Sonar Data Visualization

Intro

The sonar system produces a forward-looking intensity image that is published over ROS, which is then converted into a 2D/3D point cloud for downstream perception.

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

Testing

• Run simulation node (which should start publishing the sonar data as a point cloud message)

ros2 launch pontus_bringup odom_simulation.launch.py 

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

• Launch perception node

ros2 launch pontus_perception detection_services.launch.py auv:=sim

• CHeck for sonar topic in gazebo

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

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”

• View sonar Raycast in Gazebo

• Visualize ray hits (Switch to Points view)

  

• Check TF for sonar_0

ros2 run tf2_tools view_frames

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

• RViz with increased point size