From 7c5ba539c52e192072db6c404fb142e895c4fab6 Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Mon, 17 Mar 2025 17:31:36 +0300
Subject: [PATCH] update MainTree

---
 rbs_mill_assist/bt/xmls/FromGraver.xml   |   9 --
 rbs_mill_assist/bt/xmls/Grasp.xml        |  24 ++++-
 rbs_mill_assist/bt/xmls/MainTree.xml     | 113 ++++++++++++++++++++---
 rbs_mill_assist/bt/xmls/PickAndPlace.xml |   9 ++
 rbs_mill_assist/bt/xmls/Place.xml        |  12 ++-
 rbs_mill_assist/bt/xmls/ToGraver.xml     |   9 --
 rbs_mill_assist/bt/xmls/btmill.btproj    |  10 ++
 7 files changed, 152 insertions(+), 34 deletions(-)
 delete mode 100644 rbs_mill_assist/bt/xmls/FromGraver.xml
 create mode 100644 rbs_mill_assist/bt/xmls/PickAndPlace.xml
 delete mode 100644 rbs_mill_assist/bt/xmls/ToGraver.xml
 create mode 100644 rbs_mill_assist/bt/xmls/btmill.btproj

diff --git a/rbs_mill_assist/bt/xmls/FromGraver.xml b/rbs_mill_assist/bt/xmls/FromGraver.xml
deleted file mode 100644
index 1b42cb9..0000000
--- a/rbs_mill_assist/bt/xmls/FromGraver.xml
+++ /dev/null
@@ -1,9 +0,0 @@
-<?xml version='1.0' encoding='utf-8'?>
-<root BTCPP_format="4">
-  <BehaviorTree ID="FromGraver">
-    <Sequence>
-      <SubTree ID="Grasp" action_name="from_graver.pick" robot_name="{robot_name}"/>
-      <SubTree ID="Place" action_name="from_graver.place" robot_name="{robot_name}"/>
-    </Sequence>
-  </BehaviorTree>
-</root>
diff --git a/rbs_mill_assist/bt/xmls/Grasp.xml b/rbs_mill_assist/bt/xmls/Grasp.xml
index e058940..56ebcf1 100644
--- a/rbs_mill_assist/bt/xmls/Grasp.xml
+++ b/rbs_mill_assist/bt/xmls/Grasp.xml
@@ -2,16 +2,36 @@
 <root BTCPP_format="4">
   <BehaviorTree ID="Grasp">
     <Sequence>
-      <Action ID="GetGraspPlacePose" action_name="{action_name}" pre_pose="{pregrasp}" pose="{grasp}" post_pose="{postgrasp}" service_name="/get_pick_place_poses" />
 
       <Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="4" action_name="/mtp_moveit" />
       <Action ID="ToggleVacuumGrippper" enable="true" service_name="/gz_ros2_vacuum_gripper_plugin/toggle" />
-      <Action ID="MoveToPose" pose="{grasp}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
+
+      <Action ID="TwistCmdWithCond" action_name="/twist_cmd" robot_name="arm" twist="0;0;-4.0;0;0;0"/>
+
+      <!-- <Action ID="MoveToPose" pose="{grasp}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" /> -->
       <Action ID="MoveToPose" pose="{postgrasp}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
 
     </Sequence>
   </BehaviorTree>
 
   <TreeNodesModel>
+    <Action ID="MoveToPose" editable="true">
+      <input_port name="pose"/>
+      <input_port name="robot_name"/>
+      <input_port name="duration"/>
+      <input_port name="action_name"/>
+    </Action>
+    <Action ID="ToggleVacuumGrippper" editable="true">
+      <input_port name="enable"/>
+      <input_port name="service_name"/>
+      <input_port name="robot_name"/>
+    </Action>
+
+    <Action ID="TwistCmdWithCond" editable="true">
+      <input_port name="robot_name"/>
+      <input_port name="twist"/>
+      <!-- <input_port name="duration"/> -->
+      <input_port name="action_name"/>
+    </Action>
   </TreeNodesModel>
 </root>
