Fix BlueROV IMU bridge for ArduSub SITL

examples/dave_demos/launch/dave_robot.launch.py

@@ -1,3 +1,5 @@
+import xml.etree.ElementTree as ET
+
 from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
@@ -42,8 +44,13 @@ def launch_setup(context, *args, **kwargs):
         world_filepath = PathJoinSubstitution(
             [FindPackageShare("dave_worlds"), "worlds", world_filename]
         )
+        world_element = ET.parse(world_filepath.perform(context)).getroot().find("world")
+        if world_element is None or not world_element.get("name"):
+            raise ValueError(f"World file [{world_filename}] does not declare a world name")
+        world_entity_name = world_element.get("name")
         gz_args = [world_filepath]
     else:
+        world_entity_name = "empty"
         gz_args = [world_name]
 
     zoom_camera_value = "true" if selected_world_name == "dave_ocean_waves" else "false"
@@ -109,6 +116,7 @@ def launch_setup(context, *args, **kwargs):
             "open_virtual_joystick": open_virtual_joystick,
             "virtual_joystick_url": virtual_joystick_url,
             "ui_launch_delay": ui_launch_delay,
+            "world_name": world_entity_name,
         }.items(),
     )
 

models/dave_robot_models/config/bluerov2/robot_config.py

@@ -17,6 +17,8 @@
 
 def launch_setup(context, *args, **kwargs):
     namespace = LaunchConfiguration("namespace").perform(context)
+    world_name = LaunchConfiguration("world_name").perform(context)
+    imu_gz_topic = f"/world/{world_name}/model/{namespace}/link/base_link/" "sensor/imu_sensor/imu"
     use_ardusub = LaunchConfiguration("use_ardusub")
     use_teleop = LaunchConfiguration("use_teleop")
     use_web_joystick = LaunchConfiguration("use_web_joystick")
@@ -41,14 +43,15 @@ def launch_setup(context, *args, **kwargs):
             "nav_msgs/msg/Odometry[gz.msgs.OdometryWithCovariance"
         ),
         f"/model/{namespace}/pose@geometry_msgs/msg/PoseArray[gz.msgs.Pose_V",
-        f"/model/{namespace}/imu@sensor_msgs/msg/Imu[gz.msgs.IMU",
+        f"{imu_gz_topic}@sensor_msgs/msg/Imu[gz.msgs.IMU",
         f"/model/{namespace}/magnetometer@sensor_msgs/msg/MagneticField[gz.msgs.Magnetometer",
     ]
 
     bluerov2_bridge = Node(
         package="ros_gz_bridge",
         executable="parameter_bridge",
         arguments=bluerov2_arguments,
+        remappings=[(imu_gz_topic, f"/model/{namespace}/imu")],
         output="screen",
     )
 
@@ -59,8 +62,8 @@ def launch_setup(context, *args, **kwargs):
 
     imu_wait_cmd = (
         "while true; do "
-        f'if gz topic -l | grep -q "/model/{namespace}/imu"; then exit 0; fi; '
-        "sleep 1; "
+        f"if timeout 2 gz topic -e -t '{imu_gz_topic}' -n 1 "
+        ">/dev/null 2>&1; then exit 0; fi; "
         "done"
     )
 
@@ -331,6 +334,11 @@ def generate_launch_description():
             default_value="2.0",
             description="Delay (seconds) before launching QGC/Firefox",
         ),
+        DeclareLaunchArgument(
+            "world_name",
+            default_value="empty",
+            description="Gazebo world entity name",
+        ),
     ]
 
     return LaunchDescription(args + [OpaqueFunction(function=launch_setup)])

models/dave_robot_models/config/bluerov2_heavy/robot_config.py

@@ -17,6 +17,8 @@
 
 def launch_setup(context, *args, **kwargs):
     namespace = LaunchConfiguration("namespace").perform(context)
+    world_name = LaunchConfiguration("world_name").perform(context)
+    imu_gz_topic = f"/world/{world_name}/model/{namespace}/link/base_link/" "sensor/imu_sensor/imu"
     use_ardusub = LaunchConfiguration("use_ardusub")
     use_teleop = LaunchConfiguration("use_teleop")
     use_web_joystick = LaunchConfiguration("use_web_joystick")
@@ -39,14 +41,15 @@ def launch_setup(context, *args, **kwargs):
             "nav_msgs/msg/Odometry[gz.msgs.OdometryWithCovariance"
         ),
         f"/model/{namespace}/pose@geometry_msgs/msg/PoseArray[gz.msgs.Pose_V",
-        f"/model/{namespace}/imu@sensor_msgs/msg/Imu[gz.msgs.IMU",
+        f"{imu_gz_topic}@sensor_msgs/msg/Imu[gz.msgs.IMU",
         f"/model/{namespace}/magnetometer@sensor_msgs/msg/MagneticField[gz.msgs.Magnetometer",
     ]
 
     bluerov2_heavy_bridge = Node(
         package="ros_gz_bridge",
         executable="parameter_bridge",
         arguments=bluerov2_heavy_arguments,
+        remappings=[(imu_gz_topic, f"/model/{namespace}/imu")],
         output="screen",
     )
 
