How to Add Ultrasonic Sensors to an SDF File for Gazebo

ultrasonic-sensor-gazebo

In the cover image you can see an ultrasonic sensor that was added to a simulated robot in Gazebo. An ultrasonic sensor is useful because, unlike LIDAR, an ultrasonic sensor can detect glass. Detection of glass is important if you’re planning to build a robot for the real-world that will use the ROS 2 Navigation stack.

To add a simulated ultrasonic sensor to your SDF file, you will need to add code that looks like this:

  <!-- *********************** ULTRASONIC SENSOR ************************  -->
  <link name="ultrasonic_1_link">
    <pose>0.09 -0.139 0.595 0 0 0</pose>
    <sensor name="ultrasonic_1" type="ray">
      <always_on>true</always_on>
      <visualize>false</visualize>
      <update_rate>5</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>5</samples>
            <resolution>1.000000</resolution>
            <min_angle>-0.12</min_angle>
            <max_angle>0.12</max_angle>
          </horizontal>
          <vertical>
            <samples>5</samples>
            <resolution>1.000000</resolution>
            <min_angle>-0.01</min_angle>
            <max_angle>0.01</max_angle>
          </vertical>
        </scan>
        <range>
          <min>0.02</min>
          <max>4</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="ultrasonic_sensor_1" filename="libgazebo_ros_ray_sensor.so">
        <ros>
          <remapping>~/out:=ultrasonic_sensor_1</remapping>
        </ros>
        <output_type>sensor_msgs/Range</output_type>
        <radiation_type>ultrasound</radiation_type>
        <frame_name>ultrasonic_1_link</frame_name>
      </plugin>
    </sensor>  
  </link>

When you launch RViz along with Gazebo, you will need to add the Range sensor option so that you can visualize the ultrasonic sensor output. Be sure to select “Best Effort” for the reliability policy.