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

@@ -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"/>

rbs_mill_assist/bt/xmls/MainTree.xml

@@ -10,17 +10,19 @@
           <Sequence>
             <!-- Preparing -->
 
-            <Action ID="GetGraspPlacePose" action_name="to_graver.pick"
+            <Action ID="GetSlotGraspPoses" from_pose="slot_1" 
-              pre_pose="{pregrasp}"
+              relative_to="base_link"
-              pose="{grasp}"
+              pregrasp="{pregrasp}"
-              post_pose="{postgrasp}"
+              grasp="{grasp}"
-              service_name="/get_pick_place_poses" />
+              postgrasp="{postgrasp}"
+              service_name="/get_grasping_pose" />
 
-            <Action ID="GetGraspPlacePose" action_name="to_graver.place"
+            <Action ID="GetPlacePoses" to_pose="conductor" 
-              pre_pose="{preplace}"
+              relative_to="base_link"
-              pose="{place}"
+              preplace="{preplace}"
-              post_pose="{postplace}"
+              place="{place}"
-              service_name="/get_pick_place_poses" />
+              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}"
+            <Action ID="GetNamedPose" pose_name="waiting" relative_to="base_link" output_pose="{named_pose}"
-              service_name="/get_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"
+            <Action ID="GetSlotGraspPoses" from_pose="conductor" 
-              pre_pose="{pregrasp}"
+              relative_to="base_link"
-              pose="{grasp}"
+              pregrasp="{pregrasp}"
-              post_pose="{postgrasp}"
+              grasp="{grasp}"
-              service_name="/get_pick_place_poses" />
+              postgrasp="{postgrasp}"
+              service_name="/get_grasping_pose" />
 
-            <Action ID="GetGraspPlacePose" action_name="from_graver.place"
+            <Action ID="GetPlacePoses" to_pose="bunker" 
-              pre_pose="{preplace}"
+              relative_to="base_link"
-              pose="{place}"
+              preplace="{preplace}"
-              post_pose="{postplace}"
+              place="{place}"
-              service_name="/get_pick_place_poses" />
+              postplace="{postplace}"
+              service_name="/get_grasping_pose" />
 
             <!-- Grasp shildik from Graver and move to the box-->
             <SubTree ID="PickAndPlace"

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>

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" />