Multi-Plane SITL No Longer Works in Latest Stable Branch

[mago3421]

Hello @dagar,

My team uses the stable branch to test formation flight on live planes or in SITL. I recently checked out the latest stable so we could use RTPS instead of MAVLink but now the simulation does not work.

To get the simulation working, I clone a fresh copy of the stable branch. Afterwards, I run make posix_sitl_default gazebo_plane to build the firmware. This works and I can connect to the SITL using dronekit.

I then edit the multi_uav_mavros_sitl.launch file to use planes which reads as follows:

NOTE: The XML for the second UAV is not shown but it is identical to the first UAV except for MAVLink ports, target system numbers, and rcS files. Beforehand, I clone the plane command file into two command files: plane_1 and plane_2. These command files are identical except for MAVLink streams, MAV SYS IDs, and SITL_UDP_PORT param sets. I do the same for the plane sdf files.

After editing the multi-vehicle launch file, I edit the single_vehcile_spawn.launch to read as so:

I comment out the xacro.py line because I am using sdf files for planes that I generated some time ago.

Next, I run roslaunch px4 multi_uav_mavros_sitl.launch as shown in the next few screenshots below:

NOTE: I close the Gazebo GUI and the process finishes cleanly. I also get messages saying the uav-1_plane-1_spawn and uav-2_plane-2_spawn scripts finished successfully, however, I never get command line access to the multi-vehicle simulation as before. Also, the SITL_UDP_PORT numbers seem correct (14560/14562) even though the ROS_MASTER_URI is a different port number.

I try to connect via Dronekit in Python but I get a wait_ready timeout which signals that Dronekit did not get parameters in time. I did some debugging and found the simulation to be getting stuck at this line.

Specifically, it is line 756 which polls the UDP port for simulation data. This poll never gets anything positive and thus waits forever. Please help. My team relies on the stable firmware to test our planes. I updated the repo because we want to use RTPS but now our old environment doesn't seem to be working. Please help. I've attached the sdf, rcS, and launch files so anyone can duplicate our environment.

multi-plane-SITL-files.zip

[PX4BuildBot]

GitMate.io thinks the contributor most likely able to help you is @dagar.

[mago3421]

Just some notes on this, I think the offending issue comes from one of the submodules. I checked out v1.6.5 which would've been older than the last working stable branch I had. However, the repo forces you to update to the latest submodules despite trying git checkout --recurse-submodules in Git 2.15.1.

As a temp fix, I am using an old repo I had on a thumb drive. It works on native Ubuntu but on my VM, it forces me to check out the latest submodules. I'm looking at how to remedy this in the meantime.

[dagar]

Check the sitl_gazebo version. It changed quite a bit.

[lamping7]

@mago3421 using today's master (bda929b) I can get my multi-plane sim to work w/o issues.

Can you launch one vehicle? How about an iris? When I've had the issue you describe, it is because I forgot to source or export something.

Paste the output of (not a picture of the output please) after you launch:
rostopic echo -n1 /diagnostics
and
rosrun mavros checkid

[mago3421]

@dagar Okay, I'll check it against the backed-up copy of the Firmware that's working. It seems likely it would be that submodule as opposed to NuttX, a module, etc. Do you think I can I use a v1.6.5 version of sitl_gazebo with stable v1.7.5?

@lamping7 Yes, I can launch a single plane, a single iris but multi-iris throws an error. I will paste the output tomorrow. I suspect it might be an environment variable or some configuration. I tried to use Wireshark to monitor UDP traffic but that is troubling-enough to get working in Ubuntu.

Thank you both for helping.

[lamping7]

@mago3421 This seems to be an interfacing issue rather than a PX4 or submodule issue, but I'm sure you're not the only one so, let's figure this out. Let me be more specific on the commands above in case you're not familiar with them. A hunch is telling me there is something wrong with your port configuration or second model.

Can you launch your second plane model by itself?

You'll need to include the namespace for the checkid command and execute it for each. You should see something like:

