Multi-Robot Support

[JBVAkshaya]

Changes Made

Multi-Robot support for Gazebo with SITL in ROS 2

Simulation Overview:

We use Ardusub SITL to establish a simulation pipeline that is close to real world setup for hardware-software integration. Ardusub SITL enables running this firmware in simulation to test software modules for control and motion planning. SITL helps to establish a working pipeline which can be easily ported to actual hardware. We use Gazebo as the flight dynamic model (FDM) that simulates the robot and onboard sensor in different environment conditions and configurations.

Simulator Implementation Details:

Ardupilot plugin for Gazebo utilizes internal simulation APIs to retrieve robot’s current state and execute vehicle commands. It communicates to the ArduSub SITL over UDP using IP protocol.

Intricate details for multi-robot application:

1. ArdupilotPlugin is loaded in the gazebo.xacro file. All the sensors and actuators are configured inside the plugin body.
2. FDM out and FDM in ports are defined inside the plugin body to allocate ports for sending telemetry data and receiving actuator commands. Same port numbers are assigned on the SITL side to receive and send data respectively.

Ardusub SITL can be thought of as a virtual FCU. It is a sandwich module that communicates with both the Ardupilot gazebo plugin and ROS topics using MAVROS. mavros_node is a container node that launches mavros_router and uas_node. It is configured for individual robot using a configuration file.

Intricate details for multi-robot application:

1. tgt_system represents the system id of the SITL and is unique for each robot. Same parameter is encoded as SYSID_THISMAV in ardusub SITL. Both these parameters have to match.
2. Given the simulation is setup on single machine, all the ports for different robots have to be unique. For SITL, a block of 10 ports is reserved for a single robot and default starting port for TCP connection is 5760. Similarly, we are required to set unique fdm_out and fdm_in ports.
3. We encode the FCU_URL to define the connection with SITL in mavros.
4. Let us say n is set by --instances/-I. Then the starting port address for connect is set as base port + 10*n
5. GCS_URL is used to define a connect to the external GCS in mavros. When using this mavros enables communication between SITL (FCU) and GCS first by converting mavlink message to ros message and then converting it back to mavlink message as GCS understands mavlink messages. mavproxy is used for enabling this.

Multi-Robot Simulation Parameter setup:

Gazebo:
Setup fdm_in and fdm_out while loading Ardupilot plugin.

• Rob 1:fdm_in = 9002, fdm_out=9003
• Rob 2: fdm_in = 9012, fdm_out=9013

Mavros:

• Rob_1: fcu_url : "tcp://localhost:5760”, gcs_url: “”, tgt_system: 1
• Rob_2: fcu_url : "tcp://localhost:5770”, gcs_url: “”, tgt_system: 2

SITL:
ardusub -S -w -M JSON --defaults $(var ardusub_params_file) -I$(var ardusub_instance) --sysid $(var sysid_thismav) --home 44.65870,-124.06556,0.0,270.0

• Rob 1: ardusub_instance=0, sysid_thismav=1. Here connection address for tcp will be set to 5760 + 10*0=5760
• Rob 2: ardusub_instance=1, sysid_thismav=2. Here connection address for tcp will be set to 5760 + 10*1=5770

References:

1. Multi-robot mavros: https://github.com/Intelligent-Quads/iq_tutorials/blob/master/docs/multi_mavros_drones.md
2. Multi-robot swarm: https://github.com/Intelligent-Quads/iq_tutorials/blob/master/docs/swarming_ardupilot.md
3. Ardusub SITL: https://github.com/ArduPilot/ardupilot/tree/master/ArduSub
4. Ardusub SITL Bluerov Heavy params: https://github.com/ArduPilot/ardupilot/blob/master/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params
5. Gazebo - SITL - GCS Architecture: https://gazebosim.org/api/gazebo/3/ardupilot.html

Testing

Tested on iron. Teleop for multi-robot working fine.

[mergify]

This pull request is in conflict. Could you fix it @JBVAkshaya?

[evan-palmer]

Could you do a rebase off of main @JBVAkshaya? It looks like there are a lot of duplicate commits.