Adding SONAR-like sensors to Nav2

It is probably typical that the Navigation-2 (Nav2) setup under ROS 2 is just using LIDAR for generating costmaps. It is also possible to include SONAR-like sensors in your cost maps, that is sensors that publish a Range type of message. The process is easy.

Edit your YAML parameter configuration file for Nav2 as follows. In the “local_costmap” section, you will find a line that lists the plugins to be used to generate the local costmap. Add a “range_layer” to the list. Following the plugins line, you should see, e.g., sections describing the obstacle_layer and inflation_layer — that is, sections corresponding to the names in the plugins list. You will need to add a new section describing the range_layer.

Note, the names obstacle_layer, inflation_layer, range_layer, etc in the plugins list are arbitrary names, they don’t actually make the layer become, say, and obstacle layer. It’s what you supply as sub-parameters that make the section do what it does.

local_costmap:
  local_costmap:
    ros__parameters:
      plugins: ["obstacle_layer", "inflation_layer", "range_layer"]
      range_layer:
        plugin: "nav2_costmap_2d::RangeSensorLayer"
        topics:
          [
            "/sonar0Sensor",
            "/sonar1Sensor",
            "/sonar2Sensor",
            "/sonar3Sensor", 
            "/tof0Sensor",
            "/tof1Sensor",
            "/tof2Sensor",
            "/tof3Sensor",
            "/tof4Sensor",
            "/tof5Sensor",
            "/tof6Sensor",
            "/tof7Sensor",
          ]
      inflation_layer:
        .......rest of local_costmap YAML.....

Just like the section describing, say, the inflation_layer, you will add a new range_layer section by repeating the name given in the plugins list, followed by a colon. The first entry should describe the class name that will handle the data type. In the case of Range type values, the line should say:

plugin: "nav2_costmap_2d::RangeSensorLayer"

Then add a topics entry consisting of the word topics followed by a colon and then a single topic name in quotes, or a list of topic names formed by a left square bracket, then each quoted topic name in the list separated by commas and a closing right square bracket.

That’s it. If you bring up the nav stack, you should now see that when you wave you hand in front of your SONAR sensor, a new obstacle shows up in the local costmap.

There are a couple of caveats to consider. The published topic containing the Range message needs to have a valid frame_id in the header. And that frame_id needs to be described in the URDF for the robot, which, of course, needs to be published so that the transform system in ROS knows how to relate the Range value to the coordinate frames of the robot.

And, the time stamp in the header needs to be accurate and timely. This especially could be a problem if you are, say, publishing the data from an Arduino micro controller which may not have a notion of the time used by the rest of the ROS components. If you are publishing using micro–ROS, there are ways to synchronize your Arduino with the main ROS system.

There are a few additional parameters besides plugin and topics that you can provide for the range_layer. They are described here.