From 94e7626fdafd3ad01fe63f6089553fdd193f312b Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Sat, 11 Apr 2026 11:13:26 +0200 Subject: [PATCH] Created node for checking if in position --- toid_bt/include/toid_bt/bt_executor.hpp | 13 ++++ toid_bt/include/toid_bt/plugin.hpp | 1 + .../toid_bt/plugins/in_position_decorator.hpp | 74 +++++++++++++++++++ .../toid_bt/plugins/send_text_action.hpp | 9 ++- toid_bt/src/bt_executor.cpp | 46 ++++++++++-- 5 files changed, 136 insertions(+), 7 deletions(-) create mode 100644 toid_bt/include/toid_bt/plugins/in_position_decorator.hpp diff --git a/toid_bt/include/toid_bt/bt_executor.hpp b/toid_bt/include/toid_bt/bt_executor.hpp index 7e95045..f93fd43 100644 --- a/toid_bt/include/toid_bt/bt_executor.hpp +++ b/toid_bt/include/toid_bt/bt_executor.hpp @@ -8,6 +8,7 @@ #include "tf2_ros/transform_listener.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "std_msgs/msg/string.hpp" namespace toid { class TreeExecutor : public BT::TreeExecutionServer { @@ -21,6 +22,12 @@ namespace toid { void position(geometry_msgs::msg::PoseStamped &pose); + void acorn_position(geometry_msgs::msg::PoseStamped &msg); + + void acorn_pose_cb(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); + + void to_flip_cb(std_msgs::msg::String::ConstSharedPtr msg); + std::string describeCustomNodes(); private: @@ -28,6 +35,12 @@ namespace toid { tf2_ros::Buffer::SharedPtr tf_buffer_; std::shared_ptr tf_listener_; + rclcpp::Subscription::SharedPtr acorn_pose_sub_; + rclcpp::Subscription::SharedPtr on_rotate_sub_; + + geometry_msgs::msg::PoseStamped acorn_pose_; + std::string to_flip_ = "0000"; + std::string base_frame_; std::string global_frame_; }; diff --git a/toid_bt/include/toid_bt/plugin.hpp b/toid_bt/include/toid_bt/plugin.hpp index 6a7fc85..303551d 100644 --- a/toid_bt/include/toid_bt/plugin.hpp +++ b/toid_bt/include/toid_bt/plugin.hpp @@ -5,4 +5,5 @@ namespace toid { using PoseFunc = std::function; + using FlipFunc = std::function; } \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp b/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp new file mode 100644 index 0000000..ea95d7f --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/in_position_decorator.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" + +namespace toid +{ + +class InPositionNode : public BT::DecoratorNode +{ +public: + InPositionNode( + const std::string & name, const BT::NodeConfig & conf, PoseFunc get_pose) + : BT::DecoratorNode(name, conf), get_pose(get_pose) + { + } + + static BT::PortsList providedPorts() { + return { + BT::InputPort("timeout", 1, {}) + }; + } + + private: + bool checkIfPosesAreClose(geometry_msgs::msg::PoseStamped &poseA, geometry_msgs::msg::PoseStamped &poseB) { + const double dx = abs(poseA.pose.position.x - poseB.pose.position.x); + return dx < 0.005; + } + + bool checkIfInPose(geometry_msgs::msg::PoseStamped &pose) { + const double dx = abs(0.2575 - pose.pose.position.x); + return dx < 0.005; + } + + BT::NodeStatus tick() override + { + if(status() == BT::NodeStatus::IDLE) { + setStatus(BT::NodeStatus::RUNNING); + last_pos_change = clock.now(); + } + + double timeout = getInput("timeout").value_or(1.0); + + + geometry_msgs::msg::PoseStamped pose; + get_pose(pose); + + if(checkIfInPose(pose)) { + if(checkIfPosesAreClose(last_pos, pose)) { + if(clock.now() - last_pos_change > rclcpp::Duration::from_seconds(timeout)) { + haltChild(); + return BT::NodeStatus::SUCCESS; + } + } else { + last_pos = pose; + last_pos_change = clock.now(); + } + } + + const BT::NodeStatus child_status = child_node_->executeTick(); + return child_status; + } + + geometry_msgs::msg::PoseStamped last_pos; + PoseFunc get_pose; + rclcpp::Time last_pos_change; + rclcpp::Clock clock; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/include/toid_bt/plugins/send_text_action.hpp b/toid_bt/include/toid_bt/plugins/send_text_action.hpp index bcb18fd..27a75d1 100644 --- a/toid_bt/include/toid_bt/plugins/send_text_action.hpp +++ b/toid_bt/include/toid_bt/plugins/send_text_action.hpp @@ -15,8 +15,8 @@ class SendTextNode : public BT::RosServiceNode { public: SendTextNode( - const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params) - : BT::RosServiceNode(name, conf, params) + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, FlipFunc get_text) + : BT::RosServiceNode(name, conf, params), get_text(get_text) { } @@ -29,6 +29,9 @@ public: bool setRequest(typename Request::SharedPtr &req) override { std::string text = getInput("text").value_or(""); + if(text=="") { + get_text(text); + } req->text = text; return true; @@ -38,6 +41,8 @@ public: { return BT::NodeStatus::SUCCESS; } +protected: + FlipFunc get_text; }; } // namespace toid \ No newline at end of file diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp index 889aede..124e6ef 100644 --- a/toid_bt/src/bt_executor.cpp +++ b/toid_bt/src/bt_executor.cpp @@ -6,6 +6,7 @@ #include "rclcpp/rclcpp.hpp" #include "toid_bt/plugins/empty_srv_action.hpp" #include "toid_bt/plugins/end_calib_action.hpp" +#include "toid_bt/plugins/in_position_decorator.hpp" #include "toid_bt/plugins/move_coords_action.hpp" #include "toid_bt/plugins/rotate_action.hpp" #include "toid_bt/plugins/rotate_towards_action.hpp" @@ -32,6 +33,13 @@ TreeExecutor::TreeExecutor(const rclcpp::NodeOptions opts) nav2_util::declare_parameter_if_not_declared( node(), "global_frame", rclcpp::ParameterValue("map")); node()->get_parameter("global_frame", global_frame_); + using namespace std::placeholders; + + acorn_pose_sub_ = node()->create_subscription( + "/closest_acorn", rclcpp::QoS(1), std::bind(&TreeExecutor::acorn_pose_cb, this, _1)); + + on_rotate_sub_ = node()->create_subscription( + "/to_flip", rclcpp::QoS(1), std::bind(&TreeExecutor::to_flip_cb, this, _1)); } void TreeExecutor::onTreeCreated(BT::Tree & tree) @@ -41,7 +49,12 @@ void TreeExecutor::onTreeCreated(BT::Tree & tree) void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) { + FlipFunc get_to_flip = [this](std::string & pose) { pose = this->to_flip_; }; PoseFunc get_pose = [this](geometry_msgs::msg::PoseStamped & pose) { this->position(pose); }; + PoseFunc get_acorn_pose = [this](geometry_msgs::msg::PoseStamped & pose) { + this->acorn_position(pose); + }; + rclcpp::Node::SharedPtr nh = node(); factory.registerNodeType( "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose); @@ -62,16 +75,22 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib")); - factory.registerNodeType("Seq1", BT::RosNodeParams(nh, "/sequence1")); + BT::RosNodeParams service_params1(nh, "/sequence1"); + service_params1.server_timeout = std::chrono::seconds(15); + factory.registerNodeType("Seq1", service_params1); - BT::RosNodeParams service_params(nh, "/sequence2"); - service_params.server_timeout = std::chrono::seconds(15); - factory.registerNodeType("Seq2", service_params); + BT::RosNodeParams service_params2(nh, "/sequence2"); + service_params2.server_timeout = std::chrono::seconds(20); + factory.registerNodeType("Seq2", service_params2, get_to_flip); - factory.registerNodeType("Seq3", BT::RosNodeParams(nh, "/sequence3")); + BT::RosNodeParams service_params3(nh, "/sequence3"); + service_params3.server_timeout = std::chrono::seconds(15); + factory.registerNodeType("Seq3", service_params3); factory.registerNodeType("DetectStuck", get_pose); + factory.registerNodeType("InPose", get_acorn_pose); + std::cout << describeCustomNodes() << std::endl; } @@ -80,6 +99,23 @@ void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose) nav2_util::getCurrentPose(pose, *tf_buffer_, global_frame_, base_frame_); } +void TreeExecutor::acorn_position(geometry_msgs::msg::PoseStamped & pose) { pose = acorn_pose_; } + +void TreeExecutor::acorn_pose_cb(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) +{ + auto node = this->node(); + try { + acorn_pose_ = tf_buffer_->transform(*msg, "base_footprint"); + } catch (const tf2::TransformException & e) { + RCLCPP_ERROR_THROTTLE(node->get_logger(), *node->get_clock(), 5000, "Transform timeout"); + } +} + +void TreeExecutor::to_flip_cb(std_msgs::msg::String::ConstSharedPtr msg) +{ + this->to_flip_ = msg->data; +} + std::optional TreeExecutor::onTreeExecutionCompleted( BT::NodeStatus status, bool was_cancelled) {