Disconnection between Mavros and px4 in gazebo_sitl,

1

I want to use mavros and gazebo SITL for simulation.
I followed instructions from two sources:
http://dev.px4.io/simulation-ros-interface.html and http://dev.px4.io/ros-mavros-offboard.html.
But errors occured.
First, I run the gazebo_sitl.

 cd Firmware_clone
 make posix_sitl_default
 source ~/catkin_ws/devel/setup.bash
 source Tools/setup_gazebo.bash $(pwd) build_posix_sitl_default
 export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
 export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
 roslaunch px4 mavros_posix_sitl.launch

Output is:

started roslaunch server http://ubuntu:46399/

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /mavros/cmd/use_comp_id_system_control: False
 * /mavros/conn/heartbeat_rate: 1.0
 * /mavros/conn/system_time_rate: 1.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 10.0
 * /mavros/distance_sensor/hrlv_ez4_pub/field_of_view: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/frame_id: hrlv_ez4_sonar
 * /mavros/distance_sensor/hrlv_ez4_pub/id: 0
 * /mavros/distance_sensor/hrlv_ez4_pub/orientation: ROLL_180
 * /mavros/distance_sensor/hrlv_ez4_pub/send_tf: True
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/laser_1_sub/id: 3
 * /mavros/distance_sensor/laser_1_sub/orientation: ROLL_180
 * /mavros/distance_sensor/laser_1_sub/subscriber: True
 * /mavros/distance_sensor/lidarlite_pub/field_of_view: 0.0
 * /mavros/distance_sensor/lidarlite_pub/frame_id: lidarlite_laser
 * /mavros/distance_sensor/lidarlite_pub/id: 1
 * /mavros/distance_sensor/lidarlite_pub/orientation: ROLL_180
 * /mavros/distance_sensor/lidarlite_pub/send_tf: True
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/sonar_1_sub/id: 2
 * /mavros/distance_sensor/sonar_1_sub/orientation: ROLL_180
 * /mavros/distance_sensor/sonar_1_sub/subscriber: True
 * /mavros/fcu_url: udp://:14540@loca...
 * /mavros/gcs_url: 
 * /mavros/global_position/frame_id: fcu
 * /mavros/global_position/rot_covariance: 99999.0
 * /mavros/global_position/tf/child_frame_id: fcu_utm
 * /mavros/global_position/tf/frame_id: local_origin
 * /mavros/global_position/tf/send: False
 * /mavros/image/frame_id: px4flow
 * /mavros/imu/angular_velocity_stdev: 0.000349065850399
 * /mavros/imu/frame_id: fcu
 * /mavros/imu/linear_acceleration_stdev: 0.0003
 * /mavros/imu/magnetic_stdev: 0.0
 * /mavros/imu/orientation_stdev: 1.0
 * /mavros/local_position/frame_id: fcu
 * /mavros/local_position/tf/child_frame_id: fcu
 * /mavros/local_position/tf/frame_id: local_origin
 * /mavros/local_position/tf/send: False
 * /mavros/local_position/tf/send_fcu: False
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mocap/use_pose: True
 * /mavros/mocap/use_tf: False
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.0
 * /mavros/px4flow/ranger_max_range: 5.0
 * /mavros/px4flow/ranger_min_range: 0.3
 * /mavros/safety_area/p1/x: 1.0
 * /mavros/safety_area/p1/y: 1.0
 * /mavros/safety_area/p1/z: 1.0
 * /mavros/safety_area/p2/x: -1.0
 * /mavros/safety_area/p2/y: -1.0
 * /mavros/safety_area/p2/z: -1.0
 * /mavros/setpoint_accel/send_force: False
 * /mavros/setpoint_attitude/reverse_throttle: False
 * /mavros/setpoint_attitude/tf/child_frame_id: attitude
 * /mavros/setpoint_attitude/tf/frame_id: local_origin
 * /mavros/setpoint_attitude/tf/listen: False
 * /mavros/setpoint_attitude/tf/rate_limit: 10.0
 * /mavros/setpoint_position/tf/child_frame_id: setpoint
 * /mavros/setpoint_position/tf/frame_id: local_origin
 * /mavros/setpoint_position/tf/listen: False
 * /mavros/setpoint_position/tf/rate_limit: 50.0
 * /mavros/startup_px4_usb_quirk: True
 * /mavros/sys/disable_diag: False
 * /mavros/sys/min_voltage: 10.0
 * /mavros/target_component_id: 1
 * /mavros/target_system_id: 1
 * /mavros/tdr_radio/low_rssi: 40
 * /mavros/time/time_ref_source: fcu
 * /mavros/time/timesync_avg_alpha: 0.6
 * /mavros/vibration/frame_id: vibration
 * /mavros/vision_pose/tf/child_frame_id: vision
 * /mavros/vision_pose/tf/frame_id: local_origin
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: False
 * /rosdistro: indigo
 * /rosversion: 1.11.20
 * /use_sim_time: True

