From b154b5e719aa1bef05d408c9265cabab06c69036 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Wed, 15 Apr 2026 15:39:30 +0200
Subject: [PATCH] Changed approach acorn to not use goal angle as target

---
 .../include/toid_behaviors/approach_acorns.hpp      |  1 +
 toid_behaviors/src/approach_acorns.cpp              | 13 ++++++++++---
 2 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp
index ff17434..6cc6397 100644
--- a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp
+++ b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp
@@ -60,6 +60,7 @@ protected:
   double new_target_angle_;
 
   geometry_msgs::msg::Pose target_pose_;
+  geometry_msgs::msg::Pose initial_pose_;
   double target_angle_;
   double target_sign_;
   bool backwards_;
diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp
index 8965f1b..b022e02 100644
--- a/toid_behaviors/src/approach_acorns.cpp
+++ b/toid_behaviors/src/approach_acorns.cpp
@@ -112,11 +112,16 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
 
   pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * 0.010;
   pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * 0.010;
-  tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw), pose_global.pose.orientation);
 
-  yaw = angles::normalize_angle(yaw);
 
   std::lock_guard _lock(mutex_);
+
+  const double dx = initial_pose_.position.x - pose_global.pose.position.x;
+  const double dy = initial_pose_.position.y - pose_global.pose.position.y;
+  double yaw_to_goal = std::atan2(dy,dx);
+
+  tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw_to_goal), pose_global.pose.orientation);
+  yaw = yaw_to_goal;
   auto [a, b, c] = avg_.push(pose_global.pose);
   distance_ = x * x + y * y;
   new_target_pose_.position.x = a;
@@ -142,14 +147,16 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
 }
 
 ResultStatus ApproachAcorns::onStart(
-  const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose &,
+  const std::shared_ptr<const MoveAction::Goal> command, const geometry_msgs::msg::Pose & pose,
   const geometry_msgs::msg::Twist & vel)
 {
   std::lock_guard _lock(mutex_);
   distance_ = 1.0;
   new_target_pose_.position.x = command->x;
   new_target_pose_.position.y = command->y;
+
   backwards_ = command->backwards;
+  initial_pose_ = pose;
 
   new_target_pose_.orientation = tf2::toMsg(tf2::Quaternion::createFromRPY(0, 0, command->theta));
   new_target_angle_ = command->theta;