From b3ea552cfb50f62f1e45f27b4fc30612d9dd4eaf Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Fri, 17 Apr 2026 12:42:39 +0200
Subject: [PATCH] Strategy blue part 1

---
 toid_bt/behavior_trees/behavior1.xml          | 113 +++++++++++++++++-
 .../toid_bt/plugins/translate_x_action.hpp    |  17 ++-
 toid_bt/src/bt_executor.cpp                   |   2 +-
 3 files changed, 126 insertions(+), 6 deletions(-)

diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml
index 868ff06..d9b071a 100644
--- a/toid_bt/behavior_trees/behavior1.xml
+++ b/toid_bt/behavior_trees/behavior1.xml
@@ -1,5 +1,9 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
+  <BehaviorTree ID="pickup_crates">
+    <Seq1 service_name=""/>
+  </BehaviorTree>
+
   <BehaviorTree ID="seq">
     <Sequence>
       <InPose timeout="1.000000">
@@ -48,6 +52,83 @@
     </Sequence>
   </BehaviorTree>
 
+  <BehaviorTree ID="seq2">
+    <Sequence>
+      <ZeroOdom service_name=""/>
+      <SetInitialPose y="0.806"
+                      theta="-90"
+                      frame_id="map"
+                      x="1.328"
+                      topic_name="__default__placeholder__"/>
+      <WaitPullPin action_name=""/>
+      <InPose timeout="1.000000">
+        <MovePointSimple x="1.328"
+                         y="0.354"
+                         theta="-90"
+                         max_speed="0.300000"
+                         backwards="false"
+                         action_name=""/>
+      </InPose>
+      <Seq1 service_name=""/>
+      <Parallel failure_count="1"
+                success_count="-1">
+        <Seq2 text=""
+              service_name=""/>
+        <MovePointSimple x="0.795"
+                         y="-0.2"
+                         theta="180"
+                         max_speed="0.500000"
+                         backwards="false"
+                         action_name=""/>
+      </Parallel>
+      <Parallel failure_count="1"
+                success_count="-1">
+        <Seq3 service_name=""/>
+        <Sequence>
+          <Sleep msec="500"/>
+          <TranslateX x="-0.3"
+                      max_speed="0.000000"
+                      action_name=""/>
+          <RotateTowards x="1.328"
+                         y="0.354"
+                         max_speed="2.000000"
+                         min_angle="0.000000"
+                         action_name=""/>
+        </Sequence>
+      </Parallel>
+      <MovePointSimple x="1.328"
+                       y="0.354"
+                       theta="-90"
+                       max_speed="0.400000"
+                       backwards="true"
+                       action_name=""/>
+      <Seq1 service_name=""/>
+      <ParallelAll max_failures="1">
+        <Seq2 text=""
+              service_name=""/>
+        <MovePointSimple x="1.328"
+                         y="-0.2"
+                         theta="-90"
+                         max_speed="0.200000"
+                         backwards="false"
+                         action_name=""/>
+      </ParallelAll>
+      <Parallel failure_count="1"
+                success_count="-1">
+        <Seq3 service_name=""/>
+        <Sequence>
+          <Sleep msec="500"/>
+          <MovePointSimple x="1.328"
+                           y="0.354"
+                           theta="-90"
+                           max_speed="0.400000"
+                           backwards="true"
+                           action_name=""/>
+        </Sequence>
+      </Parallel>
+    </Sequence>
+  </BehaviorTree>
+
   <BehaviorTree ID="test_1">
     <Sequence>
       <DetectStuck timeout="1.000000">
@@ -116,10 +197,40 @@
       <input_port name="service_name"
                   type="std::string">Service name</input_port>
     </Action>
+    <Condition ID="SetInitialPose">
+      <input_port name="y"
+                  default="0.000000"
+                  type="double">Y position in meters</input_port>
+      <input_port name="theta"
+                  default="0.000000"
+                  type="double">Heading in degrees</input_port>
+      <input_port name="frame_id"
+                  default="map"
+                  type="std::string">Reference frame</input_port>
+      <input_port name="x"
+                  default="0.000000"
+                  type="double">X position in meters</input_port>
+      <input_port name="topic_name"
+                  default="__default__placeholder__"
+                  type="std::string">Topic name</input_port>
+    </Condition>
+    <Action ID="TranslateX">
+      <input_port name="x"
+                  type="double"/>
+      <input_port name="max_speed"
+                  default="0.000000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="WaitPullPin">
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
     <Action ID="ZeroOdom">
       <input_port name="service_name"
                   type="std::string">Service name</input_port>
     </Action>
   </TreeNodesModel>
 
-</root>
\ No newline at end of file
+</root>
diff --git a/toid_bt/include/toid_bt/plugins/translate_x_action.hpp b/toid_bt/include/toid_bt/plugins/translate_x_action.hpp
index 8ec9f6c..fe41c10 100644
--- a/toid_bt/include/toid_bt/plugins/translate_x_action.hpp
+++ b/toid_bt/include/toid_bt/plugins/translate_x_action.hpp
@@ -5,18 +5,18 @@
 #include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "toid_bt/plugin.hpp"
-#include "toid_msgs/action/simple_translate_x.hpp"
+#include "toid_msgs/action/simple_move_coords.hpp"
 
 namespace toid
 {
 
-class TranslateXNode : public BT::RosActionNode<toid_msgs::action::SimpleTranslateX>
+class TranslateXNode : public BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>
 {
 public:
   TranslateXNode(
     const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
     PoseFunc pose)
-  : BT::RosActionNode<toid_msgs::action::SimpleTranslateX>(name, conf, params), get_pose(pose)
+  : BT::RosActionNode<toid_msgs::action::SimpleMoveCoords>(name, conf, params), get_pose(pose)
   {
   }
 
@@ -34,7 +34,16 @@ public:
     auto x_goal = getInput<double>("x").value();
     auto max_speed = getInput<double>("max_speed").value();
 
-    goal.distance = x_goal;
+    geometry_msgs::msg::PoseStamped pose;
+    get_pose(pose);
+
+    double yaw = tf2::getYaw(pose.pose.orientation);
+
+    goal.x = pose.pose.position.x + cos(yaw) * x_goal;
+    goal.y = pose.pose.position.y + sin(yaw) * x_goal;
+    goal.theta = yaw;
+    goal.backwards = x_goal < 0;
+    goal.mode = 1;
     goal.max_speed = max_speed;
 
     return true;
diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp
index ff50348..a52ee36 100644
--- a/toid_bt/src/bt_executor.cpp
+++ b/toid_bt/src/bt_executor.cpp
@@ -75,7 +75,7 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
     "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose);
 
   factory.registerNodeType<toid::TranslateXNode>(
-    "TranslateX", BT::RosNodeParams(nh, "/translateX"), get_pose);
+    "TranslateX", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
 
   factory.registerNodeType<toid::EndCalibNode>(
     "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose);