[mergify]

Add a rdk_realtime_mode launch argument to switch the RDK control modes of Flexiv hardware interfaces from non-real-time to real-time:

• NRT_JOINT_POSITION → RT_JOINT_POSITION
• NRT_JOINT_IMPEDANCE → RT_JOINT_IMPEDANCE
• NRT_CARTESIAN_MOTION_FORCE → RT_CARTESIAN_MOTION_FORCE

In the real-time modes, commands are streamed every control cycle with the RDK API StreamJointPosition() and StreamCartesianMotionForce(), instead of being sent with SendJointPosition() and SendCartesianMotionForce() and smoothed by the robot's motion generator. The effort interface stays in RT_JOINT_TORQUE.

Changes

• flexiv_hardware
  • Mode selection — new optional hardware parameter rdk_realtime_mode (default false). It selects the joint mode from rdk_control_mode and the Cartesian mode.
  • Motion limits are NRT only — the URDF velocity limits, max_joint_acceleration and the Cartesian motion limits apply only to the non-real-time modes. ~/set_cartesian_motion_limits is refused in RT_CARTESIAN_MOTION_FORCE.
  • Hold instead of skip — a real-time mode needs a command every cycle, so a non-finite command holds the last streamed target instead of skipping the cycle.
  • Timeliness failures — once more than 2% of real-time commands arrive late within one second (SetTimelinessFailureLimit()), the RDK only logs a warning per command and keeps tracking. The driver therefore checks reached_timeliness_failure_limit() after each streamed command:
    • It withholds motion until the controller is restarted, the same as after a fault. The robot holds its position in the real-time mode while no command arrives.
    • The status topic reports the condition: Robot missed too many 1 kHz deadlines.
    • New hardware parameter withhold_on_timeliness_failure (default true). false only logs a warning and keeps streaming, as the RDK and DRDK do.
  • Joint impedance — the joint impedance services are also enabled with rdk_control_mode:=joint_impedance in RT_JOINT_IMPEDANCE.
• Dual-robot:
  • One arm starting — starting the second arm's controller no longer switches the pair's mode again. That switch blocked the control loop for about 50 ms, interrupted the arm already running and reset the mode's settings.
  • One arm stopping — DRDK streams both robots together (StreamJointPosition()), and Stop() on one robot drops it to IDLE. So stopping one arm's controller keeps both robots in the mode: the released arm holds while the other keeps streaming, and the pair stops only when no joint controller remains.
    • An arm cannot hold without torque commands, so stopping one arm's effort controller still stops both robots, with a warning.
  • Restarting one arm — after a fault or withheld motion, restarting only one arm's controller is refused. The controllers of both robots must be restarted together; otherwise the other arm's controller would resume from a stale setpoint.
• flexiv_hardware/urdf/flexiv.urdf.xacro, flexiv.ros2_control.xacro and rizon_dual.ros2_control.xacro — new rdk_realtime_mode and withhold_on_timeliness_failure arguments.
• flexiv_bringup — rdk_realtime_mode and withhold_on_timeliness_failure added to rizon.launch.py, rizon_dual.launch.py, aico1.launch.py and aico2.launch.py, and to their *_moveit.launch.py twins so move_group builds the same robot_description.
• README.md and flexiv_hardware/README.md — new launch arguments and a Real-time control modes section.

Examples

Start the driver in a real-time mode:

# RT_JOINT_POSITION for the joint trajectory controller
ros2 launch flexiv_bringup rizon.launch.py robot_sn:=[robot_sn] rdk_realtime_mode:=true

# RT_JOINT_IMPEDANCE, with the joint impedance services
ros2 launch flexiv_bringup rizon.launch.py robot_sn:=[robot_sn] rdk_realtime_mode:=true rdk_control_mode:=joint_impedance

# RT_CARTESIAN_MOTION_FORCE
ros2 launch flexiv_bringup rizon.launch.py robot_sn:=[robot_sn] rdk_realtime_mode:=true robot_controller:=cartesian_motion_force_controller

# A robot pair on a host that cannot meet the real-time streaming, so only warn about late commands
ros2 launch flexiv_bringup rizon_dual.launch.py robot_sn_left:=[robot_sn_left] robot_sn_right:=[robot_sn_right] rdk_realtime_mode:=true withhold_on_timeliness_failure:=false

---

This is an automatic backport of pull request #134 done by [Mergify](https://mergify.com).

[mergify]

Cherry-pick of 5709191 has failed:

On branch mergify/bp/release/humble-v1.9.4/pr-134
Your branch is up to date with 'origin/release/humble-v1.9.4'.

You are currently cherry-picking commit 5709191.
  (fix conflicts and run "git cherry-pick --continue")
  (use "git cherry-pick --skip" to skip this patch)
  (use "git cherry-pick --abort" to cancel the cherry-pick operation)

Changes to be committed:
	modified:   README.md
	modified:   flexiv_bringup/launch/aico1.launch.py
	modified:   flexiv_bringup/launch/aico1_moveit.launch.py
	modified:   flexiv_bringup/launch/aico2.launch.py
	modified:   flexiv_bringup/launch/aico2_moveit.launch.py
	modified:   flexiv_bringup/launch/rizon.launch.py
	modified:   flexiv_bringup/launch/rizon_dual.launch.py
	modified:   flexiv_bringup/launch/rizon_dual_moveit.launch.py
	modified:   flexiv_bringup/launch/rizon_moveit.launch.py
	modified:   flexiv_hardware/README.md
	modified:   flexiv_hardware/include/flexiv_hardware/flexiv_dual_hardware_interface.hpp
	modified:   flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp
	modified:   flexiv_hardware/include/flexiv_hardware/robot_system_control.hpp
	modified:   flexiv_hardware/ros2_control/flexiv.ros2_control.xacro
	modified:   flexiv_hardware/ros2_control/rizon_dual.ros2_control.xacro
	modified:   flexiv_hardware/urdf/flexiv.urdf.xacro

Unmerged paths:
  (use "git add <file>..." to mark resolution)
	both modified:   flexiv_hardware/src/flexiv_dual_hardware_interface.cpp
	both modified:   flexiv_hardware/src/flexiv_hardware_interface.cpp

To fix up this pull request, you can check it out locally. See documentation: https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/reviewing-changes-in-pull-requests/checking-out-pull-requests-locally