NODES
  /
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    mavros (mavros/mavros_node)
    sitl (px4/px4)
    vehicle_spawn_ubuntu_7552_4607384523363716699 (gazebo_ros/spawn_model)

auto-starting new master
process[master]: started with pid [7564]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 8a8c16f0-b0b7-11e6-9853-000c29399553
process[rosout-1]: started with pid [7577]
started core service [/rosout]
node name: sitl
data path: /home/zyq/px4/src/Firmware
commands file: /home/zyq/px4/src/Firmware/posix-configs/SITL/init/lpe/iris

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

0 WARNING: setRealtimeSched failed (not run as root?)
process[sitl-2]: started with pid [7602]
process[gazebo-3]: started with pid [7610]
process[gazebo_gui-4]: started with pid [7614]
process[vehicle_spawn_ubuntu_7552_4607384523363716699-5]: started with pid [7620]
INFO  [dataman] Unkown restart, data manager file 'rootfs/fs/microsd/dataman' size is 47640 bytes
process[mavros-6]: started with pid [7621]
[ INFO] [1479821304.393029772]: FCU URL: udp://:14540@localhost:14557
[ INFO] [1479821304.393343712]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1479821304.393436053]: udp0: Remote address: 127.0.0.1:14557
[ INFO] [1479821304.393608685]: GCS bridge disabled
[ INFO  [platforms__posix__drivers__ledsim] LED::init
[ INFO  [platforms__posix__drivers__ledsim] LED::init
[ INFO  [simulator] Waiting for initial data on UDP port 14560. Please start the flight simulator to proceed..
[ INFO] [1479821304.555421828]: Plugin 3dr_radio loaded and initialized
[ INFO] [1479821304.559220350]: Plugin actuator_control loaded and initialized
[ INFO] [1479821304.560254603]: Plugin altitude loaded and initialized
[ INFO] [1479821304.812227139]: Plugin cam_imu_sync loaded and initialized
[ INFO] [1479821304.817701056]: Plugin command loaded and initialized
[ INFO] [1479821304.820659219]: Plugin distance_sensor blacklisted
spawn_model script started
[ INFO] [1479821304.836865293]: Plugin ftp loaded and initialized
[ INFO] [1479821304.865121876]: Plugin global_position loaded and initialized
[ INFO] [1479821304.865929889]: Plugin hil_controls loaded and initialized
[ INFO] [1479821304.865980532]: Plugin image_pub blacklisted
[ INFO] [1479821304.883901255]: Plugin imu_pub loaded and initialized
[ INFO] [1479821304.907276938]: Plugin local_position loaded and initialized
[ INFO] [1479821304.909247006]: Plugin manual_control loaded and initialized
[ INFO] [1479821304.932163120]: Plugin mocap_pose_estimate loaded and initialized
[ INFO] [1479821304.936041425]: Plugin param loaded and initialized
[ INFO] [1479821304.959535264]: Plugin px4flow loaded and initialized
[ INFO] [1479821304.972318570]: Plugin rc_io loaded and initialized
[ INFO] [1479821304.972434868]: Plugin safety_area blacklisted
[ INFO] [1479821304.992986404]: Plugin setpoint_accel loaded and initialized
[ INFO] [1479821305.048057451]: Plugin setpoint_attitude loaded and initialized
[ INFO] [1479821305.079119772]: Plugin setpoint_position loaded and initialized
[ INFO] [1479821305.122359729]: Plugin setpoint_raw loaded and initialized
[ INFO] [1479821305.132584656]: Plugin setpoint_velocity loaded and initialized
[ INFO] [1479821305.171421701]: Plugin sys_status loaded and initialized
[INFO] [WallTime: 1479821305.179778] [0.000000] Loading model xml from file
[INFO] [WallTime: 1479821305.180019] [0.000000] Waiting for service /gazebo/spawn_sdf_model
[ INFO] [1479821305.194367707]: Plugin sys_time loaded and initialized
[ INFO] [1479821305.199679132]: Plugin vfr_hud loaded and initialized
[ INFO] [1479821305.199769050]: Plugin vibration blacklisted
[ INFO] [1479821305.248354842]: Plugin vision_pose_estimate loaded and initialized
[ INFO] [1479821305.261666685]: Plugin vision_speed_estimate loaded and initialized
[ INFO] [1479821305.272122837]: Plugin waypoint loaded and initialized
[ INFO] [1479821305.272229519]: Autostarting mavlink via USB on PX4
[ INFO] [1479821305.272358516]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1479821305.272402982]: Built-in MAVLink package version: 2016.5.20
[ INFO] [1479821305.272437647]: Built-in MAVLink dialect: ardupilotmega
[ INFO] [1479821305.272470903]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1479821305.543772214]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1479821305.545882892]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1479821306.019725360, 0.022000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1479821306.078201046, 0.056000000]: Physics dynamic reconfigure ready.
[INFO] [WallTime: 1479821306.091521] [0.066000] Calling service /gazebo/spawn_sdf_model
[INFO] [WallTime: 1479821306.207724] [0.132000] Spawn status: SpawnModel: Successfully spawned model
[vehicle_spawn_ubuntu_7552_4607384523363716699-5] process has finished cleanly
log file: /home/zyq/.ros/log/8a8c16f0-b0b7-11e6-9853-000c29399553/vehicle_spawn_ubuntu_7552_4607384523363716699-5*.log

