Skip to content

Sonar demo #39

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
woensug-choi wants to merge 5 commits into
base: ros2
Choose a base branch
from
Open

Sonar demo #39

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
648e594
add blueview meshes to robot model
hmoyen Aug 12, 2025
3c2ebdb
change sonar world
hmoyen Aug 12, 2025
aff48fa
add dave_ocean_waves_sonar_integrated.world
hmoyen Oct 29, 2025
a4f6676
keep both bluerov2 heavy and with sonar attached models
hmoyen Nov 27, 2025
8f6314f
Merge branch 'ros2' into sonar-demo
hmoyen Nov 27, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
52 changes: 37 additions & 15 deletions models/dave_robot_models/config/bluerov2_heavy/robot_config.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,23 +29,11 @@ def launch_setup(context, *args, **kwargs):
f"/model/{namespace}/pose@geometry_msgs/msg/[email protected]_V",
f"/model/{namespace}/imu@sensor_msgs/msg/[email protected]",
f"/model/{namespace}/magnetometer@sensor_msgs/msg/[email protected]",
f"/model/{namespace}/camera/image@sensor_msgs/msg/[email protected]",
f"/model/{namespace}/camera/image_raw@sensor_msgs/msg/[email protected]",
f"/model/{namespace}/camera/camera_info@sensor_msgs/msg/[email protected]",
f"/model/{namespace}/multibeam_sonar/point_cloud@sensor_msgs/msg/[email protected]",
]
)

tf_node = Node(
package="tf2_ros",
executable="static_transform_publisher",
arguments=[
"--frame-id",
"world",
"--child-frame-id",
"bluerov2_heavy/sonar_link/multibeam_sonar",
],
)

bluerov2_heavy_bridge = Node(
package="ros_gz_bridge",
executable="parameter_bridge",
Expand All @@ -62,7 +50,7 @@ def launch_setup(context, *args, **kwargs):
parameters=[mavros_file, {"use_sim_time": True}],
)

nodes = [bluerov2_heavy_bridge, mavros_node, tf_node]
nodes = [bluerov2_heavy_bridge, mavros_node]

ardusub_params = LaunchConfiguration("ardusub_params").perform(context)

Expand All @@ -76,6 +64,28 @@ def launch_setup(context, *args, **kwargs):

processes = [ardusub_process]

# ardusub_manager_file = LaunchConfiguration("ardusub_manager_file")

# # Include the ardusub_manager launch file
# ardusub_manager = IncludeLaunchDescription(
# PythonLaunchDescriptionSource(
# [
# PathJoinSubstitution(
# [
# FindPackageShare("ardusub_manager"),
# "launch",
# "ardusub_manager.launch.py",
# ]
# )
# ]
# ),
# launch_arguments={
# "ardusub_manager_file": ardusub_manager_file,
# }.items(),
# )

# includes = [ardusub_manager]

return nodes + processes


Expand All @@ -89,7 +99,7 @@ def generate_launch_description():
DeclareLaunchArgument(
"mavros_file",
default_value=PathJoinSubstitution(
[FindPackageShare("dave_robot_models"), "config", "mavros", "mavros.yaml"]
[FindPackageShare("dave_robot_models"), "config", "ardusub", "mavros.yaml"]
),
description="Path to mavros.yaml file",
),
Expand All @@ -100,6 +110,18 @@ def generate_launch_description():
),
description="Path to ardusub.parm file",
),
DeclareLaunchArgument(
"ardusub_manager_file",
default_value=PathJoinSubstitution(
[
FindPackageShare("dave_robot_models"),
"config",
"ardusub",
"ardusub_manager.yaml",
]
),
description="Path to ardusub_manager.yaml file",
),
]

