Task #194
Updated by Sanghoon Lee 2 months ago
Create a ros2_control controller, xela_taxel_joint_state_publisher, that publishes a JointState stream specifically for the XELA Taxel sensor.
This controller filters incoming JointState messages or initializes joint positions from a configured list, then publishes a unified JointState topic.
This controller is designed to run within ros2_control and be managed by the controller_manager.
As a first step, this publisher supports the Allegrohand v4 (ros2_control type) and 2F-85 (uSPr2F) models.
It is spawned by the controller manager when the robot is started, and upon startup, it publishes the JointState of the Taxel sensor of the specified model.
--------------------
h1. Create xela_taxel_joint_state_publisher
<pre>
xela_taxel_joint_state_publisher/
├── CMakeLists.txt
├── config
│ ├── list_2f_gripper_joint.yaml
│ ├── list_allegrohand_joint.yaml
│ └── xela_taxel_jsp_config.yaml
├── controller_plugins.xml
├── include
│ └── xela_taxel_joint_state_publisher
│ └── xela_taxel_joint_state_publisher.hpp
├── package.xml
└── src
└── xela_taxel_joint_state_publisher.cpp
</pre>
h2. 1. Overview
xela_taxel_joint_state_publisher is a ros2_control controller that publishes a filtered JointState stream for XELA taxel sensors. The controller filters incoming JointState messages or initializes joint positions from configured lists, then publishes a consolidated JointState topic for downstream applications. It is designed to run inside ros2_control and be managed by controller_manager.
h2. 2. Goals
- Provide a ros2_control controller that publishes taxel JointState for a selected device profile.
- Support multiple device profiles (e.g., allegrohand, 2f_gripper) from a single YAML config.
- Allow configuration of output topic, publish rate, input sources, and joint ordering.
- Operate without impacting the primary MoveIt control JointState stream.
- Be robust to missing inputs by using initial positions where available.
h2. 3. Non-Goals
- Parsing URDFs or XACROs directly (handled by upstream nodes or config files).
- Dynamically reloading device profiles at runtime (restart/reload expected).
- Publishing non-joint sensor data (force, tactile values, etc.).
h2. 4. Users and Use Cases
h3. 4.1 Users
- Robotics developers integrating XELA taxel sensors into MoveIt Pro configurations.
- Operators running robots that require a separate full-model joint state stream.
h3. 4.2 Use Cases
- Publish taxel joint states for a full robot model on @/joint_states_full@ while keeping @/joint_states@ lightweight for MoveIt.
- Publish taxel joint states for a 2-finger gripper package on @/joint_states@.
- Combine multiple taxel device profiles into one output stream.
h2. 5. Functional Requirements
h3. 5.1 Configuration
- Controller must read a YAML config file specified by @config_yaml@.
- Config supports @device_profiles@ with @keep_joints_files@ lists per profile.
- Config supports optional global parameters: @output_topic@, @publish_rate@, @preserve_input_order@, @source_list@.
- Controller accepts parameter @device_profiles@ to select one or more profiles.
h3. 5.2 Input JointState Handling
- Controller subscribes to topics listed in @source_list@.
- If @source_list@ is empty, the controller publishes using initial positions only.
- If a source topic equals @output_topic@, it is ignored to prevent loops.
h3. 5.3 Output JointState
- Output topic is @output_topic@ (default @/joint_states@).
- Output includes @name@ and @position@ arrays. @velocity@ and @effort@ are only included if present in inputs.
- Output ordering:
- If @ordered_keep_joints@ exists in config, it defines output order.
- Else if @preserve_input_order@ is true and input received, use input order filtered to keep joints.
- Else output order follows merged keep_joints list.
h3. 5.4 Initial Positions
- Controller supports @initial_positions@ per profile in config (optional).
- If no position has been received for a joint, initial position is used.
h3. 5.5 Package Path Resolution
- @config_yaml@ supports @package://<pkg>/path@ resolution using ament index.
h2. 6. Non-Functional Requirements
- ROS 2 Humble compatible.
- Real-time safe behavior: no blocking operations in @update()@.
- Robust to missing inputs and config errors (log and fail gracefully on configure).
- Maintain deterministic output order per configuration.
h2. 7. Configuration Schema
h3. 7.1 Example
<pre>
output_topic: /joint_states
publish_rate: 30.0
preserve_input_order: true
source_list:
- /joint_states_full
device_profiles:
allegrohand:
keep_joints_files:
- list_allegrohand_joint.yaml
2f_gripper:
keep_joints_files:
- list_2f_gripper_joint.yaml
</pre>
h3. 7.2 keep_joints_files
- Each file is YAML containing @keep_joints:@ followed by a list of joint names.
h2. 8. Interface (Parameters)
|_. Parameter |_. Type |_. Default |_. Description |
| config_yaml | string | "" | Path to config YAML (supports package://) |
| device_profiles | string[] | [] | Profiles to load from config |
| output_topic | string | /joint_states | Output JointState topic |
| source_list | string[] | [] | Input JointState topics to merge |
| publish_rate | double | 30.0 | Output publish frequency in Hz |
| preserve_input_order | bool | true | Preserve input order when possible |
h2. 9. Integration Requirements
- Controller must be listed under @controller_manager@ in the target ros2_control YAML.
- Controller parameters must be defined under a top-level node matching controller name.
- Add controller to @controllers_active_at_startup@ in MoveIt Pro config if required.
h2. 10. Failure Modes and Logging
- If @config_yaml@ is missing or invalid, controller configuration fails.
- If @device_profiles@ is empty or missing in config, configuration fails.
- If @keep_joints@ resolves to empty, configuration fails.
- All failures must log clear error messages.
h2. 11. Testing Plan
- Unit-style validation via:
- Launch controller with valid config and verify output topic publishes expected joints.
- Launch controller without @source_list@ and validate initial positions publish.
- Launch controller with invalid config and ensure configure fails gracefully.
- Integration validation with MoveIt Pro configuration:
- Confirm controller is listed by @ros2 control list_controllers@.
- Confirm output topic is populated with expected taxel joints.
h2. 12. Open Questions
- Should @source_list@ allow wildcard or namespace expansion?
- Should publish be suppressed until at least one input message is received?
- Do we need runtime profile switching without controller restart?
----------------------------
config/control/ur5e_x2f_85.ros2_control.yaml
{{collapse
<pre><code class="yaml">
controller_manager:
ros__parameters:
update_rate: 600 # Hz
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
io_and_status_controller:
type: ur_controllers/GPIOController
force_torque_sensor_broadcaster:
type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
joint_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
robotiq_gripper_controller:
type: position_controllers/GripperActionController
robotiq_activation_controller:
type: robotiq_controllers/RobotiqActivationController
servo_controller:
type: joint_trajectory_controller/JointTrajectoryController
joint_trajectory_admittance_controller:
type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController
velocity_force_controller:
type: velocity_force_controller/VelocityForceController
xela_taxel_joint_state_publisher:
type: xela_taxel_joint_state_publisher/XelaTaxelJointStatePublisher
io_and_status_controller:
ros__parameters:
tf_prefix: ""
force_torque_sensor_broadcaster:
ros__parameters:
sensor_name: tcp_fts_sensor
state_interface_names:
- force.x
- force.y
- force.z
- torque.x
- torque.y
- torque.z
frame_id: tool0
joint_trajectory_controller:
ros__parameters:
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
command_interfaces:
- position
state_interfaces:
- position
- velocity
command_joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
state_publish_rate: 100.0
action_monitor_rate: 20.0
allow_partial_joints_goal: false
constraints:
stopped_velocity_tolerance: 0.0
goal_time: 0.0
shoulder_pan_joint:
goal: 0.05
shoulder_lift_joint:
goal: 0.05
elbow_joint:
goal: 0.05
wrist_1_joint:
goal: 0.05
wrist_2_joint:
goal: 0.05
wrist_3_joint:
goal: 0.05
servo_controller:
ros__parameters:
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
command_interfaces:
- position
state_interfaces:
- position
- velocity
command_joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
state_publish_rate: 100.0
action_monitor_rate: 20.0
allow_partial_joints_goal: false
constraints:
stopped_velocity_tolerance: 0.0
goal_time: 0.0
shoulder_pan_joint:
goal: 0.05
shoulder_lift_joint:
goal: 0.05
elbow_joint:
goal: 0.05
wrist_1_joint:
goal: 0.05
wrist_2_joint:
goal: 0.05
wrist_3_joint:
goal: 0.05
robotiq_gripper_controller:
ros__parameters:
default: true
joint: robotiq_85_left_knuckle_joint
allow_stalling: true
# These need to be tuned for HW:
# stall_timeout: 0.05
# goal_tolerance: 0.02
robotiq_activation_controller:
ros__parameters:
default: true
joint_trajectory_admittance_controller:
ros__parameters:
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
base_frame: base_link
sensor_frame: tool0
ee_frame: grasp_link
ft_sensor_name: tcp_fts_sensor
# Joint accelerations chosen to match MoveIt configs.
stop_accelerations:
- 30.0
- 30.0
- 30.0
- 30.0
- 30.0
- 30.0
velocity_force_controller:
ros__parameters:
planning_group_name: manipulator
sensor_frame: tool0
ee_frame: grasp_link
ft_sensor_name: tcp_fts_sensor
ft_force_deadband: 2.0
ft_torque_deadband: 1.0
max_joint_velocity:
- 0.524
- 0.524
- 0.524
- 1.047
- 1.047
- 1.047
max_joint_acceleration:
- 52.4
- 52.4
- 52.4
- 52.4
- 52.4
- 52.4
max_cartesian_velocity:
- 0.25
- 0.25
- 0.25
- 1.5707
- 1.5707
- 1.5707
max_cartesian_acceleration:
- 20.0
- 20.0
- 20.0
- 40.0
- 40.0
- 40.0
xela_taxel_joint_state_publisher:
ros__parameters:
config_yaml: package://xela_taxel_joint_state_publisher/config/xela_taxel_jsp_config.yaml
device_profiles:
- 2f_gripper
output_topic: /joint_states
# source_list (optional): list of input joint_state topics to merge; omit to use defaults.
publish_rate: 30.0
preserve_input_order: true
</code></pre>
}}
config/config.yaml
{{collapse
<pre><code class="yaml">
based_on_package: "ur5e_x2f_85_common"
hardware:
simulated: true
robot_description:
urdf:
package: "ur5e_x2f_85_config1"
path: "description/ur5e_x2f_85.xacro"
srdf:
package: "ur5e_x2f_85_config1"
path: "config/moveit/ur5e_x2f_85.srdf"
urdf_params:
- name: "ur5e_x2f_85"
- prefix: ""
- use_fake_hardware: "true"
- use_pinch_links: "true"
- mock_sensor_commands: "false"
- headless_mode: "true"
- robot_ip: "0.0.0.0"
- has_tool_changer: "true"
- xela_taxels: "1"
- robotiq_gripper_closed_position: "0.65"
- robotiq_finger_joint_initial_value: "0.65"
- joint_limits_parameters_file:
package: "ur5e_x2f_85_config1"
path: "config/moveit/joint_limits.yaml"
- kinematics_parameters_file:
package: "ur_description"
path: "config/ur5e/default_kinematics.yaml"
- physical_parameters_file:
package: "ur_description"
path: "config/ur5e/physical_parameters.yaml"
- visual_parameters_file:
package: "ur_description"
path: "config/ur5e/visual_parameters.yaml"
moveit_params:
joint_group_name: "manipulator"
joint_limits:
package: "ur5e_x2f_85_config1"
path: "config/moveit/joint_limits.yaml"
objectives:
objective_library_paths:
common_objectives:
package_name: "ur5e_x2f_85_common"
relative_path: "objectives"
custom_objectives:
package_name: "ur5e_x2f_85_config1"
relative_path: "objectives"
ros2_control:
config:
package: "ur5e_x2f_85_config1"
path: "config/control/ur5e_x2f_85.ros2_control.yaml"
controllers_active_at_startup:
- "force_torque_sensor_broadcaster"
- "robotiq_gripper_controller"
- "joint_state_broadcaster"
- "xela_taxel_joint_state_publisher"
- "servo_controller"
- "io_and_status_controller"
- "robotiq_activation_controller"
controllers_inactive_at_startup:
- "joint_trajectory_controller"
- "joint_trajectory_admittance_controller"
- "velocity_force_controller"
controllers_not_managed: []
controller_shared_topics: []
</code></pre>
}}
----------------
<pre><code class="shell">
invokelee@🐙MoveIt Pro🐙:~/user_ws$ ros2 control list_controllers
force_torque_sensor_broadcaster force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster active
velocity_force_controller velocity_force_controller/VelocityForceController inactive
robotiq_activation_controller robotiq_controllers/RobotiqActivationController active
xela_taxel_joint_state_publisher xela_taxel_joint_state_publisher/XelaTaxelJointStatePublisher active
joint_state_broadcaster joint_state_broadcaster/JointStateBroadcaster active
robotiq_gripper_controller position_controllers/GripperActionController active
joint_trajectory_admittance_controller joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController inactive
servo_controller joint_trajectory_controller/JointTrajectoryController inactive
joint_trajectory_controller joint_trajectory_controller/JointTrajectoryController active
</code></pre>
<pre><code class="shell">
invokelee@🐙MoveIt Pro🐙:~/user_ws$ ros2 topic info /joint_states -v
Type: sensor_msgs/msg/JointState
Publisher count: 2
Node name: xela_taxel_joint_state_publisher
Node namespace: /
Topic type: sensor_msgs/msg/JointState
Endpoint type: PUBLISHER
GID: 01.10.e6.cb.8e.80.fa.90.a8.dc.e6.8f.00.00.ef.03.00.00.00.00.00.00.00.00
QoS profile:
Reliability: RELIABLE
History (Depth): KEEP_LAST (1)
Durability: VOLATILE
Lifespan: Infinite
Deadline: Infinite
Liveliness: AUTOMATIC
Liveliness lease duration: Infinite
</code></pre>