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!

[claw]

@Zachary-Lowell-0 Any luck recreating this?

[Zachary Lowell 0]

@Zachary-Lowell-0 said in VOXL2 ROS2 Starling Code Inconsistencies:

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

C
1 Reply Last reply 10 minutes ago Reply

planning on trying to recreate tomorrow in the AM - have you tested this via HITL at all as well?

[claw]

@Zachary-Lowell-0 No, we're novices and trying to just focus on the Starling. HITL, would be a distraction (unless you say differently) from the task at hand.

[Zachary Lowell 0]

@claw no worries - I recorded a loom for you watch - https://www.loom.com/share/409b4b9ca9974d54b534af7020414632 - please give it a gander and lmk if it helps - I have a strong feeling its because there is NOT a sleeper call for a second or two between swapping to offboard and arming the drone.

Also HITL/SITL testing by far and away is the standard to do first before moving to a drone especially with some experimental code - I would heavily emphasize you always debug first in this scenario before moving to the real bird - purely really just for safety reasons. Anyways, please give a watch and let me know!!

Zach

[Zachary Lowell 0]

oh also please ensure you have offboard_figure_8 set to off in voxl-vision-hub.conf in /etc/modalai.

[GlennTee]

@Zachary-Lowell-0 Thank you so much, this appears to have fixed it! I'm now able to consistently run and exit the program cleanly.

[GlennTee]

@Zachary-Lowell-0 I do have another question: would you happen to know of any resources or examples involving April Tag detection and alignment using the VOXL2 ROS2 PX4 Starling? My goal is to try to use my drone to detect an April Tag, align with it, and land on it. Preferably, I would like to do this in Python if possible.

[GlennTee]

I also was wondering if there also a way to read the battery voltage? In my case, I want to be able to read the battery voltage in my code in able to tell the drone to do specific things based on if the voltage is high or low.

[Zachary Lowell 0]

@GlennTee said in VOXL2 ROS2 Starling Code Inconsistencies:

I also was wondering if there also a way to read the battery voltage? In my case, I want to be able to read the battery voltage in my code in able to tell the drone to do specific things based on if the voltage is high or low.

Reply

@GlennTee on the voxl2 there is a program called voxl-tag-detection that will detect tags in the h36 family. You can also detect battery via voxl-inspect-battery - the reason I bring these pipes up is because you can leverage them in your ros2 code via C++ nodes. So you can create a node in CPP that will subscribe to these modalAI MPA pipes and then publish the data into a ros2 topic.

LMK if that helps - we actually did just merge in a branch of voxl-mpa-to-ros2 that will publish the tag information over a ros2 interface. In theory you can do the same with battery voltage: https://gitlab.com/voxl-public/voxl-sdk/utilities/voxl-mpa-to-ros2/-/blob/dev/colcon_ws/src/voxl_mpa_to_ros2/src/interfaces/tag_interface.cpp?ref_type=heads

Zach

[GlennTee]

@Zachary-Lowell-0 Thank you so much, I will try that out!

[claw]

@Zachary-Lowell-0 Thank you for your help and guidance. Is there a way to get the status information like battery voltage using Python?