Then I run the offb_node node in another terminal, this teminal outputed nothing.
But the SITL teminal began to give error messages:
[ERROR] [1479812333.873957573, 240.232000000]: MODE: Unsupported FCU
[ERROR] [1479812338.925361421, 245.282000000]: MODE: Unsupported FCU

And the rqt graph showed:

The mavros node had no connection with the gazebo node.

I run rostopic echo /diagnostics, the teminal outputed as follow. It showed FCU connection:message:not connected.
—
header:
seq: 150
stamp:
secs: 145
nsecs: 0
frame_id: ‘’
status:
-
level: 1
name: mavros: FCU connection
message: not connected
hardware_id: udp://:14540@127.0.0.1:14557
values:
-
key: Received packets:
value: 0
-
key: Dropped packets:
value: 0
-
key: Buffer overruns:
value: 0
-
key: Parse errors:
value: 0
-
key: Rx sequence number:
value: 0
-
key: Tx sequence number:
value: 86
-
key: Rx total bytes:
value: 0
-
key: Tx total bytes:
value: 220368
-
key: Rx speed:
value: 0.000000
-
key: Tx speed:
value: 6377.000000
-
level: 2
name: mavros: GPS
message: No satellites
hardware_id: udp://:14540@127.0.0.1:14557
values:
-
key: Satellites visible
value: 0
-
key: Fix type
value: 0
-
key: EPH (m)
value: Unknown
-
key: EPV (m)
value: Unknown
-
level: 2
name: mavros: Heartbeat
message: No events recorded.
hardware_id: udp://:14540@127.0.0.1:14557
values:
-
key: Heartbeats since startup
value: 0
-
key: Frequency (Hz)
value: 0.000000
-
key: Vehicle type
value: Generic
-
key: Autopilot type
value: Generic
-
key: Mode
value: ‘’
-
key: System status
value: Uninit
-
level: 0
name: mavros: System
message: Normal
hardware_id: udp://:14540@127.0.0.1:14557
values:
-
key: Sensor present
value: 0x00000000
-
key: Sensor enabled
value: 0x00000000
-
key: Sensor helth
value: 0x00000000
-
key: CPU Load (%)
value: 0.0
-
key: Drop rate (%)
value: 0.0
-
key: Errors comm
value: 0
-
key: Errors count #1
value: 0
-
key: Errors count #2
value: 0
-
key: Errors count #3
value: 0
-
key: Errors count #4
value: 0
-
level: 2
name: mavros: Battery
message: No data
hardware_id: udp://:14540@127.0.0.1:14557
values:
-
key: Voltage
value: -1.00
-
key: Current
value: 0.0
-
key: Remaining
value: 0.0
-
level: 2
name: mavros: Time Sync
message: No events recorded.
hardware_id: udp://:14540@127.0.0.1:14557
values:
-
key: Timesyncs since startup
value: 0
-
key: Frequency (Hz)
value: 0.000000
-
key: Last dt (ms)
value: 0.000000
-
key: Mean dt (ms)
value: 0.000000
-
key: Last system time (s)
value: 0.000000000
-
key: Time offset (s)
value: 0.000000000
—

I want to know how to solve this problem.
And whethe I can do onboard operations with gazebo sitl?
If you have good gazebo simulation instructions, could you recommend to me?
Could you help me? Thanks.

2

I’ve submitted the current workflow I use for gazebo simulation with mavros here:


The workaround presented there should be able to make everything work even if it is a little wasteful with package redundancies
2 Likes
3

@eric1221bday
Now I can use mavros to connect to the simulated drone in gazebo!
Your advice is really helpful! Thank you very much!
And when I run the command:

