I am trying to offboard transtion the gazebo standard VTOL in SITL, and am running into lots of problems. I am using PX4 v1.14.0, ROS2 humble and uDDS, all on ubuntu 22.04.
No matter what configuration I try, the VTOL does not seem to transition. It will go at my specified velocity and then slowly lose altitude and speed before the quad-chute deploys. In gazebo it looks like the pusher prop never turns on, which is very confusing. I can transition the VTOL in other modes through the QGroundControl GUI, using hold and stabalized mode with the on screen buttons to manually transition, but I really want to accomplish this in offboard mode.
I have tried many different configurations of the code below, all to no sucess. I thought the VTOL transition command should be a binary switch type of thing, so I have tried just sending the command once then continuing to send attitude setpoints, just trying to transition and then nothing, transitioning right after takeoff, etc. I have included my log file of my most recent trial, as well as my full code.
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleLocalPosition, VehicleStatus, VehicleOdometry, VehicleCommandAck, VehicleAttitudeSetpoint
import numpy as np
class OffboardControl(Node):
""" Node for controlling a vehicle in offboard mode. """
def __init__(self) -> None:
super().__init__('offboard_control_takeoff_and_land')
# Configure QoS profile for publishing and subscribing
qos_profile = QoSProfile(reliability = ReliabilityPolicy.BEST_EFFORT,
durability = DurabilityPolicy.TRANSIENT_LOCAL,
history = HistoryPolicy.KEEP_LAST,
depth = 1)
# Create publishers
self.offboard_control_mode_publisher = self.create_publisher(
OffboardControlMode, '/fmu/in/offboard_control_mode', qos_profile)
self.trajectory_setpoint_publisher = self.create_publisher(
TrajectorySetpoint, '/fmu/in/trajectory_setpoint', qos_profile)
self.vehicle_command_publisher = self.create_publisher(
VehicleCommand, '/fmu/in/vehicle_command', qos_profile)
self.vehicle_attitude_publisher = self.create_publisher(
VehicleAttitudeSetpoint, '/fmu/in/vehicle_attitude_setpoint', qos_profile)
# Create subscribers
self.vehicle_local_position_subscriber = self.create_subscription(
VehicleLocalPosition, '/fmu/out/vehicle_local_position', self.vehicle_local_position_callback, qos_profile)
self.vehicle_status_subscriber = self.create_subscription(
VehicleStatus, '/fmu/out/vehicle_status', self.vehicle_status_callback, qos_profile)
self.vehicle_odometry_subscriber = self.create_subscription(
VehicleOdometry, '/fmu/out/vehicle_odometry', self.vehicle_odometry_callback, qos_profile)
self.vehicle_command_ack_subscriber = self.create_subscription(
VehicleCommandAck, '/fmu/out/vehicle_command_ack', self.vehicle_command_ack_callback, qos_profile)
# Initialize variables
self.offboard_setpoint_counter = 0
self.vehicle_local_position = VehicleLocalPosition()
self.vehicle_status = VehicleStatus()
self.vehicle_odometry = VehicleOdometry()
self.vehicle_command_ack = VehicleCommandAck()
self.vehicle_vel_x = self.vehicle_odometry.velocity[0]
self.takeoff_height = -30.0
#Create a timer to publish control commands
self.timer = self.create_timer(0.1, self.timer_callback)
self.taken_off = None
self.first_setpoint = None
self.second_setpoint = None
self.landed = None
self.transitioned = None
self.vehicle_state = self.vehicle_status.vehicle_type
self.x_vel = self.vehicle_odometry.velocity[0]
def vehicle_local_position_callback(self, vehicle_local_position):
"""Callback function for vehicle_local_position topic subscriber"""
self.vehicle_local_position = vehicle_local_position
def vehicle_status_callback(self, vehicle_status):
"""Callback function for vehicle_status topic subscriber."""
self.vehicle_status = vehicle_status
def vehicle_odometry_callback(self, vehicle_odometry):
"""Callback function for vehicle_status topic subscriber."""
self.vehicle_odometry = vehicle_odometry
def vehicle_command_ack_callback(self, vehicle_command_ack):
"""Callback function for vehicle_status topic subscriber."""
self.vehicle_command_ack= vehicle_command_ack
def extra_callback(self):
self.x_vel = self.vehicle_odometry.velocity[0]
self.vehicle_state = self.vehicle_status.vehicle_type
def arm(self):
"""Send an arm command to the vehicle."""
self.publish_vehicle_command(
VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=1.0)
self.get_logger().info('Arm command sent')
def disarm(self):
"""Send a disarm command to the vehicle."""
self.publish_vehicle_command(
VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=0.0)
self.get_logger().info('Disarm command sent')
def engage_offboard_mode(self):
"""Switch to offboard mode."""
self.publish_vehicle_command(
VehicleCommand.VEHICLE_CMD_DO_SET_MODE, param1=1.0, param2=6.0)
self.get_logger().info("Switching to offboard mode")
def land(self):
"""Switch to land mode."""
self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_NAV_LAND)
self.get_logger().info("Switching to land mode")
def publish_offboard_control_heartbeat_signal(self):
"""Publish the offboard control mode."""
msg = OffboardControlMode()
msg.position = True
msg.velocity = True
msg.acceleration = False
msg.attitude = True
msg.body_rate = False
msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
self.offboard_control_mode_publisher.publish(msg)
def publish_position_setpoint(self, x: float, y: float, z: float):
"""Publish the trajectory setpoint."""
msg = TrajectorySetpoint()
msg.position = [x, y, z]
msg.yaw = 0.0 # (0 degree)
msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
self.trajectory_setpoint_publisher.publish(msg)
self.get_logger().info(f"Publishing position setpoints {[x, y, z]}")
def publish_vehicle_command(self, command, **params) -> None:
"""Publish a vehicle command."""
msg = VehicleCommand()
msg.command = command
msg.param1 = params.get("param1", 0.0)
msg.param2 = params.get("param2", 0.0)
msg.param3 = params.get("param3", 0.0)
msg.param4 = params.get("param4", 0.0)
msg.param5 = params.get("param5", 0.0)
msg.param6 = params.get("param6", 0.0)
msg.param7 = params.get("param7", 0.0)
msg.target_system = 1
msg.target_component = 1
msg.source_system = 1
msg.source_component = 1
msg.from_external = True
msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
self.vehicle_command_publisher.publish(msg)
def publish_velocity_setpoint(self, vx, vy, vz):
"""Publish a velocity setpoint."""
msg = TrajectorySetpoint()
msg.position = [np.nan, np.nan, np.nan]
msg.velocity = [vx, vy, vz]
msg.acceleration = [np.nan, np.nan, np.nan]
msg.jerk = [np.nan, np.nan, np.nan]
msg.yaw = 0.0
msg.yawspeed = np.nan
msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
self.trajectory_setpoint_publisher.publish(msg)
self.get_logger().info(f"Publishing velocity setpoint: [{vx}, {vy}, {vz}]")
def publish_vehicle_command_vtol_transition(self, transition_type) -> None:
self.publish_vehicle_command(command = VehicleCommand.VEHICLE_CMD_DO_VTOL_TRANSITION,
param1 = transition_type)
self.get_logger().info(f"VTOL transition command sent. Transition Type: {transition_type}")
def publish_attitude_setpoint(self):
"""Publish an attidtude setpoint"""
msg = VehicleAttitudeSetpoint()
msg.roll_body = 0.0
msg.pitch_body = 0.0
msg.yaw_body = 0.0
msg.yaw_sp_move_rate = 0.0
msg.q_d = [0.0, 0.0, 0.0, 1.0]
msg.thrust_body = [1.0, 0.0, 0.0]
msg.reset_integral = False
msg.fw_control_yaw_wheel = False
msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
self.vehicle_attitude_publisher.publish(msg)
self.get_logger().info(f"Publishing attitude setpoint")
def timer_callback(self) -> None:
"""Callback function for the timer."""
def timer_callback(self) -> None:
"""Callback function for the timer."""
self.publish_offboard_control_heartbeat_signal()
self.extra_callback()
self.get_logger().info(f"{self.vehicle_state}, {self.x_vel}")
if self.offboard_setpoint_counter == 10:
self.engage_offboard_mode()
self.arm()
if self.vehicle_local_position.z > self.takeoff_height and self.vehicle_status.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD and self.taken_off is None:
self.publish_position_setpoint(0.0, 0.0, self.takeoff_height)
self.publish_velocity_setpoint(0.0, 0.0, -3.0)
if self.vehicle_local_position.z < self.takeoff_height + 0.5:
self.taken_off = True
self.publish_velocity_setpoint(0.0, 0.0, 0.0)
print("taken off")
self.taken_off = True
if self.vehicle_local_position.z < self.takeoff_height + 0.2 and self.taken_off and self.first_setpoint is None:
self.publish_position_setpoint(400.0, 0.0, self.takeoff_height)
self.publish_velocity_setpoint(10.0, 0.0, 0.0)
if self.vehicle_local_position.x >= 39.5:
self.first_setpoint = True
self.publish_velocity_setpoint(0.0, 0.0, 0.0)
print("first setpoint")
if self.first_setpoint and self.transitioned is None:
self.publish_attitude_setpoint()
self.publish_vehicle_command_vtol_transition(4.0)
if self.vehicle_state == 2:
self.transitioned = True
if self.transitioned:
self.publish_attitude_setpoint()
if self.offboard_setpoint_counter < 11:
self.offboard_setpoint_counter += 1
def main(args=None) -> None:
print('Starting offboard control node...')
rclpy.init(args=args)
offboard_control = OffboardControl()
rclpy.spin(offboard_control)
offboard_control.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
try:
main()
except Exception as e:
print(e)
This code took inspiration from the offboard example code on the PX4 github.
Log File: https://review.px4.io/plot_app?log=cc442dc8-a1d4-4848-acbc-670e101c49b6
Any suggestions are appreciated! This is my first time using ROS2 and PX4 so I appreciate all feedback I can get. Thank you for your time in advance.