return LaunchDescription(args + [OpaqueFunction(function=launch_setup)])
94 changes: 94 additions & 0 deletions models/dave_robot_models/config/bluerov2_heavy_multibeam_sonar/ardusub.parm
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,94 @@
# These settings were copied from ardupilot/Tools/autotest/default_params/sub-6dof.parm
BARO_RND 0.02
BATT_MONITOR 4
BTN0_FUNCTION 0
BTN10_SFUNCTION 0
BTN10_FUNCTION 22
BTN10_SFUNCTION 0
BTN11_FUNCTION 42
BTN11_SFUNCTION 47
BTN12_FUNCTION 43
BTN12_SFUNCTION 46
BTN13_FUNCTION 33
BTN13_SFUNCTION 45
BTN14_FUNCTION 32
BTN14_SFUNCTION 44
BTN15_FUNCTION 0
BTN15_SFUNCTION 0
BTN1_FUNCTION 6
BTN1_SFUNCTION 0
BTN2_FUNCTION 8
BTN2_SFUNCTION 0
BTN3_FUNCTION 7
BTN3_SFUNCTION 0
BTN4_FUNCTION 4
BTN4_SFUNCTION 0
BTN5_FUNCTION 1
BTN5_SFUNCTION 0
BTN6_FUNCTION 3
BTN6_SFUNCTION 0
BTN7_FUNCTION 21
BTN7_SFUNCTION 0
BTN8_FUNCTION 48
BTN8_SFUNCTION 0
BTN9_FUNCTION 23
BTN9_SFUNCTION 0
COMPASS_OFS_X 5
COMPASS_OFS_Y 13
COMPASS_OFS_Z -18
COMPASS_OFS2_X 5
COMPASS_OFS2_Y 13
COMPASS_OFS2_Z -18
COMPASS_OFS3_X 5
COMPASS_OFS3_Y 13
COMPASS_OFS3_Z -18
INS_ACCOFFS_X 0.001
INS_ACCOFFS_Y 0.001
INS_ACCOFFS_Z 0.001
INS_ACCSCAL_X 1.001
INS_ACCSCAL_Y 1.001
INS_ACCSCAL_Z 1.001
INS_ACC2OFFS_X 0.001
INS_ACC2OFFS_Y 0.001
INS_ACC2OFFS_Z 0.001
INS_ACC2SCAL_X 1.001
INS_ACC2SCAL_Y 1.001
INS_ACC2SCAL_Z 1.001
INS_ACC3OFFS_X 0.000
INS_ACC3OFFS_Y 0.000
INS_ACC3OFFS_Z 0.000
INS_ACC3SCAL_X 1.000
INS_ACC3SCAL_Y 1.000
INS_ACC3SCAL_Z 1.000
FRAME_CONFIG 2.000

# Use EK3
EK2_ENABLE 0
EK3_ENABLE 1
AHRS_EKF_TYPE 3

# Enable external nav
# From https://ardupilot.org/copter/docs/common-vio-tracking-camera.html
GPS_TYPE 0 # Disable GPS
VISO_TYPE 1 # External vision
EK3_SRC1_POSXY 6 # External nav
EK3_SRC1_VELXY 6 # External nav
EK3_SRC1_POSZ 1 # Baro is the primary z source
EK3_SRC1_VELZ 6 # External nav z velocity influences EKF
COMPASS_USE 0 # Disable compass 1
COMPASS_USE2 0 # Disable compass 2
COMPASS_USE3 0 # Disable compass 3
EK3_SRC1_YAW 6 # External nav

# See https://github.com/clydemcqueen/bluerov2_ignition/issues/7
SIM_BARO_RND 0.01

# Store thruster parameters as backups
SERVO1_FUNCTION 33
SERVO2_FUNCTION 34
SERVO3_FUNCTION 35
SERVO4_FUNCTION 36
SERVO5_FUNCTION 37
SERVO6_FUNCTION 38
SERVO7_FUNCTION 39
SERVO8_FUNCTION 40
105 changes: 105 additions & 0 deletions models/dave_robot_models/config/bluerov2_heavy_multibeam_sonar/robot_config.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,105 @@
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
OpaqueFunction,
ExecuteProcess,
)
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def launch_setup(context, *args, **kwargs):
namespace = LaunchConfiguration("namespace").perform(context)

thruster_joints = []
for thruster in range(1, 9):
thruster_joints.append(f"/model/{namespace}/joint/thruster{thruster}_joint")

bluerov2_heavy_arguments = (
[f"{joint}/cmd_thrust@std_msgs/msg/[email protected]" for joint in thruster_joints]
+ [f"{joint}/ang_vel@std_msgs/msg/[email protected]" for joint in thruster_joints]
+ [
f"{joint}/enable_deadband@std_msgs/msg/[email protected]"
for joint in thruster_joints
]
+ [
f"/model/{namespace}/odometry@nav_msgs/msg/[email protected]",
f"/model/{namespace}/odometry_with_covariance@nav_msgs/msg/[email protected]",
f"/model/{namespace}/pose@geometry_msgs/msg/[email protected]_V",
f"/model/{namespace}/imu@sensor_msgs/msg/[email protected]",
f"/model/{namespace}/magnetometer@sensor_msgs/msg/[email protected]",
f"/model/{namespace}/camera/image@sensor_msgs/msg/[email protected]",
f"/model/{namespace}/camera/camera_info@sensor_msgs/msg/[email protected]",
f"/model/{namespace}/multibeam_sonar/point_cloud@sensor_msgs/msg/[email protected]",
]
)

tf_node = Node(
package="tf2_ros",
executable="static_transform_publisher",
arguments=[
"--frame-id",
"world",
"--child-frame-id",
"bluerov2_heavy_multibeam_sonar/sonar_link/multibeam_sonar",
],
)

bluerov2_heavy_bridge = Node(
package="ros_gz_bridge",
executable="parameter_bridge",
arguments=bluerov2_heavy_arguments,
output="screen",
)

