GTRI based Sonar Notes and Description

The GTRI-based Sonar model at https://github.com/Field-Robotics-Lab/gtri_based_sonar is based on and is forked from the SyllogismRXS Sonar model at https://github.com/SyllogismRXS/syllo-gazebo, ref. "A Computationally-Efficient 2D Imaging Sonar Model for Underwater Robotics Simulations in Gazebo", Georgia Tech . This fork has been modified as follows:

• It has been ported to work with ROS Melodic and Gazebo 9.
• The algorithm for converting the point cloud from the Gazebo Ray module to a Gazebo camera image has been merged into the Gazebo module. The image is no longer calculated and advertised on a separate ROS Node.
• Modules not related to modeling Sonar have been removed in order to simplify the repository.

Example Usage

See https://github.com/Field-Robotics-Lab/DAVE/wiki/Sonar-Models#gtri for usage and example screenshot.

.xacro Interface

Here is a proposed xacro macro for instantiating a named Sonar instance onto a parent link:

Name: gtri_based_fls. Parameters: namespace, suffix, parent_link, topic, *origin.

ROS Interface

Here are the topics published by this Sonar model:

Message type	Suggested topic name	Description
sensor_msgs::PointCloud	sonar_cloud	The point cloud is calculated from Gazebo's RaySensor interface.
sensor_msgs::Image	sonar_image	The Sonar image calculated from the point cloud.
sensor_msgs::CameraInfo	sonar_image_camera_info	The height, width, and timestamp of the currently available Sonar image.

Composition of sensor_msgs::PointCloud

PointCloud consists of a header, a number of xyz points, and the same number of values, see http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointCloud.html:

• std_msgs/Header header
  • uint32 seq
  • time stamp
  • string frame_id
• geometry_msgs/Point32[] points
  • float32 x
  • float32 y
  • float32 z
• sensor_msgs/ChannelFloat32[] channels | string name
  • float32[] values

Composition of sensor_msgs::Image:

Image consists of a header, height, width, encoding information, and uint8[] data, see http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Image.html:

• std_msgs/Header header
  • uint32 seq
  • time stamp
  • string frame_id
• uint32 height
• uint32 width
• string encoding
• uint8 is_bigendian
• uint32 step
• uint8[] data

Composition of sensor_msgs::CameraInfo:

CameraInfo contains metatata about the camera image, see http://docs.ros.org/melodic/api/sensor_msgs/html/msg/CameraInfo.html.

Implementation

The implementation is divided into two parts, 1) point cloud and 2) camera image:

Point Cloud

The point cloud is calculated from information available in Gazebo's RaySensor interface, https://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/classgazebo_1_1sensors_1_1RaySensor.html, see file ImagingSonar.cc, currently on branch basement_world at https://github.com/Field-Robotics-Lab/gtri_based_sonar/blob/basement_world/gtri_based_fls_gazebo/src/ImagingSonar.cc.

Here is the basic flow:

Initialization

Function load reads the requested topic names and sets up the Sonar so it doesn't run unless there are topic listeners. It also sets Gazebo's RaySensor to get callbacks to function OnNewLaserScans from Gazebo's MultiRayShape class for processing scans during runtime.

Runtime

• Function OnNewLaserScans takes a timestamp and, if not running behind, calls PutLaserData.

• Using angle, ray, and range information in RaySensor, function PutLaserData performs nested iterations over the 2-dimensional array of VerticalRayCount by VerticalRangeCount. For each, it computes a cloud datapoint using math on range (length of ray) and retro (intensity of ray) values and pushes the xyz point onto the points array and the intensity onto channel[0] of the channels array of arrays.

  To work with the RaySensor, we need to understand the Gazebo RaySensor class (https://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/classgazebo_1_1sensors_1_1RaySensor.html#a47aa298aceeb6084459df00a0d610ba6) and how to configure the shape of the ray using the MultiRayShape class (https://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/classgazebo_1_1physics_1_1MultiRayShape.html). The ray shape parameters are adjustable using SDF tags (http://sdformat.org/spec?ver=1.6&elem=sensor#sensor_ray). For discussion see https://answers.gazebosim.org//question/20930/where-is-the-lasershape-in-the-ray-sensor-defined/.

Sonar Image

The Sonar image is calculated from the point cloud, see file cloud_to_image.cpp, currently on branch basement_world at https://github.com/Field-Robotics-Lab/gtri_based_sonar/blob/basement_world/gtri_based_fls_gazebo/src/cloud_to_image.cpp. File ColorMaps.cpp maps the Sonar image to its rendered colors.

Initialization

Function init_cloud_to_image records the angle the image is calculated across and the Sonar's topic name for use during runtime.

Runtime

Function publish_cloud_to_image projects the point cloud onto a camera image and then publishes the camera image. Here are some of the transforms along the way:

• A Gaussion sample is applied at each point.
• A blank cv::Mat image is created
• For each point: translate to x,y coordinate and draw a circle of radius 1.
• Apply a median blur medianBlur of 3 (no salt and pepper)
• Apply coloring using function Gray2Jet_matlab provided in file ColorMaps.cpp.
• Manipulate contour points of type cv::Point.
• Rotate the image.
• Using cv_bridge::CvImage, convert the image to ROS type sensor_msgs::ImagePtr.
• Timestamp the image.
• Publish the image and its associated camera image info.