Multicopter SITL not receiving ROS2 messages

PX4 Autopilot
1

Hi everybody!
I’m currently working on a project in which I have to simulate multiple copters in an environment.
Preliminary info: I’m working with ROS2 Humble, Gazebo 7 and I’ve forked PX4 with small changes.

I’ve started from the single case:

  • execute PX4 with
    PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE='0,0' PX4_GZ_MODEL=x500 ./build/px4_sitl_default/bin/px4 -i 1
    in order to get the MAV_SYS_ID equal to the istance number (if I got that correctly)
  • start the Micro XRCE agent with MicroXRCEAgent udp4 -p 8888
  • launch my ROS2 code, which at the moment starts up the Offboard mode, arm the vehicle and gives fixed setpoints such that the drone will hover at 5m (done following the offboard example)
    Here’s a snippet of the message sent:
px4_msgs::msg::VehicleCommand msg {};
msg.param1 = param1;
msg.param2 = param2;
msg.command = command;
msg.target_system = 0;
msg.target_component = 0;
msg.source_system = 1;
msg.source_component = 1;
// msg.confirmation = 0;
msg.from_external = true;
msg.timestamp = this->gatherTime().nanoseconds() / 1000; // gatherTime() calls the node 'now()' function
this->pub_to_command_topic_->publish(msg);

Since it seems to do what I intend, I was also trying to do the same in the multi-vehicle simulation.
Through some code I create a bash script which does the following

#!/bin/bash
gnome-terminal --tab -t 'Gazebo' -- bash -c 'source /home/slim71/Documents/git/SpartanLIFT/install/odst/share/odst/resource/include_px4_models.sh && gz sim -r tugbot_depot.sdf'
gnome-terminal --tab -t 'PX4 - x500' -- bash -c 'cd PX4-Autopilot; PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE='0,0' PX4_GZ_MODEL=x500 ./build/px4_sitl_default/bin/px4 -i 1'
gnome-terminal --tab -t 'PX4 - X3' -- bash -c 'cd PX4-Autopilot; sleep 6; PX4_SYS_AUTOSTART=4242 PX4_GZ_MODEL_POSE='0,1' PX4_GZ_MODEL=X3 ./build/px4_sitl_default/bin/px4 -i 2'
gnome-terminal --tab -t 'PX4 - X3' -- bash -c 'cd PX4-Autopilot; sleep 7; PX4_SYS_AUTOSTART=4242 PX4_GZ_MODEL_POSE='0,2' PX4_GZ_MODEL=X3 ./build/px4_sitl_default/bin/px4 -i 3'
gnome-terminal --tab -t 'PX4 - emesent_hovermap' -- bash -c 'cd PX4-Autopilot; sleep 8; PX4_SYS_AUTOSTART=4007 PX4_GZ_MODEL_POSE='0,3' PX4_GZ_MODEL=emesent_hovermap ./build/px4_sitl_default/bin/px4 -i 4'
gnome-terminal --tab -t 'PX4 - typhoon_h480' -- bash -c 'cd PX4-Autopilot; sleep 9; PX4_SYS_AUTOSTART=6012 PX4_GZ_MODEL_POSE='0,4' PX4_GZ_MODEL=typhoon_h480 ./build/px4_sitl_default/bin/px4 -i 5'
gnome-terminal --tab -t 'Interactive shell' -- bash

Then, for now, I simply run the same ROS2 code manually in another terminal. I can see the messages flow through the correct topics (starting in /px4_1/, since they are for the agent 1), but nothing happens in the simulation.

So I have a couple of questions:

  • is appending -i <x> the right way to link a PX4 model to a specific MAV_SYS_ID (since this is needed for the VehicleCommand messages)?
  • is thera a way to launch PX4 as verbose to understand what’s happening differently in the two cases?
  • any other stuff to check?

I can share mode code if needed, it’s all public anyway! I haven’t done that straight away because it’s a bit much.

1 Like
2

I think I’ve found something: it all goes well when I launch PX4 on its own (which actually opens up Gazebo too, of course), but when I open up Gazebo manually with gz sim -r <world>.sdf as I do in the script, I get some warnings of Preflight checks failed:

