Simulating multiple physically connected drones in Gazebo

[hmer101]

Hi all,

I am using the (Ignition) Gazebo simulator with PX4. I’m trying to physically link multiple x500 drones with a custom ‘tether’ SDF that I have created.

In order to create joints between multiple SDF models, they must be nested together in a parent model. Doing this changes the name of the Gazebo model to ‘<parent_model_name>/model/<child_model_name>’ e.g. connected_group/model/x500. I can successfully create this structure in Gazebo by simply including multiple x500 models into an sdf world (either the default or otherwise) and adding the link in the higher-level model in the world.

The problem arrises when trying to connect PX4 SITL instances to the nested x500 models. Following the instructions here I can spawn multiple PX4 instances each connected to different x500 models included in the world (using the PX4_GZ_MODEL_NAME argument). However, although I can see that the model name in each PX4 instance is successfully set (e.g. to connected_group/model/x500_1), arming fails as the SITL instance cannot find multiple sensors (e.g. the compass, barometer - see below). Note however that the data from the imu appears to be received as the accelerometer and gyro are being found. I believe this must be because the imu data is published on a gazebo topic (see second screenshot) but I don’t understand the details of how this works/if it’s relevant. Perhaps the issue is something to do with not setting the “simulation model” in gz_bridge (seen in the screenshot below).

Does anyone know what I have to do to get the PX4 SITL to find nested models in Gazebo? An example (terminal) output of the PX4_sitl failing to connect is seen below:

I set the PX4_GZ_MODEL_NAME argument by looking at the output of ‘ign topic -l’ after the world with the nested model is spawned - see below:

Thanks in advance!

Edit: Rolling forward to a more updated version of the PX4 firmware and attempting to connect to a drone already in the simulation (not even nested as above, but simply included in the world directly and named ‘x500_1’), I get the following error message:

This is making me think that perhaps the spawn command is starting some background processes to do with gz_bridge that simply adding the model to a world is not (notice that the ‘simulation model’ is empty). Any ideas on where to look to get a better understanding of this?

[Benja]

Hi @hmer101
You are doing something very interesting here!

To be on the same page, on which PX4 commit / tag / Branch are you working?

[hmer101]

Hi @Benja, thanks for your interest!

I’m on the main branch on commit 7098970a38.

[Benja]

Thanks!

could you also share the sdf of the parent model + nested models and their interconnection? I want to try it if possible.

[hmer101]

I haven’t modelled this properly yet as I wanted to try and see if I could connect to nested drones first (I think this is the key part of my problem). Here’s some extra lines that I added to the bottom of the ‘default world’ however (under the ‘light’ tag on line 145 and nested in the ‘world’ tag), to get the group:

<model name="connected_group">
  <!-- Add drones -->
  <include>
    <uri>x500</uri>
    <name>x500_1</name>
    <pose>1.5 0 .24 0 0 0</pose>
  </include>
  <include>
    <uri>x500</uri>
    <name>x500_2</name>
    <pose>-0.75 1.3 .24 0 0 0</pose>
  </include>
  <include>
    <uri>x500</uri>
    <name>x500_3</name>
    <pose>-0.75 -1.3 .24 0 0 0</pose>
  </include>

  <!-- Add link -->
  <link name='element_1'>
    <inertial>
      <pose>0.3 0 0 0 1.5708 0</pose>
      <mass>0.02</mass>
      <inertia>
        <ixx>0.0006005</ixx>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyy>0.0006005</iyy>
        <iyz>0</iyz>
        <izz>1e-06</izz>
      </inertia>
    </inertial>
    <collision name='element_1_collision'>
      <pose>0.3 0 0 0 1.5708 0</pose>
      <geometry>
        <cylinder>
          <length>0.6</length>
          <radius>0.01</radius>
        </cylinder>
      </geometry>
    </collision>
    <visual name='element_1_visual'>
      <pose>0.3 0 0 0 1.5708 0</pose>
      <geometry>
        <cylinder>
          <length>0.6</length>
          <radius>0.01</radius>
        </cylinder>
      </geometry>
    </visual>
  </link>

  <!-- Add joints -->
  <joint name="joint_00" type="revolute">
    <parent>x500_1::base_link</parent>
    <child>element_1</child>
    <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
    <axis>
      <xyz>1.0 0.0 0.0</xyz>
    </axis>
  </joint>

  <joint name="joint_01" type="revolute">
    <parent>element_1</parent>
    <child>x500_2::base_link</child>
    <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
    <axis>
      <xyz>1.0 0.0 0.0</xyz>
    </axis>
  </joint>

