From c7caa69bfa03aa0c179815b676ba9d4e6c8aabc9 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Tue, 24 Mar 2026 20:40:14 +0100
Subject: [PATCH] Added print to print out new calculated width

---
 toid_bt/behavior_trees/behavior1.xml          | 78 +++++++++++++++++--
 .../behavior_trees/calibration_routines.xml   | 39 ++++++++--
 .../toid_bt/plugins/end_calib_action.hpp      |  4 +-
 3 files changed, 106 insertions(+), 15 deletions(-)

diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml
index b6becc0..f4ed8bd 100644
--- a/toid_bt/behavior_trees/behavior1.xml
+++ b/toid_bt/behavior_trees/behavior1.xml
@@ -1,17 +1,81 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
+  <BehaviorTree ID="seq1">
+    <Sequence>
+      <ZeroOdom service_name=""/>
+      <MovePointSimple x="1.05"
+                       y="0"
+                       theta="0"
+                       max_speed="0.300000"
+                       backwards="false"
+                       action_name=""/>
+      <MovePointSimple x="0.76"
+                       y="0.18"
+                       theta="-90"
+                       max_speed="0.250000"
+                       backwards="true"
+                       action_name=""/>
+      <TranslateX x="0.5"
+                  max_speed="0.300000"
+                  action_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
   <BehaviorTree ID="test_1">
     <Sequence>
-      <RotateSimple angle="90"
-                    min_angle="0.000000"
-                    action_name=""/>
-      <Sleep msec="1000"/>
-      <RotateSimple angle="0"
-                    min_angle="0.000000"
-                    action_name=""/>
+      <RotateTowards x="0.4"
+                     y="0.0"
+                     max_speed="0.000000"
+                     min_angle="0.000000"
+                     action_name=""/>
     </Sequence>
   </BehaviorTree>
 
   <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Action ID="MovePointSimple">
+      <input_port name="x"
+                  type="double"/>
+      <input_port name="y"
+                  type="double"/>
+      <input_port name="theta"
+                  type="double"/>
+      <input_port name="max_speed"
+                  default="0.000000"
+                  type="double"/>
+      <input_port name="backwards"
+                  default="false"
+                  type="bool"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="RotateTowards">
+      <input_port name="x"
+                  type="double"/>
+      <input_port name="y"
+                  type="double"/>
+      <input_port name="max_speed"
+                  default="0.000000"
+                  type="double"/>
+      <input_port name="min_angle"
+                  default="0.000000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <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="ZeroOdom">
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
+  </TreeNodesModel>
 
 </root>
diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml
index 35c10c9..e19211a 100644
--- a/toid_bt/behavior_trees/calibration_routines.xml
+++ b/toid_bt/behavior_trees/calibration_routines.xml
@@ -2,17 +2,22 @@
 <root BTCPP_format="4">
   <BehaviorTree ID="track_calib">
     <Sequence>
-      <SetBlackboard value="0.264"
+      <SetBlackboard value="0.265"
                      output_key="width"/>
       <Sleep msec="1000"/>
       <ZeroOdom service_name=""/>
+      <RotateTowards x="0.4"
+                     y="0.0"
+                     max_speed="0.000000"
+                     min_angle="0.000000"
+                     action_name=""/>
       <Sequence>
         <Sleep msec="1000"/>
-        <MovePointSimple x="-0.4"
+        <MovePointSimple x="0.4"
                          y="0"
                          theta="0"
                          max_speed="0.10000"
-                         backwards="true"
+                         backwards="false"
                          action_name=""/>
         <Sleep msec="500"/>
         <RotateSimple angle="0"
@@ -22,11 +27,11 @@
       </Sequence>
       <ForceSuccess>
         <Timeout msec="13000">
-          <MovePointSimple x="1.0"
+          <MovePointSimple x="-1.0"
                            y="0"
                            theta="0"
                            max_speed="0.070000"
-                           backwards="false"
+                           backwards="true"
                            action_name=""/>
         </Timeout>
       </ForceSuccess>
@@ -76,6 +81,8 @@
                            action_name=""/>
         </Timeout>
       </ForceSuccess>
+      <Sleep msec="1000"/>
+      <EndCalib service_name=""/>
     </Sequence>
   </BehaviorTree>
 
@@ -84,17 +91,21 @@
       <Sleep msec="1000"/>
       <ZeroOdom service_name=""/>
       <Sleep msec="1000"/>
-      <MovePointSimple x="-1.200"
+      <MovePointSimple x="1.200"
                        y="0"
                        theta="0"
                        max_speed="0.250000"
-                       backwards="true"
+                       backwards="false"
                        action_name=""/>
     </Sequence>
   </BehaviorTree>
 
   <!-- Description of Node Models (used by Groot) -->
   <TreeNodesModel>
+    <Action ID="EndCalib">
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
+    </Action>
     <Action ID="MovePointSimple">
       <input_port name="x"
                   type="double"/>
@@ -123,6 +134,20 @@
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
     </Action>
+    <Action ID="RotateTowards">
+      <input_port name="x"
+                  type="double"/>
+      <input_port name="y"
+                  type="double"/>
+      <input_port name="max_speed"
+                  default="0.000000"
+                  type="double"/>
+      <input_port name="min_angle"
+                  default="0.000000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
     <Action ID="SetWidth">
       <input_port name="width"
                   type="double"/>
diff --git a/toid_bt/include/toid_bt/plugins/end_calib_action.hpp b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp
index bfb4fb2..2512afc 100644
--- a/toid_bt/include/toid_bt/plugins/end_calib_action.hpp
+++ b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp
@@ -33,7 +33,7 @@ public:
   bool setRequest(typename Request::SharedPtr & request) override
   {
     auto width = getInput<double>("width").value();
-    auto count = getInput<double>("count").value();
+    double count = getInput<int>("count").value();
 
     geometry_msgs::msg::PoseStamped pose;
     get_pose(pose);
@@ -42,6 +42,8 @@ public:
     double new_width = width * (1 + (theta / (2 * M_PI * count)));
     request->data = new_width;
 
+    RCLCPP_INFO(logger(), "width is %lf", new_width);
+
     setOutput("new_width", new_width);
     return true;
   }