From ab69411bf10aa2f980617ae9398a118574ec6433 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 15 Jul 2026 21:06:19 +0100 Subject: [PATCH 01/14] chore: add distance-based formation control Signed-off-by: Beniamino Pozzan --- .../formation_control/CMakeLists.txt | 61 +++++++ .../include/formation_control.hpp | 153 ++++++++++++++++++ .../launch/formation.launch.py | 46 ++++++ .../formation_control/package.xml | 31 ++++ .../formation_control/src/main.cpp | 35 ++++ 5 files changed, 326 insertions(+) create mode 100644 px4_roscon_workshop/formation_control/CMakeLists.txt create mode 100644 px4_roscon_workshop/formation_control/include/formation_control.hpp create mode 100644 px4_roscon_workshop/formation_control/launch/formation.launch.py create mode 100644 px4_roscon_workshop/formation_control/package.xml create mode 100644 px4_roscon_workshop/formation_control/src/main.cpp diff --git a/px4_roscon_workshop/formation_control/CMakeLists.txt b/px4_roscon_workshop/formation_control/CMakeLists.txt new file mode 100644 index 0000000..cabcc1d --- /dev/null +++ b/px4_roscon_workshop/formation_control/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.5) +project(px4_formation_control) + +set(CMAKE_CXX_STANDARD 20) + +add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wno-unused-parameter) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib") +find_package(GeographicLib REQUIRED) + +find_package(px4_ros2_cpp REQUIRED) + + +include_directories( + include + ${Eigen3_INCLUDE_DIRS} + ${GeographicLib_INCLUDE_DIRS} +) + +add_executable(px4_formation_control + src/main.cpp +) + +ament_target_dependencies(px4_formation_control + rclcpp + Eigen3 + px4_ros2_cpp + geometry_msgs + tf2 + tf2_ros +) + +target_link_libraries(px4_formation_control + ${GeographicLib_LIBRARIES} +) + +target_compile_features(px4_formation_control PUBLIC c_std_99 cxx_std_20) + +install( + TARGETS px4_formation_control + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() \ No newline at end of file diff --git a/px4_roscon_workshop/formation_control/include/formation_control.hpp b/px4_roscon_workshop/formation_control/include/formation_control.hpp new file mode 100644 index 0000000..d42232a --- /dev/null +++ b/px4_roscon_workshop/formation_control/include/formation_control.hpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * Copyright (c) 2023 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "tf2_ros/transform_broadcaster.hpp" +#include "tf2_ros/transform_listener.hpp" +#include "tf2_ros/buffer.hpp" + +static const std::string kName = "Formation"; + +using namespace px4_ros2::literals; // NOLINT + +class FlightModeTest : public px4_ros2::ModeBase { + public: + explicit FlightModeTest(rclcpp::Node& node, const std::string& topic_namespace_prefix = "") + : ModeBase(node, Settings{kName}.preventArming(true), topic_namespace_prefix), + _tf_prefix(node.get_parameter("tf_prefix").as_string()), + _neighbor_distances(node.get_parameter("neighbor_distances").as_double_array()), + _neighbor_prefixes(node.get_parameter("neighbor_prefixes").as_string_array()), + _gain(node.get_parameter("gain").as_double()) + { + for (const auto& prefix : _neighbor_prefixes) { + _neighbor_base_link_frames.push_back(prefix + "base_link"); + } + _tf_broadcaster = + std::make_unique(node); + _tf_buffer = + std::make_unique(node.get_clock()); + _tf_listener = + std::make_shared(*_tf_buffer); + _trajectory_setpoint = std::make_shared(*this); + _vehicle_local_position = std::make_shared(*this); + _vehicle_local_position->onUpdate([this](const px4_msgs::msg::VehicleLocalPosition& msg) { + if (msg.xy_global && msg.z_global) { + _has_global_position = true; + if (msg.ref_timestamp != _last_global_ref_timestamp) { + _last_global_ref_timestamp = msg.ref_timestamp; + double x,y,z; + _geocentric.Forward(msg.ref_lat, msg.ref_lon, msg.ref_alt, x, y, z); + RCLCPP_INFO(this->node().get_logger(), "Global position reference updated: lat=%f, lon=%f, alt=%f", + msg.ref_lat, msg.ref_lon, msg.ref_alt); + RCLCPP_INFO(this->node().get_logger(), "ECEF position reference updated: x=%f, y=%f, z=%f", + x, y, z); + _ekf_origin.header.frame_id = "earth"; + _ekf_origin.child_frame_id = _tf_prefix + "map"; + _ekf_origin.transform.translation.x = x; + _ekf_origin.transform.translation.y = y; + _ekf_origin.transform.translation.z = z; + double cos_lat = std::cos(msg.ref_lat * M_PI / 180.0); + double sin_lat = std::sin(msg.ref_lat * M_PI / 180.0); + double cos_lon = std::cos(msg.ref_lon * M_PI / 180.0); + double sin_lon = std::sin(msg.ref_lon * M_PI / 180.0); + Eigen::Matrix3d R; + R(0,0) = -sin_lon; R(1,0) = cos_lon; R(2,0) = 0.0; + R(0,1) = -sin_lat * cos_lon; R(1,1) = -sin_lat * sin_lon; R(2,1) = cos_lat; + R(0,2) = cos_lat * cos_lon; R(1,2) = cos_lat * sin_lon; R(2,2) = sin_lat; + Eigen::Quaterniond q(R); + _ekf_origin.transform.rotation.x = q.x(); + _ekf_origin.transform.rotation.y = q.y(); + _ekf_origin.transform.rotation.z = q.z(); + _ekf_origin.transform.rotation.w = q.w(); + } + _ekf_origin.header.stamp = this->node().get_clock()->now(); + _tf_broadcaster->sendTransform(_ekf_origin); + } else { + _has_global_position = false; + } + if (msg.xy_valid && msg.z_valid) { + const Eigen::Vector3f pos_ned = _vehicle_local_position->positionNed(); + const Eigen::Vector3f pos_enu = px4_ros2::positionNedToEnu(pos_ned); + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = this->node().get_clock()->now(); + tf_msg.header.frame_id = _tf_prefix + "map"; + tf_msg.child_frame_id = _tf_prefix + "base_link"; + tf_msg.transform.translation.x = pos_enu(0); + tf_msg.transform.translation.y = pos_enu(1); + tf_msg.transform.translation.z = pos_enu(2); + tf_msg.transform.rotation.x = 0.0; + tf_msg.transform.rotation.y = 0.0; + tf_msg.transform.rotation.z = 0.0; + tf_msg.transform.rotation.w = 1.0; + _tf_broadcaster->sendTransform(tf_msg); + } + }); + RCLCPP_INFO(this->node().get_logger(), "FlightModeTest initialized with %zu neighbors, gain=%f", + _neighbor_base_link_frames.size(), _gain); + if (!this->doRegister()) { + throw px4_ros2::Exception("Registration failed"); + } + } + + void onActivate() override + { + + } + + void updateSetpoint(float dt_s) override + { + Eigen::Vector2f velocity_en{0.0, 0.0}; + for (const auto& toFrameRel : _neighbor_base_link_frames) { + geometry_msgs::msg::TransformStamped t; + try { + t = _tf_buffer->lookupTransform( + toFrameRel, _tf_prefix + "base_link", + tf2::TimePointZero); + const Eigen::Vector2f relative_en{t.transform.translation.x, t.transform.translation.y}; + const float distance = relative_en.norm(); + const Eigen::Vector2f direction = relative_en.normalized(); + const float distance_error = distance - _neighbor_distances[&toFrameRel - &_neighbor_base_link_frames[0]]; + const Eigen::Vector2f individual_control = - distance_error * direction * _gain; + velocity_en.x() += individual_control.x(); + velocity_en.y() += individual_control.y(); + } catch (const tf2::TransformException & ex) { + // RCLCPP_INFO( + // this->get_logger(), "Could not transform %s to %s: %s", + // toFrameRel.c_str(), fromFrameRel.c_str(), ex.what()); + } + + } + const px4_ros2::TrajectorySetpoint setpoint = px4_ros2::TrajectorySetpoint() + .withVelocityX(velocity_en.y()) + .withVelocityY(velocity_en.x()) + .withPositionZ(-2.0f); + _trajectory_setpoint->update(setpoint); + } + + private: + GeographicLib::Geocentric _geocentric{GeographicLib::Geocentric::WGS84()}; + std::shared_ptr _vehicle_local_position; + bool _has_global_position{false}; + uint64_t _last_global_ref_timestamp{0}; + std::unique_ptr _tf_broadcaster; + geometry_msgs::msg::TransformStamped _ekf_origin; + const std::string _tf_prefix; + const std::vector _neighbor_distances; + const std::vector _neighbor_prefixes; + const double _gain; + std::vector _neighbor_base_link_frames; + std::shared_ptr _tf_listener{nullptr}; + std::unique_ptr _tf_buffer; + std::shared_ptr _trajectory_setpoint; +}; diff --git a/px4_roscon_workshop/formation_control/launch/formation.launch.py b/px4_roscon_workshop/formation_control/launch/formation.launch.py new file mode 100644 index 0000000..e2e68d5 --- /dev/null +++ b/px4_roscon_workshop/formation_control/launch/formation.launch.py @@ -0,0 +1,46 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + return LaunchDescription([ + Node( + package='px4_formation_control', + executable='px4_formation_control', + name='formation_controller_1', + output='screen', + parameters=[{ + 'px4_ns': "/px4_1/", + 'tf_prefix': "px4_1", + 'neighbor_prefixes': ['px4_2', 'px4_3'], + 'neighbor_distances': [10.0, 10.0], + 'gain': 1.0 + }] + ), + Node( + package='px4_formation_control', + executable='px4_formation_control', + name='formation_controller_2', + output='screen', + parameters=[{ + 'px4_ns': "/px4_2/", + 'tf_prefix': "px4_2", + 'neighbor_prefixes': ['px4_1', 'px4_3'], + 'neighbor_distances': [10.0, 10.0], + 'gain': 1.0 + }] + ), + Node( + package='px4_formation_control', + executable='px4_formation_control', + name='formation_controller_3', + output='screen', + parameters=[{ + 'px4_ns': "/px4_3/", + 'tf_prefix': "px4_3", + 'neighbor_prefixes': ['px4_1', 'px4_2'], + 'neighbor_distances': [10.0, 10.0], + 'gain': 1.0 + }] + ) + ]) diff --git a/px4_roscon_workshop/formation_control/package.xml b/px4_roscon_workshop/formation_control/package.xml new file mode 100644 index 0000000..df542b9 --- /dev/null +++ b/px4_roscon_workshop/formation_control/package.xml @@ -0,0 +1,31 @@ + + + + px4_formation_control + 0.0.0 + ROSCon workshop Formation Control demo + Beniamino Pozzan + CC-BY-SA-4.0 + + eigen3_cmake_module + ament_cmake + eigen3_cmake_module + + eigen + rclcpp + geographiclib + eigen + geographiclib + + px4_ros2_cpp + geometry_msgs + tf2 + tf2_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/px4_roscon_workshop/formation_control/src/main.cpp b/px4_roscon_workshop/formation_control/src/main.cpp new file mode 100644 index 0000000..945c594 --- /dev/null +++ b/px4_roscon_workshop/formation_control/src/main.cpp @@ -0,0 +1,35 @@ +#include + +#include "rclcpp/rclcpp.hpp" + +static const std::string kNodeName = "formation_controller"; +static const bool kEnableDebugOutput = true; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(kNodeName); + node->declare_parameter("px4_ns", ""); + node->declare_parameter("tf_prefix", ""); + node->declare_parameter("neighbor_distances", std::vector{}); + node->declare_parameter("neighbor_prefixes", std::vector{}); + node->declare_parameter("gain", 1.0); + + if (kEnableDebugOutput) { + auto ret = rcutils_logging_set_logger_level(node->get_logger().get_name(), + RCUTILS_LOG_SEVERITY_DEBUG); + + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR(node->get_logger(), "Error setting severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + } + + FlightModeTest mode(*node, node->get_parameter("px4_ns").as_string()); + rclcpp::spin(node); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file From 194d5c80f0e1295f07102d8f5e9ffc68ed48cf8c Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 19 Aug 2026 00:00:33 +0100 Subject: [PATCH 02/14] feat: split launchfiles to manage gz and PX4 Signed-off-by: Beniamino Pozzan --- .../launch/gz_world.launch.py | 103 ++++++++++++++ .../px4_roscon_workshop/launch/px4.launch.py | 128 ++++++------------ .../launch/px4_vehicle.launch.py | 83 ++++++++++++ 3 files changed, 229 insertions(+), 85 deletions(-) create mode 100644 px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py create mode 100644 px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py new file mode 100644 index 0000000..f3d955c --- /dev/null +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py @@ -0,0 +1,103 @@ +from os import environ, path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, + OpaqueFunction, + SetEnvironmentVariable, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def _launch_setup(context): + px4_autopilot_path = path.expanduser( + LaunchConfiguration('px4_autopilot_path').perform(context) + ) + world_name = LaunchConfiguration('world').perform(context) + extra_gz_resource_path = LaunchConfiguration('extra_gz_resource_path').perform(context) + bridge_config_file = LaunchConfiguration('bridge_config_file').perform(context) + + ros_gz_sim_pkg_path = get_package_share_directory('ros_gz_sim') + px4_gz_resource_path = path.join(px4_autopilot_path, 'Tools', 'simulation', 'gz') + px4_gz_plugin_path = path.join( + px4_autopilot_path, + 'build', + 'px4_sitl_default', + 'src', + 'modules', + 'simulation', + 'gz_plugins', + ) + px4_gz_server_config_path = path.join( + px4_autopilot_path, + 'src', + 'modules', + 'simulation', + 'gz_bridge', + 'server.config', + ) + gz_launch_path = path.join(ros_gz_sim_pkg_path, 'launch', 'ros_gz_sim.launch.py') + + return [ + SetEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + ':'.join([ + path.join(px4_gz_resource_path, 'worlds'), + path.join(px4_gz_resource_path, 'models'), + extra_gz_resource_path, + ]), + ), + SetEnvironmentVariable( + 'GZ_SIM_SYSTEM_PLUGIN_PATH', + ':'.join([environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', ''), px4_gz_plugin_path]), + ), + SetEnvironmentVariable('GZ_SIM_SERVER_CONFIG_PATH', px4_gz_server_config_path), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(gz_launch_path), + launch_arguments={ + 'world_sdf_file': f'{world_name}.sdf', + 'bridge_name': 'gz_ros_bridge', + 'config_file': bridge_config_file, + }.items(), + ), + ExecuteProcess( + cmd=['MicroXRCEAgent', 'udp4', '--port', '8888'], + name='microxrce_agent', + output='screen', + ), + ] + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'px4_autopilot_path', + default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), + description='Path to PX4-Autopilot repository root (supports ~)', + ), + DeclareLaunchArgument( + 'world', + default_value='default', + description='Name of the world to launch', + ), + DeclareLaunchArgument( + 'extra_gz_resource_path', + default_value='', + description='Extra GZ resource path to add to GZ_SIM_RESOURCE_PATH', + ), + DeclareLaunchArgument( + 'bridge_config_file', + default_value=PathJoinSubstitution([ + FindPackageShare('px4_roscon_workshop'), + 'cfg', + 'clock_bridge.yaml', + ]), + description='Path to the ROS-GZ bridge configuration file', + ), + OpaqueFunction(function=_launch_setup), + ]) \ No newline at end of file diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/px4.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/px4.launch.py index bb72dff..f41fbb3 100644 --- a/px4_roscon_workshop/px4_roscon_workshop/launch/px4.launch.py +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/px4.launch.py @@ -1,94 +1,17 @@ -import glob -from os import path, environ -from ament_index_python.packages import get_package_share_directory +from os import path from launch import LaunchDescription -from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, IncludeLaunchDescription, ExecuteProcess, OpaqueFunction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackageShare -# launch gazebo server and PX4 -# gz gui needs to be launched separately, e.g.: gz sim -g - -def _launch_setup(context): - px4_autopilot_path = path.expanduser( - LaunchConfiguration('px4_autopilot_path').perform(context) - ) - world_name = LaunchConfiguration('world').perform(context) - model_name = LaunchConfiguration('model').perform(context) - model_path = path.expanduser( - LaunchConfiguration('model_path').perform(context) - ) - - extra_gz_resource_path = LaunchConfiguration('extra_gz_resource_path').perform(context) - - ros_gz_sim_pkg_path = get_package_share_directory('ros_gz_sim') - px4_roscon_workshop_pkg_path = get_package_share_directory('px4_roscon_workshop') - px4_gz_resource_path = path.join(px4_autopilot_path, 'Tools', 'simulation', 'gz') - px4_gz_plugin_path = path.join(px4_autopilot_path, 'build', 'px4_sitl_default', 'src', 'modules', 'simulation', 'gz_plugins') - px4_gz_server_config_path = path.join(px4_autopilot_path, 'src', 'modules', 'simulation', 'gz_bridge', 'server.config') - gz_launch_path = path.join(ros_gz_sim_pkg_path, 'launch', 'ros_gz_sim.launch.py') - gz_spawn_path = path.join(ros_gz_sim_pkg_path, 'launch', 'gz_spawn_model.launch.py') - - return [ - SetEnvironmentVariable( - 'GZ_SIM_RESOURCE_PATH', - ':'.join([path.join(px4_gz_resource_path, 'worlds'), path.join(px4_gz_resource_path, 'models'), extra_gz_resource_path]) - ), - SetEnvironmentVariable( - 'GZ_SIM_SYSTEM_PLUGIN_PATH', - ':'.join([environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', ''), px4_gz_plugin_path]) - ), - SetEnvironmentVariable( - 'GZ_SIM_SERVER_CONFIG_PATH', - px4_gz_server_config_path - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(gz_launch_path), - launch_arguments={ - 'world_sdf_file': f'{world_name}.sdf', - 'bridge_name': 'gz_ros_bridge', - 'config_file': path.join(px4_roscon_workshop_pkg_path, 'cfg', 'clock_bridge.yaml') - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource(gz_spawn_path), - launch_arguments={ - 'world': world_name, - 'file': model_path, - }.items(), - ), - ExecuteProcess( - cmd = [ - path.join(px4_autopilot_path, 'build', 'px4_sitl_default', 'bin', 'px4'), - ], - additional_env = { - 'PX4_SYS_AUTOSTART': LaunchConfiguration('px4_autostart_id').perform(context), - 'PX4_GZ_MODEL_NAME': model_name, - 'PX4_GZ_STANDALONE': '1', - 'PX4_GZ_WORLD': world_name, - }, - name = 'px4', - output = 'screen' - ), - ExecuteProcess( - cmd = [ - "MicroXRCEAgent", - "udp4", - "--port", "8888", - ], - name = 'microxrce_agent', - output = 'screen' - ) - ] - - def generate_launch_description(): + pkg_share = FindPackageShare('px4_roscon_workshop').find('px4_roscon_workshop') + return LaunchDescription([ DeclareLaunchArgument( 'px4_autopilot_path', - default_value='~/PX4-Autopilot', + default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), description='Path to PX4-Autopilot repository root (supports ~)', ), DeclareLaunchArgument( @@ -103,7 +26,15 @@ def generate_launch_description(): ), DeclareLaunchArgument( 'model_path', - default_value='~/PX4-Autopilot/Tools/simulation/gz/models/x500/model.sdf', + default_value=PathJoinSubstitution([ + LaunchConfiguration('px4_autopilot_path'), + 'Tools', + 'simulation', + 'gz', + 'models', + 'x500', + 'model.sdf', + ]), description='Path to the model SDF file', ), DeclareLaunchArgument( @@ -116,5 +47,32 @@ def generate_launch_description(): default_value='', description='Extra GZ resource path to add to GZ_SIM_RESOURCE_PATH', ), - OpaqueFunction(function=_launch_setup), + DeclareLaunchArgument( + 'bridge_config_file', + default_value=PathJoinSubstitution([ + FindPackageShare('px4_roscon_workshop'), + 'cfg', + 'clock_bridge.yaml', + ]), + description='Path to the ROS-GZ bridge configuration file', + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(path.join(pkg_share, 'launch', 'gz_world.launch.py')), + launch_arguments={ + 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), + 'world': LaunchConfiguration('world'), + 'extra_gz_resource_path': LaunchConfiguration('extra_gz_resource_path'), + 'bridge_config_file': LaunchConfiguration('bridge_config_file'), + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(path.join(pkg_share, 'launch', 'px4_vehicle.launch.py')), + launch_arguments={ + 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), + 'world': LaunchConfiguration('world'), + 'model': LaunchConfiguration('model'), + 'model_path': LaunchConfiguration('model_path'), + 'px4_autostart_id': LaunchConfiguration('px4_autostart_id'), + }.items(), + ), ]) \ No newline at end of file diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py new file mode 100644 index 0000000..c795f10 --- /dev/null +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py @@ -0,0 +1,83 @@ +from os import path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, OpaqueFunction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution + + +def _launch_setup(context): + px4_autopilot_path = path.expanduser( + LaunchConfiguration('px4_autopilot_path').perform(context) + ) + world_name = LaunchConfiguration('world').perform(context) + model_name = LaunchConfiguration('model').perform(context) + model_path = path.expanduser( + LaunchConfiguration('model_path').perform(context) + ) + + ros_gz_sim_pkg_path = get_package_share_directory('ros_gz_sim') + gz_spawn_path = path.join(ros_gz_sim_pkg_path, 'launch', 'gz_spawn_model.launch.py') + + return [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(gz_spawn_path), + launch_arguments={ + 'world': world_name, + 'file': model_path, + }.items(), + ), + ExecuteProcess( + cmd=[ + path.join(px4_autopilot_path, 'build', 'px4_sitl_default', 'bin', 'px4'), + ], + additional_env={ + 'PX4_SYS_AUTOSTART': LaunchConfiguration('px4_autostart_id').perform(context), + 'PX4_GZ_MODEL_NAME': model_name, + 'PX4_GZ_STANDALONE': '1', + 'PX4_GZ_WORLD': world_name, + }, + name='px4', + output='screen', + ), + ] + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'px4_autopilot_path', + default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), + description='Path to PX4-Autopilot repository root (supports ~)', + ), + DeclareLaunchArgument( + 'world', + default_value='default', + description='Name of the world where the model is spawned', + ), + DeclareLaunchArgument( + 'model', + default_value='x500', + description='Name of the model to start PX4 for', + ), + DeclareLaunchArgument( + 'model_path', + default_value=PathJoinSubstitution([ + LaunchConfiguration('px4_autopilot_path'), + 'Tools', + 'simulation', + 'gz', + 'models', + 'x500', + 'model.sdf', + ]), + description='Path to the model SDF file', + ), + DeclareLaunchArgument( + 'px4_autostart_id', + default_value='4001', + description='PX4 autostart ID', + ), + OpaqueFunction(function=_launch_setup), + ]) \ No newline at end of file From 918ddbde571fa6e629fe6f3d10f0dbadaf12e627 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 19 Aug 2026 00:01:00 +0100 Subject: [PATCH 03/14] chore: rover_teleop launchfile uses PX4_PATH env var Signed-off-by: Beniamino Pozzan --- px4_roscon_workshop/rover_teleop/launch/rover_launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/px4_roscon_workshop/rover_teleop/launch/rover_launch.py b/px4_roscon_workshop/rover_teleop/launch/rover_launch.py index 5ca0bf8..521259d 100644 --- a/px4_roscon_workshop/rover_teleop/launch/rover_launch.py +++ b/px4_roscon_workshop/rover_teleop/launch/rover_launch.py @@ -7,7 +7,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, EnvironmentVariable @@ -19,7 +19,7 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument( 'px4_autopilot_path', - default_value='~/PX4-Autopilot', + default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), description='Path to PX4-Autopilot repository root (supports ~)', ), IncludeLaunchDescription( From 6b143f0e2b888c75769aeaaee1118a1b4f38be1e Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 19 Aug 2026 00:48:33 +0100 Subject: [PATCH 04/14] chore: expose more params Signed-off-by: Beniamino Pozzan --- .../launch/gz_world.launch.py | 2 +- .../launch/px4_vehicle.launch.py | 48 ++++++++++++++++--- 2 files changed, 43 insertions(+), 7 deletions(-) diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py index f3d955c..778c2dd 100644 --- a/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/gz_world.launch.py @@ -66,7 +66,7 @@ def _launch_setup(context): }.items(), ), ExecuteProcess( - cmd=['MicroXRCEAgent', 'udp4', '--port', '8888'], + cmd=['MicroXRCEAgent', 'udp4', '--port', '8888', '-v', '1'], name='microxrce_agent', output='screen', ), diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py index c795f10..3206eab 100644 --- a/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py @@ -16,28 +16,39 @@ def _launch_setup(context): model_path = path.expanduser( LaunchConfiguration('model_path').perform(context) ) + px4_instance = LaunchConfiguration('px4_instance').perform(context) + px4_namespace = LaunchConfiguration('px4_ns').perform(context) ros_gz_sim_pkg_path = get_package_share_directory('ros_gz_sim') gz_spawn_path = path.join(ros_gz_sim_pkg_path, 'launch', 'gz_spawn_model.launch.py') + px4_env = { + 'PX4_SYS_AUTOSTART': LaunchConfiguration('px4_autostart_id').perform(context), + 'PX4_GZ_MODEL_NAME': model_name, + 'PX4_GZ_STANDALONE': '1', + 'PX4_GZ_WORLD': world_name, + } + if px4_namespace: + px4_env['PX4_UXRCE_DDS_NS'] = px4_namespace + return [ IncludeLaunchDescription( PythonLaunchDescriptionSource(gz_spawn_path), launch_arguments={ 'world': world_name, 'file': model_path, + 'entity_name': model_name, + 'x': LaunchConfiguration('spawn_pos_x').perform(context), + 'y': LaunchConfiguration('spawn_pos_y').perform(context), + 'z': LaunchConfiguration('spawn_pos_z').perform(context), }.items(), ), ExecuteProcess( cmd=[ path.join(px4_autopilot_path, 'build', 'px4_sitl_default', 'bin', 'px4'), + '-i', px4_instance, ], - additional_env={ - 'PX4_SYS_AUTOSTART': LaunchConfiguration('px4_autostart_id').perform(context), - 'PX4_GZ_MODEL_NAME': model_name, - 'PX4_GZ_STANDALONE': '1', - 'PX4_GZ_WORLD': world_name, - }, + additional_env=px4_env, name='px4', output='screen', ), @@ -51,6 +62,11 @@ def generate_launch_description(): default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), description='Path to PX4-Autopilot repository root (supports ~)', ), + DeclareLaunchArgument( + 'px4_instance', + default_value='0', + description='PX4 instance ID', + ), DeclareLaunchArgument( 'world', default_value='default', @@ -79,5 +95,25 @@ def generate_launch_description(): default_value='4001', description='PX4 autostart ID', ), + DeclareLaunchArgument( + 'px4_ns', + default_value='', + description='PX4 namespace', + ), + DeclareLaunchArgument( + 'spawn_pos_x', + default_value='0.0', + description='X position to spawn the model at', + ), + DeclareLaunchArgument( + 'spawn_pos_y', + default_value='0.0', + description='Y position to spawn the model at', + ), + DeclareLaunchArgument( + 'spawn_pos_z', + default_value='0.0', + description='Z position to spawn the model at', + ), OpaqueFunction(function=_launch_setup), ]) \ No newline at end of file From a5f2d89e76c9485582dab791108e0487e851e623 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 19 Aug 2026 00:49:35 +0100 Subject: [PATCH 05/14] chore: formation control launch everything Signed-off-by: Beniamino Pozzan --- .../launch/formation.launch.py | 114 +++++++++++++++--- .../formation_control/package.xml | 1 + 2 files changed, 96 insertions(+), 19 deletions(-) diff --git a/px4_roscon_workshop/formation_control/launch/formation.launch.py b/px4_roscon_workshop/formation_control/launch/formation.launch.py index e2e68d5..9c34735 100644 --- a/px4_roscon_workshop/formation_control/launch/formation.launch.py +++ b/px4_roscon_workshop/formation_control/launch/formation.launch.py @@ -1,46 +1,122 @@ +from os import path + from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import EnvironmentVariable, LaunchConfiguration from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare def generate_launch_description() -> LaunchDescription: + px4_roscon_workshop_share = FindPackageShare('px4_roscon_workshop').find( + 'px4_roscon_workshop' + ) + gz_world_launch = path.join( + px4_roscon_workshop_share, + 'launch', + 'gz_world.launch.py', + ) + px4_vehicle_launch = path.join( + px4_roscon_workshop_share, + 'launch', + 'px4_vehicle.launch.py', + ) + return LaunchDescription([ + DeclareLaunchArgument( + 'px4_autopilot_path', + default_value=EnvironmentVariable('PX4_PATH', default_value='~/PX4-Autopilot'), + description='Path to PX4-Autopilot repository root (supports ~)', + ), + DeclareLaunchArgument( + 'world', + default_value='default', + description='Name of the Gazebo world to launch', + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(gz_world_launch), + launch_arguments={ + 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), + 'world': LaunchConfiguration('world'), + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(px4_vehicle_launch), + launch_arguments={ + 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), + 'world': LaunchConfiguration('world'), + 'px4_instance': '0', + 'model': 'x500_0', + 'px4_ns': 'px4_0', + 'spawn_pos_x': '0.0', + 'spawn_pos_y': '0.0', + 'spawn_pos_z': '0.3', + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(px4_vehicle_launch), + launch_arguments={ + 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), + 'world': LaunchConfiguration('world'), + 'px4_instance': '1', + 'model': 'x500_1', + 'px4_ns': 'px4_1', + 'spawn_pos_x': '10.0', + 'spawn_pos_y': '0.0', + 'spawn_pos_z': '0.3', + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(px4_vehicle_launch), + launch_arguments={ + 'px4_autopilot_path': LaunchConfiguration('px4_autopilot_path'), + 'world': LaunchConfiguration('world'), + 'px4_instance': '2', + 'model': 'x500_2', + 'px4_ns': 'px4_2', + 'spawn_pos_x': '5.0', + 'spawn_pos_y': '8.660254', + 'spawn_pos_z': '0.3', + }.items(), + ), Node( package='px4_formation_control', executable='px4_formation_control', - name='formation_controller_1', + name='formation_controller_0', output='screen', parameters=[{ - 'px4_ns': "/px4_1/", - 'tf_prefix': "px4_1", - 'neighbor_prefixes': ['px4_2', 'px4_3'], + 'px4_ns': '/px4_0/', + 'tf_prefix': 'px4_0', + 'neighbor_prefixes': ['px4_1', 'px4_2'], 'neighbor_distances': [10.0, 10.0], - 'gain': 1.0 - }] + 'gain': 1.0, + }], ), Node( package='px4_formation_control', executable='px4_formation_control', - name='formation_controller_2', + name='formation_controller_1', output='screen', parameters=[{ - 'px4_ns': "/px4_2/", - 'tf_prefix': "px4_2", - 'neighbor_prefixes': ['px4_1', 'px4_3'], + 'px4_ns': '/px4_1/', + 'tf_prefix': 'px4_1', + 'neighbor_prefixes': ['px4_0', 'px4_2'], 'neighbor_distances': [10.0, 10.0], - 'gain': 1.0 - }] + 'gain': 1.0, + }], ), Node( package='px4_formation_control', executable='px4_formation_control', - name='formation_controller_3', + name='formation_controller_2', output='screen', parameters=[{ - 'px4_ns': "/px4_3/", - 'tf_prefix': "px4_3", - 'neighbor_prefixes': ['px4_1', 'px4_2'], + 'px4_ns': '/px4_2/', + 'tf_prefix': 'px4_2', + 'neighbor_prefixes': ['px4_0', 'px4_1'], 'neighbor_distances': [10.0, 10.0], - 'gain': 1.0 - }] - ) + 'gain': 1.0, + }], + ), ]) diff --git a/px4_roscon_workshop/formation_control/package.xml b/px4_roscon_workshop/formation_control/package.xml index df542b9..51274fb 100644 --- a/px4_roscon_workshop/formation_control/package.xml +++ b/px4_roscon_workshop/formation_control/package.xml @@ -21,6 +21,7 @@ geometry_msgs tf2 tf2_ros + px4_roscon_workshop ament_lint_auto ament_lint_common From 122033dd89ee3496c9c22ba15a0780c13892b357 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Tue, 25 Aug 2026 22:34:04 +0100 Subject: [PATCH 06/14] feat: px4_vehicle.launch.py can set env vars for PX4 process Signed-off-by: Beniamino Pozzan --- .../launch/formation.launch.py | 3 +++ .../launch/px4_vehicle.launch.py | 17 ++++++++++++++++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/px4_roscon_workshop/formation_control/launch/formation.launch.py b/px4_roscon_workshop/formation_control/launch/formation.launch.py index 9c34735..4103a42 100644 --- a/px4_roscon_workshop/formation_control/launch/formation.launch.py +++ b/px4_roscon_workshop/formation_control/launch/formation.launch.py @@ -52,6 +52,7 @@ def generate_launch_description() -> LaunchDescription: 'spawn_pos_x': '0.0', 'spawn_pos_y': '0.0', 'spawn_pos_z': '0.3', + 'px4_extra_env_vars': 'PX4_PARAM_COM_RCL_EXCEPT=9,PX4_PARAM_COM_RC_IN_MODE=1,PX4_GZ_NO_FOLLOW=1', }.items(), ), IncludeLaunchDescription( @@ -65,6 +66,7 @@ def generate_launch_description() -> LaunchDescription: 'spawn_pos_x': '10.0', 'spawn_pos_y': '0.0', 'spawn_pos_z': '0.3', + 'px4_extra_env_vars': 'PX4_PARAM_COM_RCL_EXCEPT=9,PX4_PARAM_COM_RC_IN_MODE=4,PX4_GZ_NO_FOLLOW=1', }.items(), ), IncludeLaunchDescription( @@ -78,6 +80,7 @@ def generate_launch_description() -> LaunchDescription: 'spawn_pos_x': '5.0', 'spawn_pos_y': '8.660254', 'spawn_pos_z': '0.3', + 'px4_extra_env_vars': 'PX4_PARAM_COM_RCL_EXCEPT=9,PX4_PARAM_COM_RC_IN_MODE=4,PX4_GZ_NO_FOLLOW=1', }.items(), ), Node( diff --git a/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py b/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py index 3206eab..73e1440 100644 --- a/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py +++ b/px4_roscon_workshop/px4_roscon_workshop/launch/px4_vehicle.launch.py @@ -19,6 +19,8 @@ def _launch_setup(context): px4_instance = LaunchConfiguration('px4_instance').perform(context) px4_namespace = LaunchConfiguration('px4_ns').perform(context) + px4_extra_env_vars = LaunchConfiguration('px4_extra_env_vars').perform(context) + ros_gz_sim_pkg_path = get_package_share_directory('ros_gz_sim') gz_spawn_path = path.join(ros_gz_sim_pkg_path, 'launch', 'gz_spawn_model.launch.py') @@ -28,6 +30,14 @@ def _launch_setup(context): 'PX4_GZ_STANDALONE': '1', 'PX4_GZ_WORLD': world_name, } + px4_env.update( + dict( + (key.strip(), value.strip()) + for env_var in px4_extra_env_vars.split(',') + if env_var.strip() + for key, value in [env_var.split('=', 1)] + ) + ) if px4_namespace: px4_env['PX4_UXRCE_DDS_NS'] = px4_namespace @@ -49,7 +59,7 @@ def _launch_setup(context): '-i', px4_instance, ], additional_env=px4_env, - name='px4', + name=f'px4_{px4_instance}', output='screen', ), ] @@ -100,6 +110,11 @@ def generate_launch_description(): default_value='', description='PX4 namespace', ), + DeclareLaunchArgument( + 'px4_extra_env_vars', + default_value='', + description='Extra environment variables to set for PX4', + ), DeclareLaunchArgument( 'spawn_pos_x', default_value='0.0', From d4358d73c135fcab76605fe719267267249dc9fd Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Tue, 25 Aug 2026 22:38:08 +0100 Subject: [PATCH 07/14] docs: add README for formation control package Signed-off-by: Beniamino Pozzan --- .../formation_control/README.md | 65 +++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 px4_roscon_workshop/formation_control/README.md diff --git a/px4_roscon_workshop/formation_control/README.md b/px4_roscon_workshop/formation_control/README.md new file mode 100644 index 0000000..eb03faa --- /dev/null +++ b/px4_roscon_workshop/formation_control/README.md @@ -0,0 +1,65 @@ +# Formation Control + +The `px4_formation_control` package demonstrates multi-vehicle formation control with PX4 and ROS 2. It launches three simulated X500 vehicles and runs one `Formation` flight mode per vehicle. + +Each controller: + +- publishes the vehicle's local TF frames (`map` and `base_link`), +- reads the relative TF transform to its configured neighbors, and +- commands a horizontal velocity proportional to the error from each desired neighbor distance. + +The default formation is a triangle with 10 m target distances. The vehicles hold an altitude of 2 m above the local origin. + +## Prerequisites + +- A sourced ROS 2 installation with the workspace dependencies available. +- A built PX4-Autopilot SITL target. The launch file uses the executable at `build/px4_sitl_default/bin/px4`. +- Gazebo and the PX4 ROS 2 integration packages installed and built in this workspace. +- QGroundControl, or another way to activate the flight mode and arm the vehicles. + +The PX4 repository path can be supplied explicitly, or through the `PX4_PATH` environment variable. If neither is set, the default is `~/PX4-Autopilot`. + +## Build + +From the workspace root: + +```sh +source /opt/ros/$ROS_DISTRO/setup.bash +colcon build --packages-select px4_formation_control +source install/setup.bash +``` + +## Run + +Launch the simulation and all three formation controllers: + +```sh +ros2 launch px4_formation_control formation.launch.py \ + px4_autopilot_path:=~/PX4-Autopilot +``` + +The `px4_autopilot_path` argument must point to the PX4-Autopilot repository root. The launch file also accepts a Gazebo world name: + +```sh +ros2 launch px4_formation_control formation.launch.py \ + px4_autopilot_path:=~/PX4-Autopilot \ + world:=default +``` + +After startup, use QGroundControl to activate the `Formation` mode for the vehicles and arm them according to the PX4 safety rules. The mode is configured to prevent arming from the mode itself, so manual activation and arming may be required. + +## Default configuration + +| Vehicle | Namespace | Spawn position (m) | Neighbors | +| --- | --- | --- | --- | +| 0 | `/px4_0/` | `(0, 0, 0.3)` | `px4_1`, `px4_2` | +| 1 | `/px4_1/` | `(10, 0, 0.3)` | `px4_0`, `px4_2` | +| 2 | `/px4_2/` | `(5, 8.660254, 0.3)` | `px4_0`, `px4_1` | + +Each controller uses a target distance of `10.0` m and a control gain of `1.0`. These values are configured in `launch/formation.launch.py`. + +## Notes + +- This is an experimental workshop demonstration intended for simulation. +- The controller depends on valid local-position data and TF transforms from the other vehicles. It skips a neighbor while its transform is unavailable. +- Stop the launch process before restarting it to avoid leaving PX4 or Gazebo processes running. From 0cdeeeb612390e6394336d662058c5e7c0802771 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Tue, 25 Aug 2026 22:50:08 +0100 Subject: [PATCH 08/14] chore: rename custom mode into FormationControlMode Signed-off-by: Beniamino Pozzan --- .../formation_control/include/formation_control.hpp | 6 +++--- px4_roscon_workshop/formation_control/src/main.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/px4_roscon_workshop/formation_control/include/formation_control.hpp b/px4_roscon_workshop/formation_control/include/formation_control.hpp index d42232a..414fa79 100644 --- a/px4_roscon_workshop/formation_control/include/formation_control.hpp +++ b/px4_roscon_workshop/formation_control/include/formation_control.hpp @@ -21,9 +21,9 @@ static const std::string kName = "Formation"; using namespace px4_ros2::literals; // NOLINT -class FlightModeTest : public px4_ros2::ModeBase { +class FormationControlMode : public px4_ros2::ModeBase { public: - explicit FlightModeTest(rclcpp::Node& node, const std::string& topic_namespace_prefix = "") + explicit FormationControlMode(rclcpp::Node& node, const std::string& topic_namespace_prefix = "") : ModeBase(node, Settings{kName}.preventArming(true), topic_namespace_prefix), _tf_prefix(node.get_parameter("tf_prefix").as_string()), _neighbor_distances(node.get_parameter("neighbor_distances").as_double_array()), @@ -93,7 +93,7 @@ class FlightModeTest : public px4_ros2::ModeBase { _tf_broadcaster->sendTransform(tf_msg); } }); - RCLCPP_INFO(this->node().get_logger(), "FlightModeTest initialized with %zu neighbors, gain=%f", + RCLCPP_INFO(this->node().get_logger(), "FormationControlMode initialized with %zu neighbors, gain=%f", _neighbor_base_link_frames.size(), _gain); if (!this->doRegister()) { throw px4_ros2::Exception("Registration failed"); diff --git a/px4_roscon_workshop/formation_control/src/main.cpp b/px4_roscon_workshop/formation_control/src/main.cpp index 945c594..b9add76 100644 --- a/px4_roscon_workshop/formation_control/src/main.cpp +++ b/px4_roscon_workshop/formation_control/src/main.cpp @@ -27,7 +27,7 @@ int main(int argc, char* argv[]) } } - FlightModeTest mode(*node, node->get_parameter("px4_ns").as_string()); + FormationControlMode mode(*node, node->get_parameter("px4_ns").as_string()); rclcpp::spin(node); rclcpp::shutdown(); From 60c03f85afc3e1b3d405844e3406244a76ccaf05 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Tue, 25 Aug 2026 23:19:15 +0100 Subject: [PATCH 09/14] chore: allow arm when in formation mode and register mode outside contructor Signed-off-by: Beniamino Pozzan --- .../formation_control/include/formation_control.hpp | 5 +---- px4_roscon_workshop/formation_control/src/main.cpp | 5 +++++ 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/px4_roscon_workshop/formation_control/include/formation_control.hpp b/px4_roscon_workshop/formation_control/include/formation_control.hpp index 414fa79..6e15280 100644 --- a/px4_roscon_workshop/formation_control/include/formation_control.hpp +++ b/px4_roscon_workshop/formation_control/include/formation_control.hpp @@ -24,7 +24,7 @@ using namespace px4_ros2::literals; // NOLINT class FormationControlMode : public px4_ros2::ModeBase { public: explicit FormationControlMode(rclcpp::Node& node, const std::string& topic_namespace_prefix = "") - : ModeBase(node, Settings{kName}.preventArming(true), topic_namespace_prefix), + : ModeBase(node, Settings{kName}, topic_namespace_prefix), _tf_prefix(node.get_parameter("tf_prefix").as_string()), _neighbor_distances(node.get_parameter("neighbor_distances").as_double_array()), _neighbor_prefixes(node.get_parameter("neighbor_prefixes").as_string_array()), @@ -95,9 +95,6 @@ class FormationControlMode : public px4_ros2::ModeBase { }); RCLCPP_INFO(this->node().get_logger(), "FormationControlMode initialized with %zu neighbors, gain=%f", _neighbor_base_link_frames.size(), _gain); - if (!this->doRegister()) { - throw px4_ros2::Exception("Registration failed"); - } } void onActivate() override diff --git a/px4_roscon_workshop/formation_control/src/main.cpp b/px4_roscon_workshop/formation_control/src/main.cpp index b9add76..71cc396 100644 --- a/px4_roscon_workshop/formation_control/src/main.cpp +++ b/px4_roscon_workshop/formation_control/src/main.cpp @@ -28,6 +28,11 @@ int main(int argc, char* argv[]) } FormationControlMode mode(*node, node->get_parameter("px4_ns").as_string()); + if (!mode.doRegister()) { + RCLCPP_ERROR(node->get_logger(), "Formation mode registration failed"); + rclcpp::shutdown(); + return 1; + } rclcpp::spin(node); rclcpp::shutdown(); From ab7e5b15f9b7524d5d230a71ab55d55742bb9c98 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Tue, 25 Aug 2026 23:26:02 +0100 Subject: [PATCH 10/14] chore: split formation_control.hpp in header and source Signed-off-by: Beniamino Pozzan --- .../formation_control/CMakeLists.txt | 1 + .../include/formation_control.hpp | 113 +---------------- .../src/formation_control.cpp | 116 ++++++++++++++++++ 3 files changed, 121 insertions(+), 109 deletions(-) create mode 100644 px4_roscon_workshop/formation_control/src/formation_control.cpp diff --git a/px4_roscon_workshop/formation_control/CMakeLists.txt b/px4_roscon_workshop/formation_control/CMakeLists.txt index cabcc1d..a393ba3 100644 --- a/px4_roscon_workshop/formation_control/CMakeLists.txt +++ b/px4_roscon_workshop/formation_control/CMakeLists.txt @@ -27,6 +27,7 @@ include_directories( add_executable(px4_formation_control src/main.cpp + src/formation_control.cpp ) ament_target_dependencies(px4_formation_control diff --git a/px4_roscon_workshop/formation_control/include/formation_control.hpp b/px4_roscon_workshop/formation_control/include/formation_control.hpp index 6e15280..039a4b9 100644 --- a/px4_roscon_workshop/formation_control/include/formation_control.hpp +++ b/px4_roscon_workshop/formation_control/include/formation_control.hpp @@ -17,120 +17,15 @@ #include "tf2_ros/transform_listener.hpp" #include "tf2_ros/buffer.hpp" -static const std::string kName = "Formation"; - using namespace px4_ros2::literals; // NOLINT class FormationControlMode : public px4_ros2::ModeBase { public: - explicit FormationControlMode(rclcpp::Node& node, const std::string& topic_namespace_prefix = "") - : ModeBase(node, Settings{kName}, topic_namespace_prefix), - _tf_prefix(node.get_parameter("tf_prefix").as_string()), - _neighbor_distances(node.get_parameter("neighbor_distances").as_double_array()), - _neighbor_prefixes(node.get_parameter("neighbor_prefixes").as_string_array()), - _gain(node.get_parameter("gain").as_double()) - { - for (const auto& prefix : _neighbor_prefixes) { - _neighbor_base_link_frames.push_back(prefix + "base_link"); - } - _tf_broadcaster = - std::make_unique(node); - _tf_buffer = - std::make_unique(node.get_clock()); - _tf_listener = - std::make_shared(*_tf_buffer); - _trajectory_setpoint = std::make_shared(*this); - _vehicle_local_position = std::make_shared(*this); - _vehicle_local_position->onUpdate([this](const px4_msgs::msg::VehicleLocalPosition& msg) { - if (msg.xy_global && msg.z_global) { - _has_global_position = true; - if (msg.ref_timestamp != _last_global_ref_timestamp) { - _last_global_ref_timestamp = msg.ref_timestamp; - double x,y,z; - _geocentric.Forward(msg.ref_lat, msg.ref_lon, msg.ref_alt, x, y, z); - RCLCPP_INFO(this->node().get_logger(), "Global position reference updated: lat=%f, lon=%f, alt=%f", - msg.ref_lat, msg.ref_lon, msg.ref_alt); - RCLCPP_INFO(this->node().get_logger(), "ECEF position reference updated: x=%f, y=%f, z=%f", - x, y, z); - _ekf_origin.header.frame_id = "earth"; - _ekf_origin.child_frame_id = _tf_prefix + "map"; - _ekf_origin.transform.translation.x = x; - _ekf_origin.transform.translation.y = y; - _ekf_origin.transform.translation.z = z; - double cos_lat = std::cos(msg.ref_lat * M_PI / 180.0); - double sin_lat = std::sin(msg.ref_lat * M_PI / 180.0); - double cos_lon = std::cos(msg.ref_lon * M_PI / 180.0); - double sin_lon = std::sin(msg.ref_lon * M_PI / 180.0); - Eigen::Matrix3d R; - R(0,0) = -sin_lon; R(1,0) = cos_lon; R(2,0) = 0.0; - R(0,1) = -sin_lat * cos_lon; R(1,1) = -sin_lat * sin_lon; R(2,1) = cos_lat; - R(0,2) = cos_lat * cos_lon; R(1,2) = cos_lat * sin_lon; R(2,2) = sin_lat; - Eigen::Quaterniond q(R); - _ekf_origin.transform.rotation.x = q.x(); - _ekf_origin.transform.rotation.y = q.y(); - _ekf_origin.transform.rotation.z = q.z(); - _ekf_origin.transform.rotation.w = q.w(); - } - _ekf_origin.header.stamp = this->node().get_clock()->now(); - _tf_broadcaster->sendTransform(_ekf_origin); - } else { - _has_global_position = false; - } - if (msg.xy_valid && msg.z_valid) { - const Eigen::Vector3f pos_ned = _vehicle_local_position->positionNed(); - const Eigen::Vector3f pos_enu = px4_ros2::positionNedToEnu(pos_ned); - geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = this->node().get_clock()->now(); - tf_msg.header.frame_id = _tf_prefix + "map"; - tf_msg.child_frame_id = _tf_prefix + "base_link"; - tf_msg.transform.translation.x = pos_enu(0); - tf_msg.transform.translation.y = pos_enu(1); - tf_msg.transform.translation.z = pos_enu(2); - tf_msg.transform.rotation.x = 0.0; - tf_msg.transform.rotation.y = 0.0; - tf_msg.transform.rotation.z = 0.0; - tf_msg.transform.rotation.w = 1.0; - _tf_broadcaster->sendTransform(tf_msg); - } - }); - RCLCPP_INFO(this->node().get_logger(), "FormationControlMode initialized with %zu neighbors, gain=%f", - _neighbor_base_link_frames.size(), _gain); - } - - void onActivate() override - { - - } - - void updateSetpoint(float dt_s) override - { - Eigen::Vector2f velocity_en{0.0, 0.0}; - for (const auto& toFrameRel : _neighbor_base_link_frames) { - geometry_msgs::msg::TransformStamped t; - try { - t = _tf_buffer->lookupTransform( - toFrameRel, _tf_prefix + "base_link", - tf2::TimePointZero); - const Eigen::Vector2f relative_en{t.transform.translation.x, t.transform.translation.y}; - const float distance = relative_en.norm(); - const Eigen::Vector2f direction = relative_en.normalized(); - const float distance_error = distance - _neighbor_distances[&toFrameRel - &_neighbor_base_link_frames[0]]; - const Eigen::Vector2f individual_control = - distance_error * direction * _gain; - velocity_en.x() += individual_control.x(); - velocity_en.y() += individual_control.y(); - } catch (const tf2::TransformException & ex) { - // RCLCPP_INFO( - // this->get_logger(), "Could not transform %s to %s: %s", - // toFrameRel.c_str(), fromFrameRel.c_str(), ex.what()); - } + explicit FormationControlMode( + rclcpp::Node& node, const std::string& topic_namespace_prefix = ""); - } - const px4_ros2::TrajectorySetpoint setpoint = px4_ros2::TrajectorySetpoint() - .withVelocityX(velocity_en.y()) - .withVelocityY(velocity_en.x()) - .withPositionZ(-2.0f); - _trajectory_setpoint->update(setpoint); - } + void onActivate() override; + void updateSetpoint(float dt_s) override; private: GeographicLib::Geocentric _geocentric{GeographicLib::Geocentric::WGS84()}; diff --git a/px4_roscon_workshop/formation_control/src/formation_control.cpp b/px4_roscon_workshop/formation_control/src/formation_control.cpp new file mode 100644 index 0000000..171a705 --- /dev/null +++ b/px4_roscon_workshop/formation_control/src/formation_control.cpp @@ -0,0 +1,116 @@ +#include + +using namespace px4_ros2::literals; // NOLINT + +static const std::string kName = "Formation"; + +FormationControlMode::FormationControlMode( + rclcpp::Node& node, const std::string& topic_namespace_prefix) + : ModeBase(node, Settings{kName}, topic_namespace_prefix), + _tf_prefix(node.get_parameter("tf_prefix").as_string()), + _neighbor_distances(node.get_parameter("neighbor_distances").as_double_array()), + _neighbor_prefixes(node.get_parameter("neighbor_prefixes").as_string_array()), + _gain(node.get_parameter("gain").as_double()) +{ + for (const auto& prefix : _neighbor_prefixes) { + _neighbor_base_link_frames.push_back(prefix + "base_link"); + } + _tf_broadcaster = std::make_unique(node); + _tf_buffer = std::make_unique(node.get_clock()); + _tf_listener = std::make_shared(*_tf_buffer); + _trajectory_setpoint = std::make_shared(*this); + _vehicle_local_position = std::make_shared(*this); + _vehicle_local_position->onUpdate([this](const px4_msgs::msg::VehicleLocalPosition& msg) { + if (msg.xy_global && msg.z_global) { + _has_global_position = true; + if (msg.ref_timestamp != _last_global_ref_timestamp) { + _last_global_ref_timestamp = msg.ref_timestamp; + double x, y, z; + _geocentric.Forward(msg.ref_lat, msg.ref_lon, msg.ref_alt, x, y, z); + RCLCPP_INFO(this->node().get_logger(), + "Global position reference updated: lat=%f, lon=%f, alt=%f", + msg.ref_lat, msg.ref_lon, msg.ref_alt); + RCLCPP_INFO(this->node().get_logger(), + "ECEF position reference updated: x=%f, y=%f, z=%f", x, y, z); + _ekf_origin.header.frame_id = "earth"; + _ekf_origin.child_frame_id = _tf_prefix + "map"; + _ekf_origin.transform.translation.x = x; + _ekf_origin.transform.translation.y = y; + _ekf_origin.transform.translation.z = z; + double cos_lat = std::cos(msg.ref_lat * M_PI / 180.0); + double sin_lat = std::sin(msg.ref_lat * M_PI / 180.0); + double cos_lon = std::cos(msg.ref_lon * M_PI / 180.0); + double sin_lon = std::sin(msg.ref_lon * M_PI / 180.0); + Eigen::Matrix3d R; + R(0, 0) = -sin_lon; + R(1, 0) = cos_lon; + R(2, 0) = 0.0; + R(0, 1) = -sin_lat * cos_lon; + R(1, 1) = -sin_lat * sin_lon; + R(2, 1) = cos_lat; + R(0, 2) = cos_lat * cos_lon; + R(1, 2) = cos_lat * sin_lon; + R(2, 2) = sin_lat; + Eigen::Quaterniond q(R); + _ekf_origin.transform.rotation.x = q.x(); + _ekf_origin.transform.rotation.y = q.y(); + _ekf_origin.transform.rotation.z = q.z(); + _ekf_origin.transform.rotation.w = q.w(); + } + _ekf_origin.header.stamp = this->node().get_clock()->now(); + _tf_broadcaster->sendTransform(_ekf_origin); + } else { + _has_global_position = false; + } + if (msg.xy_valid && msg.z_valid) { + const Eigen::Vector3f pos_ned = _vehicle_local_position->positionNed(); + const Eigen::Vector3f pos_enu = px4_ros2::positionNedToEnu(pos_ned); + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = this->node().get_clock()->now(); + tf_msg.header.frame_id = _tf_prefix + "map"; + tf_msg.child_frame_id = _tf_prefix + "base_link"; + tf_msg.transform.translation.x = pos_enu(0); + tf_msg.transform.translation.y = pos_enu(1); + tf_msg.transform.translation.z = pos_enu(2); + tf_msg.transform.rotation.x = 0.0; + tf_msg.transform.rotation.y = 0.0; + tf_msg.transform.rotation.z = 0.0; + tf_msg.transform.rotation.w = 1.0; + _tf_broadcaster->sendTransform(tf_msg); + } + }); + RCLCPP_INFO(this->node().get_logger(), + "FormationControlMode initialized with %zu neighbors, gain=%f", + _neighbor_base_link_frames.size(), _gain); +} + +void FormationControlMode::onActivate() +{ +} + +void FormationControlMode::updateSetpoint(float dt_s) +{ + Eigen::Vector2f velocity_en{0.0, 0.0}; + for (const auto& toFrameRel : _neighbor_base_link_frames) { + geometry_msgs::msg::TransformStamped t; + try { + t = _tf_buffer->lookupTransform( + toFrameRel, _tf_prefix + "base_link", tf2::TimePointZero); + const Eigen::Vector2f relative_en{t.transform.translation.x, t.transform.translation.y}; + const float distance = relative_en.norm(); + const Eigen::Vector2f direction = relative_en.normalized(); + const float distance_error = + distance - _neighbor_distances[&toFrameRel - &_neighbor_base_link_frames[0]]; + const Eigen::Vector2f individual_control = -distance_error * direction * _gain; + velocity_en.x() += individual_control.x(); + velocity_en.y() += individual_control.y(); + } catch (const tf2::TransformException& ex) { + (void)ex; + } + } + const px4_ros2::TrajectorySetpoint setpoint = px4_ros2::TrajectorySetpoint() + .withVelocityX(velocity_en.y()) + .withVelocityY(velocity_en.x()) + .withPositionZ(-2.0f); + _trajectory_setpoint->update(setpoint); +} From fd1ccd07b1daebf0b93bd94abe0a4c91b0d68843 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Tue, 25 Aug 2026 23:31:14 +0100 Subject: [PATCH 11/14] feat: make formation_control_lib Signed-off-by: Beniamino Pozzan --- .../formation_control/CMakeLists.txt | 42 ++++++++++++++----- 1 file changed, 31 insertions(+), 11 deletions(-) diff --git a/px4_roscon_workshop/formation_control/CMakeLists.txt b/px4_roscon_workshop/formation_control/CMakeLists.txt index a393ba3..8c357ba 100644 --- a/px4_roscon_workshop/formation_control/CMakeLists.txt +++ b/px4_roscon_workshop/formation_control/CMakeLists.txt @@ -19,18 +19,18 @@ find_package(GeographicLib REQUIRED) find_package(px4_ros2_cpp REQUIRED) -include_directories( - include - ${Eigen3_INCLUDE_DIRS} - ${GeographicLib_INCLUDE_DIRS} +add_library(px4_formation_control_lib STATIC + src/formation_control.cpp ) -add_executable(px4_formation_control - src/main.cpp - src/formation_control.cpp +target_include_directories(px4_formation_control_lib PUBLIC + $ + $ + ${Eigen3_INCLUDE_DIRS} + ${GeographicLib_INCLUDE_DIRS} ) -ament_target_dependencies(px4_formation_control +ament_target_dependencies(px4_formation_control_lib rclcpp Eigen3 px4_ros2_cpp @@ -39,17 +39,37 @@ ament_target_dependencies(px4_formation_control tf2_ros ) -target_link_libraries(px4_formation_control +target_link_libraries(px4_formation_control_lib ${GeographicLib_LIBRARIES} ) -target_compile_features(px4_formation_control PUBLIC c_std_99 cxx_std_20) +target_compile_features(px4_formation_control_lib + PUBLIC c_std_99 cxx_std_20) + +add_executable(px4_formation_control + src/main.cpp +) +target_link_libraries(px4_formation_control + px4_formation_control_lib +) +ament_target_dependencies(px4_formation_control + rclcpp + px4_ros2_cpp +) +target_compile_features(px4_formation_control + PUBLIC c_std_99 cxx_std_20) install( - TARGETS px4_formation_control + TARGETS + px4_formation_control_lib + px4_formation_control DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY include/ + DESTINATION include +) + install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) From 27d86b53370e80f53171bacfc0fd6977129df795 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Tue, 25 Aug 2026 23:32:28 +0100 Subject: [PATCH 12/14] feat: add solution to formation control exercise Signed-off-by: Beniamino Pozzan --- .../formation_control/CMakeLists.txt | 20 +++++ .../include/formation_executor_solution.hpp | 20 +++++ .../launch/formation.launch.py | 13 ++- .../formation_control/src/solution.cpp | 86 +++++++++++++++++++ 4 files changed, 136 insertions(+), 3 deletions(-) create mode 100644 px4_roscon_workshop/formation_control/include/formation_executor_solution.hpp create mode 100644 px4_roscon_workshop/formation_control/src/solution.cpp diff --git a/px4_roscon_workshop/formation_control/CMakeLists.txt b/px4_roscon_workshop/formation_control/CMakeLists.txt index 8c357ba..876fe7c 100644 --- a/px4_roscon_workshop/formation_control/CMakeLists.txt +++ b/px4_roscon_workshop/formation_control/CMakeLists.txt @@ -50,19 +50,39 @@ add_executable(px4_formation_control src/main.cpp ) +add_executable(px4_formation_control_executor_solution + src/solution.cpp +) + target_link_libraries(px4_formation_control px4_formation_control_lib ) + +target_link_libraries(px4_formation_control_executor_solution + px4_formation_control_lib +) + ament_target_dependencies(px4_formation_control rclcpp px4_ros2_cpp ) + +ament_target_dependencies(px4_formation_control_executor_solution + rclcpp + px4_ros2_cpp +) + target_compile_features(px4_formation_control PUBLIC c_std_99 cxx_std_20) + +target_compile_features(px4_formation_control_executor_solution + PUBLIC c_std_99 cxx_std_20) + install( TARGETS px4_formation_control_lib px4_formation_control + px4_formation_control_executor_solution DESTINATION lib/${PROJECT_NAME} ) diff --git a/px4_roscon_workshop/formation_control/include/formation_executor_solution.hpp b/px4_roscon_workshop/formation_control/include/formation_executor_solution.hpp new file mode 100644 index 0000000..2b0cf98 --- /dev/null +++ b/px4_roscon_workshop/formation_control/include/formation_executor_solution.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include + +class FormationExecutor : public px4_ros2::ModeExecutorBase { + public: + explicit FormationExecutor(px4_ros2::ModeBase& formation_mode); + + void onActivate() override; + void onDeactivate(DeactivateReason reason) override; + + private: + enum class State { + Takeoff, + Formation, + WaitUntilDisarmed, + }; + + void switchToState(State state, px4_ros2::Result previous_result); +}; diff --git a/px4_roscon_workshop/formation_control/launch/formation.launch.py b/px4_roscon_workshop/formation_control/launch/formation.launch.py index 4103a42..8dca4ad 100644 --- a/px4_roscon_workshop/formation_control/launch/formation.launch.py +++ b/px4_roscon_workshop/formation_control/launch/formation.launch.py @@ -7,6 +7,13 @@ from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +FORMATION_CONTROL_EXECUTABLE = ( + 'px4_formation_control', + 'px4_formation_control_executor_exercise', + 'px4_formation_control_executor_solution', +) +ACTIVE_EXECUTABLE_IDX = 2 + def generate_launch_description() -> LaunchDescription: px4_roscon_workshop_share = FindPackageShare('px4_roscon_workshop').find( @@ -85,7 +92,7 @@ def generate_launch_description() -> LaunchDescription: ), Node( package='px4_formation_control', - executable='px4_formation_control', + executable=FORMATION_CONTROL_EXECUTABLE[ACTIVE_EXECUTABLE_IDX], name='formation_controller_0', output='screen', parameters=[{ @@ -98,7 +105,7 @@ def generate_launch_description() -> LaunchDescription: ), Node( package='px4_formation_control', - executable='px4_formation_control', + executable=FORMATION_CONTROL_EXECUTABLE[ACTIVE_EXECUTABLE_IDX], name='formation_controller_1', output='screen', parameters=[{ @@ -111,7 +118,7 @@ def generate_launch_description() -> LaunchDescription: ), Node( package='px4_formation_control', - executable='px4_formation_control', + executable=FORMATION_CONTROL_EXECUTABLE[ACTIVE_EXECUTABLE_IDX], name='formation_controller_2', output='screen', parameters=[{ diff --git a/px4_roscon_workshop/formation_control/src/solution.cpp b/px4_roscon_workshop/formation_control/src/solution.cpp new file mode 100644 index 0000000..2bbab30 --- /dev/null +++ b/px4_roscon_workshop/formation_control/src/solution.cpp @@ -0,0 +1,86 @@ +#include +#include + +#include "rclcpp/rclcpp.hpp" + +static const std::string kNodeName = "formation_controller"; +static const bool kEnableDebugOutput = true; + +FormationExecutor::FormationExecutor(px4_ros2::ModeBase& formation_mode) + : ModeExecutorBase(Settings{}, formation_mode) +{ +} + +void FormationExecutor::onActivate() +{ + switchToState(State::Takeoff, px4_ros2::Result::Success); +} + +void FormationExecutor::onDeactivate(DeactivateReason reason) +{ + RCLCPP_INFO(node().get_logger(), "Formation executor deactivated: %d", + static_cast(reason)); +} + +void FormationExecutor::switchToState(State state, px4_ros2::Result previous_result) +{ + if (previous_result != px4_ros2::Result::Success) { + RCLCPP_WARN(node().get_logger(), "Formation sequence stopped in state %d with result %d", + static_cast(state), static_cast(previous_result)); + return; + } + + switch (state) { + case State::Takeoff: + takeoff( + [this](px4_ros2::Result result) { + switchToState(State::Formation, result); + }, + 2.0f); + break; + case State::Formation: + scheduleMode(ownedMode().id(), + [this](px4_ros2::Result result) { + switchToState(State::WaitUntilDisarmed, result); + }); + break; + case State::WaitUntilDisarmed: + waitUntilDisarmed([](px4_ros2::Result) {}); + break; + } +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(kNodeName); + node->declare_parameter("px4_ns", ""); + node->declare_parameter("tf_prefix", ""); + node->declare_parameter("neighbor_distances", std::vector{}); + node->declare_parameter("neighbor_prefixes", std::vector{}); + node->declare_parameter("gain", 1.0); + + if (kEnableDebugOutput) { + auto ret = rcutils_logging_set_logger_level(node->get_logger().get_name(), + RCUTILS_LOG_SEVERITY_DEBUG); + + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR(node->get_logger(), "Error setting severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + } + + FormationControlMode mode(*node, node->get_parameter("px4_ns").as_string()); + FormationExecutor executor(mode); + if (!executor.doRegister()) { + RCLCPP_ERROR(node->get_logger(), "Formation executor registration failed"); + rclcpp::shutdown(); + return 1; + } + rclcpp::spin(node); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file From 5e40c29d44fc711b871a74127c3ef521e905ce1c Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Tue, 25 Aug 2026 23:38:20 +0100 Subject: [PATCH 13/14] feat: add exercise template Signed-off-by: Beniamino Pozzan --- .../formation_control/CMakeLists.txt | 17 ++++++++ .../formation_control/src/exercise.cpp | 40 +++++++++++++++++++ 2 files changed, 57 insertions(+) create mode 100644 px4_roscon_workshop/formation_control/src/exercise.cpp diff --git a/px4_roscon_workshop/formation_control/CMakeLists.txt b/px4_roscon_workshop/formation_control/CMakeLists.txt index 876fe7c..0701bbc 100644 --- a/px4_roscon_workshop/formation_control/CMakeLists.txt +++ b/px4_roscon_workshop/formation_control/CMakeLists.txt @@ -54,6 +54,10 @@ add_executable(px4_formation_control_executor_solution src/solution.cpp ) +add_executable(px4_formation_control_executor_exercise + src/exercise.cpp +) + target_link_libraries(px4_formation_control px4_formation_control_lib ) @@ -62,6 +66,10 @@ target_link_libraries(px4_formation_control_executor_solution px4_formation_control_lib ) +target_link_libraries(px4_formation_control_executor_exercise + px4_formation_control_lib +) + ament_target_dependencies(px4_formation_control rclcpp px4_ros2_cpp @@ -72,17 +80,26 @@ ament_target_dependencies(px4_formation_control_executor_solution px4_ros2_cpp ) +ament_target_dependencies(px4_formation_control_executor_exercise + rclcpp + px4_ros2_cpp +) + target_compile_features(px4_formation_control PUBLIC c_std_99 cxx_std_20) target_compile_features(px4_formation_control_executor_solution PUBLIC c_std_99 cxx_std_20) +target_compile_features(px4_formation_control_executor_exercise + PUBLIC c_std_99 cxx_std_20) + install( TARGETS px4_formation_control_lib px4_formation_control px4_formation_control_executor_solution + px4_formation_control_executor_exercise DESTINATION lib/${PROJECT_NAME} ) diff --git a/px4_roscon_workshop/formation_control/src/exercise.cpp b/px4_roscon_workshop/formation_control/src/exercise.cpp new file mode 100644 index 0000000..1189391 --- /dev/null +++ b/px4_roscon_workshop/formation_control/src/exercise.cpp @@ -0,0 +1,40 @@ +#include + +#include "rclcpp/rclcpp.hpp" + +static const std::string kNodeName = "formation_controller"; +static const bool kEnableDebugOutput = true; + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(kNodeName); + node->declare_parameter("px4_ns", ""); + node->declare_parameter("tf_prefix", ""); + node->declare_parameter("neighbor_distances", std::vector{}); + node->declare_parameter("neighbor_prefixes", std::vector{}); + node->declare_parameter("gain", 1.0); + + if (kEnableDebugOutput) { + auto ret = rcutils_logging_set_logger_level(node->get_logger().get_name(), + RCUTILS_LOG_SEVERITY_DEBUG); + + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR(node->get_logger(), "Error setting severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + } + + /* + The node shall register a mode executor to handle the formation control mode. + The mode executor, once activated, will trigger a takeoff + and then switch to the formation control mode. + */ + + rclcpp::spin(node); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file From 42bdb90d46fd09ac7a1e03d783145474f7030e7d Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Tue, 25 Aug 2026 23:46:47 +0100 Subject: [PATCH 14/14] docs: update readme Signed-off-by: Beniamino Pozzan --- .../formation_control/README.md | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/px4_roscon_workshop/formation_control/README.md b/px4_roscon_workshop/formation_control/README.md index eb03faa..1fca38e 100644 --- a/px4_roscon_workshop/formation_control/README.md +++ b/px4_roscon_workshop/formation_control/README.md @@ -46,7 +46,26 @@ ros2 launch px4_formation_control formation.launch.py \ world:=default ``` -After startup, use QGroundControl to activate the `Formation` mode for the vehicles and arm them according to the PX4 safety rules. The mode is configured to prevent arming from the mode itself, so manual activation and arming may be required. +After startup, use QGroundControl to activate the required mode and arm the vehicles according to the PX4 safety rules. Arming is allowed while the formation mode is active. + +## Executor exercise + +The package includes three executable variants: + +- `px4_formation_control`: the original formation mode. Takeoff then activate `Formation` manually. +- `px4_formation_control_executor_exercise`: the workshop exercise. Complete the node by creating and registering a `px4_ros2::ModeExecutorBase` that takes off and then schedules the formation mode. +- `px4_formation_control_executor_solution`: the completed exercise. Its executor starts with a 2 m takeoff, activates the formation mode when takeoff completes, and waits for disarming when the formation mode finishes. + +The launch file selects the solution by default. To try another variant, change `ACTIVE_EXECUTABLE_IDX` in `launch/formation.launch.py`: + +```python +FORMATION_CONTROL_EXECUTABLE = ( + 'px4_formation_control', + 'px4_formation_control_executor_exercise', + 'px4_formation_control_executor_solution', +) +ACTIVE_EXECUTABLE_IDX = 1 # exercise +``` ## Default configuration