feat: Initial addition of SAR modes

px4_roscon_workshop/sar_auto_executor/CMakeLists.txt

@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.8)
+project(sar_auto_executor)
+
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+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)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
+
+# Executable target
+add_executable(${PROJECT_NAME}
+  SARMode.hpp
+  SARMode.cpp
+  SARModeExecutor.hpp
+  SARModeExecutor.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  px4_ros2_cpp
+  Eigen3
+  geometry_msgs
+  tf2_ros
+  tf2
+)
+
+# Install the binary
+install(TARGETS ${PROJECT_NAME}
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/launch)
+  install(DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}
+  )
+endif()
+
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/cfg)
+  install(DIRECTORY cfg
+    DESTINATION share/${PROJECT_NAME}/
+  )
+endif()
+
+# 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/sar_auto_executor/README.md

@@ -0,0 +1,41 @@
+# SAR Auto Executor
+
+This package builds on [`sar_modes`](../sar_modes/README.md): the same **SAR (V-Sweep)** and **SAR (Orbital)** formation modes, plus a **SAR (Auto)** mode that automatically switches between them based on the rover's speed.
+
+## Overview
+
+`SARModeExecutor` (`px4_ros2::ModeExecutorBase`) owns a placeholder mode, **SAR (Auto)**, the only one you select in QGC. It tracks the rover's speed from consecutive `/rover/pose` messages and schedules the active mode:
+
+- Below 0.3 m/s -> **SAR (Orbital)**
+- At or above 0.6 m/s -> **SAR (V-Sweep)**
+- In between -> stays in whichever mode is already active (hysteresis, to avoid flapping)
+
+`SAR (V-Sweep)` and `SAR (Orbital)` are still independently selectable too, for manual override or debugging.
+
+`fake_rover_mover.py` (reused from `sar_modes`) drives at two different speeds per phase, straddling both thresholds, so a single run exercises the switch both ways.
+
+## Prerequisites
+
+Same as [`sar_modes`](../sar_modes/README.md#prerequisites), build PX4 and the ROS 2 workspace as described in the [setup guide](../../docs/setup.md) ([dockerized setup](../../docs/setup_docker.md) if you'd rather run in a container), then build this package:
+
+```sh
+colcon build --symlink-install --packages-select sar_modes sar_auto_executor
+source install/setup.bash
+```
+
+## Usage
+
+1. Launch the exercise, brings up Gazebo, the bridge, `MicroXRCEAgent`, all 3 `x500` instances, one `sar_auto_executor` node per drone, and the fake rover:
+
+   ```sh
+   ros2 launch sar_auto_executor sar_auto_executor.launch.py
+   ```
+
+   Pass `px4_autopilot_path` if `PX4_PATH` isn't already set.
+
+2. Arm each vehicle and take off manually, then select **SAR (Auto)**. The executor schedules V-Sweep or Orbital based on the rover's current speed, and keeps switching as it crosses the thresholds.
+
+## Exercises
+
+1. Make `kSpeedLowThreshold`/`kSpeedHighThreshold` ROS parameters instead of compile-time constants.
+2. Add a third state to `evaluateAndSwitch()` for a rover that's been stationary for a while.

px4_roscon_workshop/sar_auto_executor/SARMode.cpp

@@ -0,0 +1,115 @@
+#include "SARMode.hpp"
+
+// BaseSARMode
+BaseSARMode::BaseSARMode(rclcpp::Node& node, const std::string& mode_name)
+	: px4_ros2::ModeBase(node, mode_name), _node(node), _mode_name(mode_name)
+{
+    _trajectory_setpoint = std::make_shared<px4_ros2::TrajectorySetpointType>(*this);
+
+    _target_sub = _node.create_subscription<geometry_msgs::msg::PoseStamped>(
+		    "/rover/pose",
+		    rclcpp::QoS(5).best_effort(),
+		    [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
+		        _target_pos.x() = static_cast<float>(msg->pose.position.y);   // North = ENU Y
+		        _target_pos.y() = static_cast<float>(msg->pose.position.x);   // East  = ENU X
+		        _target_pos.z() = -static_cast<float>(msg->pose.position.z);  // Down  = ENU -Z
+
+		        Eigen::Quaternionf q_enu(
+		            static_cast<float>(msg->pose.orientation.w),
+		            static_cast<float>(msg->pose.orientation.x),
+		            static_cast<float>(msg->pose.orientation.y),
+		            static_cast<float>(msg->pose.orientation.z));
+		        float yaw_enu = px4_ros2::quaternionToYaw(q_enu);
+		        _target_yaw = px4_ros2::wrapPi(static_cast<float>(M_PI / 2.0) - yaw_enu);
+
+		        _has_target = true;
+		    });
+}
+
+
+void BaseSARMode::onActivate() {
+    _elapsed_time = 0.0f;
+    RCLCPP_INFO(_node.get_logger(), "[SAR Mode] Activated: %s", _mode_name.c_str());
+}
+
+void BaseSARMode::onDeactivate() {
+    RCLCPP_INFO(_node.get_logger(), "[SAR Mode] Deactivated: %s", _mode_name.c_str());
+}
+
+// SARVSweepMode
+SARVSweepMode::SARVSweepMode(rclcpp::Node& node, int drone_id, int total_drones)
+        : BaseSARMode(node, "SAR (V-Sweep)"), _drone_id(drone_id), _total_drones(total_drones) {}
+
+void SARVSweepMode::updateSetpoint(float dt) {
+    (void)dt;
+    if (!_has_target) return;
+
+    float side_offset = (_drone_id - (_total_drones - 1) / 2.0f) * 8.0f;
+    float forward_offset = 10.0f - std::abs(_drone_id - (_total_drones - 1) / 2.0f) * 6.0f;
+
+    // Rotate the (forward, side) wedge offset from the rover's body frame into NED
+    // using its heading, so the V stays pointed the way the rover is facing.
+    float cos_yaw = std::cos(_target_yaw);
+    float sin_yaw = std::sin(_target_yaw);
+    float offset_north = forward_offset * cos_yaw - side_offset * sin_yaw;
+    float offset_east = forward_offset * sin_yaw + side_offset * cos_yaw;
+
+    // Same altitude layering as SAROrbitalMode, so mode switches don't change height.
+    float altitude_layer = -3.0f - (0.3f * static_cast<float>(_drone_id));
+
+    Eigen::Vector3f offset(offset_north, offset_east, altitude_layer);
+
+    px4_ros2::TrajectorySetpoint setpoint;
+    setpoint.withPosition(_target_pos + offset)
+            .withYaw(_target_yaw);
+    _trajectory_setpoint->update(setpoint);
+}
+
+
+// SAROrbitalMode
+SAROrbitalMode::SAROrbitalMode(rclcpp::Node& node, int drone_id, int total_drones)
+	: BaseSARMode(node, "SAR (Orbital)"), _drone_id(drone_id),
+	  _total_drones(total_drones > 0 ? total_drones : 1) {}
+
+void SAROrbitalMode::updateSetpoint(float dt) {
+    if (!_has_target) return;
+
+    _elapsed_time += dt;
+
+    float current_radius, current_omega;
+    {
+        std::lock_guard<std::mutex> lock(_param_mutex);
+        current_radius = _radius;
+        current_omega = _omega;
+    }
+
+    float phase_offset = static_cast<float>(_drone_id) * (2.0f * M_PI / static_cast<float>(_total_drones));
+    float angle = current_omega * _elapsed_time + phase_offset;
+    float altitude_layer = -3.0f - (0.3f * static_cast<float>(_drone_id));
+
+    Eigen::Vector3f offset(
+        current_radius * std::cos(angle),
+        current_radius * std::sin(angle),
+        altitude_layer
+    );
+
+    px4_ros2::TrajectorySetpoint setpoint;
+    setpoint.withPosition(_target_pos + offset)
+            .withYaw(px4_ros2::wrapPi(angle + static_cast<float>(M_PI)));
+    _trajectory_setpoint->update(setpoint);
+}
+
+// SARAutoMode
+SARAutoMode::SARAutoMode(rclcpp::Node& node)
+    : px4_ros2::ModeBase(node, std::string("SAR (Auto)")), _node(node)
+{
+    _trajectory_setpoint = std::make_shared<px4_ros2::TrajectorySetpointType>(*this);
+}
+
+void SARAutoMode::onActivate() {
+    RCLCPP_INFO(_node.get_logger(), "[SAR Mode] Activated: SAR (Auto)");
+}
+
+void SARAutoMode::onDeactivate() {
+    RCLCPP_INFO(_node.get_logger(), "[SAR Mode] Deactivated: SAR (Auto)");
+}

px4_roscon_workshop/sar_auto_executor/SARMode.hpp

@@ -0,0 +1,89 @@
+#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>
+#include <px4_msgs/msg/vehicle_land_detected.hpp>
+
+// ROS 2 Core
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+// C++ Std
+#include <cmath>  // for M_PI
+#include <Eigen/Eigen>
+#include <mutex>
+#include <string>
+
+// Base class declaration for Search and Rescue (SAR) Mode
+class BaseSARMode : public px4_ros2::ModeBase {
+public:
+    BaseSARMode(rclcpp::Node& node, const std::string& mode_name);
+    ~BaseSARMode() override = default;
+
+    void onActivate() override;
+    void onDeactivate() override;
+
+protected:
+    rclcpp::Node& _node;
+    const std::string _mode_name;
+    std::shared_ptr<px4_ros2::TrajectorySetpointType> _trajectory_setpoint;
+    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr _target_sub;
+    Eigen::Vector3f _target_pos{0.0f, 0.0f, 0.0f};
+    float _target_yaw{0.0f};  // Rover heading, NED convention: 0 = North, +pi/2 = East
+    bool _has_target{false};
+    float _elapsed_time{0.0f};
+};
+
+// VSweep Formation Mode
+class SARVSweepMode : public BaseSARMode {
+public:
+    explicit SARVSweepMode(rclcpp::Node& node, int drone_id=0, int total_drones=3);
+    ~SARVSweepMode() override = default;
+
+    void updateSetpoint(float dt) override;
+
+private:
+    int _drone_id;
+    int _total_drones;
+};
+
+
+// Orbital Formation Mode
+class SAROrbitalMode : public BaseSARMode {
+public:
+    explicit SAROrbitalMode(rclcpp::Node& node, int drone_id=0, int total_drones=3);
+    ~SAROrbitalMode() override = default;
+
+    void updateSetpoint(float dt) override;
+
+private:
+    int _drone_id;
+    int _total_drones;
+    float _radius{14.0f};
+    float _omega{0.4f};
+    std::mutex _param_mutex;  // guards _radius/_omega for a future dynamic-reconfigure callback
+};
+
+// The "SAR (Auto)" in QGC is the executor which immediately
+// reschedules into SARVSweepMode or SAROrbitalMode based on rover speed.
+class SARAutoMode : public px4_ros2::ModeBase {
+public:
+    explicit SARAutoMode(rclcpp::Node& node);
+    ~SARAutoMode() override = default;
+
+    void onActivate() override;
+    void onDeactivate() override;
+
+private:
+    rclcpp::Node& _node;
+
+    // Little hack (?): ModeBase requires at least one setpoint type configured before it can
+    // register/activate, even though this mode never actually issues one.
+    std::shared_ptr<px4_ros2::TrajectorySetpointType> _trajectory_setpoint;
+};