Sonar / IR / laser sensor in SITL / Gazebo simulation

Hello there,

how do I add a range finder sensor like a sonar, IR sensor or a laser to the iris copter?
I’ve been able to add a camera by adding a xacro:camera_macro section to iris_base.xacro, but there does not seem to be a macro for any distance measurement sensor.

Docs about simulation with gazebo mention make posix gazebo_iris_opt_flow, which I figure could enable the px4flow sonar, but I need to start the simulation with MAVROS capabilities, i.e. roslaunch px4 mavros_posix_sitl.launch.

Any help is appreciated.

Thanks.

OK, so I figured it out in the end, but it was very hard and I don’t feel like my solution is perfect.
It is a pity there doesn’t seem to be any documentation on that subject.

In order to edit the simulated sensors, I ended up creating a new vehicle and a new launch file spawning that particular vehicle instead of the default IRIS.

  • In the launch directory, create a new file called mavros_posix_sitl_sonar.launch containing:
    <?xml version="1.0"?> <launch> <include file="$(find px4)/launch/mavros_posix_sitl.launch"> <arg name="vehicle" value="iris_sonar"/> </include> </launch>
    This does all the stuff from mavros_posix_sitl.launch, but overrides the vehicle to be iris_sonar. This vehicle doesn’t exist yet, so create it:

  • In Tools/sitl_gazebo/models create a new folder iris_sonar with a model.config file corresponding to the ones in the other model folders and a iris_sonar.sdf file containing:

      <sdf version='1.5'>
      <model name='iris_odom'>
    
      <include>
        <uri>model://iris</uri>
      </include>
    
      <!--sonar-->
      <link name="range_down_link">
        <pose frame=''>0 0.015 -0.04 3.14159 1.57079 3.14159</pose>
        <inertial> <!-- causes issues when omitted -->
          <mass>0.05</mass>
          <inertia>
            <ixx>2.08333333333e-07</ixx>
            <iyy>5.20833333333e-07</iyy>
            <izz>5.20833333333e-07</izz>
          </inertia>
        </inertial>
        <visual name="range_down_link_visual">
          <pose frame=''>0 0 0 0 1.57079 0</pose>
          <geometry>
            <cylinder>
              <length>0.008</length>
              <radius>0.004</radius>
            </cylinder>
          </geometry>
          <material>
            <script>
              <name>Gazebo/Blue</name>
              <uri>file://media/materials/scripts/gazebo.material</uri>
            </script>
          </material>
        </visual>
        <sensor type="ray" name="iris_range_down">
          <update_rate>10</update_rate>
          <visualize>true</visualize>
          <ray>
            <scan>
              <horizontal>
                <samples>1</samples>
                <resolution>1</resolution>
                <min_angle>-0</min_angle>
                <max_angle>0</max_angle>
              </horizontal>
            </scan>
            <range>
              <min>0.06</min> <!-- do not change (smaller values cause issues) -->
              <max>35</max>   <!-- do not change (bigger values cause issues) -->
              <resolution>0.01</resolution>
            </range>
            <noise>
              <type>gaussian</type>
              <mean>0.0</mean>
              <stddev>0.01</stddev>
            </noise>
          </ray>
          <plugin name="iris_range_down_controller" filename="libgazebo_ros_range.so">
            <robotNamespace>iris_odom</robotNamespace>
            <gaussianNoise>0.005</gaussianNoise>
            <alwaysOn>true</alwaysOn>
            <updateRate>10</updateRate>
            <topicName>range_down</topicName>
            <frameName>range_down_link</frameName>
            <fov>0</fov>
            <radiation>ultrasound</radiation>
          </plugin>
        </sensor>
      </link>
    
      <joint name="range_down_joint" type="fixed">
        <parent>iris::base_link</parent>
        <child>range_down_link</child>
      </joint>
    
      </model>
      </sdf>
    
  • duplicate posix-configs/SITL/init/ekf2/iris and name it iris_sonar

I think that’s all.

By the way, in the MAVLINK shell (which is spawned in the very same terminal in which you start the mavros_posix_sitl.launch or mavros_posix_sitl_sonar.launch respectively) you can type

list_topics

and will probably already have multiple instances of distance_sensor without adding it manually, but these are not published as a ROS topic.

1 Like