roslaunch px4 mavros_posix_sitl.launch

the teminal gave me many warnings:

I want to know whether these warnings have any effction to simulation?
Can I ignore them?

4

Those warnings happen when gazebo is running too slowly, probably because your computer struggling a bit, you can mostly ignore them, just keep in mind there will be a difference between the fcu timestamp and the ros time, /diagnostics should give you the current offset

5

Yes I have checked the /diagnostics topic for current time offset.
Thanks for your answer!

6

Is there a clean solution to this problem? Or is the only solution thus far in order to launch mavros_posix_sitl.launch is to clone the sitl_gazebo package separately?

7

@han-so1omon
Now we don’t need to clone the sitl_gazebo package separately.
You can see the discussions under this link:


Here is a new way, and the instruction page about this question on PX4/Devguide had updated.

cd <Firmware_clone>
make posix_sitl_default
source ~/catkin_ws/devel/setup.bash    // (optional)
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build_posix_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
roslaunch px4 mavros_posix_sitl.launch

(optional): only source the catkin workspace if you compiled MAVROS or other ROS packages from source

8

My modified version of this in a shell script is below. The result of running px4 mavros_posix_sitl.launch after this, then using the offboard node prescribed on the dev.px4.io site is
[ERROR] [<numbers>][<numbers>] MODE: Unsupported FCU.
However, if I run, directory-independent, make -C <$PX4_FIRMWARE> posix_sitl_default gazebo. Then, from another terminal, run roslaunch mavros px4.launch fcu_url:=<udp://:14540@14557 and rosrun <mypkg> offb_node, then the simulation interfaces fine with mavros and the offboard node.

Shell script source below:

# Source px4/firmware as prescribed by
# dev.px4.io/simulation-ros-interface.html
echo "Making PX4/Firmware posix_sitl_default target"
make -C ${PX4_FIRMWARE} posix_sitl_default

echo "Sourcing catkin_ws just in case it has not been done"
source ${CATKIN_HOME}/devel/setup.bash

echo "Setting up PX4/Firmware for gazebo"
source ${PX4_FIRMWARE}/Tools/setup_gazebo.bash ${PX4_FIRMWARE} ${PX4_FIRMWARE}/build_posix_sitl_default

# These work fine. They prevent things from being added to PATH-type variables more than once.
case ":$ROS_PACKAGE_PATH:" in
  *":$PX4_FIRMWARE:"*) :;;
  *) export ROS_PACKAGE_PATH="$ROS_PACKAGE_PATH:$PX4_FIRMWARE";;
esac

case ":$ROS_PACKAGE_PATH:" in
  *":${PX4_FIRMWARE}/Tools/sitl_gazebo:"*) :;;
  *) export ROS_PACKAGE_PATH="$ROS_PACKAGE_PATH:${PX4_FIRMWARE}/Tools/sitl_gazebo";;
esac
9

Errors will also happen if you execute commands in command line directly?
I don’t know shell script very well, but I can do well with those commands.
If cloning sitl_gazebo package seperately will work for you, you can just do it.

10

Actually, now the error is different when running directly from command line. mavros complains of [ERROR] [<numbers>] [<numbers>]: udp: sendto: Invalid argument. Although, it still works the way that I mentioned previously was working (make posix_sitl_default gazebo, then starting ROS nodes).

The problem, as far as I can guess, is that the correct PX4 firmware isn’t being built or is built but is not interfaced with Gazebo if I don’t explicitly run `make -C $PX4_FIRMWARE posix_sitl_default gazebo.

The only aspect of this erroneous behavior that is perfectly clear is that the PX4/Gazebo/ROS environment is different when PX4/Gazebo is started with make -C $PX4_FIRMWARE posix_sitl_default gazebo versus when PX4/Gazebo/ROS is started with roslaunch px4 posix_sitl.launch

11

If building PX4 firmware again still can’t solve this problem, I think maybe you can ask this problem in a new topic so that more people will see.

12

So, as I mentioned in the issue linked, if you do not run make posix_sitl_default gazebo at least once, then gazebo related packages will not run, the instructions @zyq1993 posted here is actually missing the gazebo at the end of the make instruction. If you look at the devguide now directly you will see the correct instructions, that is the source of your first error I think. Your second error I think relates to the fact that you did not run the ros command correctly, could you try just running roslaunch px4 macros_posix_sitl.launch, that will set everything up.

13

The correct steps:
cd <Firmware_clone>
make posix_sitl_default gazebo
source ~/catkin_ws/devel/setup.bash // (optional)
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build_posix_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
roslaunch px4 mavros_posix_sitl.launch
(optional): only source the catkin workspace if you compiled MAVROS or other ROS packages from source