slim71@slim71-Ubuntu:~/Documents/git/SpartanLIFT/PX4-Autopilot$ PX4_SYS_AUTOSTART=4001 PX4_GZ_MODEL_POSE='0,0' PX4_GZ_MODEL=x500 ./build/px4_sitl_default/bin/px4 -i 1
INFO  [px4] instance: 1
INFO  [px4] working directory /home/slim71/Documents/git/SpartanLIFT/PX4-Autopilot/build/px4_sitl_default/roo
______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] startup script: /bin/sh etc/init.d-posix/rcS 1
env SYS_AUTOSTART: 4001
INFO  [param] selected parameter default file parameters.bson
INFO  [param] importing from 'parameters.bson'
INFO  [parameters] BSON document size 443 bytes, decoded 443 bytes (INT32:16, FLOAT:6)
INFO  [param] selected parameter backup file parameters_backup.bson
INFO  [dataman] data manager file './dataman' size is 7866640 bytes
INFO  [init] gazebo already running world: world_demo
INFO  [init] PX4_GZ_MODEL_POSE set, spawning at: 0,0
INFO  [gz_bridge] world: world_demo, model name: x500_1, simulation model: x500
INFO  [gz_bridge] Requested Model Position: 0,0
INFO  [lockstep_scheduler] setting initial absolute time to 27310000 us
INFO  [commander] LED: open /dev/led0 failed (22)
WARN  [health_and_arming_checks] Preflight Fail: barometer 0 missing
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
INFO  [tone_alarm] home set
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18571 remote port 14550
INFO  [tone_alarm] notify negative
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14581 remote port 14541
INFO  [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14281 remote port 14031
INFO  [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13031 remote port 13281
INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2023-10-25/19_49_51.ulg	
INFO  [logger] Opened full log file: ./log/2023-10-25/19_49_51.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [px4] Startup script returned successfully
pxh> INFO  [uxrce_dds_client] synchronized with time offset 1698263363654153us
INFO  [uxrce_dds_client] successfully created rt/px4_1/fmu/out/failsafe_flags data writer, topic id: 80
INFO  [uxrce_dds_client] successfully created rt/px4_1/fmu/out/position_setpoint_triplet data writer, topic iINFO  [uxrce_dds_client] successfully created rt/px4_1/fmu/out/sensor_combined data writer, topic id: 168
INFO  [uxrce_dds_client] successfully created rt/px4_1/fmu/out/timesync_status data writer, topic id: 190
INFO  [uxrce_dds_client] successfully created rt/px4_1/fmu/out/vehicle_attitude data writer, topic id: 207
INFO  [uxrce_dds_client] successfully created rt/px4_1/fmu/out/vehicle_control_mode data writer, topic id: 21INFO  [uxrce_dds_client] successfully created rt/px4_1/fmu/out/vehicle_gps_position data writer, topic id: 21INFO  [uxrce_dds_client] successfully created rt/px4_1/fmu/out/vehicle_local_position data writer, topic id: INFO  [uxrce_dds_client] successfully created rt/px4_1/fmu/out/vehicle_odometry data writer, topic id: 225
INFO  [uxrce_dds_client] successfully created rt/px4_1/fmu/out/vehicle_status data writer, topic id: 230
WARN  [health_and_arming_checks] Preflight Fail: barometer 0 missing
INFO  [uxrce_dds_client] successfully created rt/px4_1/fmu/out/vehicle_global_position data writer, topic id:WARN  [health_and_arming_checks] Preflight Fail: barometer 0 missing
WARN  [health_and_arming_checks] Preflight Fail: barometer 0 missing
WARN  [health_and_arming_checks] Preflight Fail: barometer 0 missing
INFO  [tone_alarm] notify negative
WARN  [health_and_arming_checks] Preflight Fail: barometer 0 missing

Since the commander is never ready for takeoff, nothing happens…
Any feedback about this?

3

Maybe my first idea was wrong…
I’ve tried dividing the Gazebo launch in server and GUI with gz sim -r -s and gz sim -g both manually and through the script, but I had no luck with my issue…
It just seems that sometimes the sensor are OK and sometimes not. Most of the time I end up with all PX4 instances complaining about missing sensors, even though they work out great on their own… I might inspect more and open another topic, with the correct title and info, if I find something more insightful

4

Hi @slim71,

I created a ROS 2 launch file that loads gz, PX4 instances, and offboard node for each instance, maybe you could find it useful.

This is part of the launch file:

    px4_procs = []
    followers_nodes = []
    for i in range(number_of_instances):
        px4_launch_command = f'{mode} PX4_SYS_AUTOSTART=4001 ' +  f'PX4_GZ_MODEL_POSE="0,{2*i}" ' + f'PX4_GZ_MODEL=x500 ./build/px4_sitl_default/bin/px4 -i {i}'
        sleep_time_s = 0 if i == 0 else 5 + i
        px4_procs.append(
        ExecuteProcess(
                cmd=['bash', '-c', f'sleep {sleep_time_s} && cd /workspaces/test/PX4-Autopilot && {px4_launch_command}'],
                output='screen',
                emulate_tty=True
            )
        )

        if i > 0:
            followers_nodes.append(
                Node(
                    package='px4_offboard',
                    executable='offboard_control_follower',
                    output='screen',
                    name=f'offboard_control_follower_{i}',
                    parameters=[{'id': i}],
                    emulate_tty=True
                )
            )

...
...
    elements_to_launch = [node_leader, node_dds_agent, node_rosbridge, sys_shut_down] + px4_procs + followers_nodes

    ld = LaunchDescription(elements_to_launch)
    return ld

I added a delay (sleep_time_s) between instances so that all instances connect to the same gz.

5

I have something similar, so that should be the way, thank you! :slight_smile:
it seems there’s something else for me, still trying to understand what

6

It seems I’ve found the issue!
I double checked the airframe files and found that the barometer sensor simulation was off!
So I just set param set-default SENS_EN_BAROSIM 1 and they worked fine. I still don’t really understand why manually launching a model worked while this way it didn’t, but maybe it’s because of some background stuff difference… :man_shrugging:t5:

7

After a long time, I’ve encountered this again!
Needing to activate a sensor to be able to solve this didn’t make sense at the time, but I wasn’t complaining… Turns out, since I don’t really clean the build files of PX4 a lot (not much changes in my project, PX4-wise), it was indeed something else and I just wasn’t seeing it.

In the PX4 tag I have checked out, the /fmu/out/vehicle_command_ack topic was not among the enabled ones in the dds_topics.yaml file. After manually adding it (and doing only that between two subsequent runs), I can confirm that without it no movements is initialized. Then I guess the policy is “no ack, no start”, so to speak! :joy:

1 Like
8

i have the same problem ,i found that the vehicle_odometry topic send out x=0，y=0for all drones，should be y=3 6 9for drone 1 2 3,i can see all topics with namespace /px4_instance do u know how to solve this？

9

if you get actual data in the topics, I’d say there’s something else… or maybe you either specify the wrong spawn point or don’t do that at all…

maybe try opening a new question with more details? :man_shrugging:t5:

10

I found that when i use gazebo instead of gazebo-classic the output data is correct,thank u so much