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