Sonar Data Visualization

Intro

The sonar system produces a forward-looking intensity image that is published over ROS, which is then converted into a PointCloud2 representation for downstream perception.

TODO

• Inspect simulated sonar returns in Gazebo
• Visualize sonar point clouds in RViz
• Validate TF alignment and coordinate frames
• Debug missing or incorrect sonar detections
• Compare simulated sonar output with processed perception results

Sonar Simulation (Gazebo)

• Launch simulation node (simulated sonar data in Gazebo should begin publishing to a ROS PointCloud2 topic)

ros2 launch pontus_bringup odom_simulation.launch.py 

ROS Bridge

• ROS topics mapped to Gazebo topics in bridge.yaml
  • Simulated sonar returns are published in Gazebo as gz.msgs.PointCloudPacked on /pontus/sonar_0/points
  • Which are translated into a ROS PointCloud2 message on /pontus/sonar_0/sim

bridge.yaml

# Sonar Forward
# - ros_topic_name: "/pontus/sonar_0"
#   gz_topic_name: "/pontus/sonar_0"
#   ros_type_name: "sensor_msgs/msg/LaserScan"
#   gz_type_name: "gz.msgs.LaserScan"
#   direction: GZ_TO_ROS
- ros_topic_name: "/pontus/sonar_0/sim"
  gz_topic_name: "/pontus/sonar_0/points"
  ros_type_name: "sensor_msgs/msg/PointCloud2"
  gz_type_name: "gz.msgs.PointCloudPacked"
  direction: GZ_TO_ROS

• sonoptix driver ROS topics

    cloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/pontus/sonar/pointcloud", 10);
    clustered_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/pontus/sonar/clustercloud", 10);
 
    img_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
      "/pontus/sonar_0/image_debug", qos, 
      std::bind(&SonoptixDriverNode::onImage, this, std::placeholders::_1)
    );
 
    sim_cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
      "/pontus/sonar_0/sim",
      qos,
      std::bind(&SonoptixDriverNode::onSimPointcloud, this, std::placeholders::_1)
    );

Testing & Visualization

Bridge Verfication

• List Gazebo topics

gz topic -l

/clock
/gazebo/resource_paths
/gui/camera/pose
/gui/currently_tracked
/gui/track
/model/pontus/joint/prop_joint_0/ang_vel
/model/pontus/joint/prop_joint_0/cmd_thrust
/model/pontus/joint/prop_joint_1/ang_vel
/model/pontus/joint/prop_joint_1/cmd_thrust
/model/pontus/joint/prop_joint_2/ang_vel
/model/pontus/joint/prop_joint_2/cmd_thrust
/model/pontus/joint/prop_joint_3/ang_vel
/model/pontus/joint/prop_joint_3/cmd_thrust
/model/pontus/joint/prop_joint_4/ang_vel
/model/pontus/joint/prop_joint_4/cmd_thrust
/model/pontus/joint/prop_joint_5/ang_vel
/model/pontus/joint/prop_joint_5/cmd_thrust
/model/pontus/joint/prop_joint_6/ang_vel
/model/pontus/joint/prop_joint_6/cmd_thrust
/model/pontus/joint/prop_joint_7/ang_vel
/model/pontus/joint/prop_joint_7/cmd_thrust
/model/pontus/odometry
/model/pontus/odometry_with_covariance
/model/pontus/pose
/pontus/camera_1/camera_info
/pontus/camera_1/image_raw
/pontus/camera_3/camera_info
/pontus/camera_3/image_raw
/pontus/camera_4/camera_info
/pontus/camera_4/image_raw
/pontus/camera_5/camera_info
/pontus/camera_5/image_raw
/pontus/camera_front/camera_info
/pontus/camera_front/image_raw
/pontus/imu_0
/pontus/imu_0/performance_metrics
/pontus/pressure_0
/pontus/pressure_0/performance_metrics
/pontus/sonar_0
/pontus/sonar_0/points
/pontus/sonar_1
/pontus/sonar_1/points
/sensors/marker

• Check for sonar topic in gazebo

 ros2 topic list | grep sonar

/pontus/sonar_0/sim

ros2 node info /ros_gz_bridge

There are 2 nodes in the graph with the exact name "/ros_gz_bridge". You are seeing information about only one of them.
/ros_gz_bridge
  Subscribers:
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /pontus/thruster_0/cmd_thrust: std_msgs/msg/Float64
    /pontus/thruster_1/cmd_thrust: std_msgs/msg/Float64
    /pontus/thruster_2/cmd_thrust: std_msgs/msg/Float64
    /pontus/thruster_3/cmd_thrust: std_msgs/msg/Float64
    /pontus/thruster_4/cmd_thrust: std_msgs/msg/Float64
    /pontus/thruster_5/cmd_thrust: std_msgs/msg/Float64
    /pontus/thruster_6/cmd_thrust: std_msgs/msg/Float64
    /pontus/thruster_7/cmd_thrust: std_msgs/msg/Float64
  Publishers:
    /clock: rosgraph_msgs/msg/Clock
    /parameter_events: rcl_interfaces/msg/ParameterEvent
    /pontus/camera_1/image_raw: sensor_msgs/msg/Image
    /pontus/camera_3/camera_info: sensor_msgs/msg/CameraInfo
    /pontus/camera_3/image_raw: sensor_msgs/msg/Image
    /pontus/camera_4/image_raw: sensor_msgs/msg/Image
    /pontus/camera_5/image_raw: sensor_msgs/msg/Image
    /pontus/camera_front/camera_info: sensor_msgs/msg/CameraInfo
    /pontus/camera_front/image_raw: sensor_msgs/msg/Image
    /pontus/imu_0: sensor_msgs/msg/Imu
    /pontus/pressure_0: sensor_msgs/msg/FluidPressure
    /pontus/sonar_0/sim: sensor_msgs/msg/PointCloud2
    /rosout: rcl_interfaces/msg/Log
  Service Servers:
    /ros_gz_bridge/describe_parameters: rcl_interfaces/srv/DescribeParameters
    /ros_gz_bridge/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
    /ros_gz_bridge/get_parameters: rcl_interfaces/srv/GetParameters
    /ros_gz_bridge/get_type_description: type_description_interfaces/srv/GetTypeDescription
    /ros_gz_bridge/list_parameters: rcl_interfaces/srv/ListParameters
    /ros_gz_bridge/set_parameters: rcl_interfaces/srv/SetParameters
    /ros_gz_bridge/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
  Service Clients:

  Action Servers:

  Action Clients:

“For the simulation just running the odom simulation launch file should start publishing 
sonar data to the topic as a point cloud message. 

If you are trying to get the cluster results you also need to run mapping.launch.py. 

don't try to run perception, it's only the yolo nodes for the real robot”

“I don't think you can directly echo point cloud messages to see the actual points in the 
terminal since the message array is compressed. 

If you want to see how to get the points in python check the cluster coord file in the mapping package, 
there should be an example of it getting the points out of the clustered pointcloud topic”

Sonar Visualization in Gazebo

• Use Visualize Lidar plugin → Click reload → choose topic ¹

Ray View

• View Raycast emitted by sonar in simulation

Point View

• Visualize sonar hits (Switch to Points view)

TF

• Check TF for sonar_0

ros2 run tf2_tools view_frames

RViz

• View point cloud in RViz (had to switch to TopDownOrtho view and zoom out)

• RViz with increased point size

Footnotes

1. https://articulatedrobotics.xyz/tutorials/mobile-robot/hardware/lidar#simulating-sensors-in-gazebo ↩