From 773411951d22cc1297fd4d88c4f5ed0695254f66 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Mon, 13 Apr 2026 18:33:16 +0200
Subject: [PATCH] approach acorn modifications

---
 toid_behaviors/src/approach_acorns.cpp | 11 ++++-------
 1 file changed, 4 insertions(+), 7 deletions(-)

diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp
index b3f0f3f..e1c15b3 100644
--- a/toid_behaviors/src/approach_acorns.cpp
+++ b/toid_behaviors/src/approach_acorns.cpp
@@ -96,7 +96,6 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
     return;
   }
 
-
   double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation));
 
   if(tf2::getYaw(pose_local.pose.orientation) > 0) {
@@ -110,8 +109,6 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
   pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * + 0.005;
   tf2::convert(tf2::Quaternion::createFromRPY(0,0,yaw ), pose_global.pose.orientation);
 
-
-
   yaw = angles::normalize_angle(yaw);
 
   std::lock_guard _lock(mutex_);
@@ -247,15 +244,15 @@ ResultStatus ApproachAcorns::updateVel(
 
   double dist_left = distanceToTarget(pose, target_pose_.position, target_angle_, backwards_);
 
-  if (dist_left <= 0.001) {
+  const double current_yaw = tf2::getYaw(pose.orientation);
+  double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
+
+  if (dist_left <= 0.001 && std::fabs(dist_left) <= 0.005) {
     out_vel.linear.x = 0;
     out_vel.angular.z = 0;
     return ResultStatus{Status::SUCCEEDED};
   }
 
-  const double current_yaw = tf2::getYaw(pose.orientation);
-  double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
-
   geometry_msgs::msg::Pose last_ok_pose;
   if (collisionDetection(pose, last_ok_pose)) {
     dist_left = distanceToTarget(