Add support for range sensors

[srmainwaring]

This PR adds support for up to six range sensors.

Edit: 16 Dec 2022

Rebase PR onto main and update to incorporate migration from ign -> gz.

Why only six? The SITL_JSON interface accepts data from up to six range sensors. ArduPilot will support up to ten, but the number of fields in SITL_JSON is currently six and we do not propose an upstream change at this point.

Details

The sonar sensor type from Gazebo Classic has not yet been ported to Gazebo Sim, however we can approximate a sonar sensor using a Lidar sensor as an interim solution.

The sensor is added to a sdf <link> element. For example:

<sensor name='sonar' type='gpu_lidar'>
  <always_on>1</always_on>
  <update_rate>20</update_rate>
  <visualize>1</visualize>
  <topic>sonar_front_link/sensor/sonar</topic>
  <pose>0 0 0 0 0 0</pose>
  <ray>
    <scan>
      <horizontal>
        <samples>5</samples>
        <resolution>1</resolution>
        <min_angle>-0.14835</min_angle>
        <max_angle>0.14835</max_angle>
      </horizontal>
      <vertical>
        <samples>3</samples>
        <resolution>1</resolution>
        <min_angle>0</min_angle>
        <max_angle>0.14835</max_angle>
      </vertical>
    </scan>
    <range>
      <min>0.01</min>
      <max>4</max>
      <resolution>0.02</resolution>
    </range>
    <noise>
      <type>gaussian</type>
      <mean>0.1</mean>
      <stddev>0.005</stddev>
    </noise>
  </ray>
</sensor>

To capture the sensor data in SITL the ArduPilot plugin must include an element for each sensor:

<!-- 
  RNGFND1_TYPE = 100 (SITL)
  RNGFND1_FUNCTION = 0 (LINEAR)
  RNGFND1_MIN_CM = 20 (0.2 m)
  RNGFND1_MAX_CM = 700 (7.0 m)
  RNGFND1_POS_X = 0.25
  RNGFND1_POS_Y = 0.0
  RNGFND1_POS_Z = -0.05
  RNGFND1_ORIENT = 0 (Forward)
-->
<sensor>
  <type>sonar</type>
  <index>1</index>
  <topic>sonar_front_link/sensor/sonar</topic>
</sensor>

Where we have included details of the ArduPilot parameters that must be set to enable this particular range sensor.

The topics in the sdf and ArduPilot <sensor> elements must match exactly. In current versions of Gazebo there is no equivalent to the namespace alias prefix ~/ for topic names. If the <topic> element in the sdf <sensor> is omitted then Gazebo will generate a default name of the form world/<world>/model/<model>/link/<link>/sensor/<sensor> in which case the ArduPilot plugin topic must match this. The published topics may be examined using:

gz topic --list

Other changes

There is a minor change to ArduPilotPlugin ::ApplyMotorForces which provides additional checks when accessing velocity or position component data to ensure the arrays are not empty.

Testing

The changes have been testing using a modified version of a skid-steer rover available in this branch: https://github.com/srmainwaring/SITL_Models/tree/ignition/rover-sensors.

The model is configured with six sonar sensors and has a parameter set to enable them as RNGFND1 ... RNGFND6 and use them with the Dijkstra Bendy Ruler avoidance algorithm.

Follow up

Post merge, these changes should be back-ported the ignition-edifice branch.

[khancyr]

Unless there is strict API for sonar on gazebo/ignition, I think it is better to keep the range name to not confuse people.
Most usage on drone nowadays is with lidar both 1d and 2d, so it will be best to be able to pass sonar and lidar data from gazebo/ignition.

The 6 segments on the json interface comes from the proximity lib, those are the segments we use to split 2d lidars data.

[srmainwaring]

Sure, no prob to change the name back to range. I may also rebase this PR onto ignition edifice which will be better for testing on linux (I need to use Ignition Garden as all the changes to support macOS are in that branch).

In Ignition (sdformat) there is no range sensor. The types for distance estimation / proximity are: lidar, ray and sonar. ROS has a range message with a field to denote the ranging method (ultrasound, infrared, etc). I think range is probably the better name. However in both cases laser scan message types are different from range message types.

Different SITL APIs seem to have settled on different names and data structures for range and lidar data (and most of the other fields as well as it happens):

SITL_JSON

    struct {
        double timestamp_s;
        struct {
            Vector3f gyro;
            Vector3f accel_body;
        } imu;
        Vector3d position;
        Vector3f attitude;
        Quaternion quaternion;
        Vector3f velocity;
        float rng[6];
        struct {
            float direction;
            float speed;
        } wind_vane_apparent;
        float airspeed;
        bool no_time_sync;
        struct {
            struct vector3f_array points;
            struct float_array ranges;
        } scan;
    } state;

