px4_offboard_demo_py: use a config file

Author: asherikov

px4_offboard_demo_py/package.xml

@@ -9,6 +9,7 @@
 
   <depend>rclpy</depend>
   <depend>px4_msgs</depend>
+  <depend>ament_index_python</depend>
 
   <exec_depend>ros2launch</exec_depend>
   <exec_depend>cdinit_ros2</exec_depend>

px4_offboard_demo_py/px4_offboard_demo_py/config/demo_config.yaml

@@ -0,0 +1,29 @@
+# Configuration parameters for PX4 Offboard Demo
+
+# Timer settings
+timer_frequency: 50.0          # Hz - frequency of timer callback
+timeout_sec: 0.1               # seconds - timeout for spin_once
+
+# Offboard mode settings
+offboard_set_interval: 0.2     # seconds - interval to periodically try switching to offboard mode
+
+# Arming settings
+arm_command_interval: 1.0      # seconds - interval between arm commands
+
+# Takeoff settings
+takeoff_altitude: 10.0         # meters - altitude for takeoff command
+
+# Velocity command settings
+velocity_commands:
+  forward:
+    duration: 4.0              # seconds - duration for forward velocity
+    velocity: [2.0, 0.0, 0.0]  # m/s - velocity in x, y, z directions
+  zero:
+    duration: 2.0              # seconds - duration for zero velocity
+    velocity: [0.0, 0.0, 0.0]  # m/s - velocity in x, y, z directions
+  backward:
+    duration: 4.0              # seconds - duration for backward velocity
+    velocity: [-2.0, 0.0, 0.0] # m/s - velocity in x, y, z directions
+
+# Position printing
+position_print_interval: 1.0   # seconds - interval between position prints

px4_offboard_demo_py/px4_offboard_demo_py/demo_node.py

@@ -1,8 +1,14 @@
 """ROS2 node that interacts with PX4-Autopilot using px4_msgs."""
 
+import os
+
+import ament_index_python
+
 import rclpy
 from rclpy.node import Node
 
+import yaml
+
 from .px4_communication import PX4Communication
 from .px4_state_machine import PX4TakeoffStateMachine
 
@@ -14,14 +20,33 @@ def __init__(self):
         """PX4OffboardDemoNode."""
         super().__init__('demo_node')
 
+        # Load configuration from YAML file
+        # First try to find config file using ament resource system
+        config_path = None
+        try:
+            package_share_path = ament_index_python.packages.get_package_share_directory('px4_offboard_demo_py')
+            config_path = os.path.join(package_share_path, 'config', 'demo_config.yaml')
+        except (ImportError, ament_index_python.packages.PackageNotFoundError):
+            # Fallback to the old method if ament_index_python is not available
+            config_path = os.path.join(os.path.dirname(__file__), 'config', 'demo_config.yaml')
+
+        try:
+            with open(config_path, 'r', encoding='utf-8') as file:
+                self.config = yaml.safe_load(file)
+            self.get_logger().info(f'Configuration loaded from: {config_path}')
+        except FileNotFoundError as exception:
+            self.get_logger().error(f'Configuration file not found: {config_path}')
+            raise RuntimeError(f'Configuration file not found: {config_path}') from exception
+
         # Initialize PX4 communication handler (includes subscriptions)
         self.px4_comm = PX4Communication(self)
 
-        # Initialize state machine
-        self.state_machine = PX4TakeoffStateMachine(self, self.px4_comm)
+        # Initialize state machine with configuration
+        self.state_machine = PX4TakeoffStateMachine(self, self.px4_comm, self.config)
 
-        # Create timer for sending messages (50 Hz to send constant zero velocity)
-        self.timer = self.create_timer(0.02, self.timer_callback)  # 50 Hz for faster updates
+        # Create timer for sending messages based on config
+        timer_period = 1.0 / self.config['timer_frequency']  # Convert frequency to period
+        self.timer = self.create_timer(timer_period, self.timer_callback)
 
         self.get_logger().info('PX4 Offboard Demo Node initialized')
         # Initialize takeoff_started for backward compatibility
@@ -49,9 +74,9 @@ def main(args=None):
     demo_node = PX4OffboardDemoNode()
 
     try:
-        # Use spin_once in a loop to check for completion
+        # Use spin_once in a loop to check for completion, using timeout from config
         while rclpy.ok() and not getattr(demo_node, 'completed', False):
-            rclpy.spin_once(demo_node, timeout_sec=0.1)
+            rclpy.spin_once(demo_node, timeout_sec=demo_node.config['timeout_sec'])
     except KeyboardInterrupt:
         demo_node.get_logger().info('Interrupted by user')
     finally:

px4_offboard_demo_py/px4_offboard_demo_py/px4_communication.py

@@ -70,7 +70,7 @@ def vehicle_odometry_callback(self, msg):
         """Update vehicle odometry."""
         self.vehicle_odometry = msg
 
-    def should_print_position(self, current_time_sec, print_interval=1.0):
+    def should_print_position(self, current_time_sec, print_interval=None):
         """
         Check if we should print the position based on the time interval.
 
@@ -81,6 +81,9 @@ def should_print_position(self, current_time_sec, print_interval=1.0):
         Returns:
             bool: True if position should be printed
         """
+        if print_interval is None:
+            print_interval = 1.0  # default value
+
         if current_time_sec - self._last_odom_print_time >= print_interval:
             self._last_odom_print_time = current_time_sec
             return True

px4_offboard_demo_py/px4_offboard_demo_py/px4_state_machine.py

@@ -5,16 +5,20 @@
 class PX4TakeoffStateMachine:
     """PX4TakeoffStateMachine."""
 
-    def __init__(self, node, px4_comm):
+    # pylint: disable=too-many-instance-attributes
+
+    def __init__(self, node, px4_comm, config=None):
         """
         Initialize the state machine.
 
         Args:
             node: The ROS2 node
             px4_comm: The PX4 communication handler
+            config: Configuration dictionary loaded from YAML file
         """
         self.node = node
         self.px4_comm = px4_comm
+        self.config = config or {}
 
 
         # Define the state sequence for the complete flight operation
@@ -97,7 +101,7 @@ def _handle_waiting_for_status(self, current_time_sec):
                 self.state_variables['enter_offboard_mode'] = {
                     'start_time': 0,
                     'last_offboard_set_time': 0,
-                    'offboard_set_interval': 0.2
+                    'offboard_set_interval': self.config['offboard_set_interval']
                 }
             # Set the start time for the offboard transition in the next state
             offboard_vars = self.state_variables['enter_offboard_mode']
@@ -117,7 +121,7 @@ def _handle_enter_offboard_mode(self, current_time_sec):
             self.state_variables[current_state] = {
                 'start_time': 0,
                 'last_offboard_set_time': 0,
-                'offboard_set_interval': 0.2  # seconds (200ms) to periodically try switching to offboard mode
+                'offboard_set_interval': self.config['offboard_set_interval']  # seconds to periodically try switching to offboard mode
             }
 
         offboard_vars = self.state_variables[current_state]
@@ -127,20 +131,14 @@ def _handle_enter_offboard_mode(self, current_time_sec):
             offboard_vars['start_time'] = current_time_sec
             offboard_vars['last_offboard_set_time'] = current_time_sec
 
-        last_offboard_set_time = offboard_vars['last_offboard_set_time']
-
         # Continue sending zero velocity commands while waiting for mode switch
         self.px4_comm.send_zero_velocity_setpoint()
         self.px4_comm.set_offboard_mode()  # Continue publishing offboard mode
 
         # Re-send VEHICLE_CMD_DO_SET_MODE command periodically
-        offboard_set_interval = offboard_vars['offboard_set_interval']  # Get interval from state variables
-        if current_time_sec - last_offboard_set_time > offboard_set_interval:
+        if current_time_sec - offboard_vars['last_offboard_set_time'] > offboard_vars['offboard_set_interval']:
             self.px4_comm.send_offboard_mode_command()
