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!