From f8a4b329e14af0fcf7fa13ebc1969a6dd39372ad Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Mon, 14 Apr 2025 10:14:04 +0300
Subject: [PATCH] refactor: add tree without moveit

---
 rbs_mill_assist/bt/config/bt_executor.yaml    |   3 +-
 .../bt/xmls/{ => moveit}/Grasp.xml            |   0
 .../bt/xmls/{ => moveit}/MainTree.xml         |   0
 .../bt/xmls/{ => moveit}/PickAndPlace.xml     |   0
 .../bt/xmls/{ => moveit}/Place.xml            |   0
 .../bt/xmls/{ => moveit}/btmill.btproj        |   0
 .../bt/xmls/no_moveit/FromGraver.xml          |  34 +++++
 .../bt/xmls/no_moveit/GraspNoMoveit.xml       |  36 ++++++
 .../bt/xmls/no_moveit/MainTree.xml            | 116 ++++++++++++++++++
 .../bt/xmls/no_moveit/PlaceNoMoveit.xml       |  28 +++++
 .../bt/xmls/no_moveit/ToGraver.xml            |  28 +++++
 11 files changed, 244 insertions(+), 1 deletion(-)
 rename rbs_mill_assist/bt/xmls/{ => moveit}/Grasp.xml (100%)
 rename rbs_mill_assist/bt/xmls/{ => moveit}/MainTree.xml (100%)
 rename rbs_mill_assist/bt/xmls/{ => moveit}/PickAndPlace.xml (100%)
 rename rbs_mill_assist/bt/xmls/{ => moveit}/Place.xml (100%)
 rename rbs_mill_assist/bt/xmls/{ => moveit}/btmill.btproj (100%)
 create mode 100644 rbs_mill_assist/bt/xmls/no_moveit/FromGraver.xml
 create mode 100644 rbs_mill_assist/bt/xmls/no_moveit/GraspNoMoveit.xml
 create mode 100644 rbs_mill_assist/bt/xmls/no_moveit/MainTree.xml
 create mode 100644 rbs_mill_assist/bt/xmls/no_moveit/PlaceNoMoveit.xml
 create mode 100644 rbs_mill_assist/bt/xmls/no_moveit/ToGraver.xml

diff --git a/rbs_mill_assist/bt/config/bt_executor.yaml b/rbs_mill_assist/bt/config/bt_executor.yaml
index 6feace9..069aafc 100644
--- a/rbs_mill_assist/bt/config/bt_executor.yaml
+++ b/rbs_mill_assist/bt/config/bt_executor.yaml
@@ -10,4 +10,5 @@ bt_action_server:
       - rbs_bt_executor/bt_plugins
 
     behavior_trees:
