diff --git a/robot_assets/workflow/urdf_to_xacro.py b/robot_assets/workflow/urdf_to_xacro.py index e106db9..835d70c 100644 --- a/robot_assets/workflow/urdf_to_xacro.py +++ b/robot_assets/workflow/urdf_to_xacro.py @@ -192,7 +192,31 @@ def _real_block(robot: str, group: dict, real_plugin: str) -> str: """ -def _real_macro(robot: str, groups: list[dict], backends: dict) -> str: +def _real_imu_block(imu: dict) -> str: + """A standalone block for the real base IMU. + + Unlike the sim sensor (backed by MujocoSystem inside the combined block), + the real IMU is its own hardware component reading a USB-serial driver on + ``imu_port``. It exports the SAME state interfaces as the sim sensor, so + imu_sensor_broadcaster republishes /imu/data identically on sim and real + (no standalone driver node). Emitted only when ros2_control.json's ``imu`` + carries a ``real_plugin``. + """ + name = imu["name"] + block_name = imu.get("real_block_name", "BaseIMU") + ifaces = "\n".join(f' ' for n in _IMU_STATE_INTERFACES) + return f""" + + {imu["real_plugin"]} + ${{imu_port}} + + +{ifaces} + + """ + + +def _real_macro(robot: str, groups: list[dict], backends: dict, imu: dict | None = None) -> str: real_plugin = backends["real"] blocks = [] for group in groups: @@ -207,21 +231,42 @@ def _real_macro(robot: str, groups: list[dict], backends: dict) -> str: f" " ) blocks.append(block) + real_imu = bool(imu and imu.get("real_plugin")) + if real_imu: + blocks.append(_real_imu_block(imu)) body = "\n".join(blocks) - return f""" + # imu_port is only a macro param when a real IMU block references it. + params = "mode can_interface_left can_interface_right calibration_file" + if real_imu: + params += " imu_port" + # Keep the header comment byte-identical for non-IMU robots (only the IMU + # ones gain the "(plus the base IMU sensor)" note), so regenerating a robot + # without a real IMU produces no diff. + if real_imu: + header = ( + " ") + else: + header = ( + " ") + return f"""{header} + params="{params}"> {body} """ -def _top_macro(robot: str, name: str) -> str: +def _top_macro(robot: str, name: str, imu: dict | None = None) -> str: + real_imu = bool(imu and imu.get("real_plugin")) + imu_param = " imu_port" if real_imu else "" + imu_pass = "\n imu_port=\"${imu_port}\"" if real_imu else "" return f""" + can_interface_left can_interface_right calibration_file{imu_param}"> @@ -230,7 +275,7 @@ def _top_macro(robot: str, name: str) -> str: + calibration_file="${{calibration_file}}"{imu_pass}/> """ @@ -479,9 +524,10 @@ def build_ros2_control_xacro(robot: str, ros2_control: dict, limits: dict) -> st parts = [_joint_macro(robot, command, state)] for group in groups: parts.append(_group_macro(robot, group["name"], joints_by_group.get(group["name"], []), limits)) - parts.append(_combined_macro(robot, combined_name, active_groups, backends, ros2_control.get("imu"))) - parts.append(_real_macro(robot, groups, backends)) - parts.append(_top_macro(robot, combined_name)) + imu = ros2_control.get("imu") + parts.append(_combined_macro(robot, combined_name, active_groups, backends, imu)) + parts.append(_real_macro(robot, groups, backends, imu)) + parts.append(_top_macro(robot, combined_name, imu)) body = "\n\n".join(parts) return ( @@ -514,6 +560,8 @@ def build_assembly_xacro(robot: str, ros2_control: dict | None, package: str = D args = ros2_control.get("args", {}) arg_decls = "\n".join(f' ' for k, v in args.items()) name_attr = ros2_control.get("combined_block_name", f"{robot}_system") + imu = ros2_control.get("imu") + imu_pass = '\n imu_port="$(arg imu_port)"' if (imu and imu.get("real_plugin")) else "" return f""" {_banner(robot)} @@ -531,7 +579,7 @@ def build_assembly_xacro(robot: str, ros2_control: dict | None, package: str = D mode="$(arg mode)" can_interface_left="$(arg can_interface_left)" can_interface_right="$(arg can_interface_right)" - calibration_file="$(arg calibration_file)"/> + calibration_file="$(arg calibration_file)"{imu_pass}/> """ diff --git a/robots/lite_biped/cad/ros2_control.json b/robots/lite_biped/cad/ros2_control.json index ea5e76d..3884d35 100644 --- a/robots/lite_biped/cad/ros2_control.json +++ b/robots/lite_biped/cad/ros2_control.json @@ -18,14 +18,16 @@ "mode": "legs", "can_interface_left": "can2", "can_interface_right": "can3", - "calibration_file": "" + "calibration_file": "", + "imu_port": "/dev/ttyUSB0" }, "base_link": {"name": "base_link", "child": "pelvis"}, "combined_block_name": "LiteBipedHardware", - "imu": {"name": "base_imu"}, + "_comment_imu": "Sim/mock: the base_imu 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_imu_hardware/YahboomImuSensor", "real_block_name": "LiteBipedIMU"}, "groups": [ {"name": "left_leg", "block_name": "LiteBipedLeftLeg", "can_interface_arg": "can_interface_left"}, diff --git a/robots/lite_biped/xacro/lite_biped.ros2_control.xacro b/robots/lite_biped/xacro/lite_biped.ros2_control.xacro index b195564..73ca457 100644 --- a/robots/lite_biped/xacro/lite_biped.ros2_control.xacro +++ b/robots/lite_biped/xacro/lite_biped.ros2_control.xacro @@ -121,10 +121,11 @@ - + + params="mode can_interface_left can_interface_right calibration_file imu_port"> humanoid_devices_robstride/RobstrideSystem @@ -141,13 +142,31 @@ + + + humanoid_devices_imu_hardware/YahboomImuSensor + ${imu_port} + + + + + + + + + + + + + + + can_interface_left can_interface_right calibration_file imu_port"> @@ -156,7 +175,8 @@ + calibration_file="${calibration_file}" + imu_port="${imu_port}"/> diff --git a/robots/lite_biped/xacro/lite_biped.urdf.xacro b/robots/lite_biped/xacro/lite_biped.urdf.xacro index 10e7eff..e3537a4 100644 --- a/robots/lite_biped/xacro/lite_biped.urdf.xacro +++ b/robots/lite_biped/xacro/lite_biped.urdf.xacro @@ -7,6 +7,7 @@ + @@ -20,5 +21,6 @@ mode="$(arg mode)" can_interface_left="$(arg can_interface_left)" can_interface_right="$(arg can_interface_right)" - calibration_file="$(arg calibration_file)"/> + calibration_file="$(arg calibration_file)" + imu_port="$(arg imu_port)"/>