diff --git a/README.md b/README.md index ab73245..52fc9f4 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,8 @@ 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 | +| `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 | @@ -104,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: @@ -138,6 +142,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 ``` @@ -153,6 +158,22 @@ 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 (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. Dependendency on imu_filter_madgwick. To install: + +```bash +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_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 Prints live data rates and range/intensity image timing offsets every second: diff --git a/config/default_params.yaml b/config/default_params.yaml index 0068081..5b42b6b 100644 --- a/config/default_params.yaml +++ b/config/default_params.yaml @@ -1,33 +1,38 @@ 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) topic_point_cloud: "~/point_cloud" topic_range_image: "~/range_image" topic_intensity_image: "~/intensity_image" + 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/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/config/pinchy_params.yaml b/config/pinchy_params.yaml deleted file mode 100644 index 083cf5d..0000000 --- a/config/pinchy_params.yaml +++ /dev/null @@ -1,33 +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_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_demo.launch.py b/launch/sonar_imu_orient_demo.launch.py new file mode 100644 index 0000000..ff19e16 --- /dev/null +++ b/launch/sonar_imu_orient_demo.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 = 'default_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/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 c9d2fad..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. @@ -20,13 +21,40 @@ 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) + + +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.""" @@ -46,6 +74,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 +93,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 @@ -88,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( @@ -100,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( @@ -119,7 +152,12 @@ 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('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')) @@ -132,6 +170,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 +198,12 @@ 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 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: @@ -198,6 +245,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 +300,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 # ────────────────────────────────────────────────────────────────────── @@ -335,7 +387,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): @@ -363,6 +417,16 @@ 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}') + imu_frame_id = self.get_parameter('imu_frame_id').get_parameter_value().string_value + self._publish_imu_batch(msg, imu_frame_id) # ────────────────────────────────────────────────────────────────────── # Publishers @@ -475,6 +539,62 @@ 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 is None or 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 + 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 +605,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 @@ -506,17 +627,20 @@ 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: 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)),