Seems to be correctly connected in mock mode, test on robot next

bdaiinstitute · Mar 6, 2025 · c8e1407 · c8e1407
1 parent a501044
commit c8e1407
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 0 deletions.
diff --git a/spot_ros2_control/config/spot_default_controllers_with_arm.yaml b/spot_ros2_control/config/spot_default_controllers_with_arm.yaml
@@ -8,6 +8,9 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
     forward_position_controller:
       type: forward_command_controller/ForwardCommandController
 
@@ -21,6 +24,11 @@ controller_manager:
       unconfigured:
         - SpotSystem
 
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu_sensor_frame"
+
 forward_position_controller:
   ros__parameters:
     joints:

diff --git a/spot_ros2_control/config/spot_default_controllers_without_arm.yaml b/spot_ros2_control/config/spot_default_controllers_without_arm.yaml
@@ -8,6 +8,9 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
     forward_position_controller:
       type: forward_command_controller/ForwardCommandController
 
@@ -21,6 +24,11 @@ controller_manager:
       unconfigured:
         - SpotSystem
 
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: "imu_sensor"
+    frame_id: "imu_sensor_frame"
+
 forward_position_controller:
   ros__parameters:
     joints:

diff --git a/spot_ros2_control/launch/spot_ros2_control.launch.py b/spot_ros2_control/launch/spot_ros2_control.launch.py
@@ -55,12 +55,19 @@ def create_controllers_config(spot_name: str, has_arm: bool) -> str:
             del config["controller_manager"]
             keys_to_namespace = ["forward_position_controller", "forward_state_controller", "spot_joint_controller"]
 
+            # update controller entries
             for key in keys_to_namespace:
                 key_joints = config[key]["ros__parameters"]["joints"]
                 config[key]["ros__parameters"]["joints"] = [f"{spot_name}/{joint}" for joint in key_joints]
                 config[f"{spot_name}/{key}"] = config[key]
                 del config[key]
 
+            # update IMU sensor entry
+            config[f"{spot_name}/imu_sensor_broadcaster"] = config["imu_sensor_broadcaster"]
+            imu_sensor_name = config["imu_sensor_broadcaster"]["ros__parameters"]["sensor_name"]
+            config["imu_sensor_broadcaster"]["ros__parameters"]["sensor_name"] = f"{spot_name}/{imu_sensor_name}"
+            del config["imu_sensor_broadcaster"]
+
         with NamedTemporaryFile(suffix=".yaml", mode="w", delete=False) as out_file:
             yaml.dump(config, out_file)
             return out_file.name
@@ -197,6 +204,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
                         "-c",
                         "controller_manager",
                         "joint_state_broadcaster",
+                        "imu_sensor_broadcaster",
                         LaunchConfiguration("robot_controller"),
                     ],
                     namespace=spot_name,