-            last_offboard_set_time = current_time_sec  # Update the time when command is sent
-
-        # Update the stored time
-        offboard_vars['last_offboard_set_time'] = last_offboard_set_time
+            offboard_vars['last_offboard_set_time'] = current_time_sec  # Update the time when command is sent
 
         # Check if the vehicle has switched to offboard mode
         success = (self.px4_comm.vehicle_status
@@ -166,9 +164,9 @@ def _handle_arming_vehicle(self, current_time_sec):
         if self.px4_comm.vehicle_status.arming_state == VehicleStatus.ARMING_STATE_ARMED:
             return True
 
-        # Re-send arm command periodically with proper throttling
+        # Re-send arm command periodically with proper throttling using config
         last_arm_time = self.state_variables['arming_vehicle']['last_arm_command_time']
-        if last_arm_time is None or current_time_sec - last_arm_time >= 1.0:  # Every second
+        if last_arm_time is None or current_time_sec - last_arm_time >= self.config['arm_command_interval']:
             self.px4_comm.arm_vehicle()
             self.state_variables['arming_vehicle']['last_arm_command_time'] = current_time_sec  # Update the time when command is sent
 
@@ -186,8 +184,8 @@ def _handle_sending_takeoff(self, current_time_sec):  # pylint: disable=unused-a
             return True
 
         if self.px4_comm.vehicle_status.nav_state != VehicleStatus.NAVIGATION_STATE_AUTO_TAKEOFF:
-            # Send takeoff command
-            self.px4_comm.send_takeoff_command(10.0)
+            # Send takeoff command with altitude from config
+            self.px4_comm.send_takeoff_command(self.config['takeoff_altitude'])
 
         return False
 
@@ -210,15 +208,11 @@ def _handle_performing_velocity_forward(self, current_time_sec):
             vel_vars['start_time'] = current_time_sec
             vel_vars['phase_start_time'] = current_time_sec
 
-        # Define parameters for the velocity pattern
-        forward_duration = 4.0  # seconds for forward velocity
-        forward_velocity = [2.0, 0.0, 0.0]  # m/s in x, y, z directions
-
         # Send forward velocity command
-        self.px4_comm.send_velocity_setpoint(forward_velocity)
+        self.px4_comm.send_velocity_setpoint(self.config['velocity_commands']['forward']['velocity'])
 
-        # Print position every second while executing velocity commands
-        if self.px4_comm.should_print_position(current_time_sec):
+        # Print position based on interval from config while executing velocity commands
+        if self.px4_comm.should_print_position(current_time_sec, self.config['position_print_interval']):
             position = self.px4_comm.get_current_position()
             if position:
                 x, y, z = position
@@ -227,7 +221,7 @@ def _handle_performing_velocity_forward(self, current_time_sec):
                 self.node.get_logger().info('Position unavailable')
 
         elapsed_time = current_time_sec - vel_vars['phase_start_time']
-        if elapsed_time >= forward_duration:
+        if elapsed_time >= self.config['velocity_commands']['forward']['duration']:
             return True
 
         return False
@@ -251,15 +245,11 @@ def _handle_performing_velocity_zero(self, current_time_sec):
             vel_vars['start_time'] = current_time_sec
             vel_vars['phase_start_time'] = current_time_sec
 
-        # Define parameters for the velocity pattern
-        zero_duration = 2.0     # seconds for zero velocity
-        zero_velocity = [0.0, 0.0, 0.0]     # m/s
-
         # Send zero velocity command
-        self.px4_comm.send_velocity_setpoint(zero_velocity)
+        self.px4_comm.send_velocity_setpoint(self.config['velocity_commands']['zero']['velocity'])
 
-        # Print position every second while executing velocity commands
-        if self.px4_comm.should_print_position(current_time_sec):
+        # Print position based on interval from config while executing velocity commands
+        if self.px4_comm.should_print_position(current_time_sec, self.config['position_print_interval']):
             position = self.px4_comm.get_current_position()
             if position:
                 x, y, z = position
@@ -268,7 +258,7 @@ def _handle_performing_velocity_zero(self, current_time_sec):
                 self.node.get_logger().info('Position unavailable')
 
         elapsed_time = current_time_sec - vel_vars['phase_start_time']
-        if elapsed_time >= zero_duration:
+        if elapsed_time >= self.config['velocity_commands']['zero']['duration']:
             return True
 
         return False
@@ -292,15 +282,11 @@ def _handle_performing_velocity_backward(self, current_time_sec):
             vel_vars['start_time'] = current_time_sec
             vel_vars['phase_start_time'] = current_time_sec
 
-        # Define parameters for the velocity pattern
-        backward_duration = 4.0  # seconds for backward velocity
-        backward_velocity = [-2.0, 0.0, 0.0]  # m/s in x, y, z directions
-
         # Send backward velocity command
-        self.px4_comm.send_velocity_setpoint(backward_velocity)
+        self.px4_comm.send_velocity_setpoint(self.config['velocity_commands']['backward']['velocity'])
 
-        # Print position every second while executing velocity commands
-        if self.px4_comm.should_print_position(current_time_sec):
+        # Print position based on interval from config while executing velocity commands
+        if self.px4_comm.should_print_position(current_time_sec, self.config['position_print_interval']):
             position = self.px4_comm.get_current_position()
             if position:
                 x, y, z = position
@@ -309,7 +295,7 @@ def _handle_performing_velocity_backward(self, current_time_sec):
                 self.node.get_logger().info('Position unavailable')
 
         elapsed_time = current_time_sec - vel_vars['phase_start_time']
-        if elapsed_time >= backward_duration:
+        if elapsed_time >= self.config['velocity_commands']['backward']['duration']:
             return True
 
         return False

px4_offboard_demo_py/setup.py

@@ -11,8 +11,9 @@
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
         ('share/' + package_name + '/cdinit_services', ['cdinit_services/px4_offboard_demo_py_node']),
+        ('share/' + package_name + '/config', ['px4_offboard_demo_py/config/demo_config.yaml']),
     ],
-    install_requires=['setuptools'],
+    install_requires=['setuptools', 'pyyaml'],
     zip_safe=True,
     maintainer='User',
     maintainer_email='[email protected]',

px4sitl/package.xml

@@ -12,7 +12,7 @@
 
     <buildtool_depend>cmake</buildtool_depend>
 
-    <exec_depend>cdinit_manager</exec_depend>
+    <exec_depend>cdinit</exec_depend>
     <exec_depend>dds_agent</exec_depend>
     <exec_depend>px4_msgs</exec_depend>
     <exec_depend>ros_gz</exec_depend>