initial controls program

.gitignore

@@ -0,0 +1,4 @@
+build/
+install/
+log/
+test/build/

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

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)

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

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

include/somars_controls/guidance_node.hpp

@@ -0,0 +1,95 @@
+#ifndef SOMARS_CONTROLS__GUIDANCE_NODE_HPP_
+#define SOMARS_CONTROLS__GUIDANCE_NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/vehicle_local_position.hpp>
+#include <px4_msgs/msg/trajectory_setpoint.hpp>
+#include <geometry_msgs/msg/point_stamped.hpp>
+
+#include <Eigen/Dense>
+#include <vector>
+
+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<geometry_msgs::msg::PointStamped>::SharedPtr target_sub_;
+  rclcpp::Subscription<px4_msgs::msg::VehicleLocalPosition>::SharedPtr local_pos_sub_;
+
+  // ---- publishers ----
+  rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::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<GpsWaypoint>   gps_waypoints_;    // raw from config
+  std::vector<Eigen::Vector3d> 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_