From e3a2baaa9ccf096c001765f07c920718f2f9fdf2 Mon Sep 17 00:00:00 2001 From: yeseorizi <172019512+yeseorizi@users.noreply.github.com> Date: Thu, 10 Sep 2026 22:42:03 +0900 Subject: [PATCH] fix: preserve BlueROV IMU bridge and SITL input Signed-off-by: yeseorizi <172019512+yeseorizi@users.noreply.github.com> --- examples/dave_demos/launch/dave_robot.launch.py | 8 ++++++++ .../config/bluerov2/robot_config.py | 14 +++++++++++--- .../config/bluerov2_heavy/robot_config.py | 14 +++++++++++--- .../bluerov2_heavy_multibeam_sonar/robot_config.py | 14 +++++++++++--- .../description/bluerov2/model.sdf | 1 - .../description/bluerov2_heavy/model.sdf | 1 - .../bluerov2_heavy_multibeam_sonar/model.sdf | 1 - .../launch/upload_robot.launch.py | 7 +++++++ 8 files changed, 48 insertions(+), 12 deletions(-) diff --git a/examples/dave_demos/launch/dave_robot.launch.py b/examples/dave_demos/launch/dave_robot.launch.py index 87004716..ccc2e5c3 100644 --- a/examples/dave_demos/launch/dave_robot.launch.py +++ b/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(), ) diff --git a/models/dave_robot_models/config/bluerov2/robot_config.py b/models/dave_robot_models/config/bluerov2/robot_config.py index 210df5ff..8de7df10 100644 --- a/models/dave_robot_models/config/bluerov2/robot_config.py +++ b/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,7 +43,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", ] @@ -49,6 +51,7 @@ def launch_setup(context, *args, **kwargs): 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)]) diff --git a/models/dave_robot_models/config/bluerov2_heavy/robot_config.py b/models/dave_robot_models/config/bluerov2_heavy/robot_config.py index 6a044ba3..5878464e 100644 --- a/models/dave_robot_models/config/bluerov2_heavy/robot_config.py +++ b/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,7 +41,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", ] @@ -47,6 +49,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", ) @@ -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)]) diff --git a/models/dave_robot_models/config/bluerov2_heavy_multibeam_sonar/robot_config.py b/models/dave_robot_models/config/bluerov2_heavy_multibeam_sonar/robot_config.py index 5ef40a36..a9ace8e0 100644 --- a/models/dave_robot_models/config/bluerov2_heavy_multibeam_sonar/robot_config.py +++ b/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)]) diff --git a/models/dave_robot_models/description/bluerov2/model.sdf b/models/dave_robot_models/description/bluerov2/model.sdf index 6fdc9a98..1a3a9b2e 100644 --- a/models/dave_robot_models/description/bluerov2/model.sdf +++ b/models/dave_robot_models/description/bluerov2/model.sdf @@ -45,7 +45,6 @@ 0 0 0 3.142 0 0 1 1000.0 - /model/bluerov2/imu diff --git a/models/dave_robot_models/description/bluerov2_heavy/model.sdf b/models/dave_robot_models/description/bluerov2_heavy/model.sdf index fe221776..562fbcc5 100644 --- a/models/dave_robot_models/description/bluerov2_heavy/model.sdf +++ b/models/dave_robot_models/description/bluerov2_heavy/model.sdf @@ -45,7 +45,6 @@ 0 0 0 3.142 0 0 1 1000.0 - /model/bluerov2_heavy/imu diff --git a/models/dave_robot_models/description/bluerov2_heavy_multibeam_sonar/model.sdf b/models/dave_robot_models/description/bluerov2_heavy_multibeam_sonar/model.sdf index c04edcbb..c52a4cf1 100644 --- a/models/dave_robot_models/description/bluerov2_heavy_multibeam_sonar/model.sdf +++ b/models/dave_robot_models/description/bluerov2_heavy_multibeam_sonar/model.sdf @@ -45,7 +45,6 @@ 0 0 0 3.142 0 0 1 1000.0 - /model/bluerov2_heavy_multibeam_sonar/imu diff --git a/models/dave_robot_models/launch/upload_robot.launch.py b/models/dave_robot_models/launch/upload_robot.launch.py index ea63f607..22d4801d 100755 --- a/models/dave_robot_models/launch/upload_robot.launch.py +++ b/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(), )