diff --git a/px4_roscon_workshop/sar_auto_executor/CMakeLists.txt b/px4_roscon_workshop/sar_auto_executor/CMakeLists.txt new file mode 100644 index 0000000..39857e9 --- /dev/null +++ b/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() diff --git a/px4_roscon_workshop/sar_auto_executor/README.md b/px4_roscon_workshop/sar_auto_executor/README.md new file mode 100644 index 0000000..f7d31d3 --- /dev/null +++ b/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. diff --git a/px4_roscon_workshop/sar_auto_executor/SARMode.cpp b/px4_roscon_workshop/sar_auto_executor/SARMode.cpp new file mode 100644 index 0000000..c1b7aff --- /dev/null +++ b/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(*this); + + _target_sub = _node.create_subscription( + "/rover/pose", + rclcpp::QoS(5).best_effort(), + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + _target_pos.x() = static_cast(msg->pose.position.y); // North = ENU Y + _target_pos.y() = static_cast(msg->pose.position.x); // East = ENU X + _target_pos.z() = -static_cast(msg->pose.position.z); // Down = ENU -Z + + Eigen::Quaternionf q_enu( + static_cast(msg->pose.orientation.w), + static_cast(msg->pose.orientation.x), + static_cast(msg->pose.orientation.y), + static_cast(msg->pose.orientation.z)); + float yaw_enu = px4_ros2::quaternionToYaw(q_enu); + _target_yaw = px4_ros2::wrapPi(static_cast(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(_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 lock(_param_mutex); + current_radius = _radius; + current_omega = _omega; + } + + float phase_offset = static_cast(_drone_id) * (2.0f * M_PI / static_cast(_total_drones)); + float angle = current_omega * _elapsed_time + phase_offset; + float altitude_layer = -3.0f - (0.3f * static_cast(_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(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(*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)"); +} diff --git a/px4_roscon_workshop/sar_auto_executor/SARMode.hpp b/px4_roscon_workshop/sar_auto_executor/SARMode.hpp new file mode 100644 index 0000000..8dc8c1d --- /dev/null +++ b/px4_roscon_workshop/sar_auto_executor/SARMode.hpp @@ -0,0 +1,89 @@ +#pragma once + +// PX4 Interface Library +#include +#include +#include +#include +#include +#include +#include +#include + +// ROS 2 Core +#include +#include + +// C++ Std +#include // for M_PI +#include +#include +#include + +// 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 _trajectory_setpoint; + rclcpp::Subscription::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 _trajectory_setpoint; +}; diff --git a/px4_roscon_workshop/sar_auto_executor/SARModeExecutor.cpp b/px4_roscon_workshop/sar_auto_executor/SARModeExecutor.cpp new file mode 100644 index 0000000..85c99ea --- /dev/null +++ b/px4_roscon_workshop/sar_auto_executor/SARModeExecutor.cpp @@ -0,0 +1,115 @@ +#include "SARModeExecutor.hpp" + +static const std::string kNodeName = "sar_auto_mode"; +static const bool kEnableDebugOutput = true; + +SARModeExecutor::SARModeExecutor(rclcpp::Node& node, px4_ros2::ModeBase& owned_mode, + px4_ros2::ModeBase& vsweep_mode, px4_ros2::ModeBase& orbital_mode) + : ModeExecutorBase(Settings{}, owned_mode), _node(node), + _vsweep_mode(vsweep_mode), _orbital_mode(orbital_mode) +{ + _target_sub = _node.create_subscription( + "/rover/pose", + rclcpp::QoS(5).best_effort(), + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + targetPoseCallback(msg); + }); +} + +void SARModeExecutor::targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + rclcpp::Time stamp(msg->header.stamp); + float x = static_cast(msg->pose.position.x); + float y = static_cast(msg->pose.position.y); + + if (_has_prev) { + double dt = (stamp - _prev_stamp).seconds(); + if (dt > 1e-3) { + float dx = x - _prev_x; + float dy = y - _prev_y; + _current_speed = std::sqrt(dx * dx + dy * dy) / static_cast(dt); + } + } else { + _has_prev = true; + } + + _prev_x = x; + _prev_y = y; + _prev_stamp = stamp; + + evaluateAndSwitch(); +} + +void SARModeExecutor::evaluateAndSwitch() { + if (!isInCharge()) return; + + px4_ros2::ModeBase* desired = nullptr; + + if (_active_mode_id == _orbital_mode.id()) { + if (_current_speed >= kSpeedHighThreshold) desired = &_vsweep_mode; + } else if (_active_mode_id == _vsweep_mode.id()) { + if (_current_speed < kSpeedLowThreshold) desired = &_orbital_mode; + } else { + // First decision after (re)activation: no hysteresis state yet. + desired = (_current_speed < kSpeedLowThreshold) ? &_orbital_mode : &_vsweep_mode; + } + + if (desired == nullptr) return; // inside the hysteresis band, or already correct + + _active_mode_id = desired->id(); + scheduleMode(desired->id(), [this](px4_ros2::Result result) { + RCLCPP_INFO(_node.get_logger(), "SAR Auto: scheduled mode ended (%s)", + px4_ros2::resultToString(result)); + }); +} + +void SARModeExecutor::onActivate() { + RCLCPP_INFO(_node.get_logger(), "[SAR Mode] Activated: SAR (Auto)"); + _active_mode_id = px4_ros2::ModeBase::kModeIDInvalid; // force a fresh decision + evaluateAndSwitch(); +} + +void SARModeExecutor::onDeactivate(DeactivateReason reason) { + const char* reason_str = (reason == DeactivateReason::FailsafeActivated) + ? "failsafe activated" + : "other reason"; + RCLCPP_INFO(_node.get_logger(), "[SAR Mode] Deactivated: SAR (Auto) (%s)", reason_str); +} + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(kNodeName); + + 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(); + } + } + + const int drone_id = node->declare_parameter("drone_id", 0); + const int total_drones = node->declare_parameter("total_drones", 3); + + auto auto_mode = std::make_shared(*node); + auto vsweep_mode = std::make_shared(*node, drone_id, total_drones); + auto orbital_mode = std::make_shared(*node, drone_id, total_drones); + auto executor = std::make_shared(*node, *auto_mode, *vsweep_mode, *orbital_mode); + + if (!executor->doRegister()) { + RCLCPP_ERROR(node->get_logger(), "SAR Auto executor registration failed"); + return -1; + } + if (!vsweep_mode->doRegister()) { + RCLCPP_ERROR(node->get_logger(), "SAR V-Sweep mode registration failed"); + return -1; + } + if (!orbital_mode->doRegister()) { + RCLCPP_ERROR(node->get_logger(), "SAR Orbital mode registration failed"); + return -1; + } + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/px4_roscon_workshop/sar_auto_executor/SARModeExecutor.hpp b/px4_roscon_workshop/sar_auto_executor/SARModeExecutor.hpp new file mode 100644 index 0000000..193f6b3 --- /dev/null +++ b/px4_roscon_workshop/sar_auto_executor/SARModeExecutor.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include "SARMode.hpp" +#include + +// Automatically switches between SAR (V-Sweep) and SAR (Orbital) based on the +// tracked rover's speed, computed from consecutive target's pose messages. +class SARModeExecutor : public px4_ros2::ModeExecutorBase { +public: + SARModeExecutor(rclcpp::Node& node, px4_ros2::ModeBase& owned_mode, + px4_ros2::ModeBase& vsweep_mode, px4_ros2::ModeBase& orbital_mode); + ~SARModeExecutor() override = default; + + void onActivate() override; + void onDeactivate(DeactivateReason reason) override; + +private: + void targetPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void evaluateAndSwitch(); + + rclcpp::Node& _node; + px4_ros2::ModeBase& _vsweep_mode; + px4_ros2::ModeBase& _orbital_mode; + + rclcpp::Subscription::SharedPtr _target_sub; + + bool _has_prev{false}; + float _prev_x{0.0f}; + float _prev_y{0.0f}; + rclcpp::Time _prev_stamp; + float _current_speed{0.0f}; + + // Below kSpeedLowThreshold -> Orbital, at/above kSpeedHighThreshold -> V-Sweep, + // with hysteresis in between to avoid rapid mode flapping. + static constexpr float kSpeedLowThreshold{0.3f}; // below this -> Orbital + static constexpr float kSpeedHighThreshold{0.6f}; // at/above this -> V-Sweep + + px4_ros2::ModeBase::ModeID _active_mode_id{px4_ros2::ModeBase::kModeIDInvalid}; +}; diff --git a/px4_roscon_workshop/sar_auto_executor/launch/sar_auto_executor.launch.py b/px4_roscon_workshop/sar_auto_executor/launch/sar_auto_executor.launch.py new file mode 100644 index 0000000..1b08565 --- /dev/null +++ b/px4_roscon_workshop/sar_auto_executor/launch/sar_auto_executor.launch.py @@ -0,0 +1,163 @@ +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.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare + +# Per-drone Gazebo spawn positions (ENU, spread apart so they don't overlap). +DRONE_SPAWN_POSITIONS = [(0.0, 0.0), (2.0, 0.0), (4.0, 0.0)] + + +def _float_param(launch_arg_name): + return ParameterValue(LaunchConfiguration(launch_arg_name), value_type=float) + + +def generate_launch_description(): + + px4_autopilot_path_arg = DeclareLaunchArgument( + "px4_autopilot_path", + default_value=EnvironmentVariable("PX4_PATH", default_value="~/PX4-Autopilot"), + description="Path to PX4-Autopilot repository root (supports ~)", + ) + world_arg = DeclareLaunchArgument( + "world", default_value="default", + description="Name of the Gazebo world to launch" + ) + + rover_x_arg = DeclareLaunchArgument( + "rover_start_x", default_value="5.0", + description="Fake rover starting ENU X position [m]" + ) + rover_y_arg = DeclareLaunchArgument( + "rover_start_y", default_value="3.0", + description="Fake rover starting ENU Y position [m]" + ) + rover_yaw_arg = DeclareLaunchArgument( + "rover_start_yaw_deg", default_value="45.0", + description="Fake rover starting heading [deg]" + ) + rover_min_speed_arg = DeclareLaunchArgument( + "rover_min_linear_speed", default_value="0.2", + description="Fake rover linear speed during the straight phase [m/s]" + ) + rover_max_speed_arg = DeclareLaunchArgument( + "rover_max_linear_speed", default_value="0.8", + description="Fake rover linear speed during the circle phase [m/s]" + ) + rover_start_delay_arg = DeclareLaunchArgument( + "rover_start_delay", default_value="3.0", + description="Seconds the fake rover stays still before it starts moving" + ) + rover_straight_duration_arg = DeclareLaunchArgument( + "rover_straight_duration", default_value="45.0", + description="Seconds the fake rover drives straight before circling [s]" + ) + rover_circle_radius_arg = DeclareLaunchArgument( + "rover_circle_radius", default_value="5.0", + description="Fake rover circling radius, minimum 5m [m]" + ) + rover_num_circle_turns_arg = DeclareLaunchArgument( + "rover_num_circle_turns", default_value="2.0", + description="Number of full turns the fake rover circles before holding" + ) + + # Reuses sar_modes's fake rover publisher rather than duplicating + # it here — it's a test fixture, not part of the mode-switching logic. + fake_rover_pose = Node( + package="sar_modes", + executable="fake_rover_mover.py", + name="fake_rover_mover", + output="screen", + parameters=[ + { + "start_x": _float_param("rover_start_x"), + "start_y": _float_param("rover_start_y"), + "start_yaw_deg": _float_param("rover_start_yaw_deg"), + "min_linear_speed": _float_param("rover_min_linear_speed"), + "max_linear_speed": _float_param("rover_max_linear_speed"), + "start_delay": _float_param("rover_start_delay"), + "straight_duration": _float_param("rover_straight_duration"), + "circle_radius": _float_param("rover_circle_radius"), + "num_circle_turns": _float_param("rover_num_circle_turns"), + } + ] + ) + + total_drones = 3 + + 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") + + gz_world = IncludeLaunchDescription( + PythonLaunchDescriptionSource(gz_world_launch), + launch_arguments={ + "px4_autopilot_path": LaunchConfiguration("px4_autopilot_path"), + "world": LaunchConfiguration("world"), + }.items(), + ) + + # Same 1-indexed PX4 instance / 0-indexed drone_id convention as + # sar_modes (see px4_roscon_workshop/px4_tf/README.md). + px4_vehicles = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(px4_vehicle_launch), + launch_arguments={ + "px4_autopilot_path": LaunchConfiguration("px4_autopilot_path"), + "world": LaunchConfiguration("world"), + "px4_instance": str(instance_id), + "model": f"x500_{instance_id}", + "px4_ns": f"px4_{instance_id}", + "spawn_pos_x": str(DRONE_SPAWN_POSITIONS[instance_id - 1][0]), + "spawn_pos_y": str(DRONE_SPAWN_POSITIONS[instance_id - 1][1]), + "px4_extra_env_vars": ( + "PX4_PARAM_COM_RCL_EXCEPT=9," + f"PX4_PARAM_COM_RC_IN_MODE={1 if instance_id == 1 else 4}," + "PX4_GZ_NO_FOLLOW=1" + ), + }.items(), + ) + for instance_id in range(1, total_drones + 1) + ] + + drone_nodes = [ + Node( + package="sar_auto_executor", + executable="sar_auto_executor", + name="sar_auto_executor", + namespace=f"px4_{instance_id}", + output="screen", + parameters=[ + { + "use_sim_time": True, + "drone_id": instance_id - 1, + "total_drones": total_drones, + } + ] + ) + for instance_id in range(1, total_drones + 1) + ] + + return LaunchDescription([ + px4_autopilot_path_arg, + world_arg, + rover_x_arg, + rover_y_arg, + rover_yaw_arg, + rover_min_speed_arg, + rover_max_speed_arg, + rover_start_delay_arg, + rover_straight_duration_arg, + rover_circle_radius_arg, + rover_num_circle_turns_arg, + gz_world, + *px4_vehicles, + *drone_nodes, + fake_rover_pose, + ]) diff --git a/px4_roscon_workshop/sar_auto_executor/package.xml b/px4_roscon_workshop/sar_auto_executor/package.xml new file mode 100644 index 0000000..640a89c --- /dev/null +++ b/px4_roscon_workshop/sar_auto_executor/package.xml @@ -0,0 +1,33 @@ + + + + sar_auto_executor + 0.0.1 + Auto-switching executor for the SAR swarm modes, built on top of sar_modes. + + Alexis Guijarro + CC-BY-SA-4.0 + + ament_cmake + + + rclcpp + px4_ros2_cpp + eigen3_cmake_module + geometry_msgs + tf2_ros + + launch + launch_ros + + sar_modes + px4_roscon_workshop + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/px4_roscon_workshop/sar_modes/CMakeLists.txt b/px4_roscon_workshop/sar_modes/CMakeLists.txt new file mode 100644 index 0000000..127e9d9 --- /dev/null +++ b/px4_roscon_workshop/sar_modes/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.8) +project(sar_modes) + +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 +) + +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} +) + +# Install the fake rover pose publisher used for testing without a rover model +install(PROGRAMS + scripts/fake_rover_mover.py + 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() diff --git a/px4_roscon_workshop/sar_modes/README.md b/px4_roscon_workshop/sar_modes/README.md new file mode 100644 index 0000000..c89bea8 --- /dev/null +++ b/px4_roscon_workshop/sar_modes/README.md @@ -0,0 +1,60 @@ +# SAR Modes + +This package implements Search and Rescue (SAR) swarm formation modes for PX4 using the PX4-ROS2 Interface Library. Three drones track a moving rover and hold a formation around it. + +## Overview + +`BaseSARMode` subscribes to `/rover/pose` (ENU) and converts it to NED for the derived modes: + +- **SAR (V-Sweep)**: holds a V/wedge formation around the target, rotated to match its heading. +- **SAR (Orbital)**: circles the target at a fixed radius, drones stacked at different altitudes, each yawing to face inward. + +Both modes are registered independently (no executor), so they show up as two separately selectable flight modes in QGroundControl on each vehicle. + +Spawn plumbing is shared with [`formation_control`](../formation_control/README.md); however, the coordination logic is not, because SAR tracks one external target, and `formation_control` tracks its neighbors. + +## Prerequisites + +This exercise runs 3 vehicles at once. + +1. 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). `sar_modes.launch.py` starts Gazebo and all 3 PX4 instances itself, so the setup guide's own simulation startup steps aren't needed here. +2. QGroundControl (or `commander`, see below) to arm and switch modes on the vehicles. + +## Usage + +1. Launch the exercise. This brings up Gazebo, the ROS-GZ bridge, `MicroXRCEAgent`, all 3 `x500` instances, one `sar_modes` node per drone, and the fake rover: + + ```sh + ros2 launch sar_modes sar_modes.launch.py + ``` + + Pass `px4_autopilot_path` if `PX4_PATH` isn't already set. + +2. In QGroundControl, switch to each vehicle, arm, and take off manually, there is no scripted takeoff. +3. Select **SAR (V-Sweep)** or **SAR (Orbital)** from the flight-mode dropdown. + +Check the rover is streaming with: + +```sh +ros2 topic echo /rover/pose +``` + +## Default configuration + +| PX4 instance | Namespace | `drone_id` | +| --- | --- | --- | +| 1 | `/px4_1/` | 0 | +| 2 | `/px4_2/` | 1 | +| 3 | `/px4_3/` | 2 | + +PX4 instances are 1-indexed (this repo's convention, see [`px4_tf/README.md`](../px4_tf/README.md)); `drone_id` stays 0-indexed for the formation math. + +## Exercises + +1. Retune `SARVSweepMode`'s spacing constants or `SAROrbitalMode`'s altitude step and rebuild. +2. Wire up `add_on_set_parameters_callback` on `SAROrbitalMode` so `ros2 param set` can change its orbit radius/omega while armed, the `_param_mutex` is already there for it. +3. Turn `SAROrbitalMode`'s per-drone altitude offset into deliberate multi-resolution recon: low drones close and detailed, one drone high and wide for context. +4. Port [`formation_control`](../formation_control/README.md)'s spring based separation controller into `sar_modes` as a minimum separation correction between drones. + Hint: `sar_modes` has no inter-drone awareness yet, you'll need to add TF broadcasting/lookup like `formation_control` does. + +The completed executor exercise can be found in a separate package: [SAR Auto Executor](../sar_auto_executor/README.md). diff --git a/px4_roscon_workshop/sar_modes/SARMode.cpp b/px4_roscon_workshop/sar_modes/SARMode.cpp new file mode 100644 index 0000000..c4059ac --- /dev/null +++ b/px4_roscon_workshop/sar_modes/SARMode.cpp @@ -0,0 +1,132 @@ +#include "SARMode.hpp" + +#include + +static const std::string kNodeName = "sar_mode"; +static const bool kEnableDebugOutput = true; + +// 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(*this); + + _target_sub = _node.create_subscription( + "/rover/pose", + rclcpp::QoS(5).best_effort(), + [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg) { + _target_pos.x() = static_cast(msg->pose.position.y); // North = ENU Y + _target_pos.y() = static_cast(msg->pose.position.x); // East = ENU X + _target_pos.z() = -static_cast(msg->pose.position.z); // Down = ENU -Z + + Eigen::Quaternionf q_enu( + static_cast(msg->pose.orientation.w), + static_cast(msg->pose.orientation.x), + static_cast(msg->pose.orientation.y), + static_cast(msg->pose.orientation.z)); + float yaw_enu = px4_ros2::quaternionToYaw(q_enu); + _target_yaw = px4_ros2::wrapPi(static_cast(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) * 6.0f; + float forward_offset = 8.0f - std::abs(_drone_id - (_total_drones - 1) / 2.0f) * 4.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; + + Eigen::Vector3f offset(offset_north, offset_east, -3.0f); + + 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 lock(_param_mutex); + current_radius = _radius; + current_omega = _omega; + } + + float phase_offset = static_cast(_drone_id) * (2.0f * M_PI / static_cast(_total_drones)); + float angle = current_omega * _elapsed_time + phase_offset; + float altitude_layer = -3.0f - (0.3f * static_cast(_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(M_PI))); + _trajectory_setpoint->update(setpoint); +} + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(kNodeName); + + if (kEnableDebugOutput) { + rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + } + + const int drone_id = node->declare_parameter("drone_id", 0); + const int total_drones = node->declare_parameter("total_drones", 3); + + auto vsweep_mode = std::make_shared(*node, drone_id, total_drones); + auto orbital_mode = std::make_shared(*node, drone_id, total_drones); + + if (!vsweep_mode->doRegister()) { + RCLCPP_ERROR(node->get_logger(), "SAR V-Sweep mode registration failed"); + return -1; + } + + if (!orbital_mode->doRegister()) { + RCLCPP_ERROR(node->get_logger(), "SAR Orbital mode registration failed"); + return -1; + } + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/px4_roscon_workshop/sar_modes/SARMode.hpp b/px4_roscon_workshop/sar_modes/SARMode.hpp new file mode 100644 index 0000000..3dc00c3 --- /dev/null +++ b/px4_roscon_workshop/sar_modes/SARMode.hpp @@ -0,0 +1,79 @@ +#pragma once + +// PX4 Interface Library +#include +#include +#include +#include +#include +#include +#include +#include + +// ROS 2 Core +#include +#include +#include +#include +#include +#include +#include +#include + +// C++ Std +#include // for M_PI +#include +#include +#include +#include +#include + +// 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 _trajectory_setpoint; + rclcpp::Subscription::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{3.5f}; + float _omega{0.4f}; + std::mutex _param_mutex; // guards _radius/_omega for a future dynamic-reconfigure callback +}; diff --git a/px4_roscon_workshop/sar_modes/launch/sar_modes.launch.py b/px4_roscon_workshop/sar_modes/launch/sar_modes.launch.py new file mode 100644 index 0000000..0a1f80c --- /dev/null +++ b/px4_roscon_workshop/sar_modes/launch/sar_modes.launch.py @@ -0,0 +1,164 @@ +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.parameter_descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare + +# Per-drone Gazebo spawn positions (ENU, spread apart so they don't overlap). +DRONE_SPAWN_POSITIONS = [(0.0, 0.0), (2.0, 0.0), (4.0, 0.0)] + + +def _float_param(launch_arg_name): + return ParameterValue(LaunchConfiguration(launch_arg_name), value_type=float) + + +def generate_launch_description(): + + px4_autopilot_path_arg = DeclareLaunchArgument( + "px4_autopilot_path", + default_value=EnvironmentVariable("PX4_PATH", default_value="~/PX4-Autopilot"), + description="Path to PX4-Autopilot repository root (supports ~)", + ) + world_arg = DeclareLaunchArgument( + "world", default_value="default", + description="Name of the Gazebo world to launch" + ) + + rover_x_arg = DeclareLaunchArgument( + "rover_start_x", default_value="5.0", + description="Fake rover starting ENU X position [m]" + ) + rover_y_arg = DeclareLaunchArgument( + "rover_start_y", default_value="3.0", + description="Fake rover starting ENU Y position [m]" + ) + rover_yaw_arg = DeclareLaunchArgument( + "rover_start_yaw_deg", default_value="45.0", + description="Fake rover starting heading [deg]" + ) + rover_min_speed_arg = DeclareLaunchArgument( + "rover_min_linear_speed", default_value="0.2", + description="Fake rover linear speed during the straight phase [m/s]" + ) + rover_max_speed_arg = DeclareLaunchArgument( + "rover_max_linear_speed", default_value="0.8", + description="Fake rover linear speed during the circle phase [m/s]" + ) + rover_start_delay_arg = DeclareLaunchArgument( + "rover_start_delay", default_value="3.0", + description="Seconds the fake rover stays still before it starts moving" + ) + rover_straight_duration_arg = DeclareLaunchArgument( + "rover_straight_duration", default_value="45.0", + description="Seconds the fake rover drives straight before circling [s]" + ) + rover_circle_radius_arg = DeclareLaunchArgument( + "rover_circle_radius", default_value="5.0", + description="Fake rover circling radius, minimum 5m [m]" + ) + rover_num_circle_turns_arg = DeclareLaunchArgument( + "rover_num_circle_turns", default_value="2.0", + description="Number of full turns the fake rover circles before holding" + ) + + fake_rover_pose = Node( + package="sar_modes", + executable="fake_rover_mover.py", + name="fake_rover_mover", + output="screen", + parameters=[ + { + "start_x": _float_param("rover_start_x"), + "start_y": _float_param("rover_start_y"), + "start_yaw_deg": _float_param("rover_start_yaw_deg"), + "min_linear_speed": _float_param("rover_min_linear_speed"), + "max_linear_speed": _float_param("rover_max_linear_speed"), + "start_delay": _float_param("rover_start_delay"), + "straight_duration": _float_param("rover_straight_duration"), + "circle_radius": _float_param("rover_circle_radius"), + "num_circle_turns": _float_param("rover_num_circle_turns"), + } + ] + ) + + total_drones = 3 + + 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") + + gz_world = IncludeLaunchDescription( + PythonLaunchDescriptionSource(gz_world_launch), + launch_arguments={ + "px4_autopilot_path": LaunchConfiguration("px4_autopilot_path"), + "world": LaunchConfiguration("world"), + }.items(), + ) + + # PX4 multi-vehicle instances are 1-indexed in this repo's convention + # (see px4_roscon_workshop/px4_tf/README.md: -i 1 -> /px4_1/..., -i 2 -> /px4_2/...), + # but the formation math in SARMode.cpp centers around a 0-indexed drone_id. + # So the ROS namespace uses the PX4 instance id, while the drone_id + # parameter passed to the node stays 0-indexed. + px4_vehicles = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(px4_vehicle_launch), + launch_arguments={ + "px4_autopilot_path": LaunchConfiguration("px4_autopilot_path"), + "world": LaunchConfiguration("world"), + "px4_instance": str(instance_id), + "model": f"x500_{instance_id}", + "px4_ns": f"px4_{instance_id}", + "spawn_pos_x": str(DRONE_SPAWN_POSITIONS[instance_id - 1][0]), + "spawn_pos_y": str(DRONE_SPAWN_POSITIONS[instance_id - 1][1]), + "px4_extra_env_vars": ( + "PX4_PARAM_COM_RCL_EXCEPT=9," + f"PX4_PARAM_COM_RC_IN_MODE={1 if instance_id == 1 else 4}," + "PX4_GZ_NO_FOLLOW=1" + ), + }.items(), + ) + for instance_id in range(1, total_drones + 1) + ] + + drone_nodes = [ + Node( + package="sar_modes", + executable="sar_modes", + name="sar_modes", + namespace=f"px4_{instance_id}", + output="screen", + parameters=[ + { + "use_sim_time": True, + "drone_id": instance_id - 1, + "total_drones": total_drones, + } + ] + ) + for instance_id in range(1, total_drones + 1) + ] + + return LaunchDescription([ + px4_autopilot_path_arg, + world_arg, + rover_x_arg, + rover_y_arg, + rover_yaw_arg, + rover_min_speed_arg, + rover_max_speed_arg, + rover_start_delay_arg, + rover_straight_duration_arg, + rover_circle_radius_arg, + rover_num_circle_turns_arg, + gz_world, + *px4_vehicles, + *drone_nodes, + fake_rover_pose, + ]) diff --git a/px4_roscon_workshop/sar_modes/package.xml b/px4_roscon_workshop/sar_modes/package.xml new file mode 100644 index 0000000..40d216c --- /dev/null +++ b/px4_roscon_workshop/sar_modes/package.xml @@ -0,0 +1,32 @@ + + + + sar_modes + 0.0.1 + ROS 2 SAR Swarm Tracking Workshop. + + Alexis Guijarro + CC-BY-SA-4.0 + + ament_cmake + + + rclcpp + px4_ros2_cpp + eigen3_cmake_module + geometry_msgs + tf2_ros + + launch + launch_ros + rclpy + px4_roscon_workshop + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/px4_roscon_workshop/sar_modes/scripts/fake_rover_mover.py b/px4_roscon_workshop/sar_modes/scripts/fake_rover_mover.py new file mode 100755 index 0000000..c54d94a --- /dev/null +++ b/px4_roscon_workshop/sar_modes/scripts/fake_rover_mover.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 +"""Fake rover pose publisher for testing sar_modes without a rover model. + +Publishes a moving PoseStamped on /rover/pose (ROS ENU): stands still for +start_delay seconds once at startup, then loops straight-line driving at +min_linear_speed (straight_duration seconds) and circling at max_linear_speed +(circle_radius, num_circle_turns turns) back to back until the node is +stopped, the speed change between phases lets a speed-based mode switcher +(e.g. sar_auto_executor) actually exercise both sides of its threshold in a +single run, rather than needing a relaunch with a different speed. +""" +import math + +import rclpy +from geometry_msgs.msg import PoseStamped +from rclpy.node import Node + + +class FakeRoverMover(Node): + + def __init__(self): + super().__init__('fake_rover_mover') + + self.declare_parameter('start_x', 5.0) + self.declare_parameter('start_y', 3.0) + self.declare_parameter('start_yaw_deg', 45.0) + self.declare_parameter('min_linear_speed', 0.2) # m/s, used during the straight phase + self.declare_parameter('max_linear_speed', 0.8) # m/s, used during the circle phase + self.declare_parameter('start_delay', 3.0) # seconds standing still before motion begins + self.declare_parameter('straight_duration', 45.0) # seconds driving straight (30-60s is a good range, hopefully) + self.declare_parameter('circle_radius', 5.0) # meters, clamped to a 5m minimum + self.declare_parameter('num_circle_turns', 2.0) # full 360 deg turns before holding + self.declare_parameter('rate_hz', 10.0) + + self._x = self.get_parameter('start_x').value + self._y = self.get_parameter('start_y').value + self._yaw = math.radians(self.get_parameter('start_yaw_deg').value) + self._min_speed = self.get_parameter('min_linear_speed').value + self._max_speed = self.get_parameter('max_linear_speed').value + self._start_delay = self.get_parameter('start_delay').value + self._straight_duration = self.get_parameter('straight_duration').value + self._circle_radius = max(self.get_parameter('circle_radius').value, 5.0) + self._num_circle_turns = self.get_parameter('num_circle_turns').value + rate_hz = self.get_parameter('rate_hz').value + + self._dt = 1.0 / rate_hz + + # Angular rate that makes the circle phase trace circle_radius at max_linear_speed. + self._circle_omega = self._max_speed / self._circle_radius if self._max_speed > 0.0 else 0.0 + self._circle_duration = ( + self._num_circle_turns * (2.0 * math.pi / self._circle_omega) + if self._circle_omega > 0.0 else 0.0 + ) + + # 'hold' happens once at startup; 'straight' <-> 'circle' then loop forever. + self._phase = 'hold' + self._phase_elapsed = 0.0 + + self._pub = self.create_publisher(PoseStamped, '/rover/pose', 10) + self._timer = self.create_timer(self._dt, self._on_timer) + + self.get_logger().info( + f'Fake rover starting at ({self._x:.1f}, {self._y:.1f}), yaw {math.degrees(self._yaw):.0f} deg. ' + f'Holds {self._start_delay:.1f}s, then loops: straight at {self._min_speed} m/s for ' + f'{self._straight_duration:.1f}s, circle at {self._max_speed} m/s (r={self._circle_radius:.1f}m) ' + f'for {self._num_circle_turns:.1f} turns, repeat.' + ) + + def _on_timer(self): + self._phase_elapsed += self._dt + + if self._phase == 'hold': + if self._phase_elapsed >= self._start_delay: + self._phase, self._phase_elapsed = 'straight', 0.0 + elif self._phase == 'straight': + self._x += self._min_speed * math.cos(self._yaw) * self._dt + self._y += self._min_speed * math.sin(self._yaw) * self._dt + if self._phase_elapsed >= self._straight_duration: + self._phase, self._phase_elapsed = 'circle', 0.0 + elif self._phase == 'circle': + self._yaw += self._circle_omega * self._dt + self._x += self._max_speed * math.cos(self._yaw) * self._dt + self._y += self._max_speed * math.sin(self._yaw) * self._dt + if self._phase_elapsed >= self._circle_duration: + self._phase, self._phase_elapsed = 'straight', 0.0 + + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'map' + msg.pose.position.x = self._x + msg.pose.position.y = self._y + msg.pose.position.z = 0.0 + msg.pose.orientation.z = math.sin(self._yaw / 2.0) + msg.pose.orientation.w = math.cos(self._yaw / 2.0) + self._pub.publish(msg) + + +def main(): + rclpy.init() + node = FakeRoverMover() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main()