VOXL2 ROS2 Starling Code Inconsistencies

[GlennTee]

Hello, I'm running the PX4 ROS2 offboard control example from https://docs.px4.io/main/en/ros2/offboard_control.html#foxy and am having some inconsistent behavior. When I run the program, the drone arms, takes off, and lands as expected. However, during subsequent runs, the drone never arms (The props never spin at all), but the program still continues running as indicated by the print statements that show up. Sometimes, if I close the terminal and run the program again in a new terminal, this problem is fixed as the drone will arm and fly most times, but not always. Am I missing something that cleanly shuts down after running? I'm running the code shown below:

#!/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
import time
import math

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)

    # 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)

    # Initialize variables
    self.offboard_setpoint_counter = 0
    self.vehicle_local_position = VehicleLocalPosition()
    self.vehicle_status = VehicleStatus()
    self.takeoff_height = -1.0

    # Create a timer to publish control commands
    self.timer = self.create_timer(0.1, self.timer_callback)

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 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 = False
    msg.acceleration = False
    msg.attitude = False
    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 = 1.57079  # (90 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 timer_callback(self) -> None:
    """Callback function for the timer."""
    self.publish_offboard_control_heartbeat_signal()

    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:
        self.publish_position_setpoint(0.0, 0.0, self.takeoff_height)

    elif self.vehicle_local_position.z <= self.takeoff_height:
        self.land()
        exit(0)

    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)

[Zachary Lowell 0]

Hi @GlennTee - will try and recreate this and get back to yuo!