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_);

toid_interaction/package.xml

@@ -13,6 +13,7 @@
   <test_depend>python3-pytest</test_depend>
 
   <depend>python3-serial</depend>
+  <depend>python3-gpiozero</depend>
 
   <export>
     <build_type>ament_python</build_type>

toid_interaction/setup.py

@@ -25,7 +25,9 @@ setup(
     entry_points={
         'console_scripts': [
             'sequence = toid_interaction.mechanism.sekvenca_2026:main',
-            'node = toid_interaction.interaction_node:main'
+            'node = toid_interaction.interaction_node:main',
+            'cam_calib = toid_interaction.camera:main',
+            'cam_calib1 = toid_interaction.camera1:main'
         ],
     },
 )

toid_interaction/toid_interaction/interaction_node.py

@@ -2,52 +2,60 @@ import rclpy
 from rclpy.node import Node
 from std_srvs.srv import Empty
 
+from gpiozero import Button, OutputDevice
+
 from serial.tools import list_ports
 
 from toid_interaction.mechanism.sekvenca_2026 import okreni, okreni_niz
 from toid_interaction.mechanism.zidovi_load import ZidoviAction
 from toid_interaction.mechanism.zupcanik import ZupcanikAction
+from toid_msgs.action import EmptyAction
 from toid_msgs.srv import SendString
 
+from rclpy.action.server import ServerGoalHandle
+from rclpy.action.server import ActionServer
+import asyncio
+
+
 class InteracitionNode(Node):
     step: int = 0
+    btn_: Button
+    output_pin_: OutputDevice
+    start_pin_action_: ActionServer
+
     def __init__(self):
-        super().__init__('ToidInteractionNode')
+        super().__init__("ToidInteractionNode")
 
         self.find_sigma()
 
-        self.srv = self.create_service(
-            Empty,
-            '/sequence1',
-            self.sequence1_cb
-        )
-
-        self.srv = self.create_service(
-            SendString,
-            '/sequence2',
-            self.sequence2_cb
-        )
-
-        self.srv = self.create_service(
-            Empty,
-            '/sequence3',
-            self.sequence3_cb
-        )
-
-
-
+        self.srv = self.create_service(Empty, "/sequence1", self.sequence1_cb)
         self.get_logger().info("Service 'sequence1' ready.")
 
+        self.srv = self.create_service(SendString, "/sequence2", self.sequence2_cb)
+        self.get_logger().info("Service 'sequence2' ready.")
+
+        self.srv = self.create_service(Empty, "/sequence3", self.sequence3_cb)
+        self.get_logger().info("Service 'sequence3' ready.")
+
+        self.btn_ = Button(17)
+        self.output_pin_ = OutputDevice(27)
+
+        self.start_pin_action_ = ActionServer(
+            self, EmptyAction, "/start_plug", execute_callback=self.start_plug_action_cb
+        )
+        self.get_logger().info("Action 'start_plug' ready.")
+
+
     def find_sigma(self):
         for port_info in list_ports.comports():
-            if port_info.vid == 0x1a86 and port_info.pid == 0x55d3:
+            if port_info.vid == 0x1A86 and port_info.pid == 0x55D3:
                 break
 
         print(port_info.device)
         self.st_motor_device_name = port_info.device
 
     def sequence1_cb(self, request, response):
-        if(self.step != 0):
+        if self.step != 0:
             return Empty.Response()
         okreni(5)
         zupcanik = ZupcanikAction(self.st_motor_device_name)
@@ -55,8 +63,8 @@ class InteracitionNode(Node):
         self.step = 1
         return response
 
-    def sequence2_cb(self, request : SendString.Request, response : SendString.Response):
-        if(self.step != 1):
+    def sequence2_cb(self, request: SendString.Request, response: SendString.Response):
+        if self.step != 1:
             return Empty.Response()
         zidovi = ZidoviAction(self.st_motor_device_name)
         zidovi.beli_zid(1)
@@ -70,11 +78,11 @@ class InteracitionNode(Node):
         return response
 
     def sequence3_cb(self, request, response):
-        if(self.step != 2):
+        if self.step != 2:
             return Empty.Response()
 
         zupcanik = ZupcanikAction(self.st_motor_device_name)
-        zidovi = ZidoviAction(self.st_motor_device_name) 
+        zidovi = ZidoviAction(self.st_motor_device_name)
 
         zupcanik.zupcanik(1, 1010, 25)
         zidovi.plavi_zid(0, TargetPos=150)
@@ -83,6 +91,14 @@ class InteracitionNode(Node):
         self.step = 0
         return response
 
+    async def start_plug_action_cb(self, goal_handle: ServerGoalHandle):
+        while not self.btn_.is_active:
+            await asyncio.sleep(0.1)
+        while self.btn_.is_active:
+            await asyncio.sleep(0.1)
+        goal_handle.succeed()
+        return EmptyAction.Result()
+
 
 def main(args=None):
     rclpy.init(args=args)
@@ -93,5 +109,5 @@ def main(args=None):
     rclpy.shutdown()
 
 
-if __name__ == '__main__':
-    main()
+if __name__ == "__main__":
+    main()

toid_msgs/CMakeLists.txt

@@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "action/SimpleMoveCoords.action"
   "action/SimpleRotate.action"
   "action/SimpleTranslateX.action"
+  "action/EmptyAction.action"
   DEPENDENCIES geometry_msgs
 )
 

toid_msgs/action/EmptyAction.action

@@ -0,0 +1,2 @@
+---
+---