From 97a01364f63417050f2e6ca80da143f30ac736fb Mon Sep 17 00:00:00 2001 From: "-T.K.-" Date: Tue, 25 Aug 2026 02:54:30 -0700 Subject: [PATCH] Name the sim and mock components as the real ones, and declare their safety gpio The mock and MuJoCo backends collapsed every bus into one component, so a robot was LiteHardware in simulation and LiteLeftArm + LiteRightArm on hardware. Anything keyed by component name therefore meant a different thing per backend: a controller's safety_components, the hardware_spawner, the manager's hardware_components_initial_state. The practical cost was that SafetyMonitorController, which names the real components, could not load in simulation at all -- the one place the fault-handover path can be exercised without a robot. The combined macro now emits one block per bus with the real backend's own block_name. Nothing in the vendored mujoco_ros2_control needs changing: it already iterates every block and imports each one. Mock also gains a per component carrying safety_level and safety_flags. The real drivers export those from export_unlisted_state_interface_descriptions(), which mock_components has no equivalent for; the base class parses gpio state interfaces straight from the description (hardware_component_interface.cpp:163) and GenericSystem does not override that path, so declaring them is enough. MuJoCo is deliberately left out: our MujocoSystem overrides the deprecated by-value export_state_interfaces(), which bypasses the base class's gpio handling, so a gpio there would promise interfaces that never appear. An IMU with a real component gets a matching sensor block in sim. A sim-only one has no name to match and stays nested, which is what test_imu_without_real_plugin_emits_no_sensor_component pins. combined_block_name is deleted from every cad/ros2_control.json: with the real names in use it is read by nothing. Two tests lock this in -- component names equal across sim and real, mock a subset since it cannot back an IMU, and every mock system block carrying both safety interfaces. Both were negative-controlled against the committed artifacts, since that is what they read. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01EotyWG9YmYvg5xE2v3Zkpt --- robot_assets/workflow/urdf_to_xacro.py | 64 ++++++++++++++----- robots/lite_bimanual/cad/ros2_control.json | 2 - .../xacro/lite_bimanual.ros2_control.xacro | 35 ++++++++-- .../xacro/lite_bimanual.urdf.xacro | 1 - robots/lite_biped/cad/ros2_control.json | 2 - .../xacro/lite_biped.ros2_control.xacro | 46 ++++++++++--- robots/lite_biped/xacro/lite_biped.urdf.xacro | 1 - robots/lite_biped_debug/cad/ros2_control.json | 2 - .../xacro/lite_biped_debug.ros2_control.xacro | 46 ++++++++++--- .../xacro/lite_biped_debug.urdf.xacro | 1 - robots/lite_dummy/cad/ros2_control.json | 2 - .../xacro/lite_dummy.ros2_control.xacro | 35 ++++++++-- robots/lite_dummy/xacro/lite_dummy.urdf.xacro | 1 - tests/test_ros2_artifacts.py | 62 ++++++++++++++++++ 14 files changed, 246 insertions(+), 54 deletions(-) diff --git a/robot_assets/workflow/urdf_to_xacro.py b/robot_assets/workflow/urdf_to_xacro.py index 48411a9..432d471 100644 --- a/robot_assets/workflow/urdf_to_xacro.py +++ b/robot_assets/workflow/urdf_to_xacro.py @@ -210,17 +210,23 @@ def _group_macro(robot: str, group: str, joints: list[dict], limits: dict) -> st def _combined_macro(robot: str, groups: list[dict], backends: dict, imu: dict | None) -> str: - """Return the single-block macro shared by the mock and MuJoCo backends.""" - calls = "\n".join(f' ' for g in groups) - # Only MuJoCo backs the IMU state interfaces; mock_components has no source for them. - sensor = ( - f'\n \n' - f'{_sensor_element(imu["name"], " ")}\n ' - if imu else "" - ) - return f""" -{_macro_open(f"{robot}_ros2_control_combined", ["name", SIM_ARG])} - + """Return the mock and MuJoCo macro: one block per bus, named as the real one.""" + blocks = [] + for group in groups: + # A gpio only under mock. The real backend exports these two through + # export_unlisted_state_interface_descriptions(), and MujocoSystem + # overrides the deprecated by-value export_state_interfaces(), which + # bypasses the base class's gpio handling -- declaring them there would + # promise interfaces that never appear. + safety = ( + f'\n \n' + f' \n' + f' \n' + f' \n' + f' \n' + f' ' + ) + blocks.append(f""" {backends["sim"]} @@ -229,8 +235,35 @@ def _combined_macro(robot: str, groups: list[dict], backends: dict, imu: dict | {backends["mock"]} -{calls}{sensor} - + {safety} + """) + if imu and imu.get("real_block_name"): + # The real backend gives the IMU its own sensor component, so sim matches + # the name. Only MuJoCo backs the state interfaces; mock has no source. + blocks.append( + f' \n' + f' \n' + f' \n' + f' {backends["sim"]}\n' + f' \n' + f'{_sensor_element(imu["name"], " ")}\n' + f' \n' + f' ') + elif imu: + # A sim-only IMU has no real component to match, so it stays nested in the + # first bus block rather than inventing a component name of its own. + nested = ( + f'\n \n' + f'{_sensor_element(imu["name"], " ")}\n' + f' ') + blocks[0] = blocks[0].replace("\n ", f"{nested}\n ") + body = "\n".join(blocks) + return f""" +{_macro_open(f"{robot}_ros2_control_combined", [SIM_ARG])} +{body} """ @@ -274,9 +307,9 @@ def _top_macro(robot: str, args: list[str], real_params: list[str]) -> str: return f""" -{_macro_open(f"{robot}_ros2_control", ["name", *args])} +{_macro_open(f"{robot}_ros2_control", args)} - + """ diff --git a/robots/lite_bimanual/cad/ros2_control.json b/robots/lite_bimanual/cad/ros2_control.json index 822f308..815d75c 100644 --- a/robots/lite_bimanual/cad/ros2_control.json +++ b/robots/lite_bimanual/cad/ros2_control.json @@ -22,8 +22,6 @@ "base_link": {"name": "base_link", "child": "chest"}, - "combined_block_name": "LiteHardware", - "groups": [ {"name": "left_arm", "block_name": "LiteLeftArm", "can_interface_arg": "can_interface_left"}, {"name": "right_arm", "block_name": "LiteRightArm", "can_interface_arg": "can_interface_right"} diff --git a/robots/lite_bimanual/xacro/lite_bimanual.ros2_control.xacro b/robots/lite_bimanual/xacro/lite_bimanual.ros2_control.xacro index b78dda9..209b06a 100644 --- a/robots/lite_bimanual/xacro/lite_bimanual.ros2_control.xacro +++ b/robots/lite_bimanual/xacro/lite_bimanual.ros2_control.xacro @@ -75,9 +75,12 @@ current_limit="14"/> - - - + + + mujoco_ros2_control/MujocoSystem @@ -87,7 +90,29 @@ + + + + + + + + + + + mujoco_ros2_control/MujocoSystem + + + mock_components/GenericSystem + + + + + + + + @@ -117,10 +142,10 @@ boolean per non-real backend, real hardware as the fallback when both are false. sim_mujoco wins over use_mock_hardware. --> - + lives in the combined block, backed by MujocoSystem's MJCF sensors. Real: a standalone block (real_block_name) driven by real_plugin (Yahboom WIT USB IMU on imu_port), exposing the SAME interfaces so imu_sensor_broadcaster republishes /imu/data identically -- no separate driver node.", "imu": {"name": "base_imu", "real_plugin": "humanoid_devices_witmotion/WitImuSensor", "real_block_name": "LiteBipedIMU"}, diff --git a/robots/lite_biped/xacro/lite_biped.ros2_control.xacro b/robots/lite_biped/xacro/lite_biped.ros2_control.xacro index f2c603e..e693a32 100644 --- a/robots/lite_biped/xacro/lite_biped.ros2_control.xacro +++ b/robots/lite_biped/xacro/lite_biped.ros2_control.xacro @@ -121,9 +121,12 @@ current_limit="14"/> - - - + + + mujoco_ros2_control/MujocoSystem @@ -133,8 +136,35 @@ + + + + + + + + + + + mujoco_ros2_control/MujocoSystem + + + mock_components/GenericSystem + + - + + + + + + + + + + + mujoco_ros2_control/MujocoSystem + @@ -147,8 +177,8 @@ - - + + - + lives in the combined block, backed by MujocoSystem's MJCF sensors. Real: a standalone block (real_block_name) driven by real_plugin (Yahboom WIT USB IMU on imu_port), exposing the SAME interfaces so imu_sensor_broadcaster republishes /imu/data identically -- no separate driver node.", "imu": {"name": "base_imu", "real_plugin": "humanoid_devices_witmotion/WitImuSensor", "real_block_name": "LiteBipedIMU"}, diff --git a/robots/lite_biped_debug/xacro/lite_biped_debug.ros2_control.xacro b/robots/lite_biped_debug/xacro/lite_biped_debug.ros2_control.xacro index d172c32..e49ecaf 100644 --- a/robots/lite_biped_debug/xacro/lite_biped_debug.ros2_control.xacro +++ b/robots/lite_biped_debug/xacro/lite_biped_debug.ros2_control.xacro @@ -121,9 +121,12 @@ torque_limit="5.5" current_limit="14"/> - - - + + + mujoco_ros2_control/MujocoSystem @@ -133,8 +136,35 @@ + + + + + + + + + + + mujoco_ros2_control/MujocoSystem + + + mock_components/GenericSystem + + - + + + + + + + + + + + mujoco_ros2_control/MujocoSystem + @@ -147,8 +177,8 @@ - - + + - + - - - + + + mujoco_ros2_control/MujocoSystem @@ -87,7 +90,29 @@ + + + + + + + + + + + mujoco_ros2_control/MujocoSystem + + + mock_components/GenericSystem + + + + + + + + @@ -117,10 +142,10 @@ boolean per non-real backend, real hardware as the fallback when both are false. sim_mujoco wins over use_mock_hardware. --> - + . + + The real drivers export them from + export_unlisted_state_interface_descriptions(), which mock_components has no + equivalent for, so without the gpio a safety monitor cannot be exercised off + the robot. MuJoCo is excluded on purpose: our MujocoSystem overrides the + deprecated by-value export_state_interfaces(), which bypasses the base + class's gpio handling, so declaring them there would promise interfaces that + never appear. + """ + robot = robot_dir.name + out = subprocess.run( + ["xacro", str(robot_dir / "xacro" / f"{robot}.urdf.xacro")], + capture_output=True, text=True) + assert out.returncode == 0, out.stderr + root = ET.fromstring(out.stdout) + for block in root.iter("ros2_control"): + if block.get("type") != "system": + continue + gpios = {g.get("name"): {si.get("name") for si in g.iter("state_interface")} + for g in block.iter("gpio")} + assert block.get("name") in gpios, ( + f'{robot}: mock component {block.get("name")} declares no safety gpio') + assert {"safety_level", "safety_flags"} <= gpios[block.get("name")], ( + f'{robot}: {block.get("name")} gpio is missing a safety interface') +