diff --git a/rbs_mill_assist/bt/xmls/MainTree.xml b/rbs_mill_assist/bt/xmls/MainTree.xml
index 59258cb..4a5b794 100644
--- a/rbs_mill_assist/bt/xmls/MainTree.xml
+++ b/rbs_mill_assist/bt/xmls/MainTree.xml
@@ -1,25 +1,112 @@
 <?xml version='1.0' encoding='utf-8'?>
 <root BTCPP_format="4">
   <BehaviorTree ID="Main">
-    <Repeat num_cycles="5">
+
     <Sequence>
-      <!-- <Condition ID="IsInPose" topic_name="/ee_pose"/> -->
-      <SubTree ID="ToGraver" robot_name="arm" />
+      <!-- <Action ID="GetTask" name="" data="{task}" num_cycles="{num_cycles}"/> -->
 
-      <Action ID="GetNamedPose" pose_name="waiting" output_pose="{named_pose}"
-        service_name="/get_named_pose" />
-      <Action ID="MoveToPose" pose="{named_pose}" robot_name="arm" duration="3"
-        action_name="/mtp_jtc" />
+      <Fallback>
+        <Repeat num_cycles="1">
+          <Sequence>
+            <!-- Preparing -->
 
-      <Delay delay_msec="2000">
-        <SubTree ID="FromGraver" robot_name="arm" />
-      </Delay>
-      <!-- <Action ID="MoveToJointState" joint_state="0.0;0.85;1.0;0.0;1.0;0.0" duration="3" -->
-      <!--   robot_name="arm" action_name="/mtjs_jtc" /> -->
+            <Action ID="GetGraspPlacePose" action_name="to_graver.pick"
+              pre_pose="{pregrasp}"
+              pose="{grasp}"
+              post_pose="{postgrasp}"
+              service_name="/get_pick_place_poses" />
 
+            <Action ID="GetGraspPlacePose" action_name="to_graver.place"
+              pre_pose="{preplace}"
+              pose="{place}"
+              post_pose="{postplace}"
+              service_name="/get_pick_place_poses" />
+
+            <!-- Grasp shildik and place to the Graver -->
+            <SubTree ID="PickAndPlace"
+              robot_name="arm"
+              preplace="{preplace}"
+              place="{place}"
+              postplace="{postplace}"
+              pregrasp="{pregrasp}"
+              grasp="{grasp}"
+              postgrasp="{postgrasp}"
+            />
+
+
+            <!-- Waiting position -->
+            <Action ID="GetNamedPose" pose_name="waiting" output_pose="{named_pose}"
+              service_name="/get_named_pose" />
+            <Action ID="MoveToPose" pose="{named_pose}" robot_name="arm" duration="3"
+              action_name="/mtp_jtc" />
+
+            <!-- Send task and wait Graver -->
+
+            <!-- <Action ID="SendTask" /> -->
+
+            <!-- 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="GetGraspPlacePose" action_name="from_graver.place"
+              pre_pose="{preplace}"
+              pose="{place}"
+              post_pose="{postplace}"
+              service_name="/get_pick_place_poses" />
+
+            <!-- Grasp shildik from Graver and move to the box-->
+            <SubTree ID="PickAndPlace"
+              robot_name="arm"
+              preplace="{preplace}"
+              place="{place}"
+              postplace="{postplace}"
+              pregrasp="{pregrasp}"
+              grasp="{grasp}"
+              postgrasp="{postgrasp}"
+            />
+
+            <!-- Ending -->
+          </Sequence>
+        </Repeat>
+
+        <Sequence>
+          <!-- <Action ID="NotifyOperator" /> -->
+          <AlwaysFailure />
+        </Sequence>
+      </Fallback>
     </Sequence>
-    </Repeat>
+
   </BehaviorTree>