mavros_file = LaunchConfiguration("mavros_file")

mavros_node = Node(
package="mavros",
executable="mavros_node",
output="screen",
parameters=[mavros_file, {"use_sim_time": True}],
)

nodes = [bluerov2_heavy_bridge, mavros_node, tf_node]

ardusub_params = LaunchConfiguration("ardusub_params").perform(context)

ardusub_cmd = [
"ardusub -S -w -M gazebo --defaults "
+ ardusub_params
+ " -IO --home 44.65870,-124.06556,0.0,270.0"
]

ardusub_process = ExecuteProcess(cmd=ardusub_cmd, shell=True, output="screen")

processes = [ardusub_process]

return nodes + processes


def generate_launch_description():
args = [
DeclareLaunchArgument(
"namespace",
default_value="",
description="Namespace",
),
DeclareLaunchArgument(
"mavros_file",
default_value=PathJoinSubstitution(
[FindPackageShare("dave_robot_models"), "config", "mavros", "mavros.yaml"]
),
description="Path to mavros.yaml file",
),
DeclareLaunchArgument(
"ardusub_params",
default_value=PathJoinSubstitution(
[FindPackageShare("dave_robot_models"), "config", "bluerov2", "ardusub.parm"]
),
description="Path to ardusub.parm file",
),
]

return LaunchDescription(args + [OpaqueFunction(function=launch_setup)])
2 changes: 1 addition & 1 deletion models/dave_robot_models/description/bluerov2_heavy/model.config
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,4 +16,4 @@
<description>
Blue Robotics BlueROV2 Heavy configuration model
</description>
</model>
</model>
68 changes: 1 addition & 67 deletions models/dave_robot_models/description/bluerov2_heavy/model.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,72 +118,6 @@
<child>camera_mount_link</child>
</joint>

<link name="sonar_link">
<pose relative_to="base_link">0.35 0 -0.1 0 1.5708 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.0001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0001</iyy>
<iyz>0</iyz>
<izz>0.0001</izz>
</inertia>
</inertial>
<sensor name="multibeam_sonar" type="custom" gz:type="multibeam_sonar">
<always_on>true</always_on>
<update_rate>30.0</update_rate>
<topic>/model/bluerov2_heavy/multibeam_sonar</topic>
<visualize>true</visualize>
<gz:multibeam_sonar>
<ray degrees="false">
<scan>
<horizontal>
<beams>512</beams>
<min_angle>-1.13447</min_angle>
<max_angle>1.13447</max_angle>
</horizontal>
<vertical>
<rays>300</rays>
<min_angle>-0.10472</min_angle>
<max_angle>0.10472</max_angle>
</vertical>
</scan>
<range>
<min>0.1</min>
<max>10.0</max>
</range>
</ray>
<spec>
<!-- Sonar properties -->
<verticalFOV>12</verticalFOV>
<sonarFreq>900e3</sonarFreq>
<bandwidth>29.9e3</bandwidth>
<soundSpeed>1500</soundSpeed>
<sourceLevel>220</sourceLevel>
<maxDistance>10</maxDistance>
<raySkips>10</raySkips>
<sensorGain>0.02</sensorGain>
<blazingSonarImage>true</blazingSonarImage>
<writeLog>false</writeLog>
<debugFlag>false</debugFlag>
<writeFrameInterval>5</writeFrameInterval>
<!-- ROS publication topics -->
<pointCloudTopicName>point_cloud</pointCloudTopicName>
<sonarImageRawTopicName>sonar_image_raw</sonarImageRawTopicName>
<sonarImageTopicName>sonar_image</sonarImageTopicName>
<frameName>forward_sonar_optical_link</frameName>
</spec>
</gz:multibeam_sonar>
</sensor>
</link>

<joint name="sonar_joint" type="fixed">
<parent>base_link</parent>
<child>sonar_link</child>
</joint>

<!-- Hydrodynamics plugin -->
<plugin
filename="gz-sim-hydrodynamics-system"
Expand Down Expand Up @@ -675,4 +609,4 @@
</plugin>

</model>
</sdf>
</sdf>
19 changes: 19 additions & 0 deletions models/dave_robot_models/description/bluerov2_heavy_multibeam_sonar/model.config
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
<?xml version="1.0"?>

<model>
<name>BlueROV2 Heavy With Sonar</name>
<version>1.0</version>
<sdf version="1.7">model.sdf</sdf>

<author>
<name>Evan Palmer</name>
<email>[email protected]</email>
</author>

<maintainer>Clyde McQueen</maintainer>
<maintainer email="[email protected]">Rakesh Vivekanandan</maintainer>

<description>
Blue Robotics BlueROV2 Heavy (with Sonar) configuration model
</description>
</model>
Loading