Trust and torque offboard control ros2 typhoon simulation timeout poll 0, 22

Hello everyone,

Recently iv’e been implementing an offboard control of the Typhoon model in SITL simulation.

I’m trying to send thrust and torque commands using ros2. I’m using uXRCE.
I’ve made a ros2 package where i have a control loop. In this loop i calculate the thrust and torque control signals and then i publish them using VehicleThrustSetpoint and VehicleTorqueSetpoint. (I’m using python for all of this).

I have found that the drone wont fly when i try to send the thrust and torque setpoints. In the PX4 Terminal i get these messages that say:

  • ERROR [simulator_mavlink] poll timeout 0, 22
  • poll timeout

Additionally, i have a main loop that publishes the commands at 50Hz. I checked the ros2 topics with echo to the thrust and torque topics, and the messages are being published and are synchronized with the same timestamp.

However, if i send trajectory setpoints, the drone flies. The error only appears if in the offboard msg i place false to position and true to thrust and torque. (when i do this i do not publish any trajectory setpoint, only thrust and torque ones)

I don’t know what could be happening. Also it is worth to mention that i’m not disabling the lockstep. And when sending TrajectorySetpoints i usually get this warnings:

  • WARN [timesync] time jump detected. Resetting time synchroniser.
  • WARN [uxrce_dds_client] time sync no longer converged
  • INFO [uxrce_dds_client] time sync converged
    However, the drone flies and simulation runs.

This is the QoS profile that i’m using:

    qos_profile = QoSProfile(
        reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, 
        durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, 
        history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, 
        depth=1
    )

This are the publishers:

self.publisher_trust = self.create_publisher(VehicleThrustSetpoint, '/fmu/in/vehicle_thrust_setpoint',qos_profile)

self.publisher_torque = self.create_publisher(VehicleTorqueSetpoint, '/fmu/in/vehicle_torque_setpoint', qos_profile)

I don’t know if this could be helpful but
This is the offboard control msg:

    time_stamp = int(Clock().now().nanoseconds / 1000)
    offboard_msg = OffboardControlMode()
    
    offboard_msg.timestamp = time_stamp
    offboard_msg.position=False
    offboard_msg.velocity=False
    offboard_msg.acceleration=False
    offboard_msg.attitude=False
    offboard_msg.body_rate=False
    offboard_msg.thrust_and_torque=True
    offboard_msg.direct_actuator=False

And here the thrust and torque messages:

        trust_msg = VehicleThrustSetpoint()
        trust_msg.timestamp=time_stamp
        trust_msg.xyz[0] = self.BDR_Rotated_Trust[0]
        trust_msg.xyz[1] = self.BDR_Rotated_Trust[1]
        trust_msg.xyz[2] = self.BDR_Rotated_Trust[2]

        torque_msg = VehicleTorqueSetpoint()
        torque_msg.timestamp=time_stamp
        torque_msg.xyz[0] = self.Rotated_Torque[0]
        torque_msg.xyz[1] = self.Rotated_Torque[1]
        torque_msg.xyz[2] = self.Rotated_Torque[2]

        self.publisher_trust.publish(trust_msg)
        self.publisher_torque.publish(torque_msg)

I’m a bit new to PX4 and gazebo, so i would really appreciate your help to figure out how can i solve this.

Thank you so much everyone! Best regards!

Update: I read in similar posts that disabling lockstep may help to solve this issue. So i did that, i start the simulation with lockstep disabled and also changed this property in the sdf file of the model.

Now that error doesn’t appear anymore. However i’m a bit confused, i though that lockstep disabling was only required if one is sending control signals directly to the motors, which is after the px4 control allocation module. Is it like this? or is it also requiered to control the drone with torque and thrust?

Additionally, i still get the following uxrce warnings:

WARN [timesync] time jump detected. Resetting time synchroniser.
WARN [uxrce_dds_client] time sync no longer converged
INFO [uxrce_dds_client] time sync converged
However, the drone flies and simulation runs.

I’m not sure why these appear, so i would also appreciate any feedback on this.

I’m only thinking that maybe i cant run with lockstep due to lack of computational power, because im sending the thrust and torque signals at 100Hz, so i dont think this is the issue.

Im trying to run the gazebo simulation and my ros2 control node in the same computer and i have only 8gb of ram, however i have a GTX1050 gpu and a I7-8750H (so the pc is a bit old, but not that bad… i mean maybe the ram could be a bottle neck?)

If anyone could have suggestions on how i could manage to get things to work in lockstep i would appreciate it.
Thank you so much!