• Categories
  • Recent
  • Tags
  • Popular
  • Users
  • Groups
  • Register
  • Login
ModalAI Forum
  • Categories
  • Recent
  • Tags
  • Popular
  • Users
  • Groups
    • Register
    • Login

    VOXL2 ROS2 Starling Code Inconsistencies

    ROS
    3
    14
    277
    Loading More Posts
    • Oldest to Newest
    • Newest to Oldest
    • Most Votes
    Reply
    • Reply as topic
    Log in to reply
    This topic has been deleted. Only users with topic management privileges can see it.
    • G
      GlennTee
      last edited by GlennTee 4 Apr 2025, 20:53 4 Apr 2025, 20:52

      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)

      1 Reply Last reply Reply Quote 0
      • Z
        Zachary Lowell 0 ModalAI Team
        last edited by 4 Apr 2025, 21:47

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

        C 1 Reply Last reply 7 Apr 2025, 21:30 Reply Quote 0
        • C
          claw @Zachary Lowell 0
          last edited by 7 Apr 2025, 21:30

          @Zachary-Lowell-0 Any luck recreating this?

          1 Reply Last reply Reply Quote 0
          • Z
            Zachary Lowell 0 ModalAI Team
            last edited by 7 Apr 2025, 21:41

            @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?

            C 1 Reply Last reply 7 Apr 2025, 22:26 Reply Quote 0
            • C
              claw @Zachary Lowell 0
              last edited by 7 Apr 2025, 22:26

              @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.

              1 Reply Last reply Reply Quote 0
              • Z
                Zachary Lowell 0 ModalAI Team
                last edited by 8 Apr 2025, 14:13

                @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

                1 Reply Last reply Reply Quote 0
                • Z
                  Zachary Lowell 0 ModalAI Team
                  last edited by 8 Apr 2025, 15:25

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

                  G 2 Replies Last reply 8 Apr 2025, 15:55 Reply Quote 0
                  • G
                    GlennTee @Zachary Lowell 0
                    last edited by 8 Apr 2025, 15:55

                    @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.

                    1 Reply Last reply Reply Quote 0
                    • G
                      GlennTee @Zachary Lowell 0
                      last edited by GlennTee 8 Apr 2025, 18:04 8 Apr 2025, 18:04

                      @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.

                      G 1 Reply Last reply 8 Apr 2025, 19:06 Reply Quote 0
                      • G
                        GlennTee @GlennTee
                        last edited by 8 Apr 2025, 19:06

                        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.

                        1 Reply Last reply Reply Quote 0
                        • Z
                          Zachary Lowell 0 ModalAI Team
                          last edited by 30 days ago

                          @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

                          G C 2 Replies Last reply 30 days ago Reply Quote 0
                          • G
                            GlennTee @Zachary Lowell 0
                            last edited by 30 days ago

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

                            1 Reply Last reply Reply Quote 0
                            • C
                              claw @Zachary Lowell 0
                              last edited by 30 days ago

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

                              1 Reply Last reply Reply Quote 0
                              • Z
                                Zachary Lowell 0 ModalAI Team
                                last edited by 17 days ago

                                @claw said in VOXL2 ROS2 Starling Code Inconsistencies:

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

                                Reply

                                @claw yes you can follow this post to get battery over the DDS: https://forum.modalai.com/topic/4353/voxl2-px4-ros2-mpa-to-ros2-for-battery-voltage-problems/12?_=1745355297145

                                1 Reply Last reply Reply Quote 0
                                • First post
                                  Last post
                                Powered by NodeBB | Contributors