From 6633ef41faf125f1edab331acf63e82dcdde9da2 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Mon, 13 Apr 2026 13:24:52 +0200 Subject: [PATCH] Added start plug action server --- .../toid_behaviors/approach_acorns.hpp | 2 + .../toid_behaviors/rolling_average.hpp | 67 +++++++++++++++++ toid_behaviors/src/approach_acorns.cpp | 16 +++- toid_interaction/package.xml | 1 + toid_interaction/setup.py | 4 +- .../toid_interaction/interaction_node.py | 74 +++++++++++-------- toid_msgs/CMakeLists.txt | 1 + toid_msgs/action/EmptyAction.action | 2 + 8 files changed, 134 insertions(+), 33 deletions(-) create mode 100644 toid_behaviors/include/toid_behaviors/rolling_average.hpp create mode 100644 toid_msgs/action/EmptyAction.action diff --git a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp b/toid_behaviors/include/toid_behaviors/approach_acorns.hpp index 0ce1a07..ddd0f1c 100644 --- a/toid_behaviors/include/toid_behaviors/approach_acorns.hpp +++ b/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::SharedPtr acorn_pose_sub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr target_pose_pub_; std::shared_mutex mutex_; + RollingAverage avg_ = RollingAverage(5); //Goal geometry_msgs::msg::Pose new_target_pose_; diff --git a/toid_behaviors/include/toid_behaviors/rolling_average.hpp b/toid_behaviors/include/toid_behaviors/rolling_average.hpp new file mode 100644 index 0000000..9c5b2dd --- /dev/null +++ b/toid_behaviors/include/toid_behaviors/rolling_average.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include +#include + +#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; + 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 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 \ No newline at end of file diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index c994cbf..85c0a5d 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/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_); diff --git a/toid_interaction/package.xml b/toid_interaction/package.xml index 2959e79..6fde46c 100644 --- a/toid_interaction/package.xml +++ b/toid_interaction/package.xml @@ -13,6 +13,7 @@ python3-pytest python3-serial + python3-gpiozero ament_python diff --git a/toid_interaction/setup.py b/toid_interaction/setup.py index 6473e97..aadf802 100644 --- a/toid_interaction/setup.py +++ b/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' ], }, ) diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py index 1ccdd38..b887c9a 100644 --- a/toid_interaction/toid_interaction/interaction_node.py +++ b/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() \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/toid_msgs/CMakeLists.txt b/toid_msgs/CMakeLists.txt index 6c0b3fd..2d33e00 100644 --- a/toid_msgs/CMakeLists.txt +++ b/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 ) diff --git a/toid_msgs/action/EmptyAction.action b/toid_msgs/action/EmptyAction.action new file mode 100644 index 0000000..a49ba48 --- /dev/null +++ b/toid_msgs/action/EmptyAction.action @@ -0,0 +1,2 @@ +--- +--- \ No newline at end of file