From 32e93f08d6267ebc796166559a99af1f8896bc83 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 26 Aug 2026 00:21:18 +0100 Subject: [PATCH 1/4] chore: split custom executor and custom mode demos Signed-off-by: Beniamino Pozzan --- .../custom_executor_demo/CMakeLists.txt | 51 ++++ .../custom_executor_demo/CustomMode.cpp | 251 ++++++++++++++++++ .../custom_executor_demo/CustomMode.hpp | 165 ++++++++++++ .../CustomModeExecutor.cpp | 0 .../CustomModeExecutor.hpp | 0 .../custom_executor_demo/README.md | 56 ++++ .../cfg/clock_bridge.yaml | 4 + .../launch/custom_executor_demo.launch.py | 54 ++++ .../custom_executor_demo/package.xml | 27 ++ .../custom_mode_demo/CMakeLists.txt | 2 - .../custom_mode_demo/CustomMode.cpp | 213 +-------------- .../custom_mode_demo/CustomMode.hpp | 142 +--------- .../custom_mode_demo/README.md | 30 +-- 13 files changed, 628 insertions(+), 367 deletions(-) create mode 100644 px4_roscon_workshop/custom_executor_demo/CMakeLists.txt create mode 100644 px4_roscon_workshop/custom_executor_demo/CustomMode.cpp create mode 100644 px4_roscon_workshop/custom_executor_demo/CustomMode.hpp rename px4_roscon_workshop/{custom_mode_demo => custom_executor_demo}/CustomModeExecutor.cpp (100%) rename px4_roscon_workshop/{custom_mode_demo => custom_executor_demo}/CustomModeExecutor.hpp (100%) create mode 100644 px4_roscon_workshop/custom_executor_demo/README.md create mode 100644 px4_roscon_workshop/custom_executor_demo/cfg/clock_bridge.yaml create mode 100644 px4_roscon_workshop/custom_executor_demo/launch/custom_executor_demo.launch.py create mode 100644 px4_roscon_workshop/custom_executor_demo/package.xml diff --git a/px4_roscon_workshop/custom_executor_demo/CMakeLists.txt b/px4_roscon_workshop/custom_executor_demo/CMakeLists.txt new file mode 100644 index 0000000..234ecf6 --- /dev/null +++ b/px4_roscon_workshop/custom_executor_demo/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.8) +project(custom_executor_demo) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(px4_ros2_cpp REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(geometry_msgs REQUIRED) + +# Executable target +add_executable(${PROJECT_NAME} + CustomMode.cpp + CustomMode.hpp + CustomModeExecutor.cpp + CustomModeExecutor.hpp +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + px4_ros2_cpp + Eigen3 + geometry_msgs +) + +# Install the binary +install(TARGETS ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY cfg + DESTINATION share/${PROJECT_NAME}/ +) + +# Linting +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() \ No newline at end of file diff --git a/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp b/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp new file mode 100644 index 0000000..23de1b5 --- /dev/null +++ b/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp @@ -0,0 +1,251 @@ +// ============================================================================ +// ORIGINAL VERSION - CustomMode.cpp +// ============================================================================ +#include "CustomMode.hpp" + +#include + +static const std::string kModeNameCustomWaypoints = "CustomWaypoints"; +static const std::string kModeNameCustomYaw = "CustomYaw"; + +CustomWaypoints::CustomWaypoints(rclcpp::Node &node) + : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), + _node(node) +{ + loadParameters(); + + _trajectory_setpoint = std::make_shared(*this); + _local_position = std::make_shared(*this); + + RCLCPP_INFO(node.get_logger(), "CustomWaypoints mode initialized."); + +} + +CustomYaw::CustomYaw(rclcpp::Node &node) + : px4_ros2::ModeBase(node, kModeNameCustomYaw), + _node(node) +{ + loadParameters(); + + _trajectory_setpoint = std::make_shared(*this); + _vehicle_attitude = std::make_shared(*this); + _local_position = std::make_shared(*this); + + RCLCPP_INFO(node.get_logger(), "CustomYaw mode initialized."); +} + +void CustomWaypoints::loadParameters() { + // Load parameters specific to the CustomWaypoints mode +} +void CustomYaw::loadParameters() { + // Load parameters specific to the CustomYaw mode +} + +void CustomWaypoints::onActivate() { + // Initialize waypoints + + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); + + _current_waypoint_index = 0; // Start at the first waypoint + RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); + // Set initial trajectory setpoint +} +void CustomWaypoints::onDeactivate() { + RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode deactivated"); + // Reset trajectory setpoint +} +void CustomWaypoints::updateSetpoint([[maybe_unused]] float dt_s) { + if (_current_waypoint_index < _trajectory_waypoints.size()) { + // Set the trajectory setpoint to the current waypoint + auto current_waypoint = _trajectory_waypoints[_current_waypoint_index]; + _trajectory_setpoint->updatePosition(current_waypoint); + + + // Check if we reached the current waypoint + if ((_local_position->positionNed() - current_waypoint).norm() < 0.5f) { + _current_waypoint_index++; // Move to the next waypoint + } + } else { + // All waypoints completed, reset or stop + RCLCPP_INFO(_node.get_logger(), "All waypoints completed."); + completed(px4_ros2::Result::Success); + return; // Exit the update loop + } + +} +void CustomYaw::onActivate() { + _start_yaw = _vehicle_attitude->yaw(); // Store the starting yaw angle + _yaw_accumulator = 0.0f; // Initialize yaw accumulator + RCLCPP_INFO(_node.get_logger(), "CustomYaw mode activated"); + // Set initial trajectory setpoint +} +void CustomYaw::onDeactivate() { + RCLCPP_INFO(_node.get_logger(), "CustomYaw mode deactivated"); + // Reset trajectory setpoint +} +void CustomYaw::updateSetpoint([[maybe_unused]] float dt_s) { + // Update the trajectory setpoint based on the current heading + Eigen::Vector3f velocity{0.0f, 0.0f, 0.0f}; + std::optional acceleration = std::nullopt; + std::optional yaw = std::nullopt; + std::optional yaw_rate = 0.05f; + _trajectory_setpoint->update(velocity, acceleration, yaw, yaw_rate); + _yaw_accumulator += yaw_rate.value() * dt_s; // Accumulate yaw rotation + if (std::abs(_yaw_accumulator) > 2 * M_PI - 0.1f) { // full rotation (tolerant) + RCLCPP_INFO(_node.get_logger(), "CustomYaw mode completed a full rotation."); + completed(px4_ros2::Result::Success); + return; + } +} +// ============================================================================ +// ALTERNATIVE VERSION - For exercises +// ============================================================================ +// #include "CustomMode.hpp" + +// #include + +// static const std::string kModeNameCustomWaypoints = "CustomWaypoints"; +// static const std::string kModeNameCustomYaw = "CustomYaw"; +// static const std::string kModeNameCustomAltitude = "CustomAltitude"; + +// CustomWaypoints::CustomWaypoints(rclcpp::Node &node) +// : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), +// _node(node) +// { +// loadParameters(); + +// _trajectory_setpoint = std::make_shared(*this); +// _local_position = std::make_shared(*this); + +// RCLCPP_INFO(node.get_logger(), "CustomWaypoints mode initialized."); + +// } + +// CustomYaw::CustomYaw(rclcpp::Node &node) +// : px4_ros2::ModeBase(node, kModeNameCustomYaw), +// _node(node) +// { +// // loadParameters(); + +// _trajectory_setpoint = std::make_shared(*this); +// _vehicle_attitude = std::make_shared(*this); +// _local_position = std::make_shared(*this); + +// RCLCPP_INFO(node.get_logger(), "CustomYaw mode initialized."); +// } + +// void CustomWaypoints::loadParameters() { +// // Use the node reference to declare/get parameters +// _node.declare_parameter("altitude", 2.0f); +// _node.get_parameter("altitude", _altitude); + +// RCLCPP_INFO(_node.get_logger(), "CustomWaypoints altitude parameter: %f", _altitude); +// } +// void CustomYaw::loadParameters() { +// // Load parameters specific to the CustomYaw mode +// } + +// void CustomWaypoints::onActivate() { +// // Initialize waypoints + +// _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); +// _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); +// _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); +// _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); +// _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); +// _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); +// _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); + +// _current_waypoint_index = 0; // Start at the first waypoint +// RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); +// // Set initial trajectory setpoint +// } + +// void CustomWaypoints::onDeactivate() { +// RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode deactivated"); +// // Reset trajectory setpoint +// } +// void CustomWaypoints::updateSetpoint([[maybe_unused]] float dt_s) { +// if (_current_waypoint_index < _trajectory_waypoints.size()) { +// // Set the trajectory setpoint to the current waypoint +// auto current_waypoint = _trajectory_waypoints[_current_waypoint_index]; +// _trajectory_setpoint->updatePosition(current_waypoint); + + +// // Check if we reached the current waypoint +// if ((_local_position->positionNed() - current_waypoint).norm() < 0.5f) { +// _current_waypoint_index++; // Move to the next waypoint +// } +// } else { +// // All waypoints completed, reset or stop +// RCLCPP_INFO(_node.get_logger(), "All waypoints completed."); +// completed(px4_ros2::Result::Success); +// return; // Exit the update loop +// } + +// } +// void CustomYaw::onActivate() { +// _start_yaw = _vehicle_attitude->yaw(); // Store the starting yaw angle +// _yaw_accumulator = 0.0f; // Initialize yaw accumulator +// RCLCPP_INFO(_node.get_logger(), "CustomYaw mode activated"); +// // Set initial trajectory setpoint +// } +// void CustomYaw::onDeactivate() { +// RCLCPP_INFO(_node.get_logger(), "CustomYaw mode deactivated"); +// // Reset trajectory setpoint +// } +// void CustomYaw::updateSetpoint([[maybe_unused]] float dt_s) { +// // Update the trajectory setpoint based on the current heading +// Eigen::Vector3f velocity{0.0f, 0.0f, 0.0f}; +// std::optional acceleration = std::nullopt; +// std::optional yaw = std::nullopt; +// std::optional yaw_rate = 0.05f; +// _trajectory_setpoint->update(velocity, acceleration, yaw, yaw_rate); +// _yaw_accumulator += yaw_rate.value() * dt_s; // Accumulate yaw rotation +// if (std::abs(_yaw_accumulator) > 2 * M_PI - 0.1f) { // full rotation (tolerant) +// RCLCPP_INFO(_node.get_logger(), "CustomYaw mode completed a full rotation."); +// completed(px4_ros2::Result::Success); +// return; +// } +// } + +// CustomAltitude::CustomAltitude(rclcpp::Node &node) +// : px4_ros2::ModeBase(node, kModeNameCustomAltitude), +// _node(node) +// { +// // loadParameters(); + +// _trajectory_setpoint = std::make_shared(*this); +// _local_position = std::make_shared(*this); + +// RCLCPP_INFO(node.get_logger(), "CustomAltitude mode initialized."); +// } + +// void CustomAltitude::onActivate() { +// RCLCPP_INFO(_node.get_logger(), "CustomAltitude mode activated"); +// // Set initial trajectory setpoint +// } + +// void CustomAltitude::onDeactivate() { +// RCLCPP_INFO(_node.get_logger(), "CustomAltitude mode deactivated"); +// // Reset trajectory setpoint +// } + +// void CustomAltitude::updateSetpoint([[maybe_unused]] float dt_s) { +// // Maintain current position but change altitude to -3.0 meters +// Eigen::Vector3f target_position = _local_position->positionNed(); +// target_position.z() = -3.0f; // Set target altitude to -3.0 meters +// _trajectory_setpoint->updatePosition(target_position); +// // Check if we reached the target altitude +// if (std::abs(_local_position->positionNed().z() - target_position.z()) < 0.1f) { +// RCLCPP_INFO(_node.get_logger(), "CustomAltitude mode reached target altitude."); +// completed(px4_ros2::Result::Success); +// return; // Exit the update loop +// } +// } diff --git a/px4_roscon_workshop/custom_executor_demo/CustomMode.hpp b/px4_roscon_workshop/custom_executor_demo/CustomMode.hpp new file mode 100644 index 0000000..9f64662 --- /dev/null +++ b/px4_roscon_workshop/custom_executor_demo/CustomMode.hpp @@ -0,0 +1,165 @@ +// ============================================================================ +// ORIGINAL VERSION - CustomMode.hpp +// ============================================================================ +#pragma once + +// PX4 Interface Library +#include +#include +#include +#include +#include +#include +#include + +// ROS 2 Core +#include +#include +#include + +// C++ Std +#include // for M_PI +#include +#include + +class CustomWaypoints : public px4_ros2::ModeBase { +public: + explicit CustomWaypoints(rclcpp::Node &node); + + // See ModeBase + void onActivate() override; + void onDeactivate() override; + void updateSetpoint([[maybe_unused]] float dt_s) override; + +private: + void loadParameters(); + // ROS 2 + rclcpp::Node &_node; + + + // px4_ros2_cpp + std::shared_ptr _trajectory_setpoint; + std::shared_ptr _local_position; + + std::vector _trajectory_waypoints; // Vector to hold waypoints + size_t _current_waypoint_index; // Index of the current waypoint +}; + +class CustomYaw : public px4_ros2::ModeBase { +public: + explicit CustomYaw(rclcpp::Node &node); + + // See ModeBase + void onActivate() override; + void onDeactivate() override; + void updateSetpoint([[maybe_unused]] float dt_s) override; + +private: + void loadParameters(); + // ROS 2 + rclcpp::Node &_node; + + + // px4_ros2_cpp + std::shared_ptr _vehicle_attitude; + std::shared_ptr _trajectory_setpoint; + std::shared_ptr _local_position; + + + float _start_yaw; // Starting yaw angle + float _yaw_accumulator; // Increment for yaw rotation +}; + +// ============================================================================ +// ALTERNATIVE VERSION - For exercises +// ============================================================================ +// #pragma once + +// // PX4 Interface Library +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// // ROS 2 Core +// #include +// #include +// #include + +// // C++ Std +// #include // for M_PI +// #include +// #include + +// class CustomWaypoints : public px4_ros2::ModeBase { +// public: +// explicit CustomWaypoints(rclcpp::Node &node); + +// // See ModeBase +// void onActivate() override; +// void onDeactivate() override; +// void updateSetpoint([[maybe_unused]] float dt_s) override; + +// private: +// void loadParameters(); +// // ROS 2 +// rclcpp::Node &_node; + + +// // px4_ros2_cpp +// std::shared_ptr _trajectory_setpoint; +// std::shared_ptr _local_position; + +// std::vector _trajectory_waypoints; // Vector to hold waypoints +// size_t _current_waypoint_index; // Index of the current waypoint +// // Parameters +// float _altitude; +// }; + +// class CustomYaw : public px4_ros2::ModeBase { +// public: +// explicit CustomYaw(rclcpp::Node &node); + +// // See ModeBase +// void onActivate() override; +// void onDeactivate() override; +// void updateSetpoint([[maybe_unused]] float dt_s) override; + +// private: +// void loadParameters(); +// // ROS 2 +// rclcpp::Node &_node; + + +// // px4_ros2_cpp +// std::shared_ptr _vehicle_attitude; +// std::shared_ptr _trajectory_setpoint; +// std::shared_ptr _local_position; + + +// float _start_yaw; // Starting yaw angle +// float _yaw_accumulator; // Increment for yaw rotation +// }; + +// class CustomAltitude : public px4_ros2::ModeBase { +// public: +// explicit CustomAltitude(rclcpp::Node &node); + +// // See ModeBase +// void onActivate() override; +// void onDeactivate() override; +// void updateSetpoint([[maybe_unused]] float dt_s) override; + +// private: +// void loadParameters(); +// // ROS 2 +// rclcpp::Node &_node; + + +// // px4_ros2_cpp +// std::shared_ptr _trajectory_setpoint; +// std::shared_ptr _local_position; +// }; diff --git a/px4_roscon_workshop/custom_mode_demo/CustomModeExecutor.cpp b/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.cpp similarity index 100% rename from px4_roscon_workshop/custom_mode_demo/CustomModeExecutor.cpp rename to px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.cpp diff --git a/px4_roscon_workshop/custom_mode_demo/CustomModeExecutor.hpp b/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.hpp similarity index 100% rename from px4_roscon_workshop/custom_mode_demo/CustomModeExecutor.hpp rename to px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.hpp diff --git a/px4_roscon_workshop/custom_executor_demo/README.md b/px4_roscon_workshop/custom_executor_demo/README.md new file mode 100644 index 0000000..42d9587 --- /dev/null +++ b/px4_roscon_workshop/custom_executor_demo/README.md @@ -0,0 +1,56 @@ +# Custom Executor Demo + +This package demonstrates how to create and use PX4 custom flight modes with an executor from ROS 2 using the PX4-ROS2 Interface Library. It showcases the implementation of custom flight behaviors that integrate seamlessly with PX4's mode management system. + +## Overview + +The Custom Executor Demo implements two custom flight modes using the PX4-ROS2 Interface Library: + +1. **CustomWaypoints Mode**: Autonomous waypoint navigation with predefined trajectory +2. **CustomYaw Mode**: Controlled yaw rotation while maintaining position + +These modes are orchestrated by a **CustomModeExecutor** that manages the complete flight sequence through a state machine architecture. +You have to select CustomWaypoints for the sequence to start. + +### Flight Sequence + +The demo executes the following autonomous sequence: + +1. **Takeoff** - Automatic takeoff to predefined altitude +2. **CustomWaypoints** - Navigate through rectangular waypoint pattern +3. **CustomYaw** - Perform 360-degree rotation while hovering +4. **Land** - Automatic landing +5. **WaitUntilDisarmed** - Safe completion state + +## Prerequisites + +1. Start the simulation, PX4 and QGC as described in the [setup guide](../../docs/setup.md). +2. Ensure the vehicle is armed (GPS lock, all sensors healthy) +3. Verify QGroundControl connection for mode monitoring + +## Usage + +1. Start the simulation, PX4 and QGC as described in the [setup guide](../../docs/setup.md). +2. Start the additional ROS 2 node through the [common launchfile](../px4_roscon_workshop/README.md) + + ```sh + ros2 launch px4_roscon_workshop common.launch.py + ``` + +3. Run `custom_executor_demo.launch.py` from inside the docker container + + ```sh + ros2 launch custom_executor_demo custom_executor_demo.launch.py + ``` + +4. The custom executor demo does not automatically switch to the correct external mode _CustomWaypoints_ and it does not arm the drone. + + 1. On the QGC window, fist enter _CustomWaypoints_ mode: + + ![change_mode](../../docs/assets/change_mode.png) + + 2. Then click on the `Not Ready` label and arm the vehicle: + + ![arm vehicle](../../docs/assets/arm_vehicle.png) + +The `custom_executor_demo.launch.py` can also start the _MicroXrceAgent_ and the _gz clock bridge_. Set the launch arguments `run_uxrcedds_agent` or `run_gz_clock_bridge` to `true` to run them if you don't use `common.launch.py`. diff --git a/px4_roscon_workshop/custom_executor_demo/cfg/clock_bridge.yaml b/px4_roscon_workshop/custom_executor_demo/cfg/clock_bridge.yaml new file mode 100644 index 0000000..243dab4 --- /dev/null +++ b/px4_roscon_workshop/custom_executor_demo/cfg/clock_bridge.yaml @@ -0,0 +1,4 @@ +- topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: "GZ_TO_ROS" \ No newline at end of file diff --git a/px4_roscon_workshop/custom_executor_demo/launch/custom_executor_demo.launch.py b/px4_roscon_workshop/custom_executor_demo/launch/custom_executor_demo.launch.py new file mode 100644 index 0000000..e7d3515 --- /dev/null +++ b/px4_roscon_workshop/custom_executor_demo/launch/custom_executor_demo.launch.py @@ -0,0 +1,54 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + pkg_share = get_package_share_directory("custom_executor_demo") + + clock_bridge_config_file = os.path.join(pkg_share,"cfg","clock_bridge.yaml") + + run_uxrcedds_agent_arg = DeclareLaunchArgument( + "run_uxrcedds_agent", + default_value="false", + description="Whether to run the MicroXRCEdds Agent", + ) + + run_gz_clock_bridge_arg = DeclareLaunchArgument( + "run_gz_clock_bridge", + default_value="false", + description="Whether to run the Gazebo clock bridge", + ) + + return LaunchDescription([ + run_uxrcedds_agent_arg, + run_gz_clock_bridge_arg, + Node( + package="custom_executor_demo", + executable="custom_executor_demo", + name="custom_executor_demo", + output="screen", + parameters=[ + {"use_sim_time": True} + ] + ), + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="gz_clock_bridge", + parameters=[ + {"config_file": clock_bridge_config_file} + ], + condition=IfCondition(LaunchConfiguration("run_gz_clock_bridge")) + ), + ExecuteProcess( + cmd=["MicroXRCEAgent", "udp4", "-p", "8888", "-v", "3"], + output="screen", + condition=IfCondition(LaunchConfiguration("run_uxrcedds_agent")) + ) + ]) diff --git a/px4_roscon_workshop/custom_executor_demo/package.xml b/px4_roscon_workshop/custom_executor_demo/package.xml new file mode 100644 index 0000000..7242c93 --- /dev/null +++ b/px4_roscon_workshop/custom_executor_demo/package.xml @@ -0,0 +1,27 @@ + + + + custom_executor_demo + 0.0.1 + Custom mode example using PX4 ROS 2 interface. + + patrik_ark + Beniamino Pozzan + CC-BY-SA-4.0 + + ament_cmake + + + rclcpp + px4_ros2_cpp + eigen3_cmake_module + geometry_msgs + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/px4_roscon_workshop/custom_mode_demo/CMakeLists.txt b/px4_roscon_workshop/custom_mode_demo/CMakeLists.txt index d6bd1e7..fca58d6 100644 --- a/px4_roscon_workshop/custom_mode_demo/CMakeLists.txt +++ b/px4_roscon_workshop/custom_mode_demo/CMakeLists.txt @@ -16,8 +16,6 @@ find_package(geometry_msgs REQUIRED) add_executable(${PROJECT_NAME} CustomMode.cpp CustomMode.hpp - CustomModeExecutor.cpp - CustomModeExecutor.hpp ) ament_target_dependencies(${PROJECT_NAME} diff --git a/px4_roscon_workshop/custom_mode_demo/CustomMode.cpp b/px4_roscon_workshop/custom_mode_demo/CustomMode.cpp index 23de1b5..a191b5a 100644 --- a/px4_roscon_workshop/custom_mode_demo/CustomMode.cpp +++ b/px4_roscon_workshop/custom_mode_demo/CustomMode.cpp @@ -1,12 +1,10 @@ -// ============================================================================ -// ORIGINAL VERSION - CustomMode.cpp -// ============================================================================ #include "CustomMode.hpp" #include static const std::string kModeNameCustomWaypoints = "CustomWaypoints"; -static const std::string kModeNameCustomYaw = "CustomYaw"; +static const std::string kNodeName = "custom_mode_demo"; +static const bool kEnableDebugOutput = true; CustomWaypoints::CustomWaypoints(rclcpp::Node &node) : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), @@ -21,24 +19,7 @@ CustomWaypoints::CustomWaypoints(rclcpp::Node &node) } -CustomYaw::CustomYaw(rclcpp::Node &node) - : px4_ros2::ModeBase(node, kModeNameCustomYaw), - _node(node) -{ - loadParameters(); - - _trajectory_setpoint = std::make_shared(*this); - _vehicle_attitude = std::make_shared(*this); - _local_position = std::make_shared(*this); - - RCLCPP_INFO(node.get_logger(), "CustomYaw mode initialized."); -} - void CustomWaypoints::loadParameters() { - // Load parameters specific to the CustomWaypoints mode -} -void CustomYaw::loadParameters() { - // Load parameters specific to the CustomYaw mode } void CustomWaypoints::onActivate() { @@ -52,200 +33,32 @@ void CustomWaypoints::onActivate() { _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); - _current_waypoint_index = 0; // Start at the first waypoint + _current_waypoint_index = 0; RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); - // Set initial trajectory setpoint } void CustomWaypoints::onDeactivate() { RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode deactivated"); - // Reset trajectory setpoint } void CustomWaypoints::updateSetpoint([[maybe_unused]] float dt_s) { if (_current_waypoint_index < _trajectory_waypoints.size()) { - // Set the trajectory setpoint to the current waypoint auto current_waypoint = _trajectory_waypoints[_current_waypoint_index]; _trajectory_setpoint->updatePosition(current_waypoint); - - // Check if we reached the current waypoint if ((_local_position->positionNed() - current_waypoint).norm() < 0.5f) { - _current_waypoint_index++; // Move to the next waypoint + _current_waypoint_index++; } } else { - // All waypoints completed, reset or stop - RCLCPP_INFO(_node.get_logger(), "All waypoints completed."); - completed(px4_ros2::Result::Success); - return; // Exit the update loop - } - -} -void CustomYaw::onActivate() { - _start_yaw = _vehicle_attitude->yaw(); // Store the starting yaw angle - _yaw_accumulator = 0.0f; // Initialize yaw accumulator - RCLCPP_INFO(_node.get_logger(), "CustomYaw mode activated"); - // Set initial trajectory setpoint -} -void CustomYaw::onDeactivate() { - RCLCPP_INFO(_node.get_logger(), "CustomYaw mode deactivated"); - // Reset trajectory setpoint -} -void CustomYaw::updateSetpoint([[maybe_unused]] float dt_s) { - // Update the trajectory setpoint based on the current heading - Eigen::Vector3f velocity{0.0f, 0.0f, 0.0f}; - std::optional acceleration = std::nullopt; - std::optional yaw = std::nullopt; - std::optional yaw_rate = 0.05f; - _trajectory_setpoint->update(velocity, acceleration, yaw, yaw_rate); - _yaw_accumulator += yaw_rate.value() * dt_s; // Accumulate yaw rotation - if (std::abs(_yaw_accumulator) > 2 * M_PI - 0.1f) { // full rotation (tolerant) - RCLCPP_INFO(_node.get_logger(), "CustomYaw mode completed a full rotation."); + RCLCPP_INFO_ONCE(_node.get_logger(), "All waypoints completed."); completed(px4_ros2::Result::Success); return; } } -// ============================================================================ -// ALTERNATIVE VERSION - For exercises -// ============================================================================ -// #include "CustomMode.hpp" - -// #include - -// static const std::string kModeNameCustomWaypoints = "CustomWaypoints"; -// static const std::string kModeNameCustomYaw = "CustomYaw"; -// static const std::string kModeNameCustomAltitude = "CustomAltitude"; - -// CustomWaypoints::CustomWaypoints(rclcpp::Node &node) -// : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), -// _node(node) -// { -// loadParameters(); - -// _trajectory_setpoint = std::make_shared(*this); -// _local_position = std::make_shared(*this); -// RCLCPP_INFO(node.get_logger(), "CustomWaypoints mode initialized."); - -// } - -// CustomYaw::CustomYaw(rclcpp::Node &node) -// : px4_ros2::ModeBase(node, kModeNameCustomYaw), -// _node(node) -// { -// // loadParameters(); - -// _trajectory_setpoint = std::make_shared(*this); -// _vehicle_attitude = std::make_shared(*this); -// _local_position = std::make_shared(*this); - -// RCLCPP_INFO(node.get_logger(), "CustomYaw mode initialized."); -// } - -// void CustomWaypoints::loadParameters() { -// // Use the node reference to declare/get parameters -// _node.declare_parameter("altitude", 2.0f); -// _node.get_parameter("altitude", _altitude); - -// RCLCPP_INFO(_node.get_logger(), "CustomWaypoints altitude parameter: %f", _altitude); -// } -// void CustomYaw::loadParameters() { -// // Load parameters specific to the CustomYaw mode -// } - -// void CustomWaypoints::onActivate() { -// // Initialize waypoints - -// _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); -// _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); -// _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); -// _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); -// _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); -// _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); -// _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); - -// _current_waypoint_index = 0; // Start at the first waypoint -// RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); -// // Set initial trajectory setpoint -// } - -// void CustomWaypoints::onDeactivate() { -// RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode deactivated"); -// // Reset trajectory setpoint -// } -// void CustomWaypoints::updateSetpoint([[maybe_unused]] float dt_s) { -// if (_current_waypoint_index < _trajectory_waypoints.size()) { -// // Set the trajectory setpoint to the current waypoint -// auto current_waypoint = _trajectory_waypoints[_current_waypoint_index]; -// _trajectory_setpoint->updatePosition(current_waypoint); - - -// // Check if we reached the current waypoint -// if ((_local_position->positionNed() - current_waypoint).norm() < 0.5f) { -// _current_waypoint_index++; // Move to the next waypoint -// } -// } else { -// // All waypoints completed, reset or stop -// RCLCPP_INFO(_node.get_logger(), "All waypoints completed."); -// completed(px4_ros2::Result::Success); -// return; // Exit the update loop -// } - -// } -// void CustomYaw::onActivate() { -// _start_yaw = _vehicle_attitude->yaw(); // Store the starting yaw angle -// _yaw_accumulator = 0.0f; // Initialize yaw accumulator -// RCLCPP_INFO(_node.get_logger(), "CustomYaw mode activated"); -// // Set initial trajectory setpoint -// } -// void CustomYaw::onDeactivate() { -// RCLCPP_INFO(_node.get_logger(), "CustomYaw mode deactivated"); -// // Reset trajectory setpoint -// } -// void CustomYaw::updateSetpoint([[maybe_unused]] float dt_s) { -// // Update the trajectory setpoint based on the current heading -// Eigen::Vector3f velocity{0.0f, 0.0f, 0.0f}; -// std::optional acceleration = std::nullopt; -// std::optional yaw = std::nullopt; -// std::optional yaw_rate = 0.05f; -// _trajectory_setpoint->update(velocity, acceleration, yaw, yaw_rate); -// _yaw_accumulator += yaw_rate.value() * dt_s; // Accumulate yaw rotation -// if (std::abs(_yaw_accumulator) > 2 * M_PI - 0.1f) { // full rotation (tolerant) -// RCLCPP_INFO(_node.get_logger(), "CustomYaw mode completed a full rotation."); -// completed(px4_ros2::Result::Success); -// return; -// } -// } - -// CustomAltitude::CustomAltitude(rclcpp::Node &node) -// : px4_ros2::ModeBase(node, kModeNameCustomAltitude), -// _node(node) -// { -// // loadParameters(); - -// _trajectory_setpoint = std::make_shared(*this); -// _local_position = std::make_shared(*this); - -// RCLCPP_INFO(node.get_logger(), "CustomAltitude mode initialized."); -// } - -// void CustomAltitude::onActivate() { -// RCLCPP_INFO(_node.get_logger(), "CustomAltitude mode activated"); -// // Set initial trajectory setpoint -// } - -// void CustomAltitude::onDeactivate() { -// RCLCPP_INFO(_node.get_logger(), "CustomAltitude mode deactivated"); -// // Reset trajectory setpoint -// } - -// void CustomAltitude::updateSetpoint([[maybe_unused]] float dt_s) { -// // Maintain current position but change altitude to -3.0 meters -// Eigen::Vector3f target_position = _local_position->positionNed(); -// target_position.z() = -3.0f; // Set target altitude to -3.0 meters -// _trajectory_setpoint->updatePosition(target_position); -// // Check if we reached the target altitude -// if (std::abs(_local_position->positionNed().z() - target_position.z()) < 0.1f) { -// RCLCPP_INFO(_node.get_logger(), "CustomAltitude mode reached target altitude."); -// completed(px4_ros2::Result::Success); -// return; // Exit the update loop -// } -// } +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared>( + kNodeName, kEnableDebugOutput)); + rclcpp::shutdown(); + return 0; +} diff --git a/px4_roscon_workshop/custom_mode_demo/CustomMode.hpp b/px4_roscon_workshop/custom_mode_demo/CustomMode.hpp index 9f64662..059643e 100644 --- a/px4_roscon_workshop/custom_mode_demo/CustomMode.hpp +++ b/px4_roscon_workshop/custom_mode_demo/CustomMode.hpp @@ -1,26 +1,14 @@ -// ============================================================================ -// ORIGINAL VERSION - CustomMode.hpp -// ============================================================================ #pragma once -// PX4 Interface Library #include -#include #include -#include -#include -#include #include -// ROS 2 Core #include -#include -#include -// C++ Std -#include // for M_PI #include -#include +#include +#include class CustomWaypoints : public px4_ros2::ModeBase { public: @@ -33,133 +21,11 @@ class CustomWaypoints : public px4_ros2::ModeBase { private: void loadParameters(); - // ROS 2 rclcpp::Node &_node; - - // px4_ros2_cpp - std::shared_ptr _trajectory_setpoint; - std::shared_ptr _local_position; - - std::vector _trajectory_waypoints; // Vector to hold waypoints - size_t _current_waypoint_index; // Index of the current waypoint -}; - -class CustomYaw : public px4_ros2::ModeBase { -public: - explicit CustomYaw(rclcpp::Node &node); - - // See ModeBase - void onActivate() override; - void onDeactivate() override; - void updateSetpoint([[maybe_unused]] float dt_s) override; - -private: - void loadParameters(); - // ROS 2 - rclcpp::Node &_node; - - - // px4_ros2_cpp - std::shared_ptr _vehicle_attitude; std::shared_ptr _trajectory_setpoint; std::shared_ptr _local_position; - - float _start_yaw; // Starting yaw angle - float _yaw_accumulator; // Increment for yaw rotation + std::vector _trajectory_waypoints; + size_t _current_waypoint_index{}; }; - -// ============================================================================ -// ALTERNATIVE VERSION - For exercises -// ============================================================================ -// #pragma once - -// // PX4 Interface Library -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// // ROS 2 Core -// #include -// #include -// #include - -// // C++ Std -// #include // for M_PI -// #include -// #include - -// class CustomWaypoints : public px4_ros2::ModeBase { -// public: -// explicit CustomWaypoints(rclcpp::Node &node); - -// // See ModeBase -// void onActivate() override; -// void onDeactivate() override; -// void updateSetpoint([[maybe_unused]] float dt_s) override; - -// private: -// void loadParameters(); -// // ROS 2 -// rclcpp::Node &_node; - - -// // px4_ros2_cpp -// std::shared_ptr _trajectory_setpoint; -// std::shared_ptr _local_position; - -// std::vector _trajectory_waypoints; // Vector to hold waypoints -// size_t _current_waypoint_index; // Index of the current waypoint -// // Parameters -// float _altitude; -// }; - -// class CustomYaw : public px4_ros2::ModeBase { -// public: -// explicit CustomYaw(rclcpp::Node &node); - -// // See ModeBase -// void onActivate() override; -// void onDeactivate() override; -// void updateSetpoint([[maybe_unused]] float dt_s) override; - -// private: -// void loadParameters(); -// // ROS 2 -// rclcpp::Node &_node; - - -// // px4_ros2_cpp -// std::shared_ptr _vehicle_attitude; -// std::shared_ptr _trajectory_setpoint; -// std::shared_ptr _local_position; - - -// float _start_yaw; // Starting yaw angle -// float _yaw_accumulator; // Increment for yaw rotation -// }; - -// class CustomAltitude : public px4_ros2::ModeBase { -// public: -// explicit CustomAltitude(rclcpp::Node &node); - -// // See ModeBase -// void onActivate() override; -// void onDeactivate() override; -// void updateSetpoint([[maybe_unused]] float dt_s) override; - -// private: -// void loadParameters(); -// // ROS 2 -// rclcpp::Node &_node; - - -// // px4_ros2_cpp -// std::shared_ptr _trajectory_setpoint; -// std::shared_ptr _local_position; -// }; diff --git a/px4_roscon_workshop/custom_mode_demo/README.md b/px4_roscon_workshop/custom_mode_demo/README.md index 9a8996c..33f1c2e 100644 --- a/px4_roscon_workshop/custom_mode_demo/README.md +++ b/px4_roscon_workshop/custom_mode_demo/README.md @@ -1,26 +1,10 @@ # Custom Mode Demo -This package demonstrates how to create and use PX4 custom flight modes from ROS 2 using the PX4-ROS2 Interface Library. It showcases the implementation of custom flight behaviors that integrate seamlessly with PX4's mode management system. +This package demonstrates how to create and register a PX4 custom flight mode from ROS 2 using the PX4-ROS2 Interface Library. ## Overview -The Custom Mode Demo implements two custom flight modes using the PX4-ROS2 Interface Library: - -1. **CustomWaypoints Mode**: Autonomous waypoint navigation with predefined trajectory -2. **CustomYaw Mode**: Controlled yaw rotation while maintaining position - -These modes are orchestrated by a **CustomModeExecutor** that manages the complete flight sequence through a state machine architecture. -You have to select CustomWaypoints for the sequence to start. - -### Flight Sequence - -The demo executes the following autonomous sequence: - -1. **Takeoff** - Automatic takeoff to predefined altitude -2. **CustomWaypoints** - Navigate through rectangular waypoint pattern -3. **CustomYaw** - Perform 360-degree rotation while hovering -4. **Land** - Automatic landing -5. **WaitUntilDisarmed** - Safe completion state +The Custom Mode Demo registers one mode, **CustomWaypoints**, which flies a predefined rectangular waypoint trajectory when selected in QGroundControl. ## Prerequisites @@ -43,7 +27,7 @@ The demo executes the following autonomous sequence: ros2 launch custom_mode_demo custom_mode_demo.launch.py ``` -4. Differently from the Offboard control exercise, the custom mode demo does not automatically switch to the correct external mode _CustomWaypoints_ and it does not arm the drone. +4. The custom mode demo does not automatically switch to _CustomWaypoints_ or arm the drone. 1. On the QGC window, fist enter _CustomWaypoints_ mode: @@ -55,11 +39,3 @@ The demo executes the following autonomous sequence: The `custom_mode_demo.launch.py` can also start the _MicroXrceAgent_ and the _gz clock bridge_. Set the launch arguments `run_uxrcedds_agent` or `run_gz_clock_bridge` to `true` to run them if you don't use `common.launch.py`. -## Exercises - -1. Add any kind of parameter, that affects CustomWaypoint or CustomYaw -2. Explore what happens if you want to change the Mode order in the Executor -3. Add another Mode after yaw, where you change the altitude to 3 m before landing - -The solution to the exercise is commented out at the end of the `CustomMode.cpp`, `CustomMode.hpp`, `CustomModeExecutor.cpp` and `CustomModeExecutor.hpp` -Feel free to uncomment it and recompile the package to unveil it. From 5d0f07816362e9dda6c731332cc4e0751993aada Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 26 Aug 2026 00:23:46 +0100 Subject: [PATCH 2/4] chore: remove exercise in the comments Signed-off-by: Beniamino Pozzan --- .../custom_executor_demo/CustomMode.cpp | 148 +----------------- .../custom_executor_demo/CustomMode.hpp | 96 +----------- .../CustomModeExecutor.cpp | 85 +--------- .../CustomModeExecutor.hpp | 37 +---- 4 files changed, 4 insertions(+), 362 deletions(-) diff --git a/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp b/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp index 23de1b5..646c0a3 100644 --- a/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp +++ b/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp @@ -102,150 +102,4 @@ void CustomYaw::updateSetpoint([[maybe_unused]] float dt_s) { completed(px4_ros2::Result::Success); return; } -} -// ============================================================================ -// ALTERNATIVE VERSION - For exercises -// ============================================================================ -// #include "CustomMode.hpp" - -// #include - -// static const std::string kModeNameCustomWaypoints = "CustomWaypoints"; -// static const std::string kModeNameCustomYaw = "CustomYaw"; -// static const std::string kModeNameCustomAltitude = "CustomAltitude"; - -// CustomWaypoints::CustomWaypoints(rclcpp::Node &node) -// : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), -// _node(node) -// { -// loadParameters(); - -// _trajectory_setpoint = std::make_shared(*this); -// _local_position = std::make_shared(*this); - -// RCLCPP_INFO(node.get_logger(), "CustomWaypoints mode initialized."); - -// } - -// CustomYaw::CustomYaw(rclcpp::Node &node) -// : px4_ros2::ModeBase(node, kModeNameCustomYaw), -// _node(node) -// { -// // loadParameters(); - -// _trajectory_setpoint = std::make_shared(*this); -// _vehicle_attitude = std::make_shared(*this); -// _local_position = std::make_shared(*this); - -// RCLCPP_INFO(node.get_logger(), "CustomYaw mode initialized."); -// } - -// void CustomWaypoints::loadParameters() { -// // Use the node reference to declare/get parameters -// _node.declare_parameter("altitude", 2.0f); -// _node.get_parameter("altitude", _altitude); - -// RCLCPP_INFO(_node.get_logger(), "CustomWaypoints altitude parameter: %f", _altitude); -// } -// void CustomYaw::loadParameters() { -// // Load parameters specific to the CustomYaw mode -// } - -// void CustomWaypoints::onActivate() { -// // Initialize waypoints - -// _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); -// _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); -// _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); -// _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); -// _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); -// _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); -// _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); - -// _current_waypoint_index = 0; // Start at the first waypoint -// RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); -// // Set initial trajectory setpoint -// } - -// void CustomWaypoints::onDeactivate() { -// RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode deactivated"); -// // Reset trajectory setpoint -// } -// void CustomWaypoints::updateSetpoint([[maybe_unused]] float dt_s) { -// if (_current_waypoint_index < _trajectory_waypoints.size()) { -// // Set the trajectory setpoint to the current waypoint -// auto current_waypoint = _trajectory_waypoints[_current_waypoint_index]; -// _trajectory_setpoint->updatePosition(current_waypoint); - - -// // Check if we reached the current waypoint -// if ((_local_position->positionNed() - current_waypoint).norm() < 0.5f) { -// _current_waypoint_index++; // Move to the next waypoint -// } -// } else { -// // All waypoints completed, reset or stop -// RCLCPP_INFO(_node.get_logger(), "All waypoints completed."); -// completed(px4_ros2::Result::Success); -// return; // Exit the update loop -// } - -// } -// void CustomYaw::onActivate() { -// _start_yaw = _vehicle_attitude->yaw(); // Store the starting yaw angle -// _yaw_accumulator = 0.0f; // Initialize yaw accumulator -// RCLCPP_INFO(_node.get_logger(), "CustomYaw mode activated"); -// // Set initial trajectory setpoint -// } -// void CustomYaw::onDeactivate() { -// RCLCPP_INFO(_node.get_logger(), "CustomYaw mode deactivated"); -// // Reset trajectory setpoint -// } -// void CustomYaw::updateSetpoint([[maybe_unused]] float dt_s) { -// // Update the trajectory setpoint based on the current heading -// Eigen::Vector3f velocity{0.0f, 0.0f, 0.0f}; -// std::optional acceleration = std::nullopt; -// std::optional yaw = std::nullopt; -// std::optional yaw_rate = 0.05f; -// _trajectory_setpoint->update(velocity, acceleration, yaw, yaw_rate); -// _yaw_accumulator += yaw_rate.value() * dt_s; // Accumulate yaw rotation -// if (std::abs(_yaw_accumulator) > 2 * M_PI - 0.1f) { // full rotation (tolerant) -// RCLCPP_INFO(_node.get_logger(), "CustomYaw mode completed a full rotation."); -// completed(px4_ros2::Result::Success); -// return; -// } -// } - -// CustomAltitude::CustomAltitude(rclcpp::Node &node) -// : px4_ros2::ModeBase(node, kModeNameCustomAltitude), -// _node(node) -// { -// // loadParameters(); - -// _trajectory_setpoint = std::make_shared(*this); -// _local_position = std::make_shared(*this); - -// RCLCPP_INFO(node.get_logger(), "CustomAltitude mode initialized."); -// } - -// void CustomAltitude::onActivate() { -// RCLCPP_INFO(_node.get_logger(), "CustomAltitude mode activated"); -// // Set initial trajectory setpoint -// } - -// void CustomAltitude::onDeactivate() { -// RCLCPP_INFO(_node.get_logger(), "CustomAltitude mode deactivated"); -// // Reset trajectory setpoint -// } - -// void CustomAltitude::updateSetpoint([[maybe_unused]] float dt_s) { -// // Maintain current position but change altitude to -3.0 meters -// Eigen::Vector3f target_position = _local_position->positionNed(); -// target_position.z() = -3.0f; // Set target altitude to -3.0 meters -// _trajectory_setpoint->updatePosition(target_position); -// // Check if we reached the target altitude -// if (std::abs(_local_position->positionNed().z() - target_position.z()) < 0.1f) { -// RCLCPP_INFO(_node.get_logger(), "CustomAltitude mode reached target altitude."); -// completed(px4_ros2::Result::Success); -// return; // Exit the update loop -// } -// } +} \ No newline at end of file diff --git a/px4_roscon_workshop/custom_executor_demo/CustomMode.hpp b/px4_roscon_workshop/custom_executor_demo/CustomMode.hpp index 9f64662..793a8de 100644 --- a/px4_roscon_workshop/custom_executor_demo/CustomMode.hpp +++ b/px4_roscon_workshop/custom_executor_demo/CustomMode.hpp @@ -68,98 +68,4 @@ class CustomYaw : public px4_ros2::ModeBase { float _start_yaw; // Starting yaw angle float _yaw_accumulator; // Increment for yaw rotation -}; - -// ============================================================================ -// ALTERNATIVE VERSION - For exercises -// ============================================================================ -// #pragma once - -// // PX4 Interface Library -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// // ROS 2 Core -// #include -// #include -// #include - -// // C++ Std -// #include // for M_PI -// #include -// #include - -// class CustomWaypoints : public px4_ros2::ModeBase { -// public: -// explicit CustomWaypoints(rclcpp::Node &node); - -// // See ModeBase -// void onActivate() override; -// void onDeactivate() override; -// void updateSetpoint([[maybe_unused]] float dt_s) override; - -// private: -// void loadParameters(); -// // ROS 2 -// rclcpp::Node &_node; - - -// // px4_ros2_cpp -// std::shared_ptr _trajectory_setpoint; -// std::shared_ptr _local_position; - -// std::vector _trajectory_waypoints; // Vector to hold waypoints -// size_t _current_waypoint_index; // Index of the current waypoint -// // Parameters -// float _altitude; -// }; - -// class CustomYaw : public px4_ros2::ModeBase { -// public: -// explicit CustomYaw(rclcpp::Node &node); - -// // See ModeBase -// void onActivate() override; -// void onDeactivate() override; -// void updateSetpoint([[maybe_unused]] float dt_s) override; - -// private: -// void loadParameters(); -// // ROS 2 -// rclcpp::Node &_node; - - -// // px4_ros2_cpp -// std::shared_ptr _vehicle_attitude; -// std::shared_ptr _trajectory_setpoint; -// std::shared_ptr _local_position; - - -// float _start_yaw; // Starting yaw angle -// float _yaw_accumulator; // Increment for yaw rotation -// }; - -// class CustomAltitude : public px4_ros2::ModeBase { -// public: -// explicit CustomAltitude(rclcpp::Node &node); - -// // See ModeBase -// void onActivate() override; -// void onDeactivate() override; -// void updateSetpoint([[maybe_unused]] float dt_s) override; - -// private: -// void loadParameters(); -// // ROS 2 -// rclcpp::Node &_node; - - -// // px4_ros2_cpp -// std::shared_ptr _trajectory_setpoint; -// std::shared_ptr _local_position; -// }; +}; \ No newline at end of file diff --git a/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.cpp b/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.cpp index d7fc07c..ae0768e 100644 --- a/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.cpp +++ b/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.cpp @@ -74,87 +74,4 @@ int main(int argc, char *argv[]) { rclcpp::spin(node_with_mode); rclcpp::shutdown(); return 0; -} -// ============================================================================ -// ALTERNATIVE VERSION - For exercises -// ============================================================================ -// #include "CustomModeExecutor.hpp" - -// using CustomModeWithExecutor = px4_ros2::NodeWithModeExecutor; - -// static const std::string kNodeName = "CustomModeDemo"; -// static const bool kEnableDebugOutput = true; - -// CustomModeExecutor::CustomModeExecutor(px4_ros2::ModeBase &owned_mode, px4_ros2::ModeBase &second_mode, px4_ros2::ModeBase &third_mode) -// : ModeExecutorBase(Settings{}, owned_mode), _second_mode(second_mode), _third_mode(third_mode) {} - -// void CustomModeExecutor::onActivate() { -// RCLCPP_INFO(node().get_logger(), "CustomModeExecutor activated"); -// switchToState(State::Takeoff, px4_ros2::Result::Success); -// } - -// void CustomModeExecutor::onDeactivate(DeactivateReason reason) { -// const char *reason_str = (reason == DeactivateReason::FailsafeActivated) -// ? "failsafe activated" -// : "other reason"; -// RCLCPP_INFO(node().get_logger(), "CustomModeExecutor deactivated: %s", reason_str); -// } - -// void CustomModeExecutor::switchToState(State state, px4_ros2::Result previous_result) { -// _state = state; -// if (previous_result != px4_ros2::Result::Success) { -// RCLCPP_WARN(node().get_logger(), -// "Switching to state %d due to previous result: %d", -// static_cast(state), static_cast(previous_result)); -// } - -// RCLCPP_INFO(node().get_logger(), "Switched to state: %d", static_cast(state)); - -// // Handle state-specific logic here -// switch (state) { -// case State::Takeoff: -// RCLCPP_INFO(node().get_logger(), "Initiating takeoff..."); -// takeoff( -// [this](px4_ros2::Result result) { -// switchToState(State::CustomWaypoints, result); -// }, -// 2.0f); -// break; -// case State::CustomWaypoints: -// scheduleMode(ownedMode().id(), [this](px4_ros2::Result result) { -// // This callback triggers when the mode completes -// switchToState(State::CustomYaw, result); -// }); -// break; -// case State::CustomYaw: -// scheduleMode(_second_mode.id(), [this](px4_ros2::Result result) { -// // This callback triggers when the mode completes -// switchToState(State::ChangeAltitude, result); -// }); -// break; -// case State::ChangeAltitude: -// scheduleMode(_third_mode.id(), [this](px4_ros2::Result result) { -// // This callback triggers when the mode completes -// switchToState(State::Land, result); -// }); -// break; -// case State::Land: -// land([this](px4_ros2::Result result) { -// switchToState(State::WaitUntilDisarmed, result); -// }); -// break; -// case State::WaitUntilDisarmed: -// waitUntilDisarmed([this](px4_ros2::Result result) { -// RCLCPP_INFO(node().get_logger(), "All states complete (%s)", resultToString(result)); -// }); -// break; -// } -// } - -// int main(int argc, char *argv[]) { -// rclcpp::init(argc, argv); -// auto node_with_mode = std::make_shared(kNodeName, kEnableDebugOutput); -// rclcpp::spin(node_with_mode); -// rclcpp::shutdown(); -// return 0; -// } +} \ No newline at end of file diff --git a/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.hpp b/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.hpp index ae64782..2d831e6 100644 --- a/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.hpp +++ b/px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.hpp @@ -30,39 +30,4 @@ class CustomModeExecutor : public px4_ros2::ModeExecutorBase { }; State _state; void switchToState(State state, px4_ros2::Result previous_result); -}; -// ============================================================================ -// ALTERNATIVE VERSION - For exercises -// ============================================================================ -// #pragma once - -// #include -// #include - -// #include "CustomMode.hpp" - -// class CustomModeExecutor : public px4_ros2::ModeExecutorBase { -// public: -// CustomModeExecutor(px4_ros2::ModeBase &owned_mode, -// px4_ros2::ModeBase &second_mode, px4_ros2::ModeBase &third_mode); - -// // See ModeExecutorBase -// void onActivate() override; -// void onDeactivate(DeactivateReason reason) override; - -// private: -// px4_ros2::ModeBase &_second_mode; -// px4_ros2::ModeBase &_third_mode; - -// // State management -// enum class State { -// Takeoff, // Initial state, takeoff to a predefined altitude -// CustomWaypoints, // Custom waypoints mode -// CustomYaw, // Custom yaw mode -// Land, // Land state -// WaitUntilDisarmed, // Final state, wait until the vehicle is disarmed -// ChangeAltitude, // Change altitude state -// }; -// State _state; -// void switchToState(State state, px4_ros2::Result previous_result); -// }; \ No newline at end of file +}; \ No newline at end of file From 54f57ab5b74504ce47ad7eb715a6711dc44dd5be Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 26 Aug 2026 23:12:46 +0100 Subject: [PATCH 3/4] chore(custom_executor): don't finish on takeoff and speed up yawMode Signed-off-by: Beniamino Pozzan --- px4_roscon_workshop/custom_executor_demo/CustomMode.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp b/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp index 646c0a3..ae2246e 100644 --- a/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp +++ b/px4_roscon_workshop/custom_executor_demo/CustomMode.cpp @@ -50,7 +50,6 @@ void CustomWaypoints::onActivate() { _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); - _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); _current_waypoint_index = 0; // Start at the first waypoint RCLCPP_INFO(_node.get_logger(), "CustomWaypoints mode activated"); @@ -94,7 +93,7 @@ void CustomYaw::updateSetpoint([[maybe_unused]] float dt_s) { Eigen::Vector3f velocity{0.0f, 0.0f, 0.0f}; std::optional acceleration = std::nullopt; std::optional yaw = std::nullopt; - std::optional yaw_rate = 0.05f; + std::optional yaw_rate = 0.2f; _trajectory_setpoint->update(velocity, acceleration, yaw, yaw_rate); _yaw_accumulator += yaw_rate.value() * dt_s; // Accumulate yaw rotation if (std::abs(_yaw_accumulator) > 2 * M_PI - 0.1f) { // full rotation (tolerant) From 908e8c208fbf01a71146eaa74a5ba89d6b854131 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Wed, 26 Aug 2026 23:25:16 +0100 Subject: [PATCH 4/4] chore: add solution to custom_mode exercise Signed-off-by: Beniamino Pozzan --- .../custom_mode_demo/CMakeLists.txt | 14 ++- .../custom_mode_demo/solution.cpp | 91 +++++++++++++++++++ 2 files changed, 104 insertions(+), 1 deletion(-) create mode 100644 px4_roscon_workshop/custom_mode_demo/solution.cpp diff --git a/px4_roscon_workshop/custom_mode_demo/CMakeLists.txt b/px4_roscon_workshop/custom_mode_demo/CMakeLists.txt index fca58d6..93e1c47 100644 --- a/px4_roscon_workshop/custom_mode_demo/CMakeLists.txt +++ b/px4_roscon_workshop/custom_mode_demo/CMakeLists.txt @@ -25,8 +25,20 @@ ament_target_dependencies(${PROJECT_NAME} geometry_msgs ) +# Solution executable: go-to setpoints instead of trajectory setpoints +add_executable(${PROJECT_NAME}_solution + solution.cpp +) + +ament_target_dependencies(${PROJECT_NAME}_solution + rclcpp + px4_ros2_cpp + Eigen3 + geometry_msgs +) + # Install the binary -install(TARGETS ${PROJECT_NAME} +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_solution DESTINATION lib/${PROJECT_NAME} ) diff --git a/px4_roscon_workshop/custom_mode_demo/solution.cpp b/px4_roscon_workshop/custom_mode_demo/solution.cpp new file mode 100644 index 0000000..10e2d8a --- /dev/null +++ b/px4_roscon_workshop/custom_mode_demo/solution.cpp @@ -0,0 +1,91 @@ +#include +#include +#include +#include + +#include + +#include +#include +#include + +static const std::string kModeNameCustomWaypoints = "CustomWaypoints"; +static const std::string kNodeName = "custom_mode_demo"; +static const bool kEnableDebugOutput = true; + +// Solution: same waypoint mission as CustomMode.cpp, but driven by go-to +// setpoints (smooth position/heading control) instead of trajectory setpoints. +class CustomWaypointsGoto : public px4_ros2::ModeBase { +public: + explicit CustomWaypointsGoto(rclcpp::Node &node) + : px4_ros2::ModeBase(node, kModeNameCustomWaypoints), + _node(node) + { + _goto_setpoint = std::make_shared(*this); + _local_position = std::make_shared(*this); + + RCLCPP_INFO(node.get_logger(), "CustomWaypointsGoto mode initialized."); + } + + void onActivate() override + { + _trajectory_waypoints.clear(); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, 5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(-5.0f, -5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, -5.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(5.0f, 0.0f, -1.5f)); + _trajectory_waypoints.push_back(Eigen::Vector3f(0.0f, 0.0f, -1.5f)); + + _current_waypoint_index = 0; + RCLCPP_INFO(_node.get_logger(), "CustomWaypointsGoto mode activated"); + } + + void onDeactivate() override + { + RCLCPP_INFO(_node.get_logger(), "CustomWaypointsGoto mode deactivated"); + } + + void updateSetpoint([[maybe_unused]] float dt_s) override + { + if (_current_waypoint_index < _trajectory_waypoints.size()) { + const auto ¤t_waypoint = _trajectory_waypoints[_current_waypoint_index]; + _goto_setpoint->update(current_waypoint); + + if (positionReached(current_waypoint)) { + _current_waypoint_index++; + } + } else { + RCLCPP_INFO_ONCE(_node.get_logger(), "All waypoints completed."); + completed(px4_ros2::Result::Success); + return; + } + } + +private: + bool positionReached(const Eigen::Vector3f &target_position_m) const + { + static constexpr float kPositionErrorThreshold = 0.5f; // [m] + const Eigen::Vector3f position_error_m = + target_position_m - _local_position->positionNed(); + return position_error_m.norm() < kPositionErrorThreshold; + } + + rclcpp::Node &_node; + + std::shared_ptr _goto_setpoint; + std::shared_ptr _local_position; + + std::vector _trajectory_waypoints; + size_t _current_waypoint_index{}; +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared>( + kNodeName, kEnableDebugOutput)); + rclcpp::shutdown(); + return 0; +}