From d26adb576a36ad752fed62a6b52ab920958a693d Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Fri, 17 Apr 2026 11:22:07 +0200
Subject: [PATCH] Final changes for homologation

---
 .../src/toid_bot_description.urdf             |  2 +-
 toid_bt/behavior_trees/behavior1.xml          | 60 +++++++++----------
 .../behavior_trees/calibration_routines.xml   | 23 +++----
 toid_bt/behavior_trees/toid_behaviors.btproj  | 10 ++--
 .../params/toid_general_params.yaml           |  3 +-
 5 files changed, 49 insertions(+), 49 deletions(-)

diff --git a/toid_bot_description/src/toid_bot_description.urdf b/toid_bot_description/src/toid_bot_description.urdf
index 520af05..e47500b 100644
--- a/toid_bot_description/src/toid_bot_description.urdf
+++ b/toid_bot_description/src/toid_bot_description.urdf
@@ -73,7 +73,7 @@
 <joint name="lidar_joint" type="fixed">
     <parent link="base_link"/>
     <child link="lidar"/>
-    <origin xyz="${lidar_xoff} 0 ${(base_height + lidar_height)/2}" rpy="0 0 0"/>
+    <origin xyz="${lidar_xoff} 0 ${(base_height + lidar_height)/2}" rpy="0 0 ${pi}"/>
 </joint>
 
 
diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml
index 990968a..868ff06 100644
--- a/toid_bt/behavior_trees/behavior1.xml
+++ b/toid_bt/behavior_trees/behavior1.xml
@@ -15,39 +15,35 @@
   <BehaviorTree ID="seq1">
     <Sequence>
       <ZeroOdom service_name=""/>
-      <MovePointSimple x="0.1"
-                       y="0"
-                       theta="0"
-                       max_speed="0.350000"
+      <SetInitialPose y="0.805"
+                      theta="-90"
+                      frame_id="map"
+                      x="1.325"
+                      topic_name="__default__placeholder__"/>
+      <WaitPullPin action_name=""/>
+      <InPose timeout="1.000000">
+        <MovePointSimple x="1.325"
+                         y="0.200"
+                         theta="-90"
+                         max_speed="0.200000"
+                         backwards="false"
+                         action_name=""/>
+      </InPose>
+      <Seq1 service_name=""/>
+      <Seq2 text=""
+            service_name=""/>
+      <MovePointSimple x="1.325"
+                       y="-0.05"
+                       theta="-90"
+                       max_speed="0.200000"
                        backwards="false"
                        action_name=""/>
-      <RotateTowards x="0.76"
-                     y="-0.5"
-                     max_speed="1.50000"
-                     min_angle="0.000000"
-                     action_name=""/>
-      <MovePointSimple x="0.756"
-                       y="-0.5"
-                       theta="-45"
-                       max_speed="0.350000"
-                       backwards="false"
-                       action_name=""/>
-      <RotateTowards x="0.76"
-                     y="-0.02"
-                     max_speed="1.50000"
-                     min_angle="0.000000"
-                     action_name=""/>
-      <MovePointSimple x="0.757"
-                       y="-0.02"
-                       theta="90"
-                       max_speed="0.150000"
-                       backwards="false"
-                       action_name=""/>
-      <MovePointSimple x="1.2"
-                       y="0.0"
-                       theta="0.0"
-                       max_speed="0.250000"
-                       backwards="false"
+      <Seq3 service_name=""/>
+      <MovePointSimple x="1.325"
+                       y="0.805"
+                       theta="-90"
+                       max_speed="0.200000"
+                       backwards="true"
                        action_name=""/>
     </Sequence>
   </BehaviorTree>
@@ -126,4 +122,4 @@
     </Action>
   </TreeNodesModel>
 
-</root>
+</root>
\ No newline at end of file
diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml
index b2085e1..81becab 100644
--- a/toid_bt/behavior_trees/calibration_routines.xml
+++ b/toid_bt/behavior_trees/calibration_routines.xml
@@ -4,6 +4,7 @@
     <Sequence>
       <SetBlackboard value="0.265"
                      output_key="width"/>
+      <WaitPullPin />
       <Sleep msec="1000"/>
       <ZeroOdom service_name=""/>
       <RotateTowards x="0.4"
@@ -46,52 +47,53 @@
     <Sequence>
       <Sleep msec="5000"/>
       <ZeroOdom service_name=""/>
+      <WaitPullPin />
       <Sleep msec="1000"/>
-      <Repeat num_cycles="2">
+      <Repeat num_cycles="5">
         <Sequence>
           <MovePointSimple x="1.1"
                            y="0"
                            theta="0"
-                           max_speed="0.10000"
+                           max_speed="0.30000"
                            backwards="false"
                            action_name=""/>
           <Sleep msec="1000"/>
           <RotateSimple angle="180"
-                        max_speed="0.300000"
+                        max_speed="1.300000"
                         min_angle="10"
                         action_name=""/>
           <Sleep msec="500"/>
           <MovePointSimple x="0.35"
                            y="0"
                            theta="180"
-                           max_speed="0.100000"
+                           max_speed="0.300000"
                            backwards="false"
                            action_name=""/>
           <RotateSimple angle="0"
-                        max_speed="0.300000"
+                        max_speed="1.300000"
                         min_angle="-10"
                         action_name=""/>
           <Sleep msec="500"/>
           <MovePointSimple x="1.1"
                            y="0"
                            theta="0"
-                           max_speed="0.10000"
+                           max_speed="0.30000"
                            backwards="false"
                            action_name=""/>
           <Sleep msec="500"/>
           <RotateSimple angle="180"
-                        max_speed="0.300000"
+                        max_speed="1.300000"
                         min_angle="-10"
                         action_name=""/>
           <Sleep msec="500"/>
           <MovePointSimple x="0.35"
                            y="0"
                            theta="180"
-                           max_speed="0.100000"
+                           max_speed="0.300000"
                            backwards="false"
                            action_name=""/>
           <RotateSimple angle="0"
-                        max_speed="0.300000"
+                        max_speed="1.300000"
                         min_angle="10"
                         action_name=""/>
         </Sequence>
@@ -113,6 +115,7 @@
 
   <BehaviorTree ID="wheel_size">
     <Sequence>
+      <WaitPullPin />
       <Sleep msec="1000"/>
       <ZeroOdom service_name=""/>
       <Sleep msec="1000"/>
@@ -195,4 +198,4 @@
     </Action>
   </TreeNodesModel>
 
-</root>
+</root>
\ No newline at end of file
diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj
index cd4d140..78b09b6 100644
--- a/toid_bt/behavior_trees/toid_behaviors.btproj
+++ b/toid_bt/behavior_trees/toid_behaviors.btproj
@@ -59,11 +59,11 @@
             <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
         <Condition ID="SetInitialPose">
-            <input_port name="y" type="double" default="0.000000">Y position in meters</input_port>
-            <input_port name="theta" type="double" default="0.000000">Heading in degrees</input_port>
-            <input_port name="frame_id" type="std::string" default="map">Reference frame</input_port>
-            <input_port name="x" type="double" default="0.000000">X position in meters</input_port>
-            <input_port name="topic_name" type="std::string" default="__default__placeholder__">Topic name</input_port>
+            <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="SetParameter">
             <input_port name="parameter" type="std::string"/>
diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml
index 48da697..8c32a56 100644
--- a/toid_navigation/params/toid_general_params.yaml
+++ b/toid_navigation/params/toid_general_params.yaml
@@ -29,12 +29,13 @@ behavior_server:
       plugin: "nav2_behaviors::BackUp"
     rotate:
       plugin: "toid::SimpleRotateBehavior"
-      max_angular_accel: 4.0
+      max_angular_accel: 3.0
       max_angular_decel: 1.0
     translateX:
       plugin: "toid::SimpleTranslateXBehavior"
     moveCoords:
       plugin: "toid::MoveCoords"
+      max_vel_accel: 1.0
     approachAcorns:
       plugin: "toid::ApproachAcorns"
       kphi: 1.2