initial controls program by aborjigin · Pull Request #1 · Slugbotics/somars-embedded

aborjigin · 2026-04-29T00:25:05Z

It needs some tweaking
Need help from Vision team

Copilot

Pull request overview

Introduces an initial ROS 2 (ament_cmake) controls package (somars_controls) for PX4 offboard flight, including target localization from vision detections, guidance setpoint generation, and an offboard/arming heartbeat manager.

Changes:

Added three ROS 2 C++ nodes: target_localizer, guidance_node, and offboard_manager.
Added package scaffolding (CMake/package.xml), launch file, and shared runtime parameters YAML.
Expanded README with architecture + quick start instructions.

Reviewed changes

Copilot reviewed 11 out of 11 changed files in this pull request and generated 8 comments.

Show a summary per file

File	Description
`src/target_localizer.cpp`	Implements pixel-to-ground-plane projection into NED using PX4 attitude/position.
`include/somars_controls/target_localizer.hpp`	Declares TargetLocalizer node interface and camera/vehicle state.
`src/guidance_node.cpp`	Implements target-following guidance and publishes PX4 trajectory setpoints.
`include/somars_controls/guidance_node.hpp`	Declares GuidanceNode node interface, state, and parameters.
`src/offboard_manager.cpp`	Implements offboard heartbeat publishing and optional auto-arm/offboard commands.
`include/somars_controls/offboard_manager.hpp`	Declares OffboardManager node interface and command helpers.
`launch/controls.launch.py`	Launches the three nodes with a shared params file.
`config/params.yaml`	Provides default parameters for all nodes.
`CMakeLists.txt`	Adds build/install rules for the new nodes and configs.
`package.xml`	Declares ROS package metadata and dependencies.
`README.md`	Documents architecture, usage, and dependencies.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot · 2026-04-29T00:28:31Z

+Eigen::Matrix3d TargetLocalizer::quat_to_rotation(
+  double w, double x, double y, double z)
+{
+  return Eigen::Quaterniond(w, x, y, z).normalized().toRotationMatrix();
+}


quat_to_rotation() is declared/defined but never used anywhere in this package. This adds dead code and can confuse future maintenance; either remove it or use it consistently (e.g., in attitude_cb / pixel_to_ned).

Copilot · 2026-04-29T00:28:32Z

+  auto period = std::chrono::duration<double>(1.0 / heartbeat_rate_hz_);
+  heartbeat_timer_ = this->create_wall_timer(
+    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
+    std::bind(&OffboardManager::heartbeat_loop, this));


heartbeat_rate_hz_ is used as a divisor to compute the timer period. If this parameter is set to 0 or negative, 1.0 / heartbeat_rate_hz_ becomes inf/invalid and can break timer creation. Validate that heartbeat_rate_hz_ > 0 (and fail fast with a clear log) before creating the timer.

Copilot · 2026-04-29T00:28:32Z

+  // 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);
+


This node continuously publishes a hard-coded hold-position setpoint to /fmu/in/trajectory_setpoint while guidance_node also publishes to the same topic. With two publishers, PX4 will see interleaved setpoints (origin hold vs guidance), which can cause unstable/incorrect flight behavior. Use a single setpoint publisher (recommended), or gate the hold-setpoint publishing (e.g., only before guidance is active, or behind a parameter/topic that indicates an active guidance source).

Copilot · 2026-04-29T00:28:32Z

+  auto period = std::chrono::duration<double>(1.0 / rate_hz);
+  control_timer_ = this->create_wall_timer(
+    std::chrono::duration_cast<std::chrono::nanoseconds>(period),
+    std::bind(&GuidanceNode::control_loop, this));
+


control_rate_hz is used as a divisor to compute the control loop period. If it is configured as 0 or negative, the timer period becomes inf/invalid and timer creation can fail. Add validation that rate_hz > 0 (and fail fast with a clear log) before creating the timer.

Copilot · 2026-04-29T00:28:32Z

+  if (!position_received_) {
+    // Nothing we can do until we know where we are.
+    setpoint_pub_->publish(sp);
+    return;
+  }


When position_received_ is false, this publishes a TrajectorySetpoint with default-initialized position[] (zeros). With OffboardControlMode.position = true, that can command the vehicle toward (0,0,0) before any local position has been received. Either avoid publishing a position setpoint until position is known, or explicitly set sp.position[] to NaN (and/or adjust OffboardControlMode) so PX4 ignores it.

Nodes do not need to publish every loop, and should not publish meaningless data.

Copilot · 2026-04-29T00:28:32Z

+      // 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_;


The step calculation uses a hard-coded 0.5 seconds (approach_speed_ * 0.5) even though the control loop rate is configurable. At 20 Hz this makes the commanded position jump far too quickly (effectively multiplying speed), and at low rates it slows down. Compute the step using the actual control loop dt (e.g., approach_speed_ / rate_hz) or store the timer period and use it here.

Copilot · 2026-04-29T00:28:33Z

+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+
+  <build_depend>eigen</build_depend>


<build_depend>eigen</build_depend> is not a standard ROS 2 package dependency (Eigen is typically declared as eigen3 / eigen3_cmake_module). This can break rosdep resolution and CI builds. Update the manifest to use the correct Eigen package dependency names consistent with find_package(Eigen3 REQUIRED) in CMake.

