Created node for checking if in position

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<tf2_ros::TransformListener> tf_listener_;
 
+        rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr acorn_pose_sub_;
+        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr on_rotate_sub_;
+
+        geometry_msgs::msg::PoseStamped acorn_pose_;
+        std::string to_flip_ = "0000";
+
         std::string base_frame_;
         std::string global_frame_;
     };

toid_bt/include/toid_bt/plugin.hpp

@@ -5,4 +5,5 @@
 
 namespace toid {
     using PoseFunc = std::function<void(geometry_msgs::msg::PoseStamped &)>;
+    using FlipFunc = std::function<void(std::string &)>;
 }

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<double>("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<double>("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

toid_bt/include/toid_bt/plugins/send_text_action.hpp

@@ -15,8 +15,8 @@ class SendTextNode : public BT::RosServiceNode<toid_msgs::srv::SendString>
 {
 public:
   SendTextNode(
-    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
-  : BT::RosServiceNode<toid_msgs::srv::SendString>(name, conf, params)
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, FlipFunc get_text)
+  : BT::RosServiceNode<toid_msgs::srv::SendString>(name, conf, params), get_text(get_text)
   {
   }
 
@@ -29,6 +29,9 @@ public:
 
   bool setRequest(typename Request::SharedPtr &req) override { 
     std::string text = getInput<std::string>("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

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<geometry_msgs::msg::PoseStamped>(
+    "/closest_acorn", rclcpp::QoS(1), std::bind(&TreeExecutor::acorn_pose_cb, this, _1));
+
+  on_rotate_sub_ = node()->create_subscription<std_msgs::msg::String>(
+    "/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<toid::MovePointNode>(
     "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
@@ -62,16 +75,22 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
 
   factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
 
-  factory.registerNodeType<toid::EmptySrvNode>("Seq1", BT::RosNodeParams(nh, "/sequence1"));
+  BT::RosNodeParams service_params1(nh, "/sequence1");
+  service_params1.server_timeout = std::chrono::seconds(15);
+  factory.registerNodeType<toid::EmptySrvNode>("Seq1", service_params1);
 
-  BT::RosNodeParams service_params(nh, "/sequence2");
-  service_params.server_timeout = std::chrono::seconds(15);
-  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params);
+  BT::RosNodeParams service_params2(nh, "/sequence2");
+  service_params2.server_timeout = std::chrono::seconds(20);
+  factory.registerNodeType<toid::SendTextNode>("Seq2", service_params2, get_to_flip);
 
-  factory.registerNodeType<toid::EmptySrvNode>("Seq3", BT::RosNodeParams(nh, "/sequence3"));
+  BT::RosNodeParams service_params3(nh, "/sequence3");
+  service_params3.server_timeout = std::chrono::seconds(15);
+  factory.registerNodeType<toid::EmptySrvNode>("Seq3", service_params3);
 
   factory.registerNodeType<toid::StuckDetectorNode>("DetectStuck", get_pose);
 
+  factory.registerNodeType<toid::InPositionNode>("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<std::string> TreeExecutor::onTreeExecutionCompleted(
   BT::NodeStatus status, bool was_cancelled)
 {