chore: split custom executor and custom mode demos

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()

px4_roscon_workshop/custom_executor_demo/CustomMode.cpp

@@ -0,0 +1,104 @@
+// ============================================================================
+// ORIGINAL VERSION - CustomMode.cpp
+// ============================================================================
+#include "CustomMode.hpp"
+
+#include <px4_ros2/components/node_with_mode.hpp>
+
+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<px4_ros2::TrajectorySetpointType>(*this);
+    _local_position = std::make_shared<px4_ros2::OdometryLocalPosition>(*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<px4_ros2::TrajectorySetpointType>(*this);
+    _vehicle_attitude = std::make_shared<px4_ros2::OdometryAttitude>(*this);
+    _local_position = std::make_shared<px4_ros2::OdometryLocalPosition>(*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));
+
+    _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<Eigen::Vector3f> acceleration = std::nullopt;
+    std::optional<float> yaw = std::nullopt;
+    std::optional<float> 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)
+        RCLCPP_INFO(_node.get_logger(), "CustomYaw mode completed a full rotation.");
+        completed(px4_ros2::Result::Success);
+        return;
+    }
+}

px4_roscon_workshop/custom_executor_demo/CustomMode.hpp

@@ -0,0 +1,71 @@
+// ============================================================================
+// ORIGINAL VERSION - CustomMode.hpp
+// ============================================================================
+#pragma once
+
+// PX4 Interface Library
+#include <px4_ros2/components/mode.hpp>
+#include <px4_ros2/utils/geometry.hpp>
+#include <px4_ros2/odometry/local_position.hpp>
+#include <px4_ros2/odometry/attitude.hpp>
+#include <px4_ros2/odometry/angular_velocity.hpp>
+#include <px4_msgs/msg/trajectory_setpoint.hpp>
+#include <px4_ros2/control/setpoint_types/experimental/trajectory.hpp>
+
+// ROS 2 Core
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <std_msgs/msg/bool.hpp>
+
+// C++ Std
+#include <cmath>  // for M_PI
+#include <Eigen/Eigen>
+#include <chrono>
+
+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<px4_ros2::TrajectorySetpointType> _trajectory_setpoint;
+    std::shared_ptr<px4_ros2::OdometryLocalPosition> _local_position;
+
+    std::vector<Eigen::Vector3f> _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<px4_ros2::OdometryAttitude> _vehicle_attitude;
+    std::shared_ptr<px4_ros2::TrajectorySetpointType> _trajectory_setpoint;
+    std::shared_ptr<px4_ros2::OdometryLocalPosition> _local_position;
+
+
+    float _start_yaw; // Starting yaw angle
+    float _yaw_accumulator; // Increment for yaw rotation
+};

px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.cpp

@@ -0,0 +1,77 @@
+// ============================================================================
+// ORIGINAL VERSION - CustomModeExecutor.cpp
+// ============================================================================
+#include "CustomModeExecutor.hpp"
+
+using CustomModeWithExecutor = px4_ros2::NodeWithModeExecutor<CustomModeExecutor, CustomWaypoints, CustomYaw>;
+
+static const std::string kNodeName = "CustomModeDemo";
+static const bool kEnableDebugOutput = true;
+
+CustomModeExecutor::CustomModeExecutor(px4_ros2::ModeBase &owned_mode, px4_ros2::ModeBase &second_mode)
+    : ModeExecutorBase(Settings{}, owned_mode), _second_mode(second_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<int>(state), static_cast<int>(previous_result));
+    }
+
+    RCLCPP_INFO(node().get_logger(), "Switched to state: %d", static_cast<int>(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::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<CustomModeWithExecutor>(kNodeName, kEnableDebugOutput);
+    rclcpp::spin(node_with_mode);
+    rclcpp::shutdown();
+    return 0;
+}

px4_roscon_workshop/custom_executor_demo/CustomModeExecutor.hpp

@@ -0,0 +1,33 @@
+// ============================================================================
+// ORIGINAL VERSION - CustomModeExecutor.hpp
+// ============================================================================
+#pragma once
+
+#include <px4_ros2/components/mode_executor.hpp>
+#include <px4_ros2/components/node_with_mode.hpp>
+
+#include "CustomMode.hpp"
+
+class CustomModeExecutor : public px4_ros2::ModeExecutorBase {
+public:
+    CustomModeExecutor(px4_ros2::ModeBase &owned_mode,
+                       px4_ros2::ModeBase &second_mode);
+
+    // See ModeExecutorBase
+    void onActivate() override;
+    void onDeactivate(DeactivateReason reason) override;
+
+private:
+    px4_ros2::ModeBase &_second_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
+    };
+    State _state;
+    void switchToState(State state, px4_ros2::Result previous_result);
+};