-      - rbs_mill_assist/xmls
+      - rbs_mill_assist/xmls/moveit
+      - rbs_mill_assist/xmls/no_moveit
diff --git a/rbs_mill_assist/bt/xmls/Grasp.xml b/rbs_mill_assist/bt/xmls/moveit/Grasp.xml
similarity index 100%
rename from rbs_mill_assist/bt/xmls/Grasp.xml
rename to rbs_mill_assist/bt/xmls/moveit/Grasp.xml
diff --git a/rbs_mill_assist/bt/xmls/MainTree.xml b/rbs_mill_assist/bt/xmls/moveit/MainTree.xml
similarity index 100%
rename from rbs_mill_assist/bt/xmls/MainTree.xml
rename to rbs_mill_assist/bt/xmls/moveit/MainTree.xml
diff --git a/rbs_mill_assist/bt/xmls/PickAndPlace.xml b/rbs_mill_assist/bt/xmls/moveit/PickAndPlace.xml
similarity index 100%
rename from rbs_mill_assist/bt/xmls/PickAndPlace.xml
rename to rbs_mill_assist/bt/xmls/moveit/PickAndPlace.xml
diff --git a/rbs_mill_assist/bt/xmls/Place.xml b/rbs_mill_assist/bt/xmls/moveit/Place.xml
similarity index 100%
rename from rbs_mill_assist/bt/xmls/Place.xml
rename to rbs_mill_assist/bt/xmls/moveit/Place.xml
diff --git a/rbs_mill_assist/bt/xmls/btmill.btproj b/rbs_mill_assist/bt/xmls/moveit/btmill.btproj
similarity index 100%
rename from rbs_mill_assist/bt/xmls/btmill.btproj
rename to rbs_mill_assist/bt/xmls/moveit/btmill.btproj
diff --git a/rbs_mill_assist/bt/xmls/no_moveit/FromGraver.xml b/rbs_mill_assist/bt/xmls/no_moveit/FromGraver.xml
new file mode 100644
index 0000000..65e9f5f
--- /dev/null
+++ b/rbs_mill_assist/bt/xmls/no_moveit/FromGraver.xml
@@ -0,0 +1,34 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="FromGraver">
+    <Sequence>
+      <Action ID="GetNamedPose" pose_name="to_graver_prom" relative_to="base_link"
+        output_pose="{a}"
+        service_name="/get_tf_frame_pose" />
+
+      <!-- <Action ID="MoveToPose" pose="{a}" robot_name="{robot_name}" duration="3" -->
+      <!--   try_plan_untill_success="false" action_name="/mtp_jtc_cart" /> -->
+
+      <SubTree ID="GraspNoMoveit" pregrasp="{pregrasp}" grasp="{grasp}" postgrasp="{postgrasp}"
+        robot_name="{robot_name}" />
+
+      <Action ID="MoveToPose" pose="{a}" robot_name="{robot_name}" duration="3"
+        try_plan_untill_success="false" action_name="/mtp_jtc" />
+
+      <Action ID="GetNamedPose" pose_name="preparing_grasp_slot" relative_to="base_link" output_pose="{b}"
+        service_name="/get_tf_frame_pose" />
+
+      <Action ID="MoveToPose" pose="{b}" robot_name="{robot_name}" duration="4"
+        action_name="/mtp_jtc" />
+
+      <Action ID="GetNamedPose" pose_name="preparing_place_slot" relative_to="base_link" output_pose="{c}"
+        service_name="/get_tf_frame_pose" />
+
+      <Action ID="MoveToPose" pose="{c}" robot_name="{robot_name}" duration="4"
+        action_name="/mtp_jtc" />
+
+      <SubTree ID="PlaceNoMoveit" preplace="{preplace}" place="{place}" postplace="{postplace}"
+        robot_name="{robot_name}" />
+    </Sequence>
+  </BehaviorTree>
+</root>
diff --git a/rbs_mill_assist/bt/xmls/no_moveit/GraspNoMoveit.xml b/rbs_mill_assist/bt/xmls/no_moveit/GraspNoMoveit.xml
new file mode 100644
index 0000000..6a5dd36
--- /dev/null
+++ b/rbs_mill_assist/bt/xmls/no_moveit/GraspNoMoveit.xml
@@ -0,0 +1,36 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="GraspNoMoveit">
+    <Sequence>
+      <Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="3" try_plan_untill_success="true" action_name="/mtp_jtc_cart" />
+      <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"/>
+
+      <!-- <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/no_moveit/MainTree.xml b/rbs_mill_assist/bt/xmls/no_moveit/MainTree.xml
new file mode 100644
index 0000000..86c004b
--- /dev/null
+++ b/rbs_mill_assist/bt/xmls/no_moveit/MainTree.xml
@@ -0,0 +1,116 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="MainNoMoveit">
+
+    <Sequence>
+      <!-- <Action ID="GetTask" name="" data="{task}" num_cycles="{num_cycles}"/> -->
+
+      <Fallback>
+        <Repeat num_cycles="1">
+          <Sequence>
+            <!-- Preparing -->
+
+            <Action ID="GetSlotGraspPoses" from_pose="slot_1" 
+              relative_to="base_link"
+              pregrasp="{pregrasp}"
+              grasp="{grasp}"
+              postgrasp="{postgrasp}"
+              service_name="/get_grasping_pose" />
+
+            <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="ToGraver"
+              robot_name="arm"
+              preplace="{preplace}"
+              place="{place}"
+              postplace="{postplace}"
+              pregrasp="{pregrasp}"
+              grasp="{grasp}"
+              postgrasp="{postgrasp}"
+            />
+
+
+            <!-- Waiting position -->
+            <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" />
+
+            <!-- Send task and wait Graver -->
+
+            <!-- <Action ID="SendTask" /> -->
+
+            <!-- Pick shildik from graver-->
+
+            <Action ID="GetSlotGraspPoses" from_pose="conductor" 
+              relative_to="base_link"
+              pregrasp="{pregrasp}"
+              grasp="{grasp}"
+              postgrasp="{postgrasp}"
+              service_name="/get_grasping_pose" />
+
+            <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="FromGraver"
+              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>
+
+  </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/no_moveit/PlaceNoMoveit.xml b/rbs_mill_assist/bt/xmls/no_moveit/PlaceNoMoveit.xml
new file mode 100644
index 0000000..8d6b5e9
--- /dev/null
+++ b/rbs_mill_assist/bt/xmls/no_moveit/PlaceNoMoveit.xml
@@ -0,0 +1,28 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="PlaceNoMoveit">
+    <Sequence>
+
+
+      <Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="4" action_name="/mtp_jtc" />
+      <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" />
+
+    </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>
+  </TreeNodesModel>
+</root>
diff --git a/rbs_mill_assist/bt/xmls/no_moveit/ToGraver.xml b/rbs_mill_assist/bt/xmls/no_moveit/ToGraver.xml
new file mode 100644
index 0000000..ed2042a
--- /dev/null
+++ b/rbs_mill_assist/bt/xmls/no_moveit/ToGraver.xml
@@ -0,0 +1,28 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="ToGraver">
+    <Sequence>
+      <Action ID="GetNamedPose" pose_name="preparing_grasp_slot" relative_to="base_link"
+        output_pose="{a}"
+        service_name="/get_tf_frame_pose" />
+
+      <Action ID="MoveToPose" pose="{a}" robot_name="{robot_name}" duration="3"
+        try_plan_untill_success="false" action_name="/mtp_jtc_cart" />
+
+      <SubTree ID="GraspNoMoveit" pregrasp="{pregrasp}" grasp="{grasp}" postgrasp="{postgrasp}"
+        robot_name="{robot_name}" />
+
+      <Action ID="MoveToPose" pose="{a}" robot_name="{robot_name}" duration="3"
+        try_plan_untill_success="false" action_name="/mtp_jtc_cart" />
+
+      <Action ID="GetNamedPose" pose_name="to_graver_prom" relative_to="base_link" output_pose="{b}"
+        service_name="/get_tf_frame_pose" />
+
+      <Action ID="MoveToPose" pose="{b}" robot_name="{robot_name}" duration="4"
+        action_name="/mtp_jtc" />
+
+      <SubTree ID="PlaceNoMoveit" preplace="{preplace}" place="{place}" postplace="{postplace}"
+        robot_name="{robot_name}" />
+    </Sequence>
+  </BehaviorTree>
+</root>