I’m trying to control a drone in Offboard mode. I successfully arm the drone and start sending trajectory_setpoint messages before switching to Offboard mode.
Below is the relevant part of my code:
class CustomController(Node):
def __init__(self):
super().__init__('custom_controller')
qos = QoSProfile(
reliability=QoSReliabilityPolicy.BEST_EFFORT,
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
history=HistoryPolicy.KEEP_LAST,
depth=10
)
# Паблишер режима offboard
self.offb_pub = self.create_publisher(
OffboardControlMode,
'/fmu/in/offboard_control_mode',
qos
)
# Паблишер тяг
self.thrust_pub = self.create_publisher(
VehicleThrustSetpoint,
'/fmu/in/vehicle_thrust_setpoint',
qos
)
# Подписка на разрешение управления
self.control_permission = False
self.create_subscription(
Bool,
'control_permission',
self.control_permission_callback,
qos
)
# Для графиков
self.times = []
self.thrusts = []
self.start_time = time.time()
self.get_logger().info("CustomController node started")
self.create_timer(0.05, self.control_loop)
def control_permission_callback(self, msg: Bool):
self.control_permission = msg.data
def control_loop(self):
now_us = int(self.get_clock().now().nanoseconds / 1000)
offb = OffboardControlMode()
offb.timestamp = now_us
offb.position = False
offb.velocity = False
offb.acceleration = True
offb.attitude = False
offb.body_rate = False
self.offb_pub.publish(offb)
# 2) формируем VehicleThrustSetpoint
thrust_msg = VehicleThrustSetpoint()
thrust_msg.timestamp = now_us
if not self.control_permission:
# до разрешения — нулевая тяга
thrust_msg.xyz = [0.0, 0.0, 0.0]
# сбрасываем таймер старта
self.start_time = time.time()
else:
t = time.time() - self.start_time
thrust_value = -0.5
thrust_msg.xyz = [0.0, 0.0, thrust_value]
self.thrust_pub.publish(thrust_msg)
def plot_data(self):
plt.figure()
plt.plot(self.times, self.thrusts, label='thrust_z')
plt.xlabel('Time (s)')
plt.ylabel('Thrust (NED Z)')
plt.legend()
plt.tight_layout()
plt.savefig('thrust_plot.png')
plt.show()
def main(args=None):
rclpy.init(args=args)
node = CustomController()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.plot_data()
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
In the control_loop, I publish OffboardControlMode and VehicleThrustSetpoint messages at 20 Hz. I enable only the acceleration flag in OffboardControlMode and set the thrust setpoint to a constant negative Z value (downward thrust in NED frame). Before permission is granted (via a custom Bool topic), the thrust is set to zero.
Despite successfully arming the drone and transitioning to Offboard mode, the drone does not respond or move. There are no errors, but it just hovers or remains idle.
What might I be missing in this setup? Is the thrust setpoint sufficient to trigger movement in Offboard mode, or should I be using another control interface (e.g., attitude or position)? Any advice or example would be appreciated.