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 new file mode 100644 index 0000000..149fa88 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,70 @@ +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) + +add_executable(mock_nodes test/mock_nodes.cpp) +ament_target_dependencies(mock_nodes + rclcpp px4_msgs geometry_msgs std_msgs) + +# --------------------------------------------------------------------------- +# Install +# --------------------------------------------------------------------------- + +install(TARGETS + target_localizer + guidance_node + offboard_manager + mock_nodes + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + launch + config + test + 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..7ece575 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,130 @@ # 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 for the SOMARS drone. Runs on the Jetson Orin Nano, +talks to PX4 (Pixhawk 6X) through micro-XRCE-DDS. + +--- + +## Flight Day Checklist + +### 1. Plug in waypoints + +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, ...] +``` + +Set `waypoint_laps` to the number of full laps you want to fly before +entering the search/deliver phase. + +### 2. Start the stack + +```bash +# Terminal 1 — micro-XRCE-DDS Agent (bridge PX4 ↔ ROS 2) +MicroXRCEAgent serial --dev /dev/ttyTHS1 -b 921600 + +# Terminal 2 — build & launch +cd ~/somars_ws +colcon build --packages-select somars_controls +source install/setup.bash +ros2 launch somars_controls controls.launch.py +``` + +### 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 | + +--- + +## 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 +cd test && mkdir -p build && cd build +cmake .. && make && ./test_projection_math +``` + +**Integration test** (needs ROS 2 but no Pixhawk — uses mock publishers): + +```bash +ros2 launch somars_controls integration.launch.py +``` + +--- + +## Workspace Setup + +``` +somars-ws/ +├── src/ +│ ├── 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 or Jazzy +- **[px4_msgs](https://github.com/PX4/px4_msgs)** +- **Eigen3** +- **micro-XRCE-DDS Agent** (run separately on Jetson) 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/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 new file mode 100644 index 0000000..97f1fd3 --- /dev/null +++ b/include/somars_controls/guidance_node.hpp @@ -0,0 +1,95 @@ +#ifndef SOMARS_CONTROLS__GUIDANCE_NODE_HPP_ +#define SOMARS_CONTROLS__GUIDANCE_NODE_HPP_ + +#include +#include +#include +#include + +#include +#include + +namespace somars_controls +{ + +/// 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) +/// /fmu/out/vehicle_local_position – VehicleLocalPosition (PX4) +/// +/// Publications +/// /fmu/in/trajectory_setpoint – TrajectorySetpoint (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(); + + // ---- 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_; + + // ---- timer ---- + rclcpp::TimerBase::SharedPtr control_timer_; + + // ---- 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 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_; + 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..1711c96 --- /dev/null +++ b/include/somars_controls/offboard_manager.hpp @@ -0,0 +1,83 @@ +#ifndef SOMARS_CONTROLS__OFFBOARD_MANAGER_HPP_ +#define SOMARS_CONTROLS__OFFBOARD_MANAGER_HPP_ + +#include +#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(); + + // ---- 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_; + 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; + 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_; + double arm_delay_s_; + double heartbeat_rate_hz_; +}; + +} // namespace somars_controls + +#endif // SOMARS_CONTROLS__OFFBOARD_MANAGER_HPP_ 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/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..c21a344 --- /dev/null +++ b/launch/controls.launch.py @@ -0,0 +1,36 @@ +"""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') + waypoints_file = os.path.join(pkg_share, 'config', 'waypoints.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, waypoints_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..894095a --- /dev/null +++ b/src/guidance_node.cpp @@ -0,0 +1,328 @@ +#include "somars_controls/guidance_node.hpp" + +#include +#include +#include + +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()) +{ + // ---- 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); + + // 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 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(); + + // ---- 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)); + + // ---- publisher (to PX4 via micro-XRCE-DDS) ---- + setpoint_pub_ = this->create_publisher( + "/fmu/in/trajectory_setpoint", 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"); +} + +// --------------------------------------------------------------------------- +// 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 +// --------------------------------------------------------------------------- + +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; + + // 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 — priority: vision target > waypoint > hold +// --------------------------------------------------------------------------- + +void GuidanceNode::control_loop() +{ + uint64_t ts = this->get_clock()->now().nanoseconds() / 1000; // µs + px4_msgs::msg::TrajectorySetpoint sp{}; + sp.timestamp = ts; + + // 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(); + sp.yawspeed = std::numeric_limits::quiet_NaN(); + + if (!position_received_) { + setpoint_pub_->publish(sp); + return; + } + + 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_; + + // 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 + // 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, + "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); + desired_pos = vehicle_position_ned_ + direction * step; + desired_pos.z() = loiter_altitude_; + + 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()); + 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..70aba0c --- /dev/null +++ b/src/offboard_manager.cpp @@ -0,0 +1,186 @@ +#include "somars_controls/offboard_manager.hpp" + +#include +#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)); + + 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); + + 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; +} + +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 +// --------------------------------------------------------------------------- + +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. + // Uses current position if known; otherwise a safe default. + px4_msgs::msg::TrajectorySetpoint sp{}; + sp.timestamp = ts; + 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_++; + + // 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..ff60845 --- /dev/null +++ b/src/target_localizer.cpp @@ -0,0 +1,147 @@ +#include "somars_controls/target_localizer.hpp" +#include "somars_controls/projection.hpp" + +#include +#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. + // 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", + 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; + } + + 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 = std::clamp(static_cast(pose.position.z), 0, 3); + + 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 = 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_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); + } +} + +// --------------------------------------------------------------------------- +// Projection math +// --------------------------------------------------------------------------- + +bool TargetLocalizer::pixel_to_ned( + double u, double v, Eigen::Vector3d & target_ned) const +{ + 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( + 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; +} 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; +}