Added start plug action server

toid_behaviors/include/toid_behaviors/approach_acorns.hpp

@@ -3,6 +3,7 @@
 #include "geometry_msgs/msg/pose.hpp"
 #include "toid_behaviors/scl.hpp"
 #include "toid_behaviors/simple_move.hpp"
+#include "toid_behaviors/rolling_average.hpp"
 #include "toid_msgs/action/simple_move_coords.hpp"
 #include "visualization_msgs/msg/marker.hpp"
 
@@ -52,6 +53,7 @@ protected:
   rclcpp::Subscription<PoseStamped>::SharedPtr acorn_pose_sub_;
   rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr target_pose_pub_;
   std::shared_mutex mutex_;
+  RollingAverage avg_ = RollingAverage(5);
 
   //Goal
   geometry_msgs::msg::Pose new_target_pose_;

toid_behaviors/include/toid_behaviors/rolling_average.hpp

@@ -0,0 +1,67 @@
+#pragma once
+
+#include <algorithm>
+#include <vector>
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+class RollingAverage
+{
+public:
+  using Pose2D = std::tuple<double, double, double>;
+  RollingAverage(size_t size = 0) : poses_(size), size_(size) {}
+
+  Pose2D push(geometry_msgs::msg::Pose & pose)
+  {
+    if(size_ > 0) {
+        return {};
+    }
+    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);
+      front_idx_ += 1;
+      front_idx_ %= size_;
+      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);
+
+    return {accum_x_ / data_count_, accum_y_ / data_count_, accum_theta_ / data_count_};
+  }
+
+  void reset() {
+    data_count_ = 0;
+    front_idx_  = 0;
+    back_idx_   = 0;
+    accum_x_ = 0;
+    accum_y_ = 0;
+    accum_theta_ = 0;
+  }
+
+private:
+  std::vector<geometry_msgs::msg::Pose> poses_;
+  const size_t size_;
+  size_t front_idx_ = 0;
+  size_t back_idx_ = 0;
+  size_t data_count_ = 0;
+
+  double accum_x_ = 0;
+  double accum_y_ = 0;
+  double accum_theta_ = 0;
+};
+
+}  // namespace toid

toid_behaviors/src/approach_acorns.cpp

@@ -89,9 +89,10 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
     RCLCPP_ERROR_ONCE(logger_, "Transform timeout [showing once]...");
     return;
   }
+
   double x = pose_local.pose.position.x;
   double y = pose_local.pose.position.y;
-  if (x * x + y * y > distance_ + 0.01) {
+  if (x * x + y * y > distance_ + 0.005) {
     return;
   }
 
@@ -104,10 +105,12 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
 
   yaw += M_PI/2;
 
+
   pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * + 0.005;
   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);
 
+
   if(debug_marker_) {
     visualization_msgs::msg::Marker marker;
     marker.lifetime.sec = 1.0;
@@ -124,10 +127,15 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
     target_pose_pub_->publish(marker);
   }
 
+  yaw = angles::normalize_angle(yaw);
+
   std::lock_guard _lock(mutex_);
+  auto[a,b,c] = avg_.push(pose_global.pose);
   distance_ = x * x + y * y;
-  new_target_pose_ = pose_global.pose;
-  new_target_angle_ = yaw;
+  new_target_pose_.position.x = a; 
+  new_target_pose_.position.y = b; 
+  tf2::convert(tf2::Quaternion::createFromRPY(0,0, c), new_target_pose_.orientation);
+  new_target_angle_ = c;
 }
 
 ResultStatus ApproachAcorns::onStart(
@@ -146,6 +154,8 @@ ResultStatus ApproachAcorns::onStart(
   target_sign_ = backwards_ ? -1.0 : 1.0;
   max_vel_speed_ = command->max_speed;
 
+  avg_.reset();
+
   if (command->max_speed == 0) {
     auto node = node_.lock();
     node->get_parameter(behavior_name_ + ".max_vel_speed", max_vel_speed_);