Skip to content

Commit 09dff45

Browse files
asherikovasherikov
committed
Add px4_offboard_demo_py package
1 parent 644b6f7 commit 09dff45

File tree

9 files changed

+402
-0
lines changed

9 files changed

+402
-0
lines changed

‎px4_offboard_demo_py/cdinit_services/px4_offboard_demo_py_node‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
type = process
2+
command = ros2 run px4_offboard_demo_py demo_node --ros-args -r __ns:=/px4_drone_$1 -p use_sim_time:=true
3+
4+
stop-timeout = 20.0
5+
6+
options = shares-console
7+
8+
depends-on = px4sitl_px4@$1
9+
depends-ms = cdinit_log@px4_offboard_demo_py_node@$1

‎px4_offboard_demo_py/package.xml‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>px4_offboard_demo_py</name>
5+
<version>0.0.1</version>
6+
<description>ROS2 package for PX4 takeoff and velocity control</description>
7+
<maintainer email="[email protected]">User</maintainer>
8+
<license>MIT</license>
9+
10+
<depend>rclpy</depend>
11+
<depend>px4_msgs</depend>
12+
13+
<exec_depend>ros2launch</exec_depend>
14+
15+
<export>
16+
<build_type>ament_python</build_type>
17+
</export>
18+
</package>

‎px4_offboard_demo_py/px4_offboard_demo_py/__init__.py‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
"""px4_offboard_demo_py package."""
2+
3+
from .demo_node import main
4+
5+
__all__ = ['main']

‎px4_offboard_demo_py/px4_offboard_demo_py/demo_node.py‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
"""ROS2 node that interacts with PX4-Autopilot using px4_msgs."""
2+
3+
import rclpy
4+
from rclpy.node import Node
5+
6+
from .px4_communication import PX4Communication
7+
from .px4_state_machine import PX4TakeoffStateMachine
8+
9+
10+
class PX4OffboardDemoNode(Node):
11+
"""PX4OffboardDemoNode."""
12+
13+
def __init__(self):
14+
"""PX4OffboardDemoNode."""
15+
super().__init__('demo_node')
16+
17+
# Initialize PX4 communication handler (includes subscriptions)
18+
self.px4_comm = PX4Communication(self)
19+
20+
# Initialize state machine
21+
self.state_machine = PX4TakeoffStateMachine(self, self.px4_comm)
22+
23+
# Create timer for sending messages (50 Hz to send constant zero velocity)
24+
self.timer = self.create_timer(0.02, self.timer_callback) # 50 Hz for faster updates
25+
26+
self.get_logger().info('PX4 Offboard Demo Node initialized')
27+
# Initialize takeoff_started for backward compatibility
28+
self.takeoff_started = False
29+
30+
31+
def timer_callback(self):
32+
"""Timer callback that delegates to the state machine."""
33+
self.state_machine.run_state_machine()
34+
35+
36+
def main(args=None):
37+
"""start."""
38+
rclpy.init(args=args)
39+
40+
demo_node = PX4OffboardDemoNode()
41+
42+
try:
43+
rclpy.spin(demo_node)
44+
except KeyboardInterrupt:
45+
demo_node.get_logger().info('Interrupted by user')
46+
finally:
47+
demo_node.destroy_node()
48+
rclpy.shutdown()
49+
50+
51+
if __name__ == '__main__':
52+
main()

‎px4_offboard_demo_py/px4_offboard_demo_py/px4_communication.py‎

Lines changed: 165 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,165 @@
1+
"""Handles all communication with the PX4 autopilot using px4_msgs."""
2+
from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleStatus
3+
4+
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
5+
6+
7+
class PX4Communication:
8+
"""PX4Communication."""
9+
10+
def __init__(self, node):
11+
"""
12+
Initialize the PX4 communication handler.
13+
14+
Args:
15+
node: The ROS2 node that will handle the publishers and subscribers
16+
"""
17+
self.node = node
18+
19+
# Initialize variables
20+
self.vehicle_status = None
21+
self._last_nav_state = None
22+
23+
px4_qos = QoSProfile(
24+
reliability=ReliabilityPolicy.BEST_EFFORT,
25+
durability=DurabilityPolicy.TRANSIENT_LOCAL,
26+
history=HistoryPolicy.KEEP_LAST,
27+
depth=1
28+
)
29+
30+
self.vehicle_command_publisher = self.node.create_publisher(
31+
VehicleCommand, 'fmu/in/vehicle_command', px4_qos)
32+
self.offboard_control_mode_publisher = self.node.create_publisher(
33+
OffboardControlMode, 'fmu/in/offboard_control_mode', px4_qos)
34+
self.trajectory_setpoint_publisher = self.node.create_publisher(
35+
TrajectorySetpoint, 'fmu/in/trajectory_setpoint', px4_qos)
36+
37+
self.vehicle_status_subscriber = self.node.create_subscription(
38+
VehicleStatus, 'fmu/out/vehicle_status_v1', self.vehicle_status_callback,
39+
px4_qos)
40+
41+
def vehicle_status_callback(self, msg):
42+
"""Update vehicle status."""
43+
self.vehicle_status = msg
44+
# Log navigation state changes
45+
if self._last_nav_state is None or self._last_nav_state != msg.nav_state:
46+
nav_states = {
47+
0: 'MANUAL',
48+
1: 'ALTCTL',
49+
2: 'POSCTL',
50+
3: 'AUTO_MISSION',
51+
4: 'AUTO_LOITER',
52+
5: 'AUTO_RTL',
53+
14: 'OFFBOARD',
54+
17: 'AUTO_TAKEOFF',
55+
18: 'AUTO_LAND'
56+
}
57+
current_state = nav_states.get(msg.nav_state, f'UNKNOWN({msg.nav_state})')
58+
self.node.get_logger().info(f'Navigation state changed to: {current_state}')
59+
self._last_nav_state = msg.nav_state
60+
61+
# pylint: disable=unknown-option-value too-many-positional-arguments too-many-arguments
62+
def send_command(self, command,
63+
param1=float('nan'), param2=float('nan'), param3=float('nan'), param4=float('nan'),
64+
param5=float('nan'), param6=float('nan'), param7=float('nan')):
65+
"""Send a vehicle command."""
66+
vehicle_command = VehicleCommand()
67+
vehicle_command.timestamp = int(self.node.get_clock().now().nanoseconds / 1000)
68+
vehicle_command.command = command
69+
vehicle_command.param1 = param1
70+
vehicle_command.param2 = param2
71+
vehicle_command.param3 = param3
72+
vehicle_command.param4 = param4
73+
vehicle_command.param5 = param5
74+
vehicle_command.param6 = param6
75+
vehicle_command.param7 = param7
76+
vehicle_command.target_system = 1
77+
vehicle_command.target_component = 1
78+
vehicle_command.source_system = 1
79+
vehicle_command.source_component = 1
80+
vehicle_command.from_external = True
81+
82+
self.vehicle_command_publisher.publish(vehicle_command)
83+
84+
def arm_vehicle(self):
85+
"""Send arm command to vehicle."""
86+
self.send_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, param1=1.0)
87+
88+
def set_offboard_mode(self):
89+
"""Enable offboard control mode by publishing OffboardControlMode message."""
90+
# Send offboard control mode with velocity control enabled
91+
offboard_control_mode = OffboardControlMode()
92+
offboard_control_mode.timestamp = int(self.node.get_clock().now().nanoseconds / 1000)
93+
offboard_control_mode.position = False
94+
offboard_control_mode.velocity = True # We want to control velocity
95+
offboard_control_mode.acceleration = False
96+
offboard_control_mode.attitude = False
97+
offboard_control_mode.body_rate = False
98+
offboard_control_mode.thrust_and_torque = False
99+
offboard_control_mode.direct_actuator = False
100+
101+
self.offboard_control_mode_publisher.publish(offboard_control_mode)
102+
103+
def send_offboard_mode_command(self):
104+
"""Send VEHICLE_CMD_DO_SET_MODE command to switch to offboard mode."""
105+
# Send command to set offboard mode (Base mode: 4, Custom main mode: 6)
106+
# Base mode: 4 = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED
107+
# Custom main mode: 6 = PX4_CUSTOM_MAIN_MODE_OFFBOARD
108+
self.send_command(
109+
VehicleCommand.VEHICLE_CMD_DO_SET_MODE,
110+
param1=1.0, # Base mode (1=enable custom, 0=use system defaults)
111+
param2=6.0, # Custom main mode (6 = OFFBOARD)
112+
param3=0.0 # Custom sub mode (not used for offboard)
113+
)
114+
115+
def send_takeoff_command(self, altitude):
116+
"""Send takeoff command to vehicle."""
117+
self.send_command(VehicleCommand.VEHICLE_CMD_NAV_TAKEOFF, param7=altitude)
118+
119+
def send_zero_velocity_setpoint(self):
120+
"""Send zero velocity setpoint."""
121+
trajectory_setpoint = TrajectorySetpoint()
122+
trajectory_setpoint.timestamp = int(self.node.get_clock().now().nanoseconds / 1000)
123+
124+
# Set zero velocity
125+
trajectory_setpoint.velocity[0] = 0.0
126+
trajectory_setpoint.velocity[1] = 0.0
127+
trajectory_setpoint.velocity[2] = 0.0
128+
129+
# Keep position and acceleration as NaN to not control them
130+
trajectory_setpoint.position[0] = float('nan')
131+
trajectory_setpoint.position[1] = float('nan')
132+
trajectory_setpoint.position[2] = float('nan')
133+
134+
trajectory_setpoint.acceleration[0] = float('nan')
135+
trajectory_setpoint.acceleration[1] = float('nan')
136+
trajectory_setpoint.acceleration[2] = float('nan')
137+
138+
trajectory_setpoint.yaw = float('nan') # Don't control yaw
139+
trajectory_setpoint.yawspeed = float('nan')
140+
141+
self.trajectory_setpoint_publisher.publish(trajectory_setpoint)
142+
143+
def send_velocity_setpoint(self, velocity):
144+
"""Send constant velocity setpoint."""
145+
trajectory_setpoint = TrajectorySetpoint()
146+
trajectory_setpoint.timestamp = int(self.node.get_clock().now().nanoseconds / 1000)
147+
148+
# Set constant velocity
149+
trajectory_setpoint.velocity[0] = float(velocity[0])
150+
trajectory_setpoint.velocity[1] = float(velocity[1])
151+
trajectory_setpoint.velocity[2] = float(velocity[2])
152+
153+
# Keep position and acceleration as NaN to not control them
154+
trajectory_setpoint.position[0] = float('nan')
155+
trajectory_setpoint.position[1] = float('nan')
156+
trajectory_setpoint.position[2] = float('nan')
157+
158+
trajectory_setpoint.acceleration[0] = float('nan')
159+
trajectory_setpoint.acceleration[1] = float('nan')
160+
trajectory_setpoint.acceleration[2] = float('nan')
161+
162+
trajectory_setpoint.yaw = float('nan') # Don't control yaw
163+
trajectory_setpoint.yawspeed = float('nan')
164+
165+
self.trajectory_setpoint_publisher.publish(trajectory_setpoint)

