Fixed camera based approach

toid_behaviors/include/toid_behaviors/rolling_average.hpp

@@ -21,7 +21,7 @@ public:
     if(size_ == 0) {
         return {};
     }
-    if (size_-1 == data_count_) {
+    if (size_ == data_count_) {
       accum_x_ -= poses_[front_idx_].position.x;
       accum_y_ -= poses_[front_idx_].position.y;
       accum_theta_ -= tf2::getYaw(poses_[front_idx_].orientation);
@@ -30,16 +30,16 @@ public:
       data_count_--;
     }
 
-    back_idx_ += 1;
-    back_idx_ %= size_;
-    data_count_++;
-
     poses_[back_idx_] = pose;
 
     accum_x_ += poses_[back_idx_].position.x;
     accum_y_ += poses_[back_idx_].position.y;
     accum_theta_ += tf2::getYaw(poses_[back_idx_].orientation);
 
+    back_idx_ += 1;
+    back_idx_ %= size_;
+    data_count_++;
+
     return {accum_x_ / data_count_, accum_y_ / data_count_, accum_theta_ / data_count_};
   }
 

toid_behaviors/src/approach_acorns.cpp

@@ -93,7 +93,7 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
 
   double x = pose_local.pose.position.x;
   double y = pose_local.pose.position.y;
-  if (x * x + y * y > distance_ + 0.005) {
+  if (std::sqrt(x * x + y * y) > distance_ + 0.005) {
     return;
   }
 
@@ -105,15 +105,13 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
   double yaw = angles::normalize_angle(tf2::getYaw(pose_global.pose.orientation));
 
   if (tf2::getYaw(pose_local.pose.orientation) > 0) {
-    yaw += M_PI;
+    pose_global.pose.position.x += std::cos(yaw) * 0.01 - std::sin(yaw) * 0.35;
+    pose_global.pose.position.y += std::sin(yaw) * 0.01 + std::cos(yaw) * 0.35;
+  } else {
+    pose_global.pose.position.x += std::cos(yaw) * -0.01 - std::sin(yaw) * -0.35;
+    pose_global.pose.position.y += std::sin(yaw) * -0.01 + std::cos(yaw) * -0.35;
   }
 
-  yaw += M_PI / 2;
-
-  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;
-
-
   std::lock_guard _lock(mutex_);
 
   const double dx = pose_global.pose.position.x - initial_pose_.position.x;