From cbd114ba565c14a320b8a529d3ac01a18eecf0d7 Mon Sep 17 00:00:00 2001 From: aborjigin Date: Tue, 28 Apr 2026 17:24:16 -0700 Subject: [PATCH 1/2] initial controls program --- CMakeLists.txt | 64 +++++++ README.md | 80 +++++++- config/params.yaml | 31 ++++ include/somars_controls/guidance_node.hpp | 68 +++++++ include/somars_controls/offboard_manager.hpp | 74 ++++++++ include/somars_controls/target_localizer.hpp | 71 ++++++++ launch/controls.launch.py | 35 ++++ package.xml | 33 ++++ src/guidance_node.cpp | 181 +++++++++++++++++++ src/offboard_manager.cpp | 171 ++++++++++++++++++ src/target_localizer.cpp | 158 ++++++++++++++++ 11 files changed, 965 insertions(+), 1 deletion(-) create mode 100644 CMakeLists.txt create mode 100644 config/params.yaml create mode 100644 include/somars_controls/guidance_node.hpp create mode 100644 include/somars_controls/offboard_manager.hpp create mode 100644 include/somars_controls/target_localizer.hpp create mode 100644 launch/controls.launch.py create mode 100644 package.xml create mode 100644 src/guidance_node.cpp create mode 100644 src/offboard_manager.cpp create mode 100644 src/target_localizer.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..9d617f6 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.8) +project(somars_controls) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(px4_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories( + include + ${EIGEN3_INCLUDE_DIRS} +) + +# --------------------------------------------------------------------------- +# Nodes +# --------------------------------------------------------------------------- + +add_executable(target_localizer src/target_localizer.cpp) +ament_target_dependencies(target_localizer + rclcpp px4_msgs geometry_msgs std_msgs sensor_msgs) +target_link_libraries(target_localizer Eigen3::Eigen) + +add_executable(guidance_node src/guidance_node.cpp) +ament_target_dependencies(guidance_node + rclcpp px4_msgs geometry_msgs std_msgs) +target_link_libraries(guidance_node Eigen3::Eigen) + +add_executable(offboard_manager src/offboard_manager.cpp) +ament_target_dependencies(offboard_manager + rclcpp px4_msgs std_msgs) + +# --------------------------------------------------------------------------- +# Install +# --------------------------------------------------------------------------- + +install(TARGETS + target_localizer + guidance_node + offboard_manager + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/README.md b/README.md index 74b5c3c..8522936 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,80 @@ # somars-embedded -Micro-XRCE-DDS as a strictly transparent pipeline. Turn on Agent via the CLI. Here, actual control logic in standard ROS 2 (Python or C++). + +ROS 2 C++ controls package for the SOMARS autonomous drone (C-UASC competition). + +Micro-XRCE-DDS serves as the transparent bridge between PX4 (Pixhawk 6X) and +these ROS 2 nodes running on the Jetson Orin Nano (JetPack 6.2.2). Start the +Agent via the CLI; this package contains only the control logic. + +## Architecture + +``` +Camera ──► somars-vision (detect targets, publish pixel coords) + │ /vision/detections [PoseArray] + ▼ + target_localizer (pixel → NED projection) + │ /targets/ned [PointStamped] + ▼ + guidance_node (compute trajectory setpoints) + │ /fmu/in/trajectory_setpoint + ▼ + micro-XRCE-DDS Agent ──► PX4 (Pixhawk 6X) + ▲ + offboard_manager (arm / mode switch / heartbeat) + │ /fmu/in/vehicle_command + │ /fmu/in/offboard_control_mode +``` + +## Nodes + +| Node | Description | +|------|-------------| +| `target_localizer` | Subscribes to vision detections + vehicle pose. Projects pixel detections through the camera model and intersects with the ground plane to produce NED target positions. | +| `guidance_node` | Subscribes to target NED positions. Generates `TrajectorySetpoint` messages (position mode) to fly toward the target. Holds position when no target is active. | +| `offboard_manager` | Maintains the PX4 offboard heartbeat (`OffboardControlMode` + hold setpoint). Optionally arms and switches to offboard mode. | + +## Quick Start + +```bash +# 1. Start the micro-XRCE-DDS Agent (serial example) +MicroXRCEAgent serial --dev /dev/ttyTHS1 -b 921600 + +# 2. Build this package inside your colcon workspace +cd ~/somars_ws +colcon build --packages-select somars_controls +source install/setup.bash + +# 3. Launch all controls nodes +ros2 launch somars_controls controls.launch.py +``` + +## Parameters + +All tunables live in `config/params.yaml`. Override on the CLI: + +```bash +ros2 launch somars_controls controls.launch.py \ + --ros-args -p guidance_node:approach_speed:=3.0 +``` + +## Workspace Integration + +This package is intended to be used as a submodule inside a ROS 2 colcon +workspace alongside `somars-vision`: + +``` +somars-ws/ +├── src/ +│ ├── somars_controls/ ← this repo (git submodule) +│ ├── somars_vision/ ← slugbotics/somars-vision (git submodule) +│ └── px4_msgs/ ← PX4/px4_msgs (git submodule) +├── launch/ ← system-level launch files +└── docker/ ← Jetson container setup +``` + +## Dependencies + +- ROS 2 Humble / Jazzy +- [px4_msgs](https://github.com/PX4/px4_msgs) – PX4 message definitions +- Eigen3 +- micro-XRCE-DDS Agent (run separately) diff --git a/config/params.yaml b/config/params.yaml new file mode 100644 index 0000000..c944143 --- /dev/null +++ b/config/params.yaml @@ -0,0 +1,31 @@ +# ============================================================================= +# SOMARS Controls – Runtime Parameters +# ============================================================================= +# Load via: ros2 launch somars_controls controls.launch.py +# Override on CLI: --ros-args -p camera_fx:=800.0 + +target_localizer: + ros__parameters: + # Camera intrinsics – replace with values from your calibration + camera_fx: 600.0 + camera_fy: 600.0 + camera_cx: 320.0 + camera_cy: 240.0 + + # Camera mount pitch relative to body frame (radians) + # 0 → forward-facing + # -π/2 → straight down (typical for survey) + camera_pitch_rad: -1.5708 + +guidance_node: + ros__parameters: + approach_speed: 2.0 # m/s – max horizontal approach speed + acceptance_radius: 1.0 # m – "close enough" to target + loiter_altitude: -10.0 # NED z (negative = above ground), 10 m AGL + control_rate_hz: 20.0 # Hz – main control loop frequency + +offboard_manager: + ros__parameters: + heartbeat_rate_hz: 10.0 # Hz – OffboardControlMode publish rate + auto_arm: false # !! Set true ONLY for bench/sim testing !! + arm_delay_s: 2.0 # Seconds of valid heartbeat before arm attempt diff --git a/include/somars_controls/guidance_node.hpp b/include/somars_controls/guidance_node.hpp new file mode 100644 index 0000000..8104fac --- /dev/null +++ b/include/somars_controls/guidance_node.hpp @@ -0,0 +1,68 @@ +#ifndef SOMARS_CONTROLS__GUIDANCE_NODE_HPP_ +#define SOMARS_CONTROLS__GUIDANCE_NODE_HPP_ + +#include +#include +#include +#include +#include + +#include + +namespace somars_controls +{ + +/// Generates trajectory setpoints that steer the drone towards detected +/// targets. When no target is active the node publishes a position-hold +/// at the current loiter altitude. +/// +/// Subscriptions +/// /targets/ned – PointStamped (from TargetLocalizer) +/// /fmu/out/vehicle_local_position – VehicleLocalPosition (PX4) +/// +/// Publications +/// /fmu/in/trajectory_setpoint – TrajectorySetpoint (to PX4) +/// /fmu/in/offboard_control_mode – OffboardControlMode (to PX4) +class GuidanceNode : public rclcpp::Node +{ +public: + GuidanceNode(); + +private: + // ---- callbacks ---- + void target_cb(const geometry_msgs::msg::PointStamped::SharedPtr msg); + void local_position_cb(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg); + + /// Fixed-rate control loop called by the timer. + void control_loop(); + + // ---- subscribers ---- + rclcpp::Subscription::SharedPtr target_sub_; + rclcpp::Subscription::SharedPtr local_pos_sub_; + + // ---- publishers ---- + rclcpp::Publisher::SharedPtr setpoint_pub_; + rclcpp::Publisher::SharedPtr control_mode_pub_; + + // ---- timer ---- + rclcpp::TimerBase::SharedPtr control_timer_; + + // ---- state ---- + Eigen::Vector3d vehicle_position_ned_{0.0, 0.0, 0.0}; + Eigen::Vector3d target_position_ned_{0.0, 0.0, 0.0}; + bool target_received_ = false; + bool position_received_ = false; + + /// Timestamp (steady clock) of the last target message. Used to expire + /// stale targets. + rclcpp::Time last_target_time_; + + // ---- parameters ---- + double approach_speed_; + double acceptance_radius_; + double loiter_altitude_; // NED z – negative means above ground +}; + +} // namespace somars_controls + +#endif // SOMARS_CONTROLS__GUIDANCE_NODE_HPP_ diff --git a/include/somars_controls/offboard_manager.hpp b/include/somars_controls/offboard_manager.hpp new file mode 100644 index 0000000..54b7fc6 --- /dev/null +++ b/include/somars_controls/offboard_manager.hpp @@ -0,0 +1,74 @@ +#ifndef SOMARS_CONTROLS__OFFBOARD_MANAGER_HPP_ +#define SOMARS_CONTROLS__OFFBOARD_MANAGER_HPP_ + +#include +#include +#include +#include +#include + +namespace somars_controls +{ + +/// Manages the PX4 offboard-mode lifecycle. +/// +/// Responsibilities: +/// 1. Publish OffboardControlMode + a hold-position TrajectorySetpoint at a +/// steady heartbeat rate so PX4 accepts/maintains offboard mode. +/// 2. Optionally send arm and mode-switch VehicleCommands after a +/// configurable delay (guarded by the `auto_arm` parameter). +/// 3. Monitor VehicleStatus to track arming and nav state. +/// +/// Subscriptions +/// /fmu/out/vehicle_status – VehicleStatus (PX4) +/// +/// Publications +/// /fmu/in/offboard_control_mode – OffboardControlMode +/// /fmu/in/trajectory_setpoint – TrajectorySetpoint (hold setpoint) +/// /fmu/in/vehicle_command – VehicleCommand +class OffboardManager : public rclcpp::Node +{ +public: + OffboardManager(); + +private: + // ---- callbacks ---- + void status_cb(const px4_msgs::msg::VehicleStatus::SharedPtr msg); + + /// Timer-driven heartbeat that keeps offboard mode alive. + void heartbeat_loop(); + + // ---- command helpers ---- + void publish_vehicle_command( + uint16_t command, + float param1 = 0.0f, + float param2 = 0.0f); + void arm(); + void disarm(); + void set_offboard_mode(); + + // ---- subscribers ---- + rclcpp::Subscription::SharedPtr status_sub_; + + // ---- publishers ---- + rclcpp::Publisher::SharedPtr control_mode_pub_; + rclcpp::Publisher::SharedPtr setpoint_pub_; + rclcpp::Publisher::SharedPtr command_pub_; + + // ---- timer ---- + rclcpp::TimerBase::SharedPtr heartbeat_timer_; + + // ---- state ---- + uint8_t nav_state_ = 0; + uint8_t arming_state_ = 0; + int heartbeat_count_ = 0; + + // ---- parameters ---- + bool auto_arm_; + double arm_delay_s_; + double heartbeat_rate_hz_; +}; + +} // namespace somars_controls + +#endif // SOMARS_CONTROLS__OFFBOARD_MANAGER_HPP_ diff --git a/include/somars_controls/target_localizer.hpp b/include/somars_controls/target_localizer.hpp new file mode 100644 index 0000000..8821b2f --- /dev/null +++ b/include/somars_controls/target_localizer.hpp @@ -0,0 +1,71 @@ +#ifndef SOMARS_CONTROLS__TARGET_LOCALIZER_HPP_ +#define SOMARS_CONTROLS__TARGET_LOCALIZER_HPP_ + +#include +#include +#include +#include +#include + +#include + +namespace somars_controls +{ + +/// Projects pixel-space detections from the vision pipeline into NED-frame +/// world coordinates using the drone's attitude, position and camera model. +/// +/// Subscriptions +/// /fmu/out/vehicle_attitude – VehicleAttitude (PX4) +/// /fmu/out/vehicle_local_position – VehicleLocalPosition (PX4) +/// /vision/detections – PoseArray (from somars-vision) +/// Each Pose encodes: position.x = pixel u +/// position.y = pixel v +/// position.z = class id (0=red, 1=black, 2=white) +/// +/// Publications +/// /targets/ned – PointStamped (target position in local NED frame) +class TargetLocalizer : public rclcpp::Node +{ +public: + TargetLocalizer(); + +private: + // ---- callbacks ---- + void attitude_cb(const px4_msgs::msg::VehicleAttitude::SharedPtr msg); + void local_position_cb(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg); + void detection_cb(const geometry_msgs::msg::PoseArray::SharedPtr msg); + + // ---- helpers ---- + /// Back-project a single pixel (u, v) through the camera model, rotate into + /// NED, and intersect with the ground plane (z = 0). + /// Returns false if the ray does not intersect the ground. + bool pixel_to_ned(double u, double v, Eigen::Vector3d & target_ned) const; + + /// Convert quaternion to rotation matrix (Hamilton convention, wxyz). + static Eigen::Matrix3d quat_to_rotation(double w, double x, double y, double z); + + // ---- subscribers ---- + rclcpp::Subscription::SharedPtr attitude_sub_; + rclcpp::Subscription::SharedPtr local_pos_sub_; + rclcpp::Subscription::SharedPtr detection_sub_; + + // ---- publishers ---- + rclcpp::Publisher::SharedPtr target_ned_pub_; + + // ---- state ---- + Eigen::Quaterniond vehicle_attitude_{1.0, 0.0, 0.0, 0.0}; + Eigen::Vector3d vehicle_position_ned_{0.0, 0.0, 0.0}; + bool attitude_received_ = false; + bool position_received_ = false; + + // ---- camera parameters (loaded from params.yaml) ---- + double fx_, fy_, cx_, cy_; + + /// Fixed rotation from camera optical frame to body FRD frame. + Eigen::Matrix3d R_cam_to_body_; +}; + +} // namespace somars_controls + +#endif // SOMARS_CONTROLS__TARGET_LOCALIZER_HPP_ diff --git a/launch/controls.launch.py b/launch/controls.launch.py new file mode 100644 index 0000000..f031274 --- /dev/null +++ b/launch/controls.launch.py @@ -0,0 +1,35 @@ +"""Launch all SOMARS controls nodes with shared parameters.""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_share = get_package_share_directory('somars_controls') + params_file = os.path.join(pkg_share, 'config', 'params.yaml') + + return LaunchDescription([ + Node( + package='somars_controls', + executable='offboard_manager', + name='offboard_manager', + parameters=[params_file], + output='screen', + ), + Node( + package='somars_controls', + executable='guidance_node', + name='guidance_node', + parameters=[params_file], + output='screen', + ), + Node( + package='somars_controls', + executable='target_localizer', + name='target_localizer', + parameters=[params_file], + output='screen', + ), + ]) diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..3190105 --- /dev/null +++ b/package.xml @@ -0,0 +1,33 @@ + + + + somars_controls + 0.1.0 + + SOMARS autonomous drone controls for the C-UASC competition. + Provides target localisation (pixel-to-NED projection), guidance + (trajectory setpoint generation), and offboard lifecycle management + for a Pixhawk 6X running PX4 via micro-XRCE-DDS. + + + SOMARS Team + MIT + + ament_cmake + + rclcpp + px4_msgs + geometry_msgs + std_msgs + sensor_msgs + + eigen + eigen3_cmake_module + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/guidance_node.cpp b/src/guidance_node.cpp new file mode 100644 index 0000000..7b60be3 --- /dev/null +++ b/src/guidance_node.cpp @@ -0,0 +1,181 @@ +#include "somars_controls/guidance_node.hpp" + +#include +#include +#include + +using namespace std::chrono_literals; + +namespace somars_controls +{ + +GuidanceNode::GuidanceNode() +: Node("guidance_node"), + last_target_time_(this->now()) +{ + // ---- declare & load parameters ---- + this->declare_parameter("approach_speed", 2.0); + this->declare_parameter("acceptance_radius", 1.0); + this->declare_parameter("loiter_altitude", -10.0); + this->declare_parameter("control_rate_hz", 20.0); + + approach_speed_ = this->get_parameter("approach_speed").as_double(); + acceptance_radius_ = this->get_parameter("acceptance_radius").as_double(); + loiter_altitude_ = this->get_parameter("loiter_altitude").as_double(); + double rate_hz = this->get_parameter("control_rate_hz").as_double(); + + RCLCPP_INFO(this->get_logger(), + "Guidance: speed=%.1f m/s radius=%.1f m alt=%.1f m rate=%.0f Hz", + approach_speed_, acceptance_radius_, loiter_altitude_, rate_hz); + + // ---- QoS ---- + auto px4_qos = rclcpp::SensorDataQoS(); + + // ---- subscriptions ---- + target_sub_ = this->create_subscription( + "/targets/ned", 10, + std::bind(&GuidanceNode::target_cb, this, std::placeholders::_1)); + + local_pos_sub_ = this->create_subscription( + "/fmu/out/vehicle_local_position", px4_qos, + std::bind(&GuidanceNode::local_position_cb, this, std::placeholders::_1)); + + // ---- publishers (to PX4 via micro-XRCE-DDS) ---- + setpoint_pub_ = this->create_publisher( + "/fmu/in/trajectory_setpoint", 10); + + control_mode_pub_ = this->create_publisher( + "/fmu/in/offboard_control_mode", 10); + + // ---- control loop timer ---- + auto period = std::chrono::duration(1.0 / rate_hz); + control_timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&GuidanceNode::control_loop, this)); + + RCLCPP_INFO(this->get_logger(), "GuidanceNode started"); +} + +// --------------------------------------------------------------------------- +// Callbacks +// --------------------------------------------------------------------------- + +void GuidanceNode::target_cb( + const geometry_msgs::msg::PointStamped::SharedPtr msg) +{ + target_position_ned_ = Eigen::Vector3d(msg->point.x, msg->point.y, msg->point.z); + target_received_ = true; + last_target_time_ = this->now(); +} + +void GuidanceNode::local_position_cb( + const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) +{ + vehicle_position_ned_ = Eigen::Vector3d(msg->x, msg->y, msg->z); + position_received_ = true; +} + +// --------------------------------------------------------------------------- +// Control loop +// --------------------------------------------------------------------------- + +void GuidanceNode::control_loop() +{ + // -- Always publish OffboardControlMode so PX4 stays in offboard ---------- + px4_msgs::msg::OffboardControlMode mode_msg{}; + mode_msg.position = true; + mode_msg.velocity = false; + mode_msg.acceleration = false; + mode_msg.attitude = false; + mode_msg.body_rate = false; + mode_msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; // µs + control_mode_pub_->publish(mode_msg); + + // -- Compute setpoint ----------------------------------------------------- + px4_msgs::msg::TrajectorySetpoint sp{}; + sp.timestamp = mode_msg.timestamp; + + // NaN = "don't care" for PX4 + sp.velocity[0] = std::numeric_limits::quiet_NaN(); + sp.velocity[1] = std::numeric_limits::quiet_NaN(); + sp.velocity[2] = std::numeric_limits::quiet_NaN(); + sp.acceleration[0] = std::numeric_limits::quiet_NaN(); + sp.acceleration[1] = std::numeric_limits::quiet_NaN(); + sp.acceleration[2] = std::numeric_limits::quiet_NaN(); + sp.jerk[0] = std::numeric_limits::quiet_NaN(); + sp.jerk[1] = std::numeric_limits::quiet_NaN(); + sp.jerk[2] = std::numeric_limits::quiet_NaN(); + sp.yaw = std::numeric_limits::quiet_NaN(); // hold current + sp.yawspeed = std::numeric_limits::quiet_NaN(); + + if (!position_received_) { + // Nothing we can do until we know where we are. + setpoint_pub_->publish(sp); + return; + } + + // Expire targets older than 2 seconds + bool target_stale = (this->now() - last_target_time_).seconds() > 2.0; + + if (target_received_ && !target_stale) { + // ---- Navigate towards target ---- + Eigen::Vector3d to_target = target_position_ned_ - vehicle_position_ned_; + + // Keep altitude at loiter_altitude_, only move horizontally toward target + to_target.z() = 0.0; + double dist = to_target.norm(); + + Eigen::Vector3d desired_pos; + if (dist < acceptance_radius_) { + // Close enough – hold above the target at loiter altitude + desired_pos = Eigen::Vector3d( + target_position_ned_.x(), + target_position_ned_.y(), + loiter_altitude_); + + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "Within acceptance radius (%.1f m) – holding position", dist); + } else { + // Move towards target at approach_speed_, staying at loiter altitude + Eigen::Vector3d direction = to_target.normalized(); + double step = std::min(approach_speed_ * 0.5, dist); // half-second lookahead + desired_pos = vehicle_position_ned_ + direction * step; + desired_pos.z() = loiter_altitude_; + + RCLCPP_DEBUG(this->get_logger(), + "Approaching target: dist=%.1f m setpoint=[%.1f, %.1f, %.1f]", + dist, desired_pos.x(), desired_pos.y(), desired_pos.z()); + } + + sp.position[0] = static_cast(desired_pos.x()); + sp.position[1] = static_cast(desired_pos.y()); + sp.position[2] = static_cast(desired_pos.z()); + + } else { + // ---- No active target – hold current XY at loiter altitude ---- + sp.position[0] = static_cast(vehicle_position_ned_.x()); + sp.position[1] = static_cast(vehicle_position_ned_.y()); + sp.position[2] = static_cast(loiter_altitude_); + + if (target_stale && target_received_) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "Target data stale – holding position"); + } + } + + setpoint_pub_->publish(sp); +} + +} // namespace somars_controls + +// --------------------------------------------------------------------------- +// main +// --------------------------------------------------------------------------- + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/offboard_manager.cpp b/src/offboard_manager.cpp new file mode 100644 index 0000000..9d1a5e2 --- /dev/null +++ b/src/offboard_manager.cpp @@ -0,0 +1,171 @@ +#include "somars_controls/offboard_manager.hpp" + +#include + +using namespace std::chrono_literals; + +namespace somars_controls +{ + +OffboardManager::OffboardManager() +: Node("offboard_manager") +{ + // ---- declare & load parameters ---- + this->declare_parameter("heartbeat_rate_hz", 10.0); + this->declare_parameter("auto_arm", false); + this->declare_parameter("arm_delay_s", 2.0); + + heartbeat_rate_hz_ = this->get_parameter("heartbeat_rate_hz").as_double(); + auto_arm_ = this->get_parameter("auto_arm").as_bool(); + arm_delay_s_ = this->get_parameter("arm_delay_s").as_double(); + + if (auto_arm_) { + RCLCPP_WARN(this->get_logger(), + "auto_arm is ENABLED – drone will arm automatically after %.1f s", arm_delay_s_); + } + + // ---- QoS ---- + auto px4_qos = rclcpp::SensorDataQoS(); + + // ---- subscriptions ---- + status_sub_ = this->create_subscription( + "/fmu/out/vehicle_status", px4_qos, + std::bind(&OffboardManager::status_cb, this, std::placeholders::_1)); + + // ---- publishers ---- + control_mode_pub_ = this->create_publisher( + "/fmu/in/offboard_control_mode", 10); + + setpoint_pub_ = this->create_publisher( + "/fmu/in/trajectory_setpoint", 10); + + command_pub_ = this->create_publisher( + "/fmu/in/vehicle_command", 10); + + // ---- heartbeat timer ---- + auto period = std::chrono::duration(1.0 / heartbeat_rate_hz_); + heartbeat_timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&OffboardManager::heartbeat_loop, this)); + + RCLCPP_INFO(this->get_logger(), + "OffboardManager started – heartbeat at %.0f Hz", heartbeat_rate_hz_); +} + +// --------------------------------------------------------------------------- +// Callbacks +// --------------------------------------------------------------------------- + +void OffboardManager::status_cb( + const px4_msgs::msg::VehicleStatus::SharedPtr msg) +{ + nav_state_ = msg->nav_state; + arming_state_ = msg->arming_state; +} + +// --------------------------------------------------------------------------- +// Heartbeat +// --------------------------------------------------------------------------- + +void OffboardManager::heartbeat_loop() +{ + uint64_t ts = this->get_clock()->now().nanoseconds() / 1000; // µs + + // 1. OffboardControlMode – must be published continuously + px4_msgs::msg::OffboardControlMode mode{}; + mode.position = true; + mode.velocity = false; + mode.acceleration = false; + mode.attitude = false; + mode.body_rate = false; + mode.timestamp = ts; + control_mode_pub_->publish(mode); + + // 2. Hold-position setpoint (keeps PX4 happy while no guidance is active). + // The guidance_node will overwrite this once it starts publishing. + px4_msgs::msg::TrajectorySetpoint sp{}; + sp.timestamp = ts; + sp.position[0] = 0.0f; + sp.position[1] = 0.0f; + sp.position[2] = -10.0f; // 10 m AGL (NED) + sp.yaw = 0.0f; + setpoint_pub_->publish(sp); + + heartbeat_count_++; + + // 3. Auto-arm sequence (only if enabled) + if (!auto_arm_) { + return; + } + + int required_beats = static_cast(arm_delay_s_ * heartbeat_rate_hz_); + + if (heartbeat_count_ == required_beats) { + RCLCPP_INFO(this->get_logger(), "Requesting OFFBOARD mode"); + set_offboard_mode(); + } + + if (heartbeat_count_ == required_beats + 1) { + RCLCPP_INFO(this->get_logger(), "Sending ARM command"); + arm(); + } +} + +// --------------------------------------------------------------------------- +// Vehicle commands +// --------------------------------------------------------------------------- + +void OffboardManager::publish_vehicle_command( + uint16_t command, float param1, float param2) +{ + px4_msgs::msg::VehicleCommand msg{}; + msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; + msg.param1 = param1; + msg.param2 = param2; + msg.command = command; + msg.target_system = 1; + msg.target_component = 1; + msg.source_system = 1; + msg.source_component = 1; + msg.from_external = true; + command_pub_->publish(msg); +} + +void OffboardManager::arm() +{ + publish_vehicle_command( + px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, + 1.0f); + RCLCPP_INFO(this->get_logger(), "ARM command sent"); +} + +void OffboardManager::disarm() +{ + publish_vehicle_command( + px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, + 0.0f); + RCLCPP_INFO(this->get_logger(), "DISARM command sent"); +} + +void OffboardManager::set_offboard_mode() +{ + publish_vehicle_command( + px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, + 1.0f, + 6.0f); // 6 = PX4 custom mode for offboard + RCLCPP_INFO(this->get_logger(), "OFFBOARD mode command sent"); +} + +} // namespace somars_controls + +// --------------------------------------------------------------------------- +// main +// --------------------------------------------------------------------------- + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/target_localizer.cpp b/src/target_localizer.cpp new file mode 100644 index 0000000..60f4ee3 --- /dev/null +++ b/src/target_localizer.cpp @@ -0,0 +1,158 @@ +#include "somars_controls/target_localizer.hpp" + +#include + +namespace somars_controls +{ + +TargetLocalizer::TargetLocalizer() +: Node("target_localizer") +{ + // ---- declare & load parameters ---- + this->declare_parameter("camera_fx", 600.0); + this->declare_parameter("camera_fy", 600.0); + this->declare_parameter("camera_cx", 320.0); + this->declare_parameter("camera_cy", 240.0); + this->declare_parameter("camera_pitch_rad", -M_PI_2); + + fx_ = this->get_parameter("camera_fx").as_double(); + fy_ = this->get_parameter("camera_fy").as_double(); + cx_ = this->get_parameter("camera_cx").as_double(); + cy_ = this->get_parameter("camera_cy").as_double(); + double cam_pitch = this->get_parameter("camera_pitch_rad").as_double(); + + // Build the fixed camera-to-body rotation. + // Assumes the camera optical axis is mounted in the body XZ-plane, + // pitched by cam_pitch about the body Y-axis. + // body X = forward, Y = right, Z = down (FRD) + // camera Z = optical axis (into scene) + R_cam_to_body_ = Eigen::AngleAxisd(cam_pitch, Eigen::Vector3d::UnitY()).toRotationMatrix(); + + RCLCPP_INFO(this->get_logger(), + "Camera intrinsics fx=%.1f fy=%.1f cx=%.1f cy=%.1f pitch=%.2f rad", + fx_, fy_, cx_, cy_, cam_pitch); + + // ---- QoS for PX4 topics (best-effort / volatile) ---- + auto px4_qos = rclcpp::SensorDataQoS(); + + // ---- subscriptions ---- + attitude_sub_ = this->create_subscription( + "/fmu/out/vehicle_attitude", px4_qos, + std::bind(&TargetLocalizer::attitude_cb, this, std::placeholders::_1)); + + local_pos_sub_ = this->create_subscription( + "/fmu/out/vehicle_local_position", px4_qos, + std::bind(&TargetLocalizer::local_position_cb, this, std::placeholders::_1)); + + // Vision detections – uses default reliable QoS (local ROS2 topic) + detection_sub_ = this->create_subscription( + "/vision/detections", 10, + std::bind(&TargetLocalizer::detection_cb, this, std::placeholders::_1)); + + // ---- publishers ---- + target_ned_pub_ = this->create_publisher( + "/targets/ned", 10); + + RCLCPP_INFO(this->get_logger(), "TargetLocalizer node started"); +} + +// --------------------------------------------------------------------------- +// Callbacks +// --------------------------------------------------------------------------- + +void TargetLocalizer::attitude_cb( + const px4_msgs::msg::VehicleAttitude::SharedPtr msg) +{ + // PX4 quaternion order: [w, x, y, z] stored in q[0..3] + vehicle_attitude_ = Eigen::Quaterniond(msg->q[0], msg->q[1], msg->q[2], msg->q[3]); + attitude_received_ = true; +} + +void TargetLocalizer::local_position_cb( + const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) +{ + vehicle_position_ned_ = Eigen::Vector3d(msg->x, msg->y, msg->z); + position_received_ = true; +} + +void TargetLocalizer::detection_cb( + const geometry_msgs::msg::PoseArray::SharedPtr msg) +{ + if (!attitude_received_ || !position_received_) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "Waiting for vehicle attitude & position before localising targets"); + return; + } + + for (const auto & pose : msg->poses) { + double u = pose.position.x; // pixel column + double v = pose.position.y; // pixel row + // int class_id = static_cast(pose.position.z); // TODO: use class + + Eigen::Vector3d target_ned; + if (!pixel_to_ned(u, v, target_ned)) { + RCLCPP_DEBUG(this->get_logger(), "Ray did not intersect ground plane"); + continue; + } + + geometry_msgs::msg::PointStamped out; + out.header.stamp = this->now(); + out.header.frame_id = "map_ned"; + out.point.x = target_ned.x(); + out.point.y = target_ned.y(); + out.point.z = target_ned.z(); + target_ned_pub_->publish(out); + + RCLCPP_DEBUG(this->get_logger(), + "Target NED: [%.2f, %.2f, %.2f]", + target_ned.x(), target_ned.y(), target_ned.z()); + } +} + +// --------------------------------------------------------------------------- +// Projection math +// --------------------------------------------------------------------------- + +bool TargetLocalizer::pixel_to_ned( + double u, double v, Eigen::Vector3d & target_ned) const +{ + // 1. Pixel → normalised camera-frame ray + Eigen::Vector3d ray_cam((u - cx_) / fx_, (v - cy_) / fy_, 1.0); + ray_cam.normalize(); + + // 2. Camera frame → body frame → NED frame + Eigen::Matrix3d R_body_to_ned = vehicle_attitude_.toRotationMatrix(); + Eigen::Vector3d ray_ned = R_body_to_ned * R_cam_to_body_ * ray_cam; + + // 3. Intersect with ground plane (NED z = 0). + // Drone is at vehicle_position_ned_.z() (negative when above ground). + // We need the ray to point downward (positive z in NED) to hit ground. + if (ray_ned.z() <= 1e-6) { + return false; // ray is parallel to or pointing away from ground + } + + double t = -vehicle_position_ned_.z() / ray_ned.z(); + target_ned = vehicle_position_ned_ + t * ray_ned; + target_ned.z() = 0.0; // on the ground plane + return true; +} + +Eigen::Matrix3d TargetLocalizer::quat_to_rotation( + double w, double x, double y, double z) +{ + return Eigen::Quaterniond(w, x, y, z).normalized().toRotationMatrix(); +} + +} // namespace somars_controls + +// --------------------------------------------------------------------------- +// main +// --------------------------------------------------------------------------- + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From cffad2080a5a57e3c914ffdf0f9784a1dd8dda40 Mon Sep 17 00:00:00 2001 From: aborjigin Date: Fri, 1 May 2026 17:55:05 -0700 Subject: [PATCH 2/2] fixing issues and adding in waypoint navigation --- .gitignore | 4 + CMakeLists.txt | 6 + README.md | 146 ++++++---- config/waypoints.yaml | 52 ++++ include/somars_controls/guidance_node.hpp | 45 +++- include/somars_controls/offboard_manager.hpp | 9 + include/somars_controls/projection.hpp | 80 ++++++ launch/controls.launch.py | 5 +- src/guidance_node.cpp | 201 ++++++++++++-- src/offboard_manager.cpp | 23 +- src/target_localizer.cpp | 47 ++-- test/CMakeLists.txt | 24 ++ test/integration.launch.py | 63 +++++ test/mock_nodes.cpp | 150 +++++++++++ test/test_projection_math.cpp | 269 +++++++++++++++++++ 15 files changed, 1005 insertions(+), 119 deletions(-) create mode 100644 .gitignore create mode 100644 config/waypoints.yaml create mode 100644 include/somars_controls/projection.hpp create mode 100644 test/CMakeLists.txt create mode 100644 test/integration.launch.py create mode 100644 test/mock_nodes.cpp create mode 100644 test/test_projection_math.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d020176 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +build/ +install/ +log/ +test/build/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 9d617f6..149fa88 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,10 @@ add_executable(offboard_manager src/offboard_manager.cpp) ament_target_dependencies(offboard_manager rclcpp px4_msgs std_msgs) +add_executable(mock_nodes test/mock_nodes.cpp) +ament_target_dependencies(mock_nodes + rclcpp px4_msgs geometry_msgs std_msgs) + # --------------------------------------------------------------------------- # Install # --------------------------------------------------------------------------- @@ -47,12 +51,14 @@ install(TARGETS target_localizer guidance_node offboard_manager + mock_nodes DESTINATION lib/${PROJECT_NAME} ) install(DIRECTORY launch config + test DESTINATION share/${PROJECT_NAME} ) diff --git a/README.md b/README.md index 8522936..7ece575 100644 --- a/README.md +++ b/README.md @@ -1,80 +1,130 @@ # somars-embedded -ROS 2 C++ controls package for the SOMARS autonomous drone (C-UASC competition). +ROS 2 C++ controls for the SOMARS drone. Runs on the Jetson Orin Nano, +talks to PX4 (Pixhawk 6X) through micro-XRCE-DDS. -Micro-XRCE-DDS serves as the transparent bridge between PX4 (Pixhawk 6X) and -these ROS 2 nodes running on the Jetson Orin Nano (JetPack 6.2.2). Start the -Agent via the CLI; this package contains only the control logic. +--- -## Architecture +## Flight Day Checklist -``` -Camera ──► somars-vision (detect targets, publish pixel coords) - │ /vision/detections [PoseArray] - ▼ - target_localizer (pixel → NED projection) - │ /targets/ned [PointStamped] - ▼ - guidance_node (compute trajectory setpoints) - │ /fmu/in/trajectory_setpoint - ▼ - micro-XRCE-DDS Agent ──► PX4 (Pixhawk 6X) - ▲ - offboard_manager (arm / mode switch / heartbeat) - │ /fmu/in/vehicle_command - │ /fmu/in/offboard_control_mode -``` +### 1. Plug in waypoints -## Nodes +Open **`config/waypoints.yaml`** and replace the example coordinates with the +ones given at check-in: + +```yaml +waypoints_lat: [38.315339, 38.315805, ...] +waypoints_lon: [-76.548108, -76.550537, ...] +waypoints_alt_ft: [200.0, 250.0, ...] +``` -| Node | Description | -|------|-------------| -| `target_localizer` | Subscribes to vision detections + vehicle pose. Projects pixel detections through the camera model and intersects with the ground plane to produce NED target positions. | -| `guidance_node` | Subscribes to target NED positions. Generates `TrajectorySetpoint` messages (position mode) to fly toward the target. Holds position when no target is active. | -| `offboard_manager` | Maintains the PX4 offboard heartbeat (`OffboardControlMode` + hold setpoint). Optionally arms and switches to offboard mode. | +Set `waypoint_laps` to the number of full laps you want to fly before +entering the search/deliver phase. -## Quick Start +### 2. Start the stack ```bash -# 1. Start the micro-XRCE-DDS Agent (serial example) +# Terminal 1 — micro-XRCE-DDS Agent (bridge PX4 ↔ ROS 2) MicroXRCEAgent serial --dev /dev/ttyTHS1 -b 921600 -# 2. Build this package inside your colcon workspace +# Terminal 2 — build & launch cd ~/somars_ws colcon build --packages-select somars_controls source install/setup.bash - -# 3. Launch all controls nodes ros2 launch somars_controls controls.launch.py ``` -## Parameters +### 3. Verify topics + +```bash +ros2 topic echo /fmu/out/vehicle_local_position # PX4 position +ros2 topic echo /targets/ned # vision localisation +ros2 topic echo /fmu/in/trajectory_setpoint # setpoints going to PX4 +``` + +--- + +## Mission Flow + +``` +Takeoff + │ + ├─ Phase 1: Waypoint laps (GPS coords from config/waypoints.yaml) + │ guidance_node sequences through waypoints, counts laps + │ + ├─ Phase 2: Search / detect / deliver + │ somars-vision detects targets → target_localizer projects to NED + │ → guidance_node steers drone to target → payload drop + │ + └─ Land +``` + +**Priority in `guidance_node`:** + +| Priority | Condition | Action | +|----------|-----------|--------| +| 1 | Vision target active (< 2 s old) | Track target for delivery | +| 2 | Waypoints remaining | Fly to next waypoint | +| 3 | Nothing to do | Hold position at loiter altitude | + +--- -All tunables live in `config/params.yaml`. Override on the CLI: +## Config Files + +| File | What to edit | When | +|------|-------------|------| +| **`config/waypoints.yaml`** | GPS waypoints, number of laps | **Competition day** | +| `config/params.yaml` | Camera intrinsics, speeds, altitudes | Tuning / calibration | + +--- + +## Nodes + +| Node | Purpose | +|------|---------| +| `offboard_manager` | PX4 heartbeat, arm/disarm, mode switching | +| `guidance_node` | Waypoint navigation + vision-target tracking → publishes `TrajectorySetpoint` | +| `target_localizer` | Pixel detections → NED world coordinates (camera ray–ground intersection) | + +--- + +## Testing Without Hardware + +**Math tests** (runs on any machine with Eigen — no ROS 2 needed): ```bash -ros2 launch somars_controls controls.launch.py \ - --ros-args -p guidance_node:approach_speed:=3.0 +cd test && mkdir -p build && cd build +cmake .. && make && ./test_projection_math ``` -## Workspace Integration +**Integration test** (needs ROS 2 but no Pixhawk — uses mock publishers): -This package is intended to be used as a submodule inside a ROS 2 colcon -workspace alongside `somars-vision`: +```bash +ros2 launch somars_controls integration.launch.py +``` + +--- + +## Workspace Setup ``` somars-ws/ ├── src/ -│ ├── somars_controls/ ← this repo (git submodule) -│ ├── somars_vision/ ← slugbotics/somars-vision (git submodule) -│ └── px4_msgs/ ← PX4/px4_msgs (git submodule) -├── launch/ ← system-level launch files -└── docker/ ← Jetson container setup +│ ├── somars-embedded/ ← this repo +│ ├── somars-vision/ ← slugbotics/somars-vision +│ └── px4_msgs/ ← PX4/px4_msgs +└── ... +``` + +```bash +cd ~/somars_ws +colcon build +source install/setup.bash ``` ## Dependencies -- ROS 2 Humble / Jazzy -- [px4_msgs](https://github.com/PX4/px4_msgs) – PX4 message definitions -- Eigen3 -- micro-XRCE-DDS Agent (run separately) +- **ROS 2** Humble or Jazzy +- **[px4_msgs](https://github.com/PX4/px4_msgs)** +- **Eigen3** +- **micro-XRCE-DDS Agent** (run separately on Jetson) diff --git a/config/waypoints.yaml b/config/waypoints.yaml new file mode 100644 index 0000000..f81ce7b --- /dev/null +++ b/config/waypoints.yaml @@ -0,0 +1,52 @@ +# ===================================================================== +# +# >>> COMPETITION WAYPOINTS — EDIT THIS FILE ON FLIGHT DAY <<< +# +# ===================================================================== +# +# At check-in you will receive a list of GPS waypoints. +# Paste the coordinates into the three arrays below (lat, lon, alt). +# They MUST be the same length and in the same order. +# +# Format: +# Latitude — decimal degrees (e.g. 38.315339) +# Longitude — decimal degrees (e.g. -76.548108) +# Altitude — feet MSL (e.g. 200.0) +# +# SUAS 2026: up to 15 waypoints per lap, must fly within 100 ft (30 m) +# C-UASC: similar format, given at check-in +# +# ===================================================================== + +guidance_node: + ros__parameters: + + # ---------- MISSION SETTINGS ---------- + waypoint_laps: 4 # number of full laps before search phase + waypoint_radius_m: 30.0 # "reached" radius in meters (SUAS: 100 ft ≈ 30 m) + + # ---------- WAYPOINTS (paste competition coords here) ---------- + # Example: SUAS sample lap around Maryland (NOT real competition data) + waypoints_lat: + - 38.315339 + - 38.315805 + - 38.316847 + - 38.318212 + - 38.319640 + - 38.318400 + + waypoints_lon: + - -76.548108 + - -76.550537 + - -76.553244 + - -76.550048 + - -76.547900 + - -76.545200 + + waypoints_alt_ft: + - 200.0 + - 250.0 + - 200.0 + - 300.0 + - 250.0 + - 200.0 diff --git a/include/somars_controls/guidance_node.hpp b/include/somars_controls/guidance_node.hpp index 8104fac..97f1fd3 100644 --- a/include/somars_controls/guidance_node.hpp +++ b/include/somars_controls/guidance_node.hpp @@ -4,17 +4,24 @@ #include #include #include -#include #include #include +#include namespace somars_controls { -/// Generates trajectory setpoints that steer the drone towards detected -/// targets. When no target is active the node publishes a position-hold -/// at the current loiter altitude. +/// Unified guidance node — handles both waypoint navigation and +/// vision-target tracking with the following priority: +/// +/// 1. Vision target active → track it (for payload delivery) +/// 2. Waypoints remaining → fly to next waypoint +/// 3. Nothing to do → hold position at loiter altitude +/// +/// Waypoints are loaded from config/waypoints.yaml (GPS lat/lon/alt). +/// GPS coordinates are converted to NED once the PX4 reference origin +/// is received. /// /// Subscriptions /// /targets/ned – PointStamped (from TargetLocalizer) @@ -22,7 +29,6 @@ namespace somars_controls /// /// Publications /// /fmu/in/trajectory_setpoint – TrajectorySetpoint (to PX4) -/// /fmu/in/offboard_control_mode – OffboardControlMode (to PX4) class GuidanceNode : public rclcpp::Node { public: @@ -36,27 +42,48 @@ class GuidanceNode : public rclcpp::Node /// Fixed-rate control loop called by the timer. void control_loop(); + // ---- waypoint helpers ---- + void load_waypoints(); + void convert_waypoints_to_ned(); + Eigen::Vector3d gps_to_ned(double lat_deg, double lon_deg, double alt_ft_msl) const; + // ---- subscribers ---- rclcpp::Subscription::SharedPtr target_sub_; rclcpp::Subscription::SharedPtr local_pos_sub_; // ---- publishers ---- rclcpp::Publisher::SharedPtr setpoint_pub_; - rclcpp::Publisher::SharedPtr control_mode_pub_; // ---- timer ---- rclcpp::TimerBase::SharedPtr control_timer_; - // ---- state ---- + // ---- vehicle state ---- Eigen::Vector3d vehicle_position_ned_{0.0, 0.0, 0.0}; Eigen::Vector3d target_position_ned_{0.0, 0.0, 0.0}; bool target_received_ = false; bool position_received_ = false; - /// Timestamp (steady clock) of the last target message. Used to expire - /// stale targets. + /// Timestamp of the last vision target. Used to expire stale targets. rclcpp::Time last_target_time_; + // ---- waypoint state ---- + struct GpsWaypoint { double lat, lon, alt_ft; }; + + std::vector gps_waypoints_; // raw from config + std::vector ned_waypoints_; // computed after ref received + size_t current_wp_index_ = 0; + int current_lap_ = 0; + int total_laps_ = 1; + double waypoint_radius_ = 30.0; // meters + bool waypoints_loaded_ = false; + bool ned_waypoints_ready_ = false; + + // PX4 NED reference origin (captured once from VehicleLocalPosition) + double ref_lat_ = 0.0; + double ref_lon_ = 0.0; + double ref_alt_m_ = 0.0; // MSL in meters + bool ref_received_ = false; + // ---- parameters ---- double approach_speed_; double acceptance_radius_; diff --git a/include/somars_controls/offboard_manager.hpp b/include/somars_controls/offboard_manager.hpp index 54b7fc6..1711c96 100644 --- a/include/somars_controls/offboard_manager.hpp +++ b/include/somars_controls/offboard_manager.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include namespace somars_controls @@ -47,8 +48,12 @@ class OffboardManager : public rclcpp::Node void disarm(); void set_offboard_mode(); + // ---- callbacks ---- + void local_position_cb(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg); + // ---- subscribers ---- rclcpp::Subscription::SharedPtr status_sub_; + rclcpp::Subscription::SharedPtr local_pos_sub_; // ---- publishers ---- rclcpp::Publisher::SharedPtr control_mode_pub_; @@ -62,6 +67,10 @@ class OffboardManager : public rclcpp::Node uint8_t nav_state_ = 0; uint8_t arming_state_ = 0; int heartbeat_count_ = 0; + float hold_x_ = 0.0f; + float hold_y_ = 0.0f; + float hold_z_ = -10.0f; // NED, default 10 m AGL + bool position_received_ = false; // ---- parameters ---- bool auto_arm_; diff --git a/include/somars_controls/projection.hpp b/include/somars_controls/projection.hpp new file mode 100644 index 0000000..638a28b --- /dev/null +++ b/include/somars_controls/projection.hpp @@ -0,0 +1,80 @@ +#ifndef SOMARS_CONTROLS__PROJECTION_HPP_ +#define SOMARS_CONTROLS__PROJECTION_HPP_ + +#include +#include + +namespace somars_controls +{ +namespace projection +{ + +/// Build the fixed rotation matrix from camera optical frame to body FRD frame. +/// +/// Camera optical convention: X = right, Y = down, Z = into scene +/// Body FRD convention: X = forward, Y = right, Z = down +/// +/// When cam_pitch_rad = 0 → camera looks forward (along body X) +/// When cam_pitch_rad = -π/2 → camera looks straight down (along body Z) +inline Eigen::Matrix3d build_cam_to_body_rotation(double cam_pitch_rad) +{ + // Step 1: Base alignment when camera is forward-looking (pitch = 0). + // body X (fwd) = cam Z (into scene) + // body Y (right) = cam X (right) + // body Z (down) = cam Y (down) + Eigen::Matrix3d R_base; + R_base << 0, 0, 1, + 1, 0, 0, + 0, 1, 0; + + // Step 2: Apply pitch rotation about the body Y-axis. + Eigen::Matrix3d R_pitch = + Eigen::AngleAxisd(cam_pitch_rad, Eigen::Vector3d::UnitY()).toRotationMatrix(); + + return R_pitch * R_base; +} + +/// Back-project a single pixel (u, v) through a pinhole camera model, +/// rotate into NED via the vehicle attitude, and intersect with the +/// ground plane (NED z = 0). +/// +/// @param[in] u, v Pixel coordinates (col, row) +/// @param[in] fx, fy, cx, cy Camera intrinsics +/// @param[in] R_cam_to_body Camera-to-body rotation (from build_cam_to_body_rotation) +/// @param[in] vehicle_attitude Body-to-NED quaternion (from PX4 VehicleAttitude) +/// @param[in] vehicle_position_ned Drone position in NED (z < 0 when above ground) +/// @param[out] target_ned Resulting ground-plane intersection in NED +/// @return true if the ray intersects the ground plane (z = 0) +inline bool project_pixel_to_ned( + double u, double v, + double fx, double fy, double cx, double cy, + const Eigen::Matrix3d & R_cam_to_body, + const Eigen::Quaterniond & vehicle_attitude, + const Eigen::Vector3d & vehicle_position_ned, + Eigen::Vector3d & target_ned) +{ + // 1. Pixel → normalised camera-frame ray + Eigen::Vector3d ray_cam((u - cx) / fx, (v - cy) / fy, 1.0); + ray_cam.normalize(); + + // 2. Camera frame → body frame → NED frame + Eigen::Matrix3d R_body_to_ned = vehicle_attitude.toRotationMatrix(); + Eigen::Vector3d ray_ned = R_body_to_ned * R_cam_to_body * ray_cam; + + // 3. Intersect with ground plane (NED z = 0). + // Drone z is negative when above ground. + // Ray must point downward (positive z component) to hit ground. + if (ray_ned.z() <= 1e-6) { + return false; + } + + double t = -vehicle_position_ned.z() / ray_ned.z(); + target_ned = vehicle_position_ned + t * ray_ned; + target_ned.z() = 0.0; + return true; +} + +} // namespace projection +} // namespace somars_controls + +#endif // SOMARS_CONTROLS__PROJECTION_HPP_ diff --git a/launch/controls.launch.py b/launch/controls.launch.py index f031274..c21a344 100644 --- a/launch/controls.launch.py +++ b/launch/controls.launch.py @@ -8,7 +8,8 @@ def generate_launch_description(): pkg_share = get_package_share_directory('somars_controls') - params_file = os.path.join(pkg_share, 'config', 'params.yaml') + params_file = os.path.join(pkg_share, 'config', 'params.yaml') + waypoints_file = os.path.join(pkg_share, 'config', 'waypoints.yaml') return LaunchDescription([ Node( @@ -22,7 +23,7 @@ def generate_launch_description(): package='somars_controls', executable='guidance_node', name='guidance_node', - parameters=[params_file], + parameters=[params_file, waypoints_file], output='screen', ), Node( diff --git a/src/guidance_node.cpp b/src/guidance_node.cpp index 7b60be3..894095a 100644 --- a/src/guidance_node.cpp +++ b/src/guidance_node.cpp @@ -9,6 +9,10 @@ using namespace std::chrono_literals; namespace somars_controls { +static constexpr double R_EARTH = 6371000.0; // meters +static constexpr double FT_TO_M = 0.3048; +static constexpr double DEG_TO_RAD = M_PI / 180.0; + GuidanceNode::GuidanceNode() : Node("guidance_node"), last_target_time_(this->now()) @@ -19,14 +23,25 @@ GuidanceNode::GuidanceNode() this->declare_parameter("loiter_altitude", -10.0); this->declare_parameter("control_rate_hz", 20.0); + // Waypoint parameters (loaded from config/waypoints.yaml) + this->declare_parameter("waypoint_laps", 1); + this->declare_parameter("waypoint_radius_m", 30.0); + this->declare_parameter("waypoints_lat", std::vector{}); + this->declare_parameter("waypoints_lon", std::vector{}); + this->declare_parameter("waypoints_alt_ft", std::vector{}); + approach_speed_ = this->get_parameter("approach_speed").as_double(); acceptance_radius_ = this->get_parameter("acceptance_radius").as_double(); loiter_altitude_ = this->get_parameter("loiter_altitude").as_double(); double rate_hz = this->get_parameter("control_rate_hz").as_double(); + total_laps_ = this->get_parameter("waypoint_laps").as_int(); + waypoint_radius_ = this->get_parameter("waypoint_radius_m").as_double(); + + load_waypoints(); RCLCPP_INFO(this->get_logger(), - "Guidance: speed=%.1f m/s radius=%.1f m alt=%.1f m rate=%.0f Hz", - approach_speed_, acceptance_radius_, loiter_altitude_, rate_hz); + "Guidance: speed=%.1f m/s wp_radius=%.0f m loiter_alt=%.1f m rate=%.0f Hz", + approach_speed_, waypoint_radius_, loiter_altitude_, rate_hz); // ---- QoS ---- auto px4_qos = rclcpp::SensorDataQoS(); @@ -40,13 +55,10 @@ GuidanceNode::GuidanceNode() "/fmu/out/vehicle_local_position", px4_qos, std::bind(&GuidanceNode::local_position_cb, this, std::placeholders::_1)); - // ---- publishers (to PX4 via micro-XRCE-DDS) ---- + // ---- publisher (to PX4 via micro-XRCE-DDS) ---- setpoint_pub_ = this->create_publisher( "/fmu/in/trajectory_setpoint", 10); - control_mode_pub_ = this->create_publisher( - "/fmu/in/offboard_control_mode", 10); - // ---- control loop timer ---- auto period = std::chrono::duration(1.0 / rate_hz); control_timer_ = this->create_wall_timer( @@ -56,6 +68,74 @@ GuidanceNode::GuidanceNode() RCLCPP_INFO(this->get_logger(), "GuidanceNode started"); } +// --------------------------------------------------------------------------- +// Waypoint loading +// --------------------------------------------------------------------------- + +void GuidanceNode::load_waypoints() +{ + auto lats = this->get_parameter("waypoints_lat").as_double_array(); + auto lons = this->get_parameter("waypoints_lon").as_double_array(); + auto alts = this->get_parameter("waypoints_alt_ft").as_double_array(); + + if (lats.empty()) { + RCLCPP_WARN(this->get_logger(), + "No waypoints loaded — skipping waypoint phase"); + return; + } + + if (lats.size() != lons.size() || lats.size() != alts.size()) { + RCLCPP_ERROR(this->get_logger(), + "waypoints_lat/lon/alt arrays must be the same length! " + "Got %zu / %zu / %zu", lats.size(), lons.size(), alts.size()); + return; + } + + gps_waypoints_.clear(); + for (size_t i = 0; i < lats.size(); ++i) { + gps_waypoints_.push_back({lats[i], lons[i], alts[i]}); + } + + waypoints_loaded_ = true; + RCLCPP_INFO(this->get_logger(), + "Loaded %zu waypoints, %d laps planned", gps_waypoints_.size(), total_laps_); + + for (size_t i = 0; i < gps_waypoints_.size(); ++i) { + RCLCPP_INFO(this->get_logger(), " WP %zu: lat=%.6f lon=%.6f alt=%.0f ft", + i + 1, gps_waypoints_[i].lat, gps_waypoints_[i].lon, gps_waypoints_[i].alt_ft); + } +} + +void GuidanceNode::convert_waypoints_to_ned() +{ + ned_waypoints_.clear(); + for (const auto & wp : gps_waypoints_) { + ned_waypoints_.push_back(gps_to_ned(wp.lat, wp.lon, wp.alt_ft)); + } + ned_waypoints_ready_ = true; + + RCLCPP_INFO(this->get_logger(), "Waypoints converted to NED (ref: %.6f, %.6f, %.1f m MSL):", + ref_lat_, ref_lon_, ref_alt_m_); + for (size_t i = 0; i < ned_waypoints_.size(); ++i) { + const auto & n = ned_waypoints_[i]; + RCLCPP_INFO(this->get_logger(), " WP %zu NED: [%.1f, %.1f, %.1f]", + i + 1, n.x(), n.y(), n.z()); + } +} + +Eigen::Vector3d GuidanceNode::gps_to_ned( + double lat_deg, double lon_deg, double alt_ft_msl) const +{ + double dlat = (lat_deg - ref_lat_) * DEG_TO_RAD; + double dlon = (lon_deg - ref_lon_) * DEG_TO_RAD; + + double north = dlat * R_EARTH; + double east = dlon * R_EARTH * std::cos(ref_lat_ * DEG_TO_RAD); + double down = -(alt_ft_msl * FT_TO_M - ref_alt_m_); + + return Eigen::Vector3d(north, east, down); +} + // --------------------------------------------------------------------------- // Callbacks // --------------------------------------------------------------------------- @@ -73,27 +153,33 @@ void GuidanceNode::local_position_cb( { vehicle_position_ned_ = Eigen::Vector3d(msg->x, msg->y, msg->z); position_received_ = true; + + // Capture the NED reference origin once (needs valid global frame) + if (!ref_received_ && msg->xy_global && msg->z_global) { + ref_lat_ = msg->ref_lat; + ref_lon_ = msg->ref_lon; + ref_alt_m_ = static_cast(msg->ref_alt); + ref_received_ = true; + + RCLCPP_INFO(this->get_logger(), + "NED reference origin: lat=%.6f lon=%.6f alt=%.1f m MSL", + ref_lat_, ref_lon_, ref_alt_m_); + + if (waypoints_loaded_ && !ned_waypoints_ready_) { + convert_waypoints_to_ned(); + } + } } // --------------------------------------------------------------------------- -// Control loop +// Control loop — priority: vision target > waypoint > hold // --------------------------------------------------------------------------- void GuidanceNode::control_loop() { - // -- Always publish OffboardControlMode so PX4 stays in offboard ---------- - px4_msgs::msg::OffboardControlMode mode_msg{}; - mode_msg.position = true; - mode_msg.velocity = false; - mode_msg.acceleration = false; - mode_msg.attitude = false; - mode_msg.body_rate = false; - mode_msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; // µs - control_mode_pub_->publish(mode_msg); - - // -- Compute setpoint ----------------------------------------------------- + uint64_t ts = this->get_clock()->now().nanoseconds() / 1000; // µs px4_msgs::msg::TrajectorySetpoint sp{}; - sp.timestamp = mode_msg.timestamp; + sp.timestamp = ts; // NaN = "don't care" for PX4 sp.velocity[0] = std::numeric_limits::quiet_NaN(); @@ -105,18 +191,20 @@ void GuidanceNode::control_loop() sp.jerk[0] = std::numeric_limits::quiet_NaN(); sp.jerk[1] = std::numeric_limits::quiet_NaN(); sp.jerk[2] = std::numeric_limits::quiet_NaN(); - sp.yaw = std::numeric_limits::quiet_NaN(); // hold current + sp.yaw = std::numeric_limits::quiet_NaN(); sp.yawspeed = std::numeric_limits::quiet_NaN(); if (!position_received_) { - // Nothing we can do until we know where we are. setpoint_pub_->publish(sp); return; } - // Expire targets older than 2 seconds bool target_stale = (this->now() - last_target_time_).seconds() > 2.0; + bool waypoints_active = ned_waypoints_ready_ && (current_lap_ < total_laps_); + // =================================================================== + // PRIORITY 1: Vision target detected → track it for payload delivery + // =================================================================== if (target_received_ && !target_stale) { // ---- Navigate towards target ---- Eigen::Vector3d to_target = target_position_ned_ - vehicle_position_ned_; @@ -128,29 +216,88 @@ void GuidanceNode::control_loop() Eigen::Vector3d desired_pos; if (dist < acceptance_radius_) { // Close enough – hold above the target at loiter altitude + // TODO: trigger payload drop signal here desired_pos = Eigen::Vector3d( target_position_ned_.x(), target_position_ned_.y(), loiter_altitude_); RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, - "Within acceptance radius (%.1f m) – holding position", dist); + "TARGET: within %.1f m – holding for delivery", dist); } else { // Move towards target at approach_speed_, staying at loiter altitude Eigen::Vector3d direction = to_target.normalized(); - double step = std::min(approach_speed_ * 0.5, dist); // half-second lookahead + double step = std::min(approach_speed_ * 0.5, dist); desired_pos = vehicle_position_ned_ + direction * step; desired_pos.z() = loiter_altitude_; - RCLCPP_DEBUG(this->get_logger(), - "Approaching target: dist=%.1f m setpoint=[%.1f, %.1f, %.1f]", - dist, desired_pos.x(), desired_pos.y(), desired_pos.z()); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + "TARGET: approaching, dist=%.1f m", dist); } sp.position[0] = static_cast(desired_pos.x()); sp.position[1] = static_cast(desired_pos.y()); sp.position[2] = static_cast(desired_pos.z()); + // =================================================================== + // PRIORITY 2: Waypoints remaining → fly the mission lap + // =================================================================== + } else if (waypoints_active) { + const auto & wp = ned_waypoints_[current_wp_index_]; + + Eigen::Vector3d to_wp = wp - vehicle_position_ned_; + double horiz_dist = Eigen::Vector2d(to_wp.x(), to_wp.y()).norm(); + + // Check if current waypoint is reached + if (horiz_dist < waypoint_radius_) { + RCLCPP_INFO(this->get_logger(), + "WAYPOINT %zu/%zu reached (%.0f m) — lap %d/%d", + current_wp_index_ + 1, ned_waypoints_.size(), horiz_dist, + current_lap_ + 1, total_laps_); + + current_wp_index_++; + if (current_wp_index_ >= ned_waypoints_.size()) { + current_wp_index_ = 0; + current_lap_++; + RCLCPP_INFO(this->get_logger(), + "===== LAP %d/%d COMPLETE =====", current_lap_, total_laps_); + + if (current_lap_ >= total_laps_) { + RCLCPP_INFO(this->get_logger(), + "All %d laps done — entering search/detect phase", total_laps_); + } + } + } + + // Navigate towards the current (or next) waypoint + if (current_lap_ < total_laps_) { + const auto & next_wp = ned_waypoints_[current_wp_index_]; + Eigen::Vector3d to_next = next_wp - vehicle_position_ned_; + double dist = to_next.norm(); + Eigen::Vector3d direction = to_next.normalized(); + double step = std::min(approach_speed_ * 0.5, dist); + + Eigen::Vector3d desired = vehicle_position_ned_ + direction * step; + desired.z() = next_wp.z(); // use waypoint altitude + + sp.position[0] = static_cast(desired.x()); + sp.position[1] = static_cast(desired.y()); + sp.position[2] = static_cast(desired.z()); + + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 3000, + "WAYPOINT %zu/%zu dist=%.0f m lap %d/%d", + current_wp_index_ + 1, ned_waypoints_.size(), dist, + current_lap_ + 1, total_laps_); + } else { + // All laps done — hold position + sp.position[0] = static_cast(vehicle_position_ned_.x()); + sp.position[1] = static_cast(vehicle_position_ned_.y()); + sp.position[2] = static_cast(loiter_altitude_); + } + + // =================================================================== + // PRIORITY 3: Nothing to do → hold position + // =================================================================== } else { // ---- No active target – hold current XY at loiter altitude ---- sp.position[0] = static_cast(vehicle_position_ned_.x()); diff --git a/src/offboard_manager.cpp b/src/offboard_manager.cpp index 9d1a5e2..70aba0c 100644 --- a/src/offboard_manager.cpp +++ b/src/offboard_manager.cpp @@ -1,6 +1,7 @@ #include "somars_controls/offboard_manager.hpp" #include +#include using namespace std::chrono_literals; @@ -32,6 +33,10 @@ OffboardManager::OffboardManager() "/fmu/out/vehicle_status", px4_qos, std::bind(&OffboardManager::status_cb, this, std::placeholders::_1)); + local_pos_sub_ = this->create_subscription( + "/fmu/out/vehicle_local_position", px4_qos, + std::bind(&OffboardManager::local_position_cb, this, std::placeholders::_1)); + // ---- publishers ---- control_mode_pub_ = this->create_publisher( "/fmu/in/offboard_control_mode", 10); @@ -63,6 +68,15 @@ void OffboardManager::status_cb( arming_state_ = msg->arming_state; } +void OffboardManager::local_position_cb( + const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) +{ + hold_x_ = msg->x; + hold_y_ = msg->y; + hold_z_ = msg->z; + position_received_ = true; +} + // --------------------------------------------------------------------------- // Heartbeat // --------------------------------------------------------------------------- @@ -83,12 +97,13 @@ void OffboardManager::heartbeat_loop() // 2. Hold-position setpoint (keeps PX4 happy while no guidance is active). // The guidance_node will overwrite this once it starts publishing. + // Uses current position if known; otherwise a safe default. px4_msgs::msg::TrajectorySetpoint sp{}; sp.timestamp = ts; - sp.position[0] = 0.0f; - sp.position[1] = 0.0f; - sp.position[2] = -10.0f; // 10 m AGL (NED) - sp.yaw = 0.0f; + sp.position[0] = hold_x_; + sp.position[1] = hold_y_; + sp.position[2] = hold_z_; + sp.yaw = std::numeric_limits::quiet_NaN(); // hold current heading setpoint_pub_->publish(sp); heartbeat_count_++; diff --git a/src/target_localizer.cpp b/src/target_localizer.cpp index 60f4ee3..ff60845 100644 --- a/src/target_localizer.cpp +++ b/src/target_localizer.cpp @@ -1,6 +1,8 @@ #include "somars_controls/target_localizer.hpp" +#include "somars_controls/projection.hpp" #include +#include namespace somars_controls { @@ -22,11 +24,11 @@ TargetLocalizer::TargetLocalizer() double cam_pitch = this->get_parameter("camera_pitch_rad").as_double(); // Build the fixed camera-to-body rotation. - // Assumes the camera optical axis is mounted in the body XZ-plane, - // pitched by cam_pitch about the body Y-axis. - // body X = forward, Y = right, Z = down (FRD) - // camera Z = optical axis (into scene) - R_cam_to_body_ = Eigen::AngleAxisd(cam_pitch, Eigen::Vector3d::UnitY()).toRotationMatrix(); + // Handles the base alignment (camera optical → body FRD) PLUS the + // pitch offset for the physical mount angle. + // camera: X = right, Y = down, Z = into scene + // body: X = forward, Y = right, Z = down (FRD) + R_cam_to_body_ = projection::build_cam_to_body_rotation(cam_pitch); RCLCPP_INFO(this->get_logger(), "Camera intrinsics fx=%.1f fy=%.1f cx=%.1f cy=%.1f pitch=%.2f rad", @@ -84,10 +86,12 @@ void TargetLocalizer::detection_cb( return; } + static const char * class_names[] = {"red", "black", "white", "unknown"}; + for (const auto & pose : msg->poses) { double u = pose.position.x; // pixel column double v = pose.position.y; // pixel row - // int class_id = static_cast(pose.position.z); // TODO: use class + int class_id = std::clamp(static_cast(pose.position.z), 0, 3); Eigen::Vector3d target_ned; if (!pixel_to_ned(u, v, target_ned)) { @@ -97,15 +101,15 @@ void TargetLocalizer::detection_cb( geometry_msgs::msg::PointStamped out; out.header.stamp = this->now(); - out.header.frame_id = "map_ned"; + out.header.frame_id = std::string("target_") + class_names[class_id]; out.point.x = target_ned.x(); out.point.y = target_ned.y(); out.point.z = target_ned.z(); target_ned_pub_->publish(out); - RCLCPP_DEBUG(this->get_logger(), - "Target NED: [%.2f, %.2f, %.2f]", - target_ned.x(), target_ned.y(), target_ned.z()); + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Target [%s] NED: [%.2f, %.2f, %.2f] from pixel [%.0f, %.0f]", + class_names[class_id], target_ned.x(), target_ned.y(), target_ned.z(), u, v); } } @@ -116,25 +120,10 @@ void TargetLocalizer::detection_cb( bool TargetLocalizer::pixel_to_ned( double u, double v, Eigen::Vector3d & target_ned) const { - // 1. Pixel → normalised camera-frame ray - Eigen::Vector3d ray_cam((u - cx_) / fx_, (v - cy_) / fy_, 1.0); - ray_cam.normalize(); - - // 2. Camera frame → body frame → NED frame - Eigen::Matrix3d R_body_to_ned = vehicle_attitude_.toRotationMatrix(); - Eigen::Vector3d ray_ned = R_body_to_ned * R_cam_to_body_ * ray_cam; - - // 3. Intersect with ground plane (NED z = 0). - // Drone is at vehicle_position_ned_.z() (negative when above ground). - // We need the ray to point downward (positive z in NED) to hit ground. - if (ray_ned.z() <= 1e-6) { - return false; // ray is parallel to or pointing away from ground - } - - double t = -vehicle_position_ned_.z() / ray_ned.z(); - target_ned = vehicle_position_ned_ + t * ray_ned; - target_ned.z() = 0.0; // on the ground plane - return true; + return projection::project_pixel_to_ned( + u, v, fx_, fy_, cx_, cy_, + R_cam_to_body_, vehicle_attitude_, vehicle_position_ned_, + target_ned); } Eigen::Matrix3d TargetLocalizer::quat_to_rotation( diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..33f513b --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,24 @@ +# ============================================================================ +# Standalone test build – runs on any machine with CMake + Eigen3. +# No ROS2 required. +# +# Usage: +# cd test && mkdir build && cd build +# cmake .. && make && ./test_projection_math +# ============================================================================ + +cmake_minimum_required(VERSION 3.8) +project(somars_controls_tests) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(Eigen3 REQUIRED) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR}/../include + ${EIGEN3_INCLUDE_DIRS} +) + +add_executable(test_projection_math test_projection_math.cpp) +target_link_libraries(test_projection_math Eigen3::Eigen) diff --git a/test/integration.launch.py b/test/integration.launch.py new file mode 100644 index 0000000..788964a --- /dev/null +++ b/test/integration.launch.py @@ -0,0 +1,63 @@ +"""Launch mock nodes + all controls nodes for hardware-free integration testing. + +Usage: + ros2 launch somars_controls integration.launch.py + +Then in another terminal: + ros2 topic echo /targets/ned + ros2 topic echo /fmu/in/trajectory_setpoint +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_share = get_package_share_directory('somars_controls') + params_file = os.path.join(pkg_share, 'config', 'params.yaml') + + return LaunchDescription([ + # Mock PX4 + vision publisher + Node( + package='somars_controls', + executable='mock_nodes', + name='mock_nodes', + parameters=[{ + 'altitude_m': 20.0, + 'north_m': 100.0, + 'east_m': 50.0, + 'yaw_deg': 0.0, + 'detection_u': 400.0, # offset right of center → East + 'detection_v': 300.0, # offset below center → North + 'detection_class': 0, # red target + 'publish_rate_hz': 10.0, + 'enable_detections': True, + }], + output='screen', + ), + + # Real controls nodes + Node( + package='somars_controls', + executable='offboard_manager', + name='offboard_manager', + parameters=[params_file], + output='screen', + ), + Node( + package='somars_controls', + executable='guidance_node', + name='guidance_node', + parameters=[params_file], + output='screen', + ), + Node( + package='somars_controls', + executable='target_localizer', + name='target_localizer', + parameters=[params_file], + output='screen', + ), + ]) diff --git a/test/mock_nodes.cpp b/test/mock_nodes.cpp new file mode 100644 index 0000000..5788f50 --- /dev/null +++ b/test/mock_nodes.cpp @@ -0,0 +1,150 @@ +// ============================================================================ +// Mock PX4 + Vision publisher for integration testing without hardware. +// Requires ROS2 + px4_msgs (but NOT a real Pixhawk or micro-XRCE-DDS Agent). +// +// Publishes: +// /fmu/out/vehicle_attitude – identity quaternion (level, heading N) +// /fmu/out/vehicle_local_position – hovering at configurable NED position +// /fmu/out/vehicle_status – armed, in offboard mode +// /vision/detections – one detection at configurable pixel coords +// +// Usage (after building in a ROS2 workspace): +// ros2 run somars_controls mock_nodes +// ros2 run somars_controls mock_nodes --ros-args \ +// -p altitude_m:=25.0 -p detection_u:=400.0 -p detection_v:=300.0 +// ============================================================================ + +#include +#include +#include +#include +#include + +#include +#include + +using namespace std::chrono_literals; + +class MockNodes : public rclcpp::Node +{ +public: + MockNodes() : Node("mock_nodes") + { + this->declare_parameter("altitude_m", 20.0); + this->declare_parameter("north_m", 100.0); + this->declare_parameter("east_m", 50.0); + this->declare_parameter("yaw_deg", 0.0); + this->declare_parameter("detection_u", 320.0); + this->declare_parameter("detection_v", 240.0); + this->declare_parameter("detection_class", 0); // 0=red, 1=black, 2=white + this->declare_parameter("publish_rate_hz", 10.0); + this->declare_parameter("enable_detections", true); + + alt_ = this->get_parameter("altitude_m").as_double(); + north_ = this->get_parameter("north_m").as_double(); + east_ = this->get_parameter("east_m").as_double(); + double yaw_deg = this->get_parameter("yaw_deg").as_double(); + yaw_rad_ = yaw_deg * M_PI / 180.0; + det_u_ = this->get_parameter("detection_u").as_double(); + det_v_ = this->get_parameter("detection_v").as_double(); + det_class_ = this->get_parameter("detection_class").as_int(); + enable_det_ = this->get_parameter("enable_detections").as_bool(); + double rate = this->get_parameter("publish_rate_hz").as_double(); + + // Publishers + att_pub_ = this->create_publisher( + "/fmu/out/vehicle_attitude", rclcpp::SensorDataQoS()); + pos_pub_ = this->create_publisher( + "/fmu/out/vehicle_local_position", rclcpp::SensorDataQoS()); + stat_pub_ = this->create_publisher( + "/fmu/out/vehicle_status", rclcpp::SensorDataQoS()); + det_pub_ = this->create_publisher( + "/vision/detections", 10); + + auto period = std::chrono::duration(1.0 / rate); + timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&MockNodes::publish_all, this)); + + RCLCPP_INFO(this->get_logger(), + "MockNodes: pos=[%.0f,%.0f,%.0f] yaw=%.0f° det=[%.0f,%.0f] class=%ld @ %.0fHz", + north_, east_, -alt_, yaw_deg, det_u_, det_v_, det_class_, rate); + } + +private: + void publish_all() + { + uint64_t ts = this->get_clock()->now().nanoseconds() / 1000; + + // -- VehicleAttitude -- + px4_msgs::msg::VehicleAttitude att{}; + att.timestamp = ts; + // Quaternion for yaw rotation about NED Z-axis + double half_yaw = yaw_rad_ / 2.0; + att.q[0] = static_cast(std::cos(half_yaw)); // w + att.q[1] = 0.0f; // x + att.q[2] = 0.0f; // y + att.q[3] = static_cast(std::sin(half_yaw)); // z + att_pub_->publish(att); + + // -- VehicleLocalPosition -- + px4_msgs::msg::VehicleLocalPosition pos{}; + pos.timestamp = ts; + pos.x = static_cast(north_); + pos.y = static_cast(east_); + pos.z = static_cast(-alt_); // NED: negative = above ground + pos.vx = 0.0f; + pos.vy = 0.0f; + pos.vz = 0.0f; + pos.xy_valid = true; + pos.z_valid = true; + pos.heading = static_cast(yaw_rad_); + pos_pub_->publish(pos); + + // -- VehicleStatus -- + px4_msgs::msg::VehicleStatus stat{}; + stat.timestamp = ts; + stat.arming_state = 2; // armed + stat.nav_state = 14; // offboard + stat_pub_->publish(stat); + + // -- Vision detections -- + if (enable_det_) { + geometry_msgs::msg::PoseArray det{}; + det.header.stamp = this->now(); + det.header.frame_id = "camera"; + + geometry_msgs::msg::Pose p; + p.position.x = det_u_; + p.position.y = det_v_; + p.position.z = static_cast(det_class_); + det.poses.push_back(p); + det_pub_->publish(det); + } + + tick_++; + if (tick_ % 50 == 0) { + RCLCPP_INFO(this->get_logger(), "Published %d mock cycles", tick_); + } + } + + rclcpp::Publisher::SharedPtr att_pub_; + rclcpp::Publisher::SharedPtr pos_pub_; + rclcpp::Publisher::SharedPtr stat_pub_; + rclcpp::Publisher::SharedPtr det_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + double alt_, north_, east_, yaw_rad_; + double det_u_, det_v_; + int64_t det_class_; + bool enable_det_; + int tick_ = 0; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/test/test_projection_math.cpp b/test/test_projection_math.cpp new file mode 100644 index 0000000..15be2f6 --- /dev/null +++ b/test/test_projection_math.cpp @@ -0,0 +1,269 @@ +// ============================================================================ +// Standalone projection math tests – no ROS2, only Eigen. +// +// Build & run: +// cd test && mkdir build && cd build +// cmake .. && make && ./test_projection_math +// ============================================================================ + +#include "somars_controls/projection.hpp" + +#include +#include +#include + +using namespace somars_controls::projection; + +static int g_pass = 0; +static int g_fail = 0; + +static void check(bool cond, const char * label) +{ + if (cond) { + std::printf(" [PASS] %s\n", label); + g_pass++; + } else { + std::printf(" [FAIL] %s\n", label); + g_fail++; + } +} + +static void check_near(double a, double b, double tol, const char * label) +{ + bool ok = std::fabs(a - b) < tol; + if (!ok) { + std::printf(" [FAIL] %s (got %.6f, expected %.6f, tol %.6f)\n", label, a, b, tol); + g_fail++; + } else { + std::printf(" [PASS] %s\n", label); + g_pass++; + } +} + +// --------------------------------------------------------------------------- +// Test: camera-to-body rotation matrix +// --------------------------------------------------------------------------- +static void test_cam_to_body_rotation() +{ + std::printf("\n--- test_cam_to_body_rotation ---\n"); + + // Forward-looking camera (pitch = 0) + { + Eigen::Matrix3d R = build_cam_to_body_rotation(0.0); + // Camera Z (into scene) should map to body X (forward) + Eigen::Vector3d cam_z(0, 0, 1); + Eigen::Vector3d result = R * cam_z; + check_near(result.x(), 1.0, 1e-9, "pitch=0: cam_Z → body_X"); + check_near(result.y(), 0.0, 1e-9, "pitch=0: cam_Z → body_X (y=0)"); + check_near(result.z(), 0.0, 1e-9, "pitch=0: cam_Z → body_X (z=0)"); + + // Camera X (right) should map to body Y (right) + Eigen::Vector3d cam_x(1, 0, 0); + result = R * cam_x; + check_near(result.x(), 0.0, 1e-9, "pitch=0: cam_X → body_Y (x=0)"); + check_near(result.y(), 1.0, 1e-9, "pitch=0: cam_X → body_Y"); + check_near(result.z(), 0.0, 1e-9, "pitch=0: cam_X → body_Y (z=0)"); + + // Camera Y (down in image) should map to body Z (down) + Eigen::Vector3d cam_y(0, 1, 0); + result = R * cam_y; + check_near(result.x(), 0.0, 1e-9, "pitch=0: cam_Y → body_Z (x=0)"); + check_near(result.y(), 0.0, 1e-9, "pitch=0: cam_Y → body_Z (y=0)"); + check_near(result.z(), 1.0, 1e-9, "pitch=0: cam_Y → body_Z"); + } + + // Straight-down camera (pitch = -π/2) + { + Eigen::Matrix3d R = build_cam_to_body_rotation(-M_PI_2); + // Camera Z (into scene) should map to body Z (down) + Eigen::Vector3d cam_z(0, 0, 1); + Eigen::Vector3d result = R * cam_z; + check_near(result.x(), 0.0, 1e-9, "pitch=-90°: cam_Z → body_Z (x=0)"); + check_near(result.y(), 0.0, 1e-9, "pitch=-90°: cam_Z → body_Z (y=0)"); + check_near(result.z(), 1.0, 1e-9, "pitch=-90°: cam_Z → body_Z"); + + // Camera X (right) should still map to body Y (right) + Eigen::Vector3d cam_x(1, 0, 0); + result = R * cam_x; + check_near(result.x(), 0.0, 1e-9, "pitch=-90°: cam_X → body_Y (x=0)"); + check_near(result.y(), 1.0, 1e-9, "pitch=-90°: cam_X → body_Y"); + check_near(result.z(), 0.0, 1e-9, "pitch=-90°: cam_X → body_Y (z=0)"); + } +} + +// --------------------------------------------------------------------------- +// Test: pixel projection with downward camera at level hover +// --------------------------------------------------------------------------- +static void test_pixel_projection_straight_down() +{ + std::printf("\n--- test_pixel_projection_straight_down ---\n"); + + double fx = 600.0, fy = 600.0, cx = 320.0, cy = 240.0; + Eigen::Matrix3d R_cam_to_body = build_cam_to_body_rotation(-M_PI_2); + Eigen::Quaterniond attitude = Eigen::Quaterniond::Identity(); // level, heading north + Eigen::Vector3d pos_ned(100.0, 50.0, -20.0); // 20 m above ground + + Eigen::Vector3d target; + + // Case 1: Center pixel → target directly below drone + { + bool hit = project_pixel_to_ned( + cx, cy, fx, fy, cx, cy, + R_cam_to_body, attitude, pos_ned, target); + check(hit, "center pixel hits ground"); + check_near(target.x(), 100.0, 0.1, "center pixel → target N = drone N"); + check_near(target.y(), 50.0, 0.1, "center pixel → target E = drone E"); + check_near(target.z(), 0.0, 1e-9, "center pixel → target D = 0 (ground)"); + } + + // Case 2: Pixel shifted right in image → target shifted East (body Y = NED East when heading North) + { + double u_right = cx + 300.0; // 300 pixels right of center + bool hit = project_pixel_to_ned( + u_right, cy, fx, fy, cx, cy, + R_cam_to_body, attitude, pos_ned, target); + check(hit, "right-of-center pixel hits ground"); + check(target.y() > 50.0, "right pixel → target East > drone East"); + // Expected offset: 20m * tan(atan(300/600)) = 20 * 0.5 = 10m east + check_near(target.y(), 60.0, 0.5, "right pixel offset ≈ 10m east"); + check_near(target.x(), 100.0, 0.5, "right pixel → same North"); + } + + // Case 3: Pixel shifted DOWN in image (v > cy) → target shifted SOUTH. + // For a downward camera the top of the image faces forward (body +X). + // Image bottom → body -X → South when heading North. + { + double v_down = cy + 300.0; // 300 pixels below center + bool hit = project_pixel_to_ned( + cx, v_down, fx, fy, cx, cy, + R_cam_to_body, attitude, pos_ned, target); + check(hit, "below-center pixel hits ground"); + check(target.x() < 100.0, "below pixel → target South (behind drone)"); + check_near(target.x(), 90.0, 0.5, "below pixel offset ≈ 10m south"); + check_near(target.y(), 50.0, 0.5, "below pixel → same East"); + } + + // Case 4: Pixel shifted UP in image (v < cy) → target shifted NORTH (forward). + { + double v_up = cy - 300.0; // 300 pixels above center + bool hit = project_pixel_to_ned( + cx, v_up, fx, fy, cx, cy, + R_cam_to_body, attitude, pos_ned, target); + check(hit, "above-center pixel hits ground"); + check(target.x() > 100.0, "above pixel → target North (ahead of drone)"); + check_near(target.x(), 110.0, 0.5, "above pixel offset ≈ 10m north"); + } +} + +// --------------------------------------------------------------------------- +// Test: pixel projection with yawed drone +// --------------------------------------------------------------------------- +static void test_pixel_projection_yawed() +{ + std::printf("\n--- test_pixel_projection_yawed ---\n"); + + double fx = 600.0, fy = 600.0, cx = 320.0, cy = 240.0; + Eigen::Matrix3d R_cam_to_body = build_cam_to_body_rotation(-M_PI_2); + Eigen::Vector3d pos_ned(0.0, 0.0, -20.0); + + // Drone yawed 90° clockwise (heading East): body X = NED East, body Y = NED South + Eigen::Quaterniond attitude( + Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ())); // yaw +90° in NED + + Eigen::Vector3d target; + + // Center pixel → still directly below + { + bool hit = project_pixel_to_ned( + cx, cy, fx, fy, cx, cy, + R_cam_to_body, attitude, pos_ned, target); + check(hit, "yawed: center pixel hits ground"); + check_near(target.x(), 0.0, 0.1, "yawed: center → N = 0"); + check_near(target.y(), 0.0, 0.1, "yawed: center → E = 0"); + } + + // Pixel shifted right → now body Y = NED South, so target goes South (negative N) + { + double u_right = cx + 300.0; + bool hit = project_pixel_to_ned( + u_right, cy, fx, fy, cx, cy, + R_cam_to_body, attitude, pos_ned, target); + check(hit, "yawed: right pixel hits ground"); + check(target.x() < -0.1, "yawed: right pixel → South (negative North)"); + check_near(target.x(), -10.0, 0.5, "yawed: right pixel ≈ 10m south"); + } +} + +// --------------------------------------------------------------------------- +// Test: ray that misses ground +// --------------------------------------------------------------------------- +static void test_ray_miss() +{ + std::printf("\n--- test_ray_miss ---\n"); + + double fx = 600.0, fy = 600.0, cx = 320.0, cy = 240.0; + // Forward-looking camera (pitch = 0) + Eigen::Matrix3d R_cam_to_body = build_cam_to_body_rotation(0.0); + Eigen::Quaterniond attitude = Eigen::Quaterniond::Identity(); + Eigen::Vector3d pos_ned(0.0, 0.0, -20.0); + Eigen::Vector3d target; + + // Center pixel with forward-looking camera → ray goes forward (horizontal), + // never hits ground (z component ≈ 0) + bool hit = project_pixel_to_ned( + cx, cy, fx, fy, cx, cy, + R_cam_to_body, attitude, pos_ned, target); + check(!hit, "forward-looking camera, center pixel: ray misses ground"); +} + +// --------------------------------------------------------------------------- +// Test: altitude scaling +// --------------------------------------------------------------------------- +static void test_altitude_scaling() +{ + std::printf("\n--- test_altitude_scaling ---\n"); + + double fx = 600.0, fy = 600.0, cx = 320.0, cy = 240.0; + Eigen::Matrix3d R_cam_to_body = build_cam_to_body_rotation(-M_PI_2); + Eigen::Quaterniond attitude = Eigen::Quaterniond::Identity(); + + double u_offset = cx + 300.0; // same pixel + Eigen::Vector3d target_10, target_40; + + // At 10m altitude + Eigen::Vector3d pos_10(0, 0, -10); + project_pixel_to_ned(u_offset, cy, fx, fy, cx, cy, + R_cam_to_body, attitude, pos_10, target_10); + + // At 40m altitude + Eigen::Vector3d pos_40(0, 0, -40); + project_pixel_to_ned(u_offset, cy, fx, fy, cx, cy, + R_cam_to_body, attitude, pos_40, target_40); + + // Offset should scale linearly with altitude + double ratio = target_40.y() / target_10.y(); + check_near(ratio, 4.0, 0.01, "4x altitude → 4x ground offset"); +} + +// --------------------------------------------------------------------------- +// main +// --------------------------------------------------------------------------- + +int main() +{ + std::printf("========================================\n"); + std::printf(" SOMARS Projection Math Tests\n"); + std::printf("========================================\n"); + + test_cam_to_body_rotation(); + test_pixel_projection_straight_down(); + test_pixel_projection_yawed(); + test_ray_miss(); + test_altitude_scaling(); + + std::printf("\n========================================\n"); + std::printf(" Results: %d passed, %d failed\n", g_pass, g_fail); + std::printf("========================================\n"); + + return g_fail > 0 ? 1 : 0; +}