@@ -57,8 +60,8 @@ def launch_setup(context, *args, **kwargs):
 
     imu_wait_cmd = (
         "while true; do "
-        f'if gz topic -l | grep -q "/model/{namespace}/imu"; then exit 0; fi; '
-        "sleep 1; "
+        f"if timeout 2 gz topic -e -t '{imu_gz_topic}' -n 1 "
+        ">/dev/null 2>&1; then exit 0; fi; "
         "done"
     )
 
@@ -329,6 +332,11 @@ def generate_launch_description():
             default_value="2.0",
             description="Delay (seconds) before launching QGC/Firefox",
         ),
+        DeclareLaunchArgument(
+            "world_name",
+            default_value="empty",
+            description="Gazebo world entity name",
+        ),
     ]
 
     return LaunchDescription(args + [OpaqueFunction(function=launch_setup)])

models/dave_robot_models/config/bluerov2_heavy_multibeam_sonar/robot_config.py

@@ -17,6 +17,8 @@
 
 def launch_setup(context, *args, **kwargs):
     namespace = LaunchConfiguration("namespace").perform(context)
+    world_name = LaunchConfiguration("world_name").perform(context)
+    imu_gz_topic = f"/world/{world_name}/model/{namespace}/link/base_link/" "sensor/imu_sensor/imu"
     use_ardusub = LaunchConfiguration("use_ardusub")
     use_teleop = LaunchConfiguration("use_teleop")
     use_web_joystick = LaunchConfiguration("use_web_joystick")
@@ -37,7 +39,7 @@ def launch_setup(context, *args, **kwargs):
             "nav_msgs/msg/Odometry[gz.msgs.OdometryWithCovariance"
         ),
         f"/model/{namespace}/pose@geometry_msgs/msg/PoseArray[gz.msgs.Pose_V",
-        f"/model/{namespace}/imu@sensor_msgs/msg/Imu[gz.msgs.IMU",
+        f"{imu_gz_topic}@sensor_msgs/msg/Imu[gz.msgs.IMU",
         f"/model/{namespace}/magnetometer@sensor_msgs/msg/MagneticField[gz.msgs.Magnetometer",
         (
             f"/model/{namespace}/multibeam_sonar/point_cloud@"
@@ -60,6 +62,7 @@ def launch_setup(context, *args, **kwargs):
         package="ros_gz_bridge",
         executable="parameter_bridge",
         arguments=bluerov2_heavy_arguments,
+        remappings=[(imu_gz_topic, f"/model/{namespace}/imu")],
         output="screen",
     )
 
@@ -70,8 +73,8 @@ def launch_setup(context, *args, **kwargs):
 
     imu_wait_cmd = (
         "while true; do "
-        f'if gz topic -l | grep -q "/model/{namespace}/imu"; then exit 0; fi; '
-        "sleep 1; "
+        f"if timeout 2 gz topic -e -t '{imu_gz_topic}' -n 1 "
+        ">/dev/null 2>&1; then exit 0; fi; "
         "done"
     )
 
@@ -313,6 +316,11 @@ def generate_launch_description():
             default_value="2.0",
             description="Delay (seconds) before launching QGC/Firefox",
         ),
+        DeclareLaunchArgument(
+            "world_name",
+            default_value="empty",
+            description="Gazebo world entity name",
+        ),
     ]
 
     return LaunchDescription(args + [OpaqueFunction(function=launch_setup)])

models/dave_robot_models/description/bluerov2/model.sdf

@@ -45,7 +45,6 @@
         <pose>0 0 0 3.142 0 0</pose>
         <always_on>1</always_on>
         <update_rate>1000.0</update_rate>
-        <topic>/model/bluerov2/imu</topic>
       </sensor>
 
     </link>

models/dave_robot_models/description/bluerov2_heavy/model.sdf

@@ -45,7 +45,6 @@
         <pose>0 0 0 3.142 0 0</pose>
         <always_on>1</always_on>
         <update_rate>1000.0</update_rate>
-        <topic>/model/bluerov2_heavy/imu</topic>
       </sensor>
 
     </link>

models/dave_robot_models/description/bluerov2_heavy_multibeam_sonar/model.sdf

@@ -45,7 +45,6 @@
         <pose>0 0 0 3.142 0 0</pose>
         <always_on>1</always_on>
         <update_rate>1000.0</update_rate>
-        <topic>/model/bluerov2_heavy_multibeam_sonar/imu</topic>
       </sensor>
 
     </link>

models/dave_robot_models/launch/upload_robot.launch.py

@@ -33,6 +33,7 @@ def generate_launch_description():
     open_virtual_joystick = LaunchConfiguration("open_virtual_joystick")
     virtual_joystick_url = LaunchConfiguration("virtual_joystick_url")
     ui_launch_delay = LaunchConfiguration("ui_launch_delay")
+    world_name = LaunchConfiguration("world_name")
 
     args = [
         DeclareLaunchArgument(
@@ -138,6 +139,11 @@ def generate_launch_description():
             default_value="2.0",
             description="Delay (seconds) before moving the GUI camera",
         ),
+        DeclareLaunchArgument(
+            "world_name",
+            default_value="empty",
+            description="Gazebo world entity name",
+        ),
     ]
 
     description_file = PathJoinSubstitution(
@@ -221,6 +227,7 @@ def generate_launch_description():
             "open_virtual_joystick": open_virtual_joystick,
             "virtual_joystick_url": virtual_joystick_url,
             "ui_launch_delay": ui_launch_delay,
+            "world_name": world_name,
         }.items(),
     )