Task #211
Updated by Sanghoon Lee about 2 months ago
This is a marker publisher for 2F sensor data visualization named xela_taxel_viz_2f.
I plan to develop a visualization app that subscribes to the sensor data topic provided by xela_server2_2f and publishes visualized markers. Using the sensor data's force value (if unavailable, the taxel value will be used) and sensor model (frame_id) information, I will convert the x, y, and z values into markers and visualize them on a grid-based sensor base. As an additional feature, I will link it with the xela_model information to display the corresponding values as markers at each sensor location in the visualized model.
This app will publish data to the Marker topic, allowing RViz and other ROS apps to easily subscribe to this marker topic for visualization.
In the first step, xela_taxel_viz_2f supports three target models:
- uSPr2F,
- uSPrDS,
- uSPrHE35
These supported models can be selected as parameters during runtime, using pre-prepared profiles for each model.
----------------
!{width:800px}clipboard-202602071740-fhjcv.png!
--------------
h1. xela_taxel_viz_2f
h2. Goals
* Visualize @/x_taxel_2f@ (@xela_taxel_msgs/XTaxelSensorTArray@) in RViz2 as two 4x6 grids.
* Use a fixed background grid; encode force magnitude with circle size and color.
* Encode force direction with arrows whose length scales with the same magnitude.
* Apply right-module 180-degree rotation mapping (row + column flip) in grid mode (configurable).
h2. Scope
* 2D grid visualization and optional URDF-based visualization using taxel frame IDs.
* Optional grid overlay in URDF mode.
* Designed for ROBOTIQ 2F-140 fingertip taxels.
h2. Inputs
* Topic: @/x_taxel_2f@
* Message: @xela_taxel_msgs/XTaxelSensorTArray@
** @x_modules[i].forces[j]@ provides force vector (x, y, z).
** @x_modules[i].frame_ids@ can be used for debug labels.
* In URDF mode, @frame_ids@ drive per-taxel marker frames.
** If a frame_id ends with @_joint@, it is converted to the corresponding @_link@.
h2. Outputs
* Topic: @/x_taxel_2f/markers@
* Message: @visualization_msgs/MarkerArray@
h2. Visualization Style
* Grid cell background: fixed color (no per-cell color).
* Optional faint grid lines between cells.
* Per-taxel circle: size and color reflect force magnitude.
* Per-taxel arrow: direction reflects force vector; length matches magnitude scale.
* URDF mode places circles/arrows at the taxel frame origin.
h2. Grid Layout
* Rows: 4, Cols: 6
* Indexing (left module): row-major, left-to-right, top-to-bottom
** Row 1: 1..6
** Row 2: 7..12
** Row 3: 13..18
** Row 4: 19..24
h3. Left/Right Module Mapping
* Grid mode:
** Left module uses base grid mapping.
** Right module uses 180-degree rotation (row + column flip):
** @rowR = (rows - 1) - rowL@
** @colR = (cols - 1) - colL@
* Example mapping:
** L1 -> R24, L2 -> R23, ... L6 -> R19
** L7 -> R18, L8 -> R17, ... L12 -> R13
** L13 -> R12, L14 -> R11, ... L18 -> R7
** L19 -> R6, L20 -> R5, ... L24 -> R1
* URDF mode:
** No index remapping; @x_modules[].frame_ids[]@ order is used as-is.
h2. Force Mapping
* Magnitude: @|F| = sqrt(Fx^2 + Fy^2 + Fz^2)@ (default)
* Axis-normalized magnitude (recommended for sensitivity ranges):
** @nx = Fx / xy_force_range@, @ny = Fy / xy_force_range@
** @nz = max(Fz, 0) / z_force_range@
** @normalized = clamp(sqrt(nx^2 + ny^2 + nz^2), 0..1)@
* Direction:
** Arrow direction uses @(Fx, Fy)@ projected onto the grid plane.
** If @(Fx, Fy)@ is near zero, arrow length becomes zero (or skip arrow).
* Scale reference: 0.1 gf = @0.000980665 N@
h2. Baseline (Zero-Point) Handling
* After node start, collect 1-2 seconds of samples per taxel.
* Compute baseline for both @forces@ and @taxels@.
* Visualization uses @current - baseline@ for forces.
* While baseline is collecting, render zero magnitude.
h2. Node Architecture
* Node name: @xela_taxel_viz_2f@
* Subscribes to @/x_taxel_2f@
* Publishes @/x_taxel_2f/markers@
* Marker types:
** Grid tiles: @CUBE@
** Circles: @CYLINDER@ or @SPHERE@
** Arrows: @ARROW@
** Optional grid overlay in URDF mode
h3. Marker ID Scheme
Use stable IDs to avoid flicker:
* @module_offset = module_index * 1000@
* @grid_id = module_offset + 0 + idx@
* @circle_id = module_offset + 100 + idx@
* @arrow_id = module_offset + 200 + idx@
h3. Update Strategy
* Grid tiles are static; publish once at startup and republish if parameters change.
* Circles and arrows update on each incoming message.
h2. Parameters (draft defaults)
* @frame_id@: @x_taxel_viz@
* @viz_mode@: @grid@ | @urdf@
* @overlay_grid_in_urdf@: @false@
* @grid_rows@: @4@
* @grid_cols@: @6@
* @cell_size@: @0.015@
* @module_gap@: @0.04@
* @left_module_index@: @0@
* @right_module_index@: @1@
* @row_flip_right@: @true@
* @col_flip_right@: @true@
* @baseline_duration_sec@: @2.0@
* @use_axis_normalization@: @true@
* @xy_force_range@: @0.8@
* @z_force_range@: @14.0@
* @max_force@: @0.02@ (clamp)
* @use_cell_scale@: @true@
* @circle_area_scale@: @2.0@ (max circle area = cell area * scale)
* @arrow_length_scale@: @2.0@ (max arrow length = cell_size * scale)
* @use_fz_only@: @false@ (if true, use |Fz| for magnitude)
* @use_xy_direction@: @true@
* @grid_color@: @#e8e3dc@
* @grid_lines_enabled@: @true@
* @grid_line_width@: @0.001@
* @grid_line_alpha@: @0.2@
* @grid_line_color@: @[0.2, 0.2, 0.2]@
* @circle_colormap@: @blue_to_red@
* @left_force_x_sign@: @1.0@
* @left_force_y_sign@: @-1.0@
* @right_force_x_sign@: @-1.0@
* @right_force_y_sign@: @1.0@
* @urdf_left_force_x_sign@: @-1.0@
* @urdf_left_force_y_sign@: @1.0@
* @urdf_right_force_x_sign@: @-1.0@
* @urdf_right_force_y_sign@: @1.0@
h2. URDF Mode Requirements
* Launch @robot_state_publisher@ with @description/xela_uSPr2F_2_modules.xacro@.
* Launch @xela_taxel_joint_state_publisher@ to publish @/joint_states@ for taxel joints.
* @x_modules[].frame_ids[]@ must match URDF joint/link names.
* Direction signs are controlled by @urdf_left_force_*@ and @urdf_right_force_*@.
h2. Data Handling Rules
* If @forces.size()@ < @grid_rows * grid_cols@, fill remaining with zeros.
* If @forces.size()@ > @grid_rows * grid_cols@, ignore extras.
* If @right_module_index@ is out of range, only left module is shown.
h2. RViz2 Setup
* Add @MarkerArray@ display for @/x_taxel_2f/markers@.
* Set Fixed Frame to @frame_id@.
h2. Validation Plan
* Unit test: mapping index -> (row, col) for left/right.
* Playback test: feed known forces and verify circle size/color and arrow direction.
* Visual check: confirm 180-degree mapping (L1 aligns with R24).
---------------
h1. xela_taxel_viz_2f
RViz2 visualization for @/x_taxel_2f@ (@xela_taxel_msgs/XTaxelSensorTArray@).
Supports a 2x module 4x6 grid mode and a URDF mode that uses taxel frames.
h2. Build
<pre><code>
cd ~/xela_robotics/02_dev_ws
colcon build --packages-select xela_taxel_viz_2f
</code></pre>
If you modify @xela_models@ xacros, rebuild that package too:
<pre><code>
colcon build --packages-select xela_models
</code></pre>
h2. Run (Grid mode)
<pre><code>
source ~/xela_robotics/02_dev_ws/install/setup.bash
ros2 launch xela_taxel_viz_2f xela_taxel_viz_2f.launch.py viz_mode:=grid
</code></pre>
h2. Run (URDF mode)
<pre><code>
source ~/xela_robotics/02_dev_ws/install/setup.bash
ros2 launch xela_taxel_viz_2f xela_taxel_viz_2f.launch.py viz_mode:=urdf model_name:=uSPr2F
</code></pre>
URDF mode also launches:
* @robot_state_publisher@
* @ros2_control_node@
* @xela_taxel_joint_state_publisher@ (via spawner)
h2. Launch arguments
* @viz_mode@: @grid@ or @urdf@
* @model_name@: model name used for @description/xela_<model>_2_modules.xacro@ and device profile selection
* URDF mode uses @config/models/<model>/ros2_controllers.yaml@ for ros2_control.
* @overlay_grid_in_urdf@: show grid overlay in URDF mode (default: @false@)
* @style_preset@: grid style preset (@default@, @cool_steel@, @deep_navy@, @warm_graphite@)
* @params_file@: override params file
* @urdf_xacro_path@: override xacro path for URDF mode
Example:
<pre><code>
ros2 launch xela_taxel_viz_2f xela_taxel_viz_2f.launch.py viz_mode:=urdf model_name:=uSPr2F overlay_grid_in_urdf:=true
</code></pre>
h2. Grid style preset
<pre><code>
ros2 launch xela_taxel_viz_2f xela_taxel_viz_2f.launch.py viz_mode:=grid model_name:=uSPrDS style_preset:=cool_steel
</code></pre>
h2. Parameters
See @config/base.yaml@ and @config/models/<model>/<mode>.yaml@ for defaults. Key groups:
* Topics: @in_topic@, @out_topic@, @frame_id@
* Grid layout: @grid_rows@, @grid_cols@, @cell_size@, @module_gap@, @row_flip_right@, @col_flip_right@
* Grid map overrides: @grid_index_map_left@, @grid_index_map_right@, @grid_separator_cols_left@, @grid_separator_cols_right@
* Baseline and scaling: @baseline_duration_sec@, @baseline_deadband_xy@, @baseline_deadband_z@, @baseline_deadband_taxel_xy@, @baseline_deadband_taxel_z@, @use_axis_normalization@, @xy_force_range@, @z_force_range@
* Marker style: @circle_*@, @arrow_*@, @grid_*@, @color_*@
* Marker timestamps: @marker_stamp_mode@ (@keep@, @now@, @zero@)
* Marker visibility floor: @min_marker_scale@ (avoids RViz scale=0 warnings)
* Marker timestamp offset: @marker_time_offset_sec@ (negative values reduce TF extrapolation warnings)
* Direction signs (grid): @left_force_*@, @right_force_*@
* Direction signs (URDF): @urdf_left_force_*@, @urdf_right_force_*@
* Model overrides: @config/models/<model>/grid.yaml@ and @config/models/<model>/urdf.yaml@
* Legacy config (archived): @config/legacy/xela_taxel_viz_2f.yaml@
h2. RViz2
<pre><code>
rviz2 -d ~/xela_robotics/02_dev_ws/src/xela_apps/xela_taxel_viz_2f/config/xeal_taxel_viz_2f.rviz
</code></pre>
h2. Troubleshooting: URDF mode uses wrong joint profile
If @/joint_states@ shows the wrong model (e.g., @uSPa46@ when launching @model_name:=uSPrHE35@):
# Confirm controller params:
<pre><code>
ros2 param get /controller_manager xela_taxel_joint_state_publisher.ros__parameters.device_profile
ros2 param get /controller_manager xela_taxel_joint_state_publisher.ros__parameters.config_yaml
</code></pre>
# Confirm the controller log shows the loaded profile and file:
<pre><code>
Profile '<model>' loaded keep_joints file '<...>/<model>_joint.yaml'
</code></pre>
h2. Data source behavior
* @xela_server2_2f@: when @calibrated@ is missing, @forces@ is left empty and only @taxels@ are populated.
* @xela_taxel_viz_2f@: when @forces@ is empty, it falls back to @taxels@ for visualization.
* Axis ranges follow the project FSD/Design documents.
---------------
{{video("xela_viz_2f_n_sim_server.mp4",800, 400)}} {{video("xela_viz_2f_n_sim_server.mp4", 800)}}