{
    "timestemp": 0,
    "imu": {
        "gyro": [0, 0, 0],
        "accel_body": [0, 0, 0],
    },
    "position": [0, 0, 0],
    "attitude": [0, 0, 0],
    "quaternion": [1, 0, 0, 0],
    "velocity": [0, 0, 0],
    "rgn1": 0,
    "rgn2": 0,
    "rgn3": 0,
    "rgn4": 0,
    "rgn5": 0,
    "rgn6": 0,
    "windvane": {
        "direction": 0,
        "speed": 0
    }
}

SITL_AirSim

    struct {
        uint64_t timestamp;
        struct {
            Vector3f angular_velocity;
            Vector3f linear_acceleration;
        } imu;
        struct {
            double lat, lon, alt;
        } gps;
        struct {
            float roll, pitch, yaw;
        } pose;
        struct {
            Vector3f world_linear_velocity;
        } velocity;
        struct {
            struct vector3f_array points;
        } lidar;
        struct {
            struct float_array rc_channels;
        } rc;
        struct {
            struct float_array rng_distances;
        } rng;
    } state;

{
    "timestemp" : 0,
    "imu" : {
        "angular_velocity": [0, 0, 0], 
        "linear_acceleration": [0, 0, 0]
    },
    "gps" : {
        "lat": 0,
        "lon": 0,
        "alt": 0
    },
    "pose" : {
        "roll": 0,
        "pitch": 0,
        "yaw": 0
    },
    "velocity" : {
        "world_linear_velocity": [0, 0, 0]
    },
    "lidar" : {
        "point_cloud": [[0, 0, 0], [0, 0, 0]]
    },
    "rc" : {
        "channels": [1500, 1500, 1500, 1500, 1500, 1500]
    },
    "rng" : {
        "distances": [0, 0, 0, 0, 0, 0]
    },
}

Morse

    struct {
        double timestamp;
        struct {
            Vector3f angular_velocity;
            Vector3f linear_acceleration;
            Vector3f magnetic_field;
        } imu;
        struct {
            float x, y, z;
        } gps;
        struct {
            float roll, pitch, yaw;
        } pose;
        struct {
            Vector3f world_linear_velocity;
        } velocity;
        struct {
            struct vector3f_array points;
            struct float_array ranges;
        } scanner;
    } state, last_state;

{
    "timestamp": 0,
    "imu" : {
        "angular_velocity": [0, 0, 0],
        "linear_acceleration": [0, 0, 0],
        "magnetic_field": [0, 0, 0]
    },
    "gps": {"x": 0, "y": 0, "z": 0},
    "pose": {"roll": 0, "pitch": 0, "yaw": 0},
    "velocity": [0, 0, 0],
    "scan": {
        "point_list": [],
        "range_list": []
    }
}

I'm not planning any upstream changes for this PR, but we will need an extension for SITL_JSON to support lidar. At that point it would be nice to revisit the JSON schema for FDM and sensor data and try to come up with a plan for how to standardise that.

Also, it may not make sense to try and jam all the sensor data into the FDM packet as that is not going to scale very well.

[srmainwaring]

@khancyr I've moved this to draft as it would be better to slot it in after #6. The nested model PR will make it much more efficient to assemble examples with sensor arrays.

[rmackay9]

not directly related to this PR but in general it is nice to see this work on gazebo and I hope to make use of it myself in the coming months to verify some of our ROS<->AP non-gps features.

[srmainwaring]

not directly related to this PR but in general it is nice to see this work on gazebo and I hope to make use of it myself in the coming months to verify some of our ROS<->AP non-gps features.

Thanks @rmackay9. Be happy to help getting the ROS projects integrated, including some of the good work done by the gsoc students. I have a fully worked example using ROS2 Humble / Gazebo / mavros using this environment if people are interested in making the move from ROS1 to ROS2. There's also some boat / marine environment support as well in different plugins that may be useful for testing the auto docking work.

[rmackay9]

@srmainwaring,

Sounds great.

For me, I think one of the key problems with our gazebo integration is that it's difficult to install and then start AP+gazebo.

I created PR ArduPilot/ardupilot#21680 to try and fixup the install script but it still has problems I think according to @jmachuca77. TBH, I'm totally unqualified in this area so I just end up bouncing back and forth trying to fix it up issues based on feedback without really understanding what I'm doing.

[srmainwaring]

@rmackay9 thanks for the link, I'll take a look at your script. I agree, the initial set up can be painful. For linux users with Nvidia cards, Docker is probably the way to go. I develop on a mac, so am used to having to build everything from source to get a working ROS / Gazebo environment and so tend to forget how involved it can get. VMware have recently provided support for modern versions of OpenGL so that gives me access to an Ubuntu machine that will render the modern Gazebo scenes. I had a Gazebo11 / ROS / ArduPilot docker container running simulations previously, so that might be worth revisiting as a starting point.

[srmainwaring]

As noted in #40 it would be good to have an example using range sensors included in this repo for completeness. As this change does not impact other functionality (and effectively replaces commented out code) will merge and defer the model examples to a follow up PR.