Suggested change

<build_depend>eigen</build_depend>

<build_depend>eigen3</build_depend>

Copilot · 2026-04-29T00:28:33Z

+  Eigen::Vector3d ray_cam((u - cx_) / fx_, (v - cy_) / fy_, 1.0);
+  ray_cam.normalize();


camera_fx/camera_fy are used as divisors when computing the normalized ray. If either parameter is configured as 0 (or very close to 0), this will generate inf/NaN rays and break localization. Add parameter validation (e.g., reject non-positive values with a clear log + throw/early return) before using them.

Kyle-Eldridge

This is a good first version of the code, but there are several major bugs that will prevent it from working.

Kyle-Eldridge · 2026-04-30T17:48:33Z

+  // Expire targets older than 2 seconds
+  bool target_stale = (this->now() - last_target_time_).seconds() > 2.0;


The target does not move, so this could be longer. Instead, adding something like a debouncer would help filter out false targets. It could either be implemented here, in the target callback, or in the target localization node.

This line could be completely removed, since we might see a target while doing another task but not want to fly to it yet.

Kyle-Eldridge · 2026-04-30T17:52:20Z

+    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);


We want to either drop something on the target or land on it. We can keep code similar to this to decide when we are close, but then we need to go to the correct height and send a signal to dop the payload.

Kyle-Eldridge · 2026-04-30T17:58:13Z

+      // 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


I'm not sure how the autonomous controller works, but could we just update the desired pose to be directly above the target? As it gets closer, this desired pose will update, probably becoming more exact.

Kyle-Eldridge · 2026-04-30T18:00:18Z

+    // ---- No active target – hold current XY at loiter altitude ----
+    sp.position[0] = static_cast<float>(vehicle_position_ned_.x());
+    sp.position[1] = static_cast<float>(vehicle_position_ned_.y());
+    sp.position[2] = static_cast<float>(loiter_altitude_);


The competition gives us a position where we should be able to see the target. If we haven't seen a target yet, we should go there.

Kyle-Eldridge · 2026-04-30T18:02:45Z

+
+  setpoint_pub_->publish(sp);
+}


This node should not always control the drone. Either this node should not always publish or the main node should ignore its messages until it's ready to drop the payload. After dropping, this node should be ignored again.

Kyle-Eldridge · 2026-04-30T18:54:42Z

+  // 1. Pixel → normalised camera-frame ray
+  Eigen::Vector3d ray_cam((u - cx_) / fx_, (v - cy_) / fy_, 1.0);


This could be done on the pi/Jetson. Currently, it sends the data as angles, but this calculation is simpler and better for this use. I will change the somars-vision code to send this data instead.

Kyle-Eldridge · 2026-04-30T18:57:36Z

+  // 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
+  }


I thought down was defined as negative in NED, but this returns false for negative z.

Kyle-Eldridge · 2026-04-30T19:10:43Z

+  // 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();


I think this rotation is wrong. The vector [0, 0, 1] in the camera frame should be rotated π/2 if the pitch is 0 to become [1, 0, 0]. You might have to do something like pitch+M_PI_2, depending on the convention for rotation direction.

Kyle-Eldridge · 2026-04-30T19:13:49Z

+  target_ned = vehicle_position_ned_ + t * ray_ned;
+  target_ned.z() = 0.0;  // on the ground plane


If the calculation was correct, this second line is not necessary. Maybe check that it is approximately 0 and either throw or log an error if not.

Kyle-Eldridge · 2026-04-30T19:15:12Z

+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<somars_controls::TargetLocalizer>());
+  rclcpp::shutdown();
+  return 0;


See earlier comment about exceptions.

initial controls program

cbd114b

Copilot AI review requested due to automatic review settings April 29, 2026 00:25

Copilot started reviewing on behalf of aborjigin April 29, 2026 00:25 View session

aborjigin requested a review from AkshajGG April 29, 2026 00:25

Copilot AI reviewed Apr 29, 2026

View reviewed changes

Kyle-Eldridge self-requested a review April 30, 2026 16:33

Kyle-Eldridge requested changes Apr 30, 2026

View reviewed changes

fixing issues and adding in waypoint navigation

cffad20

	<build_depend>eigen</build_depend>
	<build_depend>eigen3</build_depend>

		Eigen::Vector3d ray_cam((u - cx_) / fx_, (v - cy_) / fy_, 1.0);
		ray_cam.normalize();

		// Expire targets older than 2 seconds
		bool target_stale = (this->now() - last_target_time_).seconds() > 2.0;

		// 1. Pixel → normalised camera-frame ray
		Eigen::Vector3d ray_cam((u - cx_) / fx_, (v - cy_) / fy_, 1.0);

		target_ned = vehicle_position_ned_ + t * ray_ned;
		target_ned.z() = 0.0; // on the ground plane

Conversation

aborjigin commented Apr 29, 2026

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Apr 29, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 29, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 29, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 29, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 29, 2026

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 29, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 29, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 29, 2026

Choose a reason for hiding this comment

Uh oh!

Kyle-Eldridge left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants