Offboard mode AttitudeSetpoint failure over ROS2 and FastRTPS

Hey,

We are trying to control a drone from ROS2 over FastRTPS using the topic VehicleAttitudeSetPoint_PubSubTopic, but our drone is failing miserably the instant we switch to OFFBOARD mode as seen in the attached video.

Taking your advice @tsc21 (from this thread) we looked at how the MAVLink message is handled and in addition went to look in Mavros how the different flags are set to reproduce the exact same sequences as when the mavlink messages are sent from Mavros. We also took the bit from the px4_ros_com offboard example for timestamp synchronisation.

But whenever we switch to OFFBOARD mode, setpoints for the body rate controllers stop being sent (you can see the red curve stutter in the video) and the drone crashes. I would have expected to still have an attitude controller running and sending setpoints to the underlying controlers, but for some reason this is not happening even after reproducing the exact same behavior (in terms of uORB topics) as when a mavlink message is sent by Mavros and received by PX4.

I also saw that other people faced the same issue without finding answers. @Andrew_Saba did you find a solution to your problem ?


What did we miss ?


  • px4_ros_com version: 958ccdb
  • PX4-Autopilot version : 378b41a

Please find in this public Google Drive :

  • A video showing the problem
  • The ulog files for this flight
  • The code used during this flight (spidrone_control.py, spidrone_control.yaml)

https://review.px4.io/plot_app?log=bd61e22e-3ec1-477c-ba27-4bebd95d7156

@deb0ch Looking at your code, you are setting the attitude via roll_body pitch_body yaw_body. However these fields are used for body rates.

But then you specify that you are ignoring body rate setpoints and enable attitude setpoints. So you have conflicting masks with data

Hello @Jaeyoung-Lim, first thank you for your swift answer !

I looked in more details and it seems to me that there is more to it than your first answer.

Firstly, you are talking about masks so I want to make sure that there is no confusion: neither Mavlink nor Mavros are involved in my example, I am only using FastRTPS publishing directly to uORB topics, and the VehicleAttitudeSetpoint does not have the type_mask field that the Mavlink message has.

I also find it highly unlikely that the fields that I am using are for angular rates :

  • From the comments in the code, the fields that I am using seem to be documented for setpoints of attitude and not angular rates.
  • Moreover, if you look at mavlink_receiver.cpp, you can see that attitude and angular rate setpoints are sent in two different steps and topics: here and here.

However, when I read the code again after your response I saw that there is indeed a step that I am not doing the same which is filling up the quaternion field ; seeing both euler and quaternion being documented for the same use led me to assume that you could use either the one or the other. Notice that the roll_body pitch_body and yaw_body are filled with the same angles as the ones held by the q_d quaternion, so seeing that I doubt even more that these should hold an attitude rate.

I will get back to you whenever we have tried filling this quaternion field as well.