‎px4_offboard_demo_py/px4_offboard_demo_py/px4_state_machine.py‎

Lines changed: 119 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,119 @@
1+
"""Dedicated class for state machine logic."""
2+
from px4_msgs.msg import VehicleStatus
3+
4+
import rclpy
5+
6+
7+
class PX4TakeoffStateMachine:
8+
"""PX4TakeoffStateMachine."""
9+
10+
def __init__(self, node, px4_comm):
11+
"""
12+
Initialize the state machine.
13+
14+
Args:
15+
node: The ROS2 node
16+
px4_comm: The PX4 communication handler
17+
"""
18+
self.node = node
19+
self.px4_comm = px4_comm
20+
21+
22+
# State machine variables
23+
# waiting_for_status, sending_zero_velocity, setting_offboard_mode,
24+
# waiting_for_offboard, arming_vehicle, waiting_for_offboard,
25+
# sending_takeoff, waiting_takeoff_completion
26+
self.state = 'waiting_for_status'
27+
self.zero_velocity_start_time = 0
28+
self.zero_velocity_duration = 2.0 # seconds to send zero velocity before offboard mode
29+
self.last_offboard_set_time = 0
30+
self.last_arm_command_time = None # Track time of last arm command to implement proper throttling
31+
32+
33+
def run_state_machine(self):
34+
"""Run state machine execution function."""
35+
current_time_sec = self.node.get_clock().now().nanoseconds / 1e9
36+
37+
if self.state == 'waiting_for_status':
38+
# Wait to receive the first vehicle status before proceeding
39+
if self.px4_comm.vehicle_status:
40+
self.node.get_logger().info('Received vehicle status, proceeding with initialization')
41+
self.state = 'sending_zero_velocity'
42+
# Set the start time for the zero velocity duration
43+
self.zero_velocity_start_time = current_time_sec
44+
self.node.get_logger().info('Starting zero velocity and offboard control commands')
45+
else:
46+
self.node.get_logger().log('Waiting for initial vehicle status...', rclpy.logging.LoggingSeverity.INFO, throttle_duration_sec=2.0)
47+
48+
elif self.state == 'sending_zero_velocity':
49+
# Send zero velocity and offboard control mode for 2 seconds to switch to offboard control
50+
self.px4_comm.set_offboard_mode()
51+
self.px4_comm.send_zero_velocity_setpoint()
52+
53+
elapsed_time = current_time_sec - self.zero_velocity_start_time
54+
if elapsed_time >= self.zero_velocity_duration:
55+
self.node.get_logger().info('Zero velocity and offboard control sent for required duration, switching to offboard mode using VEHICLE_CMD_DO_SET_MODE')
56+
self.state = 'setting_offboard_mode'
57+
# Send the VEHICLE_CMD_DO_SET_MODE command to switch to offboard mode
58+
self.px4_comm.send_offboard_mode_command()
59+
else:
60+
self.node.get_logger().log('Sending zero velocity and offboard control', rclpy.logging.LoggingSeverity.INFO, throttle_duration_sec=2.0)
61+
62+
elif self.state == 'setting_offboard_mode':
63+
# Continue sending zero velocity commands while waiting for mode switch
64+
self.px4_comm.send_zero_velocity_setpoint()
65+
66+
# Wait until the vehicle switches to offboard mode
67+
if self.px4_comm.vehicle_status.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD:
68+
self.node.get_logger().info('Successfully switched to offboard mode, proceeding to arming')
69+
self.state = 'arming_vehicle'
70+
else:
71+
# Keep sending zero velocity and ensure offboard mode is set
72+
self.px4_comm.set_offboard_mode()
73+
# Re-send VEHICLE_CMD_DO_SET_MODE command periodically
74+
if current_time_sec - self.last_offboard_set_time > 1.0:
75+
self.px4_comm.send_offboard_mode_command()
76+
self.last_offboard_set_time = current_time_sec # Update the time when command is sent
77+
78+
nav_state_str = self.px4_comm.vehicle_status.nav_state if self.px4_comm.vehicle_status else 'Unknown'
79+
self.node.get_logger().log(f'Waiting for offboard mode, current state: {nav_state_str}', rclpy.logging.LoggingSeverity.INFO, throttle_duration_sec=2.0)
80+
81+
elif self.state == 'arming_vehicle':
82+
# Continue sending zero velocity while arming
83+
self.px4_comm.send_zero_velocity_setpoint()
84+
self.px4_comm.set_offboard_mode() # Continue publishing offboard mode
85+
86+
# Check if vehicle is now armed
87+
if self.px4_comm.vehicle_status.arming_state == VehicleStatus.ARMING_STATE_ARMED:
88+
self.node.get_logger().info('Vehicle successfully armed, proceeding to takeoff')
89+
self.state = 'sending_takeoff'
90+
else:
91+
# Re-send arm command periodically with proper throttling
92+
if self.last_arm_command_time is None or current_time_sec - self.last_arm_command_time >= 1.0: # Every second
93+
self.px4_comm.arm_vehicle()
94+
self.last_arm_command_time = current_time_sec # Update the time when command is sent
95+
96+
arming_state_str = 'Unknown' if not self.px4_comm.vehicle_status else (
97+
'ARMED' if self.px4_comm.vehicle_status.arming_state == VehicleStatus.ARMING_STATE_ARMED else 'DISARMED'
98+
)
99+
self.node.get_logger().log(f'Waiting for arming, current arming state: {arming_state_str}', rclpy.logging.LoggingSeverity.INFO, throttle_duration_sec=2.0)
100+
101+
elif self.state == 'sending_takeoff':
102+
# Continue to send offboard control mode
103+
self.px4_comm.set_offboard_mode()
104+
105+
# Wait for takeoff to complete
106+
if self.px4_comm.vehicle_status.nav_state in [VehicleStatus.NAVIGATION_STATE_AUTO_LOITER,
107+
VehicleStatus.NAVIGATION_STATE_POSCTL]:
108+
self.node.get_logger().info('Takeoff completed')
109+
self.state = 'completed'
110+
self.node.get_logger().info('Takeoff sequence completed successfully')
111+
elif self.px4_comm.vehicle_status.nav_state != VehicleStatus.NAVIGATION_STATE_AUTO_TAKEOFF:
112+
# Send takeoff command
113+
self.px4_comm.send_takeoff_command(10.0)
114+
115+
elif self.state == 'completed':
116+
# After takeoff is completed, continue sending zero velocity`
117+
# self.px4_comm.send_zero_velocity_setpoint()
118+
# self.px4_comm.set_offboard_mode()
119+
self.node.get_logger().log('Takeoff completed, continuing to send zero velocity commands', rclpy.logging.LoggingSeverity.INFO, throttle_duration_sec=2.0)

‎px4_offboard_demo_py/resource/px4_offboard_demo_py‎

Whitespace-only changes.

‎px4_offboard_demo_py/setup.cfg‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
[develop]
2+
script_dir=$base/lib/px4_offboard_demo_py
3+
4+
[install]
5+
install_scripts=$base/lib/px4_offboard_demo_py

0 commit comments

Comments
 (0)