toid_behaviors/src/approach_acorns.cpp

@@ -98,7 +98,7 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
   }
 
   // Smooth out approach
-  if (x * x + y * y < 0.42) {
+  if (x * x + y * y < 0.42 * 0.42) {
     return;
   }
 
@@ -275,7 +275,7 @@ ResultStatus ApproachAcorns::updateVel(
     return ResultStatus{Status::RUNNING};
   }
 
-  if (dist_left <= 0.02) {
+  if (dist_left >= 0.001 && dist_left <= 0.02) {
     out_vel.linear.x = velocityTarget(dist_left);
     out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
     last_speed_ = out_vel.linear.x;