</model>

It’s simply the default world with 3 x500 drones included and a element that ‘links’ two of them (but hasn’t been modelled properly yet). In order to open the default world with ‘gz sim’, you have to move the world to the ‘models’ folder so it can find the x500 model (I don’t yet understand how to change Gazebo’s model search path which it appears that the startup script can do).

P.S. I’ve figured out a workaround to the ‘edit’ on my previous post. By simply spawning a drone with the regular start command ‘PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE=“0,0” PX4_GZ_MODEL=x500 ./build/px4_sitl_default/bin/px4 -i 0’ (with the edited default world so that 3 additional drones are created), it appears that the gz_bridge is properly started for me. Using this hack, and non-nested drones included in the default world, I am able to connect to the other drones (e.g. x500_1) using the command ‘PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_NAME=x500_1 build/px4_sitl_default/bin/px4’. The only problem that remains is the original one: how to connect to drones when they’re nested.

[Benja]

ok!
I got you.

First, that commit is very recent, therefore PX4 requires gazebo garden, not ignition. That is the the source of your

timed out waiting for clock message

Please follow https://docs.px4.io/main/en/sim_gazebo_gz/#installation-ubuntu-linux to install gazebo garden.

I’m not that expert in Gazebo but I think your issue is due to different plugins that uses different conventions for the name of their topics

• IMU publishes at

  /world/default/model/connected_group/model/x500_1/link/base_link/sensor/imu_sensor/imu
  

  therefore when on PX4 you set PX4_GZ_MODEL_NAME=connected_group/model/x500_1 everything is fine.

• Airspeed publishes at

  /world/default/model/connected_group/model/x500_1/link/base_link/sensor/air_pressure_sensor/air_pressure
  

  therefore when on PX4 you set PX4_GZ_MODEL_NAME=connected_group/model/x500_1 everything is fine.

• The motor controllers instead listen at

  /x500_1/command/motor_speed
  

  And they ignore any nesting!
  You can check it, it is pretty fun, but starting gazebo with the three nested models, NO PX4, and then spawing another model with the same name of one of the other three (but not nested this time)… well you will have two models moving and responding to the same commands!

• The position groundthruth which is used by the fake gps and fake magnetometer is obtained listening to

  /world/default/pose/info
  

  and extracting from it the pose of the drone. However, it doesn’t matter if the model is nested, the name remains x500_1

[hmer101]

Great thanks for your reply! I can replicate your results and confirm everything that you’re saying (found the Gazebo Garden update earlier but forgot to update my post).

It appears that all of the topics that repond to the change in ‘PX4_GZ_MODEL_NAME’ do so because they’re set in GZBridge with the _model_name parameter (makes sense).

As you aptly pointed out, the motor controllers ignore nesting and listen directly to:

/x500_1/command/motor_speed

Although this does not change when launching with the ‘PX4_GZ_MODEL_NAME’ command, this still appears to be set on the receving/gazebo side correctly (hence why you see the ‘two drones responding to the same commands’ phenomenon I suppose).

I was initially looking at the
px4-rc.simulator start-up sequence to try solve this problem. But perhaps with this new information, I should be searching elsewhere in the code i.e. somewhere that is expecting ‘connected_group/model/x500_1/command/motor_speed’ rather than simply ‘x500_1/command/motor_speed’? Any idea where to look to try and solve this problem?

[Benja]

you have to look for the MulticopterMotorModel gazebo plugin.
Let me ping @bperseghetti

By the way, I realized I wasn’t clear: also acquiring the position groundthruth is an issue as you provide a PX4_GZ_MODEL_NAME=connected_group/model/x500_1 then gz_bridge looks for a link named connected_group/model/x500_1 and finds nothing.

[hmer101]

Great thanks for pinging @bperseghetti! I have seen this issue and wonder if it is related.

In the meantime, I’ve tried to implement a “quick fix” by extracting the x500_1 from the model name connected_group/model/x500_1 and using that instead of the _model_name parameter to pass into the esc and servo .init calls (lines 184 and 189 of GZBridge respectively). This appears to give me the correct /model/x500_1/servo_<x> and /x500_1/command/motor_speed topics in the gz topic -l output. It also allows me to send commands to the nested drone and have it take off (without a ‘shadow drone’)!

Further, in an attempt to quick fix the fake GPS/magnetometer issue, I also changed line 475 to the extracted name x500_1. Without this fix, the commander never gives me the ‘ready for takeoff’ message. With it however, although I am able to take-off, the drone just continues to rise so I guess I am not getting accurate position feedback. Any idea on how to either quickly or properly fix this?

I have also tried the opposite approach i.e. setting PX4_GZ_MODEL_NAME=x500_1 and prefixing all other topics in GZBridge with connected_group/model/ and get the same result. I guess I don’t understand how PX4 is acquiring the position groundtruth?

Thanks for your help so far - much appreciated!

[Benja]

The position groundthruth is acquired here

PX4/PX4-Autopilot/blob/bd5838faf0730b40d5d971b798563c82eeecdb5f/src/modules/simulation/gz_bridge/GZBridge.cpp#L466-L475

and extracted from the gazebo topic "/world/" + _world_name + "/pose/info", see

PX4/PX4-Autopilot/blob/bd5838faf0730b40d5d971b798563c82eeecdb5f/src/modules/simulation/gz_bridge/GZBridge.cpp#L139-L140

That topic streams the pose of all links in the world. However, it does not take into account nesting and pose.pose(p).name() in line 475 returns x500_01, x500_2 and x500_3 and not connected_group/model/x500_1, ecc

You should be able to use the exact same strategy you adopted for the motors here!

[hmer101]

Hmm yeah, if I understand you correctly, I believe that’s what I tried. The nested drone is able to take off correctly but it doesn’t stop rising when it reaches the takeoff setpoint like non-nested drones do when using commander takeoff. Further, after using commander land, the drone approaches the ground at very high speed. This is making me think that it’s getting the incorrect ground truth feedback (even with the similar fix to the motors), but I don’t really understand why.

[hmer101]

I’ve found why the incorrect ground truth feedback is received. As you mentioned before, the /world/$WORLD/pose/info topic doesn’t take into account nesting. I printed all of the names found with pose.pose(p).name() below to confirm this:

Printing the pose of the nested x500_1 after it has taken off shows no update. However, the pose of connected_group shows an update to the z value as x500_1 rises. I’m assuming this is because whatever is publishing the topic "/world/" + _world_name + "/pose/info" is using movements from the lowest-level model to change the pose of the highest-level model. Any idea where this topic might be published so I could rectify this? I’m assuming it’s in Gazebo somewhere rather than PX4…

Edit: SOLVED!!
Turns out that the connected_group model’s pose is taken relative to the first link nested in it. Because x500_1 was the first link, when it was taking off, connected_group was moving too. Simply adding a reference link before the first drone in connected_group solves this problem. See a screenshot of the result below (drones mid-flight)!

Thanks so much for your help @Benja!

[Benja]

I don’t know if this is the “correct” way to address the problem (I mean nesting), but I’m glad that it worked!