[TASK] Sonar Image PCL conversion Node

[jorgenfj]

Description of task

Description

Create a ROS node that converts a 2D sonar image into a PointCloud (PCL) expressed in the sonar sensor frame, where:

• Each pixel is converted from image coordinates to metric coordinates (meters)
• The resulting point cloud lies in the XY-plane of the sonar frame
• The Z-coordinate is fixed to 0
• Range limits and pixel scaling are derived from a SonarInfo message
• A ROS param should decide if the PCL should be dense, with point field PointXYZI, or if the pixels should be thresholded by a static threshold and the PCL should then not be dense but point field can still be PointXYZI.

Inputs

The node shall subscribe to:

1. Sonar image topic

   • Type: sensor_msgs/Image
   • Pixels Represents intensity

2. Sonar metadata topic

   • Type: SonarInfo

   • Fields:

     std_msgs/Header header
     uint32 height
     uint32 width
     float64 meters_per_pixel_x
     float64 meters_per_pixel_y
     float64 max_range
     float64 min_range
     float64 vertical_fov
     

---

Outputs

The node shall publish:

• Point cloud topic

  • Type: sensor_msgs/PointCloud2
  • Frame ID: same as input image
  • Point type: pcl::PointXYZ (or PointXYZI if intensity is included)

---

Expected Behavior

• Subscripe to one message of SonarInfo, then destroy subscriber.
• Generate one 3D point per valid image pixel
• Publish a dense or filtered point cloud
• Preserve timestamp consistency with the sonar image

---

Example: Creating PCL Points from Image Pixels

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
    new pcl::PointCloud<pcl::PointXYZ>);

cloud->header.frame_id = sonar_frame;
cloud->height = 1;
cloud->is_dense = false;

for (uint32_t v = 0; v < sonar_info.height; ++v) {
    for (uint32_t u = 0; u < sonar_info.width; ++u) {

        double x = pixel_to_meter(sonar_info.width, sonar_info.meters_per_pixel_x, u);

        double y = pixel_to_meter(sonar_info.height, sonar_info.meters_per_pixel_y, v);

        pcl::PointXYZ p;
        p.x = x;
        p.y = y;
        p.z = 0.0;

        cloud->points.push_back(p);
    }
}

cloud->width = cloud->points.size();

Conversion to ROS message:

sensor_msgs::PointCloud2 cloud_msg;
pcl::toROSMsg(*cloud, cloud_msg);
cloud_msg.header.stamp = image_msg->header.stamp;
cloud_msg.header.frame_id = sonar_frame;
pointcloud_pub.publish(cloud_msg);

---

Acceptance Criteria

• Node subscribes to Image and SonarInfo
• Publishes PointCloud2
• Coordinates are correctly converted to meters
• Z-coordinate is always zero
• Frame ID is correctly set
• No processing occurs without valid SonarInfo

Suggested Workflow

No response

Specifications

No response

Contacts

No response

Code Quality

• Every function in header files are documented (inputs/returns/exceptions)
• The project has automated tests that cover MOST of the functions and branches in functions (pytest/gtest)
• The code is documented on the wiki (provide link)

[jorgenfj]

For inspiration on how to setup/retrieve SonarInfo you can follow the example in this node that is functionally equivalent using CameraInfo as the message type: https://github.com/vortexntnu/vortex-aruco-detection/blob/main/aruco-detector/src/aruco_detector_ros.cpp

[jorgenfj]

https://github.com/vortexntnu/vortex-navigation/blob/main/mapping_interface_example/src/mapping_interface_example_ros.cpp the publish_grid_block function in this node can be used as inspiration for how to convert a contiguous block of memory to a pointcloud