$ rosrun mavros checkid __ns:=/uav1
OK. I got messages from 1:1.

---
Received 6605 messages, from 1 addresses
sys:comp   list of messages
  1:1     32, 1, 2, 245, 230, 102, 105, 74, 76, 141, 77, 111, 241, 140, 147, 30, 22, 0, 44, 62, 31
Exception in thread Thread-5 (most likely raised during interpreter shutdown):
Traceback (most recent call last):
  File "/usr/lib/python2.7/threading.py", line 801, in __bootstrap_inner

$ rosrun mavros checkid __ns:=/uav2
OK. I got messages from 2:1.

---
Received 6284 messages, from 1 addresses
sys:comp   list of messages
  2:1     32, 0, 2, 245, 230, 102, 1, 105, 74, 140, 141, 77, 76, 241, 147, 62, 111, 30, 31
Exception in thread Thread-4 (most likely raised during interpreter shutdown):

For the other command, you'll need to grab two messages (one for each instance), so the n1 becomes n2 -- rostopic echo -n2 /diagnostics This is going to be long, so paste the output to a gist or pastebin.

[lamping7]

@mago3421 I figured this out, my hunch was on target. So, no need to run those commands above. I'm not sure how this setup worked previously (I don't think it should have worked before, but I could be wrong.), but the issue is a both a combination of port configuration and your sdf model. Let me try to explain this best I can.

Your setting:
param set SITL_UDP_PRT 14562 in the config -- this is good and is necessary

But:
You're including <arg name="mavlink_udp_port" value="14562"/> in multi_uav_mavros_sitl.launch to be passed to single_vehcile_spawn.launch. The problem here is that you're doing nothing with it after that. This argument is meant to be used to generate models with xacro (which you're not doing obviously). You can remove that arg as it isn't being used.

What you should be doing:
You need to manually edit the sdf model you're using to include this info. Specifically, you need to add a couple elements to the mavlink_interface plugin. So, scroll down to the bottom of the file where this plugin is and make sure it begins like this:

<plugin name='mavlink_interface' filename='librotors_gazebo_mavlink_interface.so'>      
      <mavlink_addr>INADDR_ANY</mavlink_addr>
      <mavlink_udp_port>14562</mavlink_udp_port>
      <robotNamespace></robotNamespace>
      <imuSubTopic>/imu</imuSubTopic>

and so on.

I added this to your model, configured my launch to match your fcu_url and it ran normally with your config and the sdf with these two additional elements. You probably need to do this with both of your models, and make sure the config SITL port matches the one in the sdf. If you still have issues, run the commands above.

[mago3421]

@lamping7 Much thanks Anthony! I thought that re-declaration of SITL_UDP_PRT would be benign if left in the launch file. This was a relic after getting multi-iris SITL to work last year. Will try and report back so we can hopefully close the bug.

[lamping7]

The iris model is different in that the model is generated with xacro.

That argument in the launch file is used to populate the value here:
https://github.com/PX4/sitl_gazebo/blob/5e3375facdfae687c4eb3d20aa9da65aea0ec5d1/models/rotors_description/urdf/iris_base.xacro#L9
which then goes here:
https://github.com/PX4/sitl_gazebo/blob/5e3375facdfae687c4eb3d20aa9da65aea0ec5d1/models/rotors_description/urdf/component_snippets.xacro#L145

[mago3421]

@lamping7 Just an update, I cloned the stable branch today and copied my sdf, rcS, and launch files that were posted here. I deleted the mavlink_udp_port arguments and ensured that each plane sdf file had the mavlink_interface plugin configured per your specification. The sdf file did have the mavlink_interface plugins configured like above except the robotNamespace and imuSubTopic lines preceded the MAVLink lines. I tried it both ways and still could not get the command line on the multi-vehicle SITL. I'm going to try on my home machine and report back.

[lamping7]

The order of the elements shouldn't matter. If this still isn't working, maybe you need to start with a clean environment and start over to rule out setup issues. Maybe re-read this old issue thread -- #6408