From 4849bb8c0905c3a813726affaabdf112128e2f97 Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Mon, 7 Apr 2025 12:28:28 +0300
Subject: [PATCH] fix: Improve robot movement reliability in behavior tree XMLs

- Grasp.xml:
  - Added `try_plan_until_success` attribute to ensure reliable path planning
- MainTree.xml:
  - Replaced generic pick-and-place actions with slot-based movements
  - Updated service calls for consistent pose calculations
  - Set all poses relative to `base_link` for accuracy
- PickAndPlace.xml:
  - Corrected postgrasp reference to prevent bugs
- Place.xml:
  - Enhanced with retry logic and increased planning time
---
 rbs_mill_assist/bt/xmls/Grasp.xml        |  2 +-
 rbs_mill_assist/bt/xmls/MainTree.xml     | 48 +++++++++++++-----------
 rbs_mill_assist/bt/xmls/PickAndPlace.xml |  2 +-
 rbs_mill_assist/bt/xmls/Place.xml        |  2 +-
 4 files changed, 29 insertions(+), 25 deletions(-)

diff --git a/rbs_mill_assist/bt/xmls/Grasp.xml b/rbs_mill_assist/bt/xmls/Grasp.xml
index 56ebcf1..69f7a23 100644
--- a/rbs_mill_assist/bt/xmls/Grasp.xml
+++ b/rbs_mill_assist/bt/xmls/Grasp.xml
@@ -3,7 +3,7 @@
   <BehaviorTree ID="Grasp">
     <Sequence>
 
-      <Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="4" action_name="/mtp_moveit" />
+      <Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="4" try_plan_untill_success="true" action_name="/mtp_moveit" />
       <Action ID="ToggleVacuumGrippper" enable="true" service_name="/gz_ros2_vacuum_gripper_plugin/toggle" />
 
       <Action ID="TwistCmdWithCond" action_name="/twist_cmd" robot_name="arm" twist="0;0;-4.0;0;0;0"/>
diff --git a/rbs_mill_assist/bt/xmls/MainTree.xml b/rbs_mill_assist/bt/xmls/MainTree.xml
index 4a5b794..bf9987b 100644
--- a/rbs_mill_assist/bt/xmls/MainTree.xml
+++ b/rbs_mill_assist/bt/xmls/MainTree.xml
@@ -10,17 +10,19 @@
           <Sequence>
             <!-- Preparing -->
 
-            <Action ID="GetGraspPlacePose" action_name="to_graver.pick"
-              pre_pose="{pregrasp}"
-              pose="{grasp}"
-              post_pose="{postgrasp}"
-              service_name="/get_pick_place_poses" />
+            <Action ID="GetSlotGraspPoses" from_pose="slot_1" 
+              relative_to="base_link"
+              pregrasp="{pregrasp}"
+              grasp="{grasp}"
+              postgrasp="{postgrasp}"
+              service_name="/get_grasping_pose" />
 
-            <Action ID="GetGraspPlacePose" action_name="to_graver.place"
-              pre_pose="{preplace}"
-              pose="{place}"
-              post_pose="{postplace}"
-              service_name="/get_pick_place_poses" />
+            <Action ID="GetPlacePoses" to_pose="conductor" 
+              relative_to="base_link"
+              preplace="{preplace}"
+              place="{place}"
+              postplace="{postplace}"
+              service_name="/get_grasping_pose" />
 
             <!-- Grasp shildik and place to the Graver -->
             <SubTree ID="PickAndPlace"
@@ -35,8 +37,8 @@
 
 
             <!-- Waiting position -->
-            <Action ID="GetNamedPose" pose_name="waiting" output_pose="{named_pose}"
-              service_name="/get_named_pose" />
+            <Action ID="GetNamedPose" pose_name="waiting" relative_to="base_link" output_pose="{named_pose}"
+              service_name="/get_tf_frame_pose" />
             <Action ID="MoveToPose" pose="{named_pose}" robot_name="arm" duration="3"
               action_name="/mtp_jtc" />
 
@@ -46,17 +48,19 @@
 
             <!-- Pick shildik from graver-->
 
-            <Action ID="GetGraspPlacePose" action_name="from_graver.pick"
-              pre_pose="{pregrasp}"
-              pose="{grasp}"
-              post_pose="{postgrasp}"
-              service_name="/get_pick_place_poses" />
+            <Action ID="GetSlotGraspPoses" from_pose="conductor" 
+              relative_to="base_link"
+              pregrasp="{pregrasp}"
+              grasp="{grasp}"
+              postgrasp="{postgrasp}"
+              service_name="/get_grasping_pose" />
 
-            <Action ID="GetGraspPlacePose" action_name="from_graver.place"
-              pre_pose="{preplace}"
-              pose="{place}"
-              post_pose="{postplace}"
-              service_name="/get_pick_place_poses" />
+            <Action ID="GetPlacePoses" to_pose="bunker" 
+              relative_to="base_link"
+              preplace="{preplace}"
+              place="{place}"
+              postplace="{postplace}"
+              service_name="/get_grasping_pose" />
 
             <!-- Grasp shildik from Graver and move to the box-->
             <SubTree ID="PickAndPlace"
diff --git a/rbs_mill_assist/bt/xmls/PickAndPlace.xml b/rbs_mill_assist/bt/xmls/PickAndPlace.xml
index 67231e5..a9f084e 100644
--- a/rbs_mill_assist/bt/xmls/PickAndPlace.xml
+++ b/rbs_mill_assist/bt/xmls/PickAndPlace.xml
@@ -2,7 +2,7 @@
 <root BTCPP_format="4">
   <BehaviorTree ID="PickAndPlace">
     <Sequence>
-      <SubTree ID="Grasp" pregrasp="{pregrasp}" grasp="{grasp}" postgrasp="{grasp}" robot_name="{robot_name}"/>
+      <SubTree ID="Grasp" pregrasp="{pregrasp}" grasp="{grasp}" postgrasp="{postgrasp}" robot_name="{robot_name}"/>
       <SubTree ID="Place" preplace="{preplace}" place="{place}" postplace="{postplace}" robot_name="{robot_name}"/>
     </Sequence>
   </BehaviorTree>
diff --git a/rbs_mill_assist/bt/xmls/Place.xml b/rbs_mill_assist/bt/xmls/Place.xml
index c35ffa7..a89c864 100644
--- a/rbs_mill_assist/bt/xmls/Place.xml
+++ b/rbs_mill_assist/bt/xmls/Place.xml
@@ -3,7 +3,7 @@
   <BehaviorTree ID="Place">
     <Sequence>
 
-      <Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="4" action_name="/mtp_moveit" />
+      <Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="4" try_plan_untill_success="true" allowed_planning_time="10.0" action_name="/mtp_moveit" />
       <Action ID="MoveToPose" pose="{place}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
       <Action ID="ToggleVacuumGrippper" enable="false" service_name="/gz_ros2_vacuum_gripper_plugin/toggle" />
       <Action ID="MoveToPose" pose="{postplace}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />