Imu output enabled

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:

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

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

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')

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,
+        ),
+    ])

package.xml

@@ -2,10 +2,10 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>waterlinked_sonar_3d15</name>
-  <version>0.1.0</version>
+  <version>0.2.0</version>
   <description>ROS 2 driver for the Water Linked Sonar 3D-15, built on the official wlsonar Python library.</description>
 
-  <maintainer email="julian.valdez@example.com">Julian Valdez</maintainer>
+  <maintainer email="support@waterlinked.com">Water Linked</maintainer>
   <license>MIT</license>
 
   <url type="repository">https://github.com/smarc-project/waterlinked_sonar_3d15</url>

setup.py

@@ -17,7 +17,7 @@
     ],
     install_requires=[
         'setuptools',
-        'wlsonar>=0.5.0',
+        'wlsonar>=0.6.0',
         'numpy',
     ],
     zip_safe=True,