offboard_control.py still uses old vehicle_command method

[PetervdPerk-NXP]

PX4/PX4-Autopilot#22313 introduces ROS2 vehicle_command service.
The Cpp example here got updated but the python version has not.

[AWickersGit]

Hey,

today I faced the same problem as I build a new environment with ubuntu 24.04, ros2 jazzy, gazebo harmonic (8.9) and the new versions from px4_msgs.
For anyone who might also stumble across this issue, I want to provide a simple example, that I used to test the simulation. It works for me, although it might not be, what you're searching for as a merge for this git? But since I didn't see a solution yet, it might be helpful for others, who just want to test their environment.

#!/usr/bin/env python3
#offboard_control.py

import sys
import array
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy

from px4_msgs.msg import OffboardControlMode
from px4_msgs.msg import TrajectorySetpoint
from px4_msgs.msg import VehicleCommand


class OffboardControl(Node):
    def __init__(self):
        super().__init__('offboard_control')

        # PX4-Topics QoS Profile
        px4_qos = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE
        )

        # Publisher
        self.offboard_control_mode_pub = self.create_publisher(
            OffboardControlMode, '/fmu/in/offboard_control_mode', px4_qos
        )
        self.trajectory_setpoint_pub = self.create_publisher(
            TrajectorySetpoint, '/fmu/in/trajectory_setpoint', px4_qos
        )
        self.vehicle_command_pub = self.create_publisher(
            VehicleCommand, '/fmu/in/vehicle_command', px4_qos
        )

        # Counter for Offboard-Handshake
        self.offboard_setpoint_counter = 0

        # 100 ms Timer (10 Hz)
        self.timer = self.create_timer(0.1, self._on_timer)

        self.get_logger().info('Starting offboard control node...')

    # --- Public commands ---
    def arm(self):
        self._publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0)
        self.get_logger().info('Arm command sent')

    def disarm(self):
        self._publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0)
        self.get_logger().info('Disarm command sent')

    # --- Timer callback ---
    def _on_timer(self):
        if self.offboard_setpoint_counter == 10:
            # Wait for 10 Setpoints before switching to Offboard
            self._publish_vehicle_command(VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 1.0, 6.0)
            self.arm()

        # OffboardControlMode + TrajectorySetpoint need to be send together at all time. 
        self._publish_offboard_control_mode()
        self._publish_trajectory_setpoint()

        if self.offboard_setpoint_counter < 11:
            self.offboard_setpoint_counter += 1

    # --- Publishers ---
    def _publish_offboard_control_mode(self):
        msg = OffboardControlMode()
        msg.position = True
        msg.velocity = False
        msg.acceleration = False
        msg.attitude = False
        msg.body_rate = False
        msg.timestamp = self._now_us()
        self.offboard_control_mode_pub.publish(msg)

    def _publish_trajectory_setpoint(self):
        msg = TrajectorySetpoint()
        # PX4 erwartet array.array('f', [x, y, z]) für position
        msg.position = array.array('f', [0.0, 0.0, -10.0])
        msg.yaw = 0.0  # [-PI, PI]
        msg.timestamp = self._now_us()
        self.trajectory_setpoint_pub.publish(msg)

    def _publish_vehicle_command(self, command, param1=0.0, param2=0.0):
        msg = VehicleCommand()
        msg.param1 = float(param1)
        msg.param2 = float(param2)
        msg.command = int(command)
        msg.target_system = 1
        msg.target_component = 1
        msg.source_system = 1
        msg.source_component = 1
        msg.from_external = True
        msg.timestamp = self._now_us()
        self.vehicle_command_pub.publish(msg)

    # --- Helpers ---
    def _now_us(self) -> int:
        return self.get_clock().now().nanoseconds // 1000


def main(argv=None):
    rclpy.init(args=argv)
    node = OffboardControl()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main(sys.argv)