+
+  <TreeNodesModel>
+    <Action ID="GetGraspPlacePose" editable="true">
+      <input_port name="action_name"/>
+      <input_port name="pre_pose"/>
+      <input_port name="pose"/>
+      <input_port name="post_pose"/>
+      <input_port name="service_name"/>
+    </Action>
+
+    <Action ID="GetNamedPose" editable="true">
+      <input_port name="pose_name"/>
+      <input_port name="output_pose"/>
+      <input_port name="service_name"/>
+    </Action>
+
+    <Action ID="NotifyOperator" editable="true" />
+
+    <Action ID="GetTask" editable="true">
+      <input_port name="name"/> 
+      <input_port name="data"/>
+      <input_port name="num_cycles"/> 
+    </Action>
+
+    <Action ID="SendTask" editable="true" />
+  </TreeNodesModel>
 </root>
 
 
diff --git a/rbs_mill_assist/bt/xmls/PickAndPlace.xml b/rbs_mill_assist/bt/xmls/PickAndPlace.xml
new file mode 100644
index 0000000..67231e5
--- /dev/null
+++ b/rbs_mill_assist/bt/xmls/PickAndPlace.xml
@@ -0,0 +1,9 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="PickAndPlace">
+    <Sequence>
+      <SubTree ID="Grasp" pregrasp="{pregrasp}" grasp="{grasp}" postgrasp="{grasp}" robot_name="{robot_name}"/>
+      <SubTree ID="Place" preplace="{preplace}" place="{place}" postplace="{postplace}" robot_name="{robot_name}"/>
+    </Sequence>
+  </BehaviorTree>
+</root>
diff --git a/rbs_mill_assist/bt/xmls/Place.xml b/rbs_mill_assist/bt/xmls/Place.xml
index ebea90a..c35ffa7 100644
--- a/rbs_mill_assist/bt/xmls/Place.xml
+++ b/rbs_mill_assist/bt/xmls/Place.xml
@@ -2,7 +2,6 @@
 <root BTCPP_format="4">
   <BehaviorTree ID="Place">
     <Sequence>
-      <Action ID="GetGraspPlacePose" action_name="{action_name}" pre_pose="{preplace}" pose="{place}" post_pose="{postplace}" service_name="/get_pick_place_poses" />
 
       <Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="4" action_name="/mtp_moveit" />
       <Action ID="MoveToPose" pose="{place}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
@@ -13,5 +12,16 @@
   </BehaviorTree>
 
   <TreeNodesModel>
+    <Action ID="MoveToPose" editable="true">
+      <input_port name="pose"/>
+      <input_port name="robot_name"/>
+      <input_port name="duration"/>
+      <input_port name="action_name"/>
+    </Action>
+    <Action ID="ToggleVacuumGrippper" editable="true">
+      <input_port name="enable"/>
+      <input_port name="service_name"/>
+      <input_port name="robot_name"/>
+    </Action>
   </TreeNodesModel>
 </root>
diff --git a/rbs_mill_assist/bt/xmls/ToGraver.xml b/rbs_mill_assist/bt/xmls/ToGraver.xml
deleted file mode 100644
index ac9f090..0000000
--- a/rbs_mill_assist/bt/xmls/ToGraver.xml
+++ /dev/null
@@ -1,9 +0,0 @@
-<?xml version='1.0' encoding='utf-8'?>
-<root BTCPP_format="4">
-  <BehaviorTree ID="ToGraver">
-    <Sequence>
-      <SubTree ID="Grasp" action_name="to_graver.pick" robot_name="{robot_name}"/>
-      <SubTree ID="Place" action_name="to_graver.place" robot_name="{robot_name}"/>
-    </Sequence>
-  </BehaviorTree>
-</root>
diff --git a/rbs_mill_assist/bt/xmls/btmill.btproj b/rbs_mill_assist/bt/xmls/btmill.btproj
new file mode 100644
index 0000000..48fa0b0
--- /dev/null
+++ b/rbs_mill_assist/bt/xmls/btmill.btproj
@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" project_name="Project">
+    <include path="Grasp.xml"/>
+    <include path="Place.xml"/>
+    <include path="MainTree.xml"/>
+    <include path="PickAndPlace.xml"/>
+    <!-- Description of Node Models (used by Groot) -->
+    <TreeNodesModel>
+    </TreeNodesModel>
+</root>