From 78bb19655dbd85cb4e288d35da9c4bcc49d20e35 Mon Sep 17 00:00:00 2001 From: Trygve Lunheim Date: Fri, 7 Aug 2026 16:05:16 +0200 Subject: [PATCH 1/5] added imu message output to driver --- config/imu_filter_params.yaml | 11 +++ launch/sonar_with_imu_orientation.launch.py | 56 ++++++++++++ waterlinked_sonar_3d15/sonar_node.py | 95 ++++++++++++++++++++- 3 files changed, 160 insertions(+), 2 deletions(-) create mode 100644 config/imu_filter_params.yaml create mode 100644 launch/sonar_with_imu_orientation.launch.py diff --git a/config/imu_filter_params.yaml b/config/imu_filter_params.yaml new file mode 100644 index 0000000..5b8c181 --- /dev/null +++ b/config/imu_filter_params.yaml @@ -0,0 +1,11 @@ +imu_filter: + ros__parameters: + use_mag: false + publish_tf: true + reverse_tf: false + fixed_frame: world + world_frame: enu + gain: 0.1 + zeta: 0.0 + remove_gravity_vector: false + constant_dt: 0.0 \ No newline at end of file diff --git a/launch/sonar_with_imu_orientation.launch.py b/launch/sonar_with_imu_orientation.launch.py new file mode 100644 index 0000000..8089c53 --- /dev/null +++ b/launch/sonar_with_imu_orientation.launch.py @@ -0,0 +1,56 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +CONFIG_FILE = 'pinchy_params.yaml' +IMU_FILTER_CONFIG_FILE = 'imu_filter_params.yaml' + + +def generate_launch_description(): + pkg_share = get_package_share_directory('waterlinked_sonar_3d15') + default_params_file = os.path.join(pkg_share, 'config', CONFIG_FILE) + default_filter_params_file = os.path.join(pkg_share, 'config', IMU_FILTER_CONFIG_FILE) + + return LaunchDescription([ + DeclareLaunchArgument( + 'params_file', + default_value=default_params_file, + description='Full path to the sonar parameter YAML file', + ), + DeclareLaunchArgument( + 'imu_filter_params_file', + default_value=default_filter_params_file, + description='Full path to the IMU filter parameter YAML file', + ), + DeclareLaunchArgument( + 'namespace', + default_value='', + description='Node namespace', + ), + Node( + package='waterlinked_sonar_3d15', + executable='sonar_node', + name='sonar_node', + namespace=LaunchConfiguration('namespace'), + parameters=[LaunchConfiguration('params_file')], + output='screen', + emulate_tty=True, + ), + Node( + package='imu_filter_madgwick', + executable='imu_filter_madgwick_node', + name='imu_filter', + namespace=LaunchConfiguration('namespace'), + parameters=[LaunchConfiguration('imu_filter_params_file')], + remappings=[ + ('imu/data_raw', 'sonar_node/imu'), + ('imu/data', 'imu/data'), + ], + output='screen', + emulate_tty=True, + ), + ]) \ No newline at end of file diff --git a/waterlinked_sonar_3d15/sonar_node.py b/waterlinked_sonar_3d15/sonar_node.py index c9d2fad..6d9da3e 100644 --- a/waterlinked_sonar_3d15/sonar_node.py +++ b/waterlinked_sonar_3d15/sonar_node.py @@ -20,13 +20,15 @@ from rclpy.node import Node from rclpy.parameter import Parameter from rcl_interfaces.msg import ParameterDescriptor, ParameterType, SetParametersResult -from sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField +from sensor_msgs.msg import CameraInfo, Image, Imu, PointCloud2, PointField from std_msgs.msg import Header from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue import wlsonar import wlsonar.range_image_protocol as rip +RIP_IMU_BATCH_TYPE = getattr(rip, 'ImuBatch', None) + class SonarNode(Node): """Water Linked Sonar 3D-15 driver node.""" @@ -46,6 +48,7 @@ def __init__(self): self._stats_udp_packets = 0 self._stats_range_images = 0 self._stats_bitmap_images = 0 + self._stats_imu_batches = 0 self._stats_unknown_packets = 0 self._stats_decode_errors = 0 self._stats_timeouts = 0 @@ -64,6 +67,9 @@ def __init__(self): self._pub_camera_info = self.create_publisher( CameraInfo, self.get_parameter('topic_camera_info').get_parameter_value().string_value, 10) + self._pub_imu = self.create_publisher( + Imu, + self.get_parameter('topic_imu').get_parameter_value().string_value, 10) self._pub_diagnostics = self.create_publisher(DiagnosticArray, '/diagnostics', 10) diag_period = self.get_parameter('diagnostics_period').get_parameter_value().double_value @@ -119,6 +125,9 @@ def _declare_parameters(self): self.declare_parameter('diagnostics_period', 5.0, ParameterDescriptor( type=ParameterType.PARAMETER_DOUBLE, description='Period in seconds between diagnostic queries')) + self.declare_parameter('imu_output_enabled', True, ParameterDescriptor( + type=ParameterType.PARAMETER_BOOL, + description='Enable IMU batch output from sonar (requires firmware >= 1.8.0)')) self.declare_parameter('topic_point_cloud', '~/point_cloud', ParameterDescriptor( type=ParameterType.PARAMETER_STRING, @@ -132,6 +141,9 @@ def _declare_parameters(self): self.declare_parameter('topic_camera_info', '~/camera_info', ParameterDescriptor( type=ParameterType.PARAMETER_STRING, description='Topic name for CameraInfo output')) + self.declare_parameter('topic_imu', '~/imu', ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description='Topic name for IMU output (sensor_msgs/Imu)')) def _on_parameter_change(self, params: list[Parameter]) -> SetParametersResult: for param in params: @@ -157,6 +169,10 @@ def _on_parameter_change(self, params: list[Parameter]) -> SetParametersResult: rmax = param.value self._sonar.set_range(rmin, rmax) self.get_logger().info(f'Range set to [{rmin}, {rmax}] m') + elif param.name == 'imu_output_enabled' and self._sonar: + if self._set_imu_output_enabled(param.value): + self.get_logger().info( + f'IMU output {"enabled" if param.value else "disabled"}') except wlsonar.VersionException as e: self.get_logger().warn(str(e)) except Exception as e: @@ -198,6 +214,7 @@ def _apply_initial_configuration(self): rmin = self.get_parameter('range_min').get_parameter_value().double_value rmax = self.get_parameter('range_max').get_parameter_value().double_value udp_mode = self.get_parameter('udp_mode').get_parameter_value().string_value + imu_output_enabled = self.get_parameter('imu_output_enabled').get_parameter_value().bool_value try: self._sonar.set_speed_of_sound(sos) @@ -252,6 +269,10 @@ def _apply_initial_configuration(self): except Exception as e: self.get_logger().error(f'Could not configure UDP output: {e}') + if self._set_imu_output_enabled(imu_output_enabled): + self.get_logger().info( + f'IMU batch output: {"enabled" if imu_output_enabled else "disabled"}') + # ────────────────────────────────────────────────────────────────────── # UDP receiver # ────────────────────────────────────────────────────────────────────── @@ -363,6 +384,15 @@ def _udp_receive_loop(self): f'seq={msg.header.sequence_id}') self._publish_camera_info(msg, header) self._publish_intensity_image(msg, header) + elif RIP_IMU_BATCH_TYPE is not None and isinstance(msg, rip.ImuBatch): + with self._lock: + self._stats_imu_batches += 1 + imu_count = self._stats_imu_batches + if imu_count <= 3: + self.get_logger().info( + f'ImuBatch: samples={msg.samples}, ' + f'batch_seq={msg.batch_sequence_id}') + self._publish_imu_batch(msg, frame_id) # ────────────────────────────────────────────────────────────────────── # Publishers @@ -475,6 +505,63 @@ def _publish_camera_info(self, msg, header: Header): self._pub_camera_info.publish(ci) + def _publish_imu_batch(self, msg, frame_id: str): + if self._pub_imu.get_subscription_count() == 0: + return + + if len(msg.timestamp) != msg.samples: + self.get_logger().warn('Invalid ImuBatch: len(timestamp) != samples') + return + if len(msg.specific_force) != msg.samples * 3: + self.get_logger().warn('Invalid ImuBatch: len(specific_force) != samples*3') + return + if len(msg.rate_of_turn) != msg.samples * 3: + self.get_logger().warn('Invalid ImuBatch: len(rate_of_turn) != samples*3') + return + + for i in range(msg.samples): + idx = i * 3 + + imu_msg = Imu() + # Timestamps are provided in seconds/nanoseconds for each sample + # TODO How to use this together with timestamp of pointcloud data for the transform. + imu_msg.header.stamp.sec = msg.timestamp[i].seconds + imu_msg.header.stamp.nanosec = msg.timestamp[i].nanos + imu_msg.header.frame_id = frame_id + + # Orientation is not provided by ImuBatch, mark as unavailable. + imu_msg.orientation_covariance[0] = -1.0 + + imu_msg.linear_acceleration.x = msg.specific_force[idx] + imu_msg.linear_acceleration.y = msg.specific_force[idx + 1] + imu_msg.linear_acceleration.z = msg.specific_force[idx + 2] + + imu_msg.angular_velocity.x = msg.rate_of_turn[idx] + imu_msg.angular_velocity.y = msg.rate_of_turn[idx + 1] + imu_msg.angular_velocity.z = msg.rate_of_turn[idx + 2] + + self._pub_imu.publish(imu_msg) + + def _set_imu_output_enabled(self, enabled: bool) -> bool: + if self._sonar is None: + return False + + setter = getattr(self._sonar, 'set_output_imu_batch_enabled', None) + if setter is None: + self.get_logger().warn( + 'wlsonar package does not expose IMU output control; skipping configuration.') + return False + + try: + setter(enabled) + return True + except wlsonar.VersionException: + self.get_logger().warn( + 'Firmware too old for IMU output setting (requires >= 1.8.0). Skipping.') + except Exception as e: + self.get_logger().warn(f'Could not set IMU output: {e}') + return False + # ────────────────────────────────────────────────────────────────────── # Heartbeat # ────────────────────────────────────────────────────────────────────── @@ -485,6 +572,7 @@ def _heartbeat_callback(self): udp_pkts = self._stats_udp_packets range_imgs = self._stats_range_images bitmap_imgs = self._stats_bitmap_images + imu_batches = self._stats_imu_batches unknown = self._stats_unknown_packets decode_err = self._stats_decode_errors timeouts = self._stats_timeouts @@ -511,12 +599,15 @@ def _heartbeat_callback(self): status.message = f'Packets received but none decoded ({unknown} unknown)' else: status.level = DiagnosticStatus.OK - status.message = f'Receiving ({range_imgs} range, {bitmap_imgs} bitmap images)' + status.message = ( + f'Receiving ({range_imgs} range, {bitmap_imgs} bitmap, {imu_batches} imu batches)' + ) status.values = [ KeyValue(key='udp_packets_total', value=str(udp_pkts)), KeyValue(key='range_images', value=str(range_imgs)), KeyValue(key='bitmap_images', value=str(bitmap_imgs)), + KeyValue(key='imu_batches', value=str(imu_batches)), KeyValue(key='unknown_packets', value=str(unknown)), KeyValue(key='decode_errors', value=str(decode_err)), KeyValue(key='timeouts', value=str(timeouts)), From e893b45dde5382ddccaeb4001e640ec378f1cc50 Mon Sep 17 00:00:00 2001 From: Trygve Lunheim Date: Mon, 10 Aug 2026 08:51:50 +0200 Subject: [PATCH 2/5] use timestamp from rip header --- ...n.launch.py => sonar_imu_orient.launch.py} | 0 waterlinked_sonar_3d15/sonar_node.py | 30 +++++++++++++++++-- 2 files changed, 28 insertions(+), 2 deletions(-) rename launch/{sonar_with_imu_orientation.launch.py => sonar_imu_orient.launch.py} (100%) diff --git a/launch/sonar_with_imu_orientation.launch.py b/launch/sonar_imu_orient.launch.py similarity index 100% rename from launch/sonar_with_imu_orientation.launch.py rename to launch/sonar_imu_orient.launch.py diff --git a/waterlinked_sonar_3d15/sonar_node.py b/waterlinked_sonar_3d15/sonar_node.py index 6d9da3e..de5257d 100644 --- a/waterlinked_sonar_3d15/sonar_node.py +++ b/waterlinked_sonar_3d15/sonar_node.py @@ -30,6 +30,31 @@ RIP_IMU_BATCH_TYPE = getattr(rip, 'ImuBatch', None) +def _proto_timestamp_to_ros_time(proto_ts, fallback_stamp): + """Convert protobuf Timestamp to ROS2 builtin time message. + + RIP message timestamps use protobuf (seconds, nanos). ROS2 expects + builtin_interfaces/msg/Time (sec, nanosec). + """ + if proto_ts is None: + return fallback_stamp + + try: + sec = int(proto_ts.seconds) + nanos = int(proto_ts.nanos) + except Exception: + return fallback_stamp + + # Normalize nanoseconds to [0, 1e9). + sec += nanos // 1_000_000_000 + nanos = nanos % 1_000_000_000 + + stamp = fallback_stamp + stamp.sec = sec + stamp.nanosec = nanos + return stamp + + class SonarNode(Node): """Water Linked Sonar 3D-15 driver node.""" @@ -356,7 +381,9 @@ def _udp_receive_loop(self): self.get_logger().warn(f'Unexpected decode error: {type(e).__name__}: {e}') continue - stamp = self.get_clock().now().to_msg() + msg_header = getattr(msg, 'header', None) + msg_timestamp = getattr(msg_header, 'timestamp', None) + stamp = _proto_timestamp_to_ros_time(msg_timestamp, self.get_clock().now().to_msg()) header = Header(stamp=stamp, frame_id=frame_id) if isinstance(msg, rip.RangeImage): @@ -524,7 +551,6 @@ def _publish_imu_batch(self, msg, frame_id: str): imu_msg = Imu() # Timestamps are provided in seconds/nanoseconds for each sample - # TODO How to use this together with timestamp of pointcloud data for the transform. imu_msg.header.stamp.sec = msg.timestamp[i].seconds imu_msg.header.stamp.nanosec = msg.timestamp[i].nanos imu_msg.header.frame_id = frame_id From 1e76c6ce47815340d3f9c4c1c0591e0535bae66a Mon Sep 17 00:00:00 2001 From: Trygve Lunheim Date: Thu, 20 Aug 2026 15:53:02 +0200 Subject: [PATCH 3/5] Update params and README --- README.md | 7 +++++-- config/default_params.yaml | 1 + config/pinchy_params.yaml | 1 + 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index ab73245..4dfbd28 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ ROS 2 driver for the [Water Linked Sonar 3D-15](https://www.waterlinked.com/3dso ## Features - **High / low frequency mode** switching (firmware >= 1.7.0) -- **PointCloud2**, **range image**, and **intensity image** publishing +- **PointCloud2**, **range image**, **intensity image** and **IMU message** publishing - Full sonar configuration via ROS parameters (speed of sound, range, salinity, UDP mode) - Runtime parameter reconfiguration - Diagnostic publishing (temperature, system status) @@ -16,7 +16,7 @@ ROS 2 driver for the [Water Linked Sonar 3D-15](https://www.waterlinked.com/3dso - ROS 2 (Humble / Jazzy / Rolling) - Python >= 3.10 -- Water Linked Sonar 3D-15 with firmware >= 1.5.1 (>= 1.7.0 for mode/salinity features) +- Water Linked Sonar 3D-15 with firmware >= 1.5.1 (>= 1.7.0 for mode/salinity features and >= 1.8.0 for IMU output) ## Installation @@ -66,6 +66,7 @@ ros2 run waterlinked_sonar_3d15 sonar_node --ros-args \ | `~/range_image` | `sensor_msgs/Image` (32FC1) | Range image as float32 distances in meters | | `~/intensity_image` | `sensor_msgs/Image` (8UC1) | Logarithmic signal strength image | | `~/camera_info` | `sensor_msgs/CameraInfo` | Sonar lens model (pinhole projection) | +| `~/imu` | `sensor_msgs/Imu` | Imu message | | `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Temperature, firmware, system status | Topic names are configurable via the `topic_*` parameters (see below). The defaults above use the `~/` prefix, which resolves relative to the node name. @@ -89,6 +90,7 @@ Topic names are configurable via the `topic_*` parameters (see below). The defau | `topic_point_cloud` | string | `~/point_cloud` | Topic name for PointCloud2 output | | `topic_range_image` | string | `~/range_image` | Topic name for range image output | | `topic_intensity_image` | string | `~/intensity_image` | Topic name for intensity image output | +| `topic_imu` | string | `~/imu` | Topic name for IMU output | | `topic_camera_info` | string | `~/camera_info` | Topic name for CameraInfo output | | `diagnostics_period` | double | `5.0` | Seconds between diagnostic queries | @@ -138,6 +140,7 @@ graph TD C -- "wlsonar.range_image_protocol.unpackb()" --> D[PointCloud2 publisher] C -- "wlsonar.range_image_protocol.unpackb()" --> E[range_image publisher] C -- "wlsonar.range_image_protocol.unpackb()" --> F[intensity publisher] + C -- "wlsonar.range_image_protocol.unpackb()" --> G[imu publisher] end ``` diff --git a/config/default_params.yaml b/config/default_params.yaml index 0068081..4ce678b 100644 --- a/config/default_params.yaml +++ b/config/default_params.yaml @@ -27,6 +27,7 @@ sonar_node: topic_point_cloud: "~/point_cloud" topic_range_image: "~/range_image" topic_intensity_image: "~/intensity_image" + topic_imu: "~/imu" topic_camera_info: "~/camera_info" # Diagnostics timer period in seconds diff --git a/config/pinchy_params.yaml b/config/pinchy_params.yaml index 083cf5d..04e9024 100644 --- a/config/pinchy_params.yaml +++ b/config/pinchy_params.yaml @@ -27,6 +27,7 @@ sonar_node: topic_point_cloud: "~/point_cloud" topic_range_image: "~/range_image" topic_intensity_image: "~/intensity_image" + topic_imu: "~/imu" topic_camera_info: "~/camera_info" # Diagnostics timer period in seconds From c9a6382d282899e9e1652a63250bc2f29b0a5d6d Mon Sep 17 00:00:00 2001 From: Trygve Lunheim Date: Fri, 21 Aug 2026 12:30:34 +0200 Subject: [PATCH 4/5] changes after copilot review --- README.md | 13 +++++++++++++ waterlinked_sonar_3d15/sonar_node.py | 10 ++++++---- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 4dfbd28..4ac3c4e 100644 --- a/README.md +++ b/README.md @@ -91,6 +91,7 @@ Topic names are configurable via the `topic_*` parameters (see below). The defau | `topic_range_image` | string | `~/range_image` | Topic name for range image output | | `topic_intensity_image` | string | `~/intensity_image` | Topic name for intensity image output | | `topic_imu` | string | `~/imu` | Topic name for IMU output | +| `imu_output_enabled` | bool | `true` | Enable IMU batch output (firmware >= 1.8.0) | | `topic_camera_info` | string | `~/camera_info` | Topic name for CameraInfo output | | `diagnostics_period` | double | `5.0` | Seconds between diagnostic queries | @@ -106,6 +107,7 @@ The following parameters can be changed while the node is running using `ros2 pa | `salinity` | `salt` / `fresh` | Requires firmware >= 1.7.0 | | `range_min` | `0.5` | Minimum imaging range (meters) | | `range_max` | `10.0` | Maximum imaging range (meters) | +| `imu_output_enabled` | `true` / `false` | Enable IMU batch output (firmware >= 1.8.0) | First, find the node name: @@ -156,6 +158,17 @@ ros2 launch waterlinked_sonar_3d15 sonar_diag.launch.py ros2 launch waterlinked_sonar_3d15 sonar_diag.launch.py params_file:=/path/to/diag_params.yaml ``` +### Launch with orientation transformation + +A launch file is provided to run the sonar with IMU data enabled together with `imu_filter_madgwick` to estimate orientation and publish +the transform to `world`, so the point cloud can be viewed rotated according to IMU input. + +```bash +ros2 launch waterlinked_sonar_3d15 sonar_imu_orient.launch.py +# or with a custom params file: +ros2 launch waterlinked_sonar_3d15 sonar_imu_orient.launch.py params_file:=/path/to/imu_filter_params.yaml +``` + ### Continuous monitoring Prints live data rates and range/intensity image timing offsets every second: diff --git a/waterlinked_sonar_3d15/sonar_node.py b/waterlinked_sonar_3d15/sonar_node.py index de5257d..8cd6395 100644 --- a/waterlinked_sonar_3d15/sonar_node.py +++ b/waterlinked_sonar_3d15/sonar_node.py @@ -195,9 +195,11 @@ def _on_parameter_change(self, params: list[Parameter]) -> SetParametersResult: self._sonar.set_range(rmin, rmax) self.get_logger().info(f'Range set to [{rmin}, {rmax}] m') elif param.name == 'imu_output_enabled' and self._sonar: - if self._set_imu_output_enabled(param.value): - self.get_logger().info( - f'IMU output {"enabled" if param.value else "disabled"}') + if not self._set_imu_output_enabled(param.value): + return SetParametersResult( + successful=False, reason='Could not apply IMU output setting') + self.get_logger().info( + f'IMU output {"enabled" if param.value else "disabled"}') except wlsonar.VersionException as e: self.get_logger().warn(str(e)) except Exception as e: @@ -620,7 +622,7 @@ def _heartbeat_callback(self): elif not receiving and elapsed > 10.0: status.level = DiagnosticStatus.WARN status.message = f'No data (timeouts: {timeouts})' - elif udp_pkts > 0 and range_imgs == 0 and bitmap_imgs == 0: + elif udp_pkts > 0 and range_imgs == 0 and bitmap_imgs == 0 and imu_batches == 0: status.level = DiagnosticStatus.WARN status.message = f'Packets received but none decoded ({unknown} unknown)' else: From e634d7bcdebb52e1e7183634f14721fca4b2b344 Mon Sep 17 00:00:00 2001 From: Trygve Lunheim Date: Mon, 24 Aug 2026 17:29:36 +0200 Subject: [PATCH 5/5] changes after tholok input --- README.md | 13 ++++--- config/default_params.yaml | 14 +++++--- config/pinchy_params.yaml | 34 ------------------- launch/sonar_3d15.launch.py | 2 +- ...nch.py => sonar_imu_orient_demo.launch.py} | 2 +- package.xml | 4 +-- setup.py | 2 +- waterlinked_sonar_3d15/sonar_node.py | 15 +++++--- 8 files changed, 33 insertions(+), 53 deletions(-) delete mode 100644 config/pinchy_params.yaml rename launch/{sonar_imu_orient.launch.py => sonar_imu_orient_demo.launch.py} (98%) diff --git a/README.md b/README.md index 4ac3c4e..52fc9f4 100644 --- a/README.md +++ b/README.md @@ -158,16 +158,21 @@ ros2 launch waterlinked_sonar_3d15 sonar_diag.launch.py ros2 launch waterlinked_sonar_3d15 sonar_diag.launch.py params_file:=/path/to/diag_params.yaml ``` -### Launch with orientation transformation +### Launch with orientation transformation (DEMO) A launch file is provided to run the sonar with IMU data enabled together with `imu_filter_madgwick` to estimate orientation and publish -the transform to `world`, so the point cloud can be viewed rotated according to IMU input. +the transform to `world`, so the point cloud can be viewed rotated according to IMU input. Dependendency on imu_filter_madgwick. To install: ```bash -ros2 launch waterlinked_sonar_3d15 sonar_imu_orient.launch.py +sudo apt install ros-humble-imu-filter-madgwick +``` +Then to run the node: +```bash +ros2 launch waterlinked_sonar_3d15 sonar_imu_orient_demo.launch.py # or with a custom params file: -ros2 launch waterlinked_sonar_3d15 sonar_imu_orient.launch.py params_file:=/path/to/imu_filter_params.yaml +ros2 launch waterlinked_sonar_3d15 sonar_imu_orient_demo.launch.py params_file:=/path/to/imu_filter_params.yaml ``` +To view transformed point cloud: start `rviz2`, then add PointCloud2 with topic /sonar_node/point_cloud and use `world` as Fixed Frame in rviz2. ### Continuous monitoring diff --git a/config/default_params.yaml b/config/default_params.yaml index 4ce678b..5b42b6b 100644 --- a/config/default_params.yaml +++ b/config/default_params.yaml @@ -1,26 +1,26 @@ sonar_node: ros__parameters: - sonar_ip: "192.168.2.190" # "192.168.194.96" + sonar_ip: "10.1.2.180" # "192.168.194.96" frame_id: "sonar_link" # Acoustics acoustics_enabled: true - speed_of_sound: 1480.0 + speed_of_sound: 0.0 # mode: "low-frequency" or "high-frequency" (requires firmware >= 1.7.0) mode: "low-frequency" # salinity: "salt" or "fresh" (requires firmware >= 1.7.0) salinity: "salt" range_min: 0.3 - range_max: 15.0 + range_max: 16.0 # UDP streaming # Use "unicast" when running in WSL2 (multicast doesn't traverse the VM boundary) udp_mode: "unicast" # Local interface IP for multicast join / unicast bind. # Set to the IP on the same subnet as the sonar on multi-homed machines. - interface_ip: "192.168.2.1" + interface_ip: "10.1.2.124" # Unicast: sonar sends UDP to this IP:port (must be reachable from sonar) - unicast_destination_ip: "192.168.2.1" + unicast_destination_ip: "10.1.2.124" unicast_destination_port: 4747 # Topic names (use ~/ prefix for node-relative, or absolute like /sonar/points) @@ -30,5 +30,9 @@ sonar_node: topic_imu: "~/imu" topic_camera_info: "~/camera_info" + # IMU output and rigid offset correction (IMU origin relative to sonar_link) + imu_frame_id: "sonar_imu_link" + imu_output_enabled: true + # Diagnostics timer period in seconds diagnostics_period: 5.0 diff --git a/config/pinchy_params.yaml b/config/pinchy_params.yaml deleted file mode 100644 index 04e9024..0000000 --- a/config/pinchy_params.yaml +++ /dev/null @@ -1,34 +0,0 @@ -sonar_node: - ros__parameters: - sonar_ip: "192.168.32.109" # "192.168.194.96" - frame_id: "sonar_link" - - # Acoustics - acoustics_enabled: true - speed_of_sound: 1480.0 - # mode: "low-frequency" or "high-frequency" (requires firmware >= 1.7.0) - mode: "low-frequency" - # salinity: "salt" or "fresh" (requires firmware >= 1.7.0) - salinity: "salt" - range_min: 0.3 - range_max: 15.0 - - # UDP streaming - # Use "unicast" when running in WSL2 (multicast doesn't traverse the VM boundary) - udp_mode: "unicast" - # Local interface IP for multicast join / unicast bind. - # Set to the IP on the same subnet as the sonar on multi-homed machines. - interface_ip: "192.168.32.190" - # Unicast: sonar sends UDP to this IP:port (must be reachable from sonar) - unicast_destination_ip: "192.168.32.190" - unicast_destination_port: 4747 - - # Topic names (use ~/ prefix for node-relative, or absolute like /sonar/points) - topic_point_cloud: "~/point_cloud" - topic_range_image: "~/range_image" - topic_intensity_image: "~/intensity_image" - topic_imu: "~/imu" - topic_camera_info: "~/camera_info" - - # Diagnostics timer period in seconds - diagnostics_period: 5.0 diff --git a/launch/sonar_3d15.launch.py b/launch/sonar_3d15.launch.py index d4fe64c..4a94b66 100644 --- a/launch/sonar_3d15.launch.py +++ b/launch/sonar_3d15.launch.py @@ -6,7 +6,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -CONFIG_FILE = 'pinchy_params.yaml' +CONFIG_FILE = 'default_params.yaml' def generate_launch_description(): pkg_share = get_package_share_directory('waterlinked_sonar_3d15') diff --git a/launch/sonar_imu_orient.launch.py b/launch/sonar_imu_orient_demo.launch.py similarity index 98% rename from launch/sonar_imu_orient.launch.py rename to launch/sonar_imu_orient_demo.launch.py index 8089c53..ff19e16 100644 --- a/launch/sonar_imu_orient.launch.py +++ b/launch/sonar_imu_orient_demo.launch.py @@ -6,7 +6,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -CONFIG_FILE = 'pinchy_params.yaml' +CONFIG_FILE = 'default_params.yaml' IMU_FILTER_CONFIG_FILE = 'imu_filter_params.yaml' diff --git a/package.xml b/package.xml index 89746bb..2a157c2 100644 --- a/package.xml +++ b/package.xml @@ -2,10 +2,10 @@ waterlinked_sonar_3d15 - 0.1.0 + 0.2.0 ROS 2 driver for the Water Linked Sonar 3D-15, built on the official wlsonar Python library. - Julian Valdez + Water Linked MIT https://github.com/smarc-project/waterlinked_sonar_3d15 diff --git a/setup.py b/setup.py index b544b23..ddcf1f7 100644 --- a/setup.py +++ b/setup.py @@ -17,7 +17,7 @@ ], install_requires=[ 'setuptools', - 'wlsonar>=0.5.0', + 'wlsonar>=0.6.0', 'numpy', ], zip_safe=True, diff --git a/waterlinked_sonar_3d15/sonar_node.py b/waterlinked_sonar_3d15/sonar_node.py index 8cd6395..1938339 100644 --- a/waterlinked_sonar_3d15/sonar_node.py +++ b/waterlinked_sonar_3d15/sonar_node.py @@ -1,4 +1,5 @@ # Copyright 2025 Julian Valdez +# Copyright 2026 Water Linked AS # # Licensed under the MIT License. @@ -119,7 +120,8 @@ def _declare_parameters(self): self.declare_parameter('acoustics_enabled', True, ParameterDescriptor( type=ParameterType.PARAMETER_BOOL, description='Enable acoustic imaging on startup')) - self.declare_parameter('speed_of_sound', 1480.0, ParameterDescriptor( + # Setting the default value of speed_of_sound to 0.0 means internal default speed of sound is used if the user does not specify a value. + self.declare_parameter('speed_of_sound', 0.0, ParameterDescriptor( type=ParameterType.PARAMETER_DOUBLE, description='Speed of sound in m/s')) self.declare_parameter('mode', 'low-frequency', ParameterDescriptor( @@ -131,7 +133,7 @@ def _declare_parameters(self): self.declare_parameter('range_min', 0.3, ParameterDescriptor( type=ParameterType.PARAMETER_DOUBLE, description='Minimum imaging range in meters')) - self.declare_parameter('range_max', 15.0, ParameterDescriptor( + self.declare_parameter('range_max', 16.0, ParameterDescriptor( type=ParameterType.PARAMETER_DOUBLE, description='Maximum imaging range in meters')) self.declare_parameter('udp_mode', 'multicast', ParameterDescriptor( @@ -153,7 +155,9 @@ def _declare_parameters(self): self.declare_parameter('imu_output_enabled', True, ParameterDescriptor( type=ParameterType.PARAMETER_BOOL, description='Enable IMU batch output from sonar (requires firmware >= 1.8.0)')) - + self.declare_parameter('imu_frame_id', 'sonar_imu_link', ParameterDescriptor( + type=ParameterType.PARAMETER_STRING, + description='TF frame ID for published IMU messages')) self.declare_parameter('topic_point_cloud', '~/point_cloud', ParameterDescriptor( type=ParameterType.PARAMETER_STRING, description='Topic name for PointCloud2 output')) @@ -421,7 +425,8 @@ def _udp_receive_loop(self): self.get_logger().info( f'ImuBatch: samples={msg.samples}, ' f'batch_seq={msg.batch_sequence_id}') - self._publish_imu_batch(msg, frame_id) + imu_frame_id = self.get_parameter('imu_frame_id').get_parameter_value().string_value + self._publish_imu_batch(msg, imu_frame_id) # ────────────────────────────────────────────────────────────────────── # Publishers @@ -535,7 +540,7 @@ def _publish_camera_info(self, msg, header: Header): self._pub_camera_info.publish(ci) def _publish_imu_batch(self, msg, frame_id: str): - if self._pub_imu.get_subscription_count() == 0: + if self._pub_imu is None or self._pub_imu.get_subscription_count() == 0: return if len(msg.timestamp) != msg.samples: