From 12a1bfbfe8bc12fb72d50d9a274ee7df3f2e75fc Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Thu, 8 Feb 2024 13:45:11 +0300
Subject: [PATCH] add skill interfaces description and change robot name in bt
 tree

---
 .../example_skills/env_manager_starter.xml    | 19 ++++++++++++++
 .../example_skills/move_to_joint_states.xml   | 23 ++++++++++++++++
 .../bt_trees/example_skills/move_to_pose.xml  | 23 ++++++++++++++++
 .../example_skills/move_to_pose_array.xml     | 26 +++++++++++++++++++
 rbs_bt_executor/bt_trees/test_tree.xml        |  2 +-
 5 files changed, 92 insertions(+), 1 deletion(-)
 create mode 100644 rbs_bt_executor/bt_trees/example_skills/env_manager_starter.xml
 create mode 100644 rbs_bt_executor/bt_trees/example_skills/move_to_joint_states.xml
 create mode 100644 rbs_bt_executor/bt_trees/example_skills/move_to_pose.xml
 create mode 100644 rbs_bt_executor/bt_trees/example_skills/move_to_pose_array.xml

diff --git a/rbs_bt_executor/bt_trees/example_skills/env_manager_starter.xml b/rbs_bt_executor/bt_trees/example_skills/env_manager_starter.xml
new file mode 100644
index 0000000..41fd05c
--- /dev/null
+++ b/rbs_bt_executor/bt_trees/example_skills/env_manager_starter.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root main_tree_to_execute="Main">
+  <Action ID="EnvStarter"
+    env_class="gz_enviroment::GzEnviroment"
+    env_name="gz_enviroment"
+    server_name="/env_manager/start_env"
+    server_timeout="1000"
+    workspace="{workspace}" />
+
+  <TreeNodesModel>
+    <Action ID="EnvStarter">
+      <input_port name="env_class" />
+      <input_port name="env_name" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+      <output_port name="workspace" />
+    </Action>
+  </TreeNodesModel>
+</root>
diff --git a/rbs_bt_executor/bt_trees/example_skills/move_to_joint_states.xml b/rbs_bt_executor/bt_trees/example_skills/move_to_joint_states.xml
new file mode 100644
index 0000000..b07ff12
--- /dev/null
+++ b/rbs_bt_executor/bt_trees/example_skills/move_to_joint_states.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root main_tree_to_execute="Main">
+  <Action ID="MoveToPose"
+    robot_name="{robot_name}"
+    JointState="{joint_state}"
+    server_name="{action_server}"
+    server_timeout="{time_s}"
+    cancel_timeout="{time_c}" />
+
+  <TreeNodesModel>
+    <Action ID="MoveToPose">
+      <!-- Массив положений суставов робота-->
+      <!-- std::vector<double>-->
+      <input_port name="JointState" />
+      <!-- std::string название робота. Нужен для MoveIt2 -->
+      <input_port name="robot_name" />
+      <!-- Для ros2_action-->
+      <input_port name="cancel_timeout" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+    </Action>
+  </TreeNodesModel>
+</root>
diff --git a/rbs_bt_executor/bt_trees/example_skills/move_to_pose.xml b/rbs_bt_executor/bt_trees/example_skills/move_to_pose.xml
new file mode 100644
index 0000000..8715ab5
--- /dev/null
+++ b/rbs_bt_executor/bt_trees/example_skills/move_to_pose.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root main_tree_to_execute="Main">
+  <Action ID="MoveToPose"
+    robot_name="{robot_name}"
+    pose="{PostGraspPose}"
+    server_name="{action_server}"
+    server_timeout="{time_s}"
+    cancel_timeout="{time_c}" />
+
+  <TreeNodesModel>
+    <Action ID="MoveToPose">
+      <!-- Непосредственно сама позиция в формате ROS2 сообщения-->
+      <!-- geometry_msgs::msgs::Pose-->
+      <input_port name="pose" />
+      <!-- std::string название робота. Нужен для MoveIt2 -->
+      <input_port name="robot_name" />
+      <!-- Для ros2_action-->
+      <input_port name="cancel_timeout" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+    </Action>
+  </TreeNodesModel>
+</root>
diff --git a/rbs_bt_executor/bt_trees/example_skills/move_to_pose_array.xml b/rbs_bt_executor/bt_trees/example_skills/move_to_pose_array.xml
new file mode 100644
index 0000000..c9e3414
--- /dev/null
+++ b/rbs_bt_executor/bt_trees/example_skills/move_to_pose_array.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root main_tree_to_execute="Main">
+  <Action ID="MoveToPoseArray"
+    cancel_timeout="5000" 
+    pose_vec_in="{goal_pose}"
+    pose_vec_out="{goal_pose}"
+    robot_name="{robot_name}" 
+    server_name="{server_name}" 
+    server_timeout="5000" />
+  <TreeNodesModel>
+    <Action ID="MoveToPose">
+      <!-- Непосредственно сама позиция в формате ROS2 сообщения-->
+      <!-- geometry_msgs::msgs::PoseArray-->
+      <input_port name="pose_vec_in" />
+      <!-- Выдает вычтенную первую позицию из массива-->
+      <!-- geometry_msgs::msgs::PoseArray-->
+      <output_port name="pose_vec_out" />
+      <!-- std::string название робота. Нужен для MoveIt2 -->
+      <input_port name="robot_name" />
+      <!-- Для ros2_action-->
+      <input_port name="cancel_timeout" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+    </Action>
+  </TreeNodesModel>
+</root>
diff --git a/rbs_bt_executor/bt_trees/test_tree.xml b/rbs_bt_executor/bt_trees/test_tree.xml
index 8e11ecc..79b71fa 100644
--- a/rbs_bt_executor/bt_trees/test_tree.xml
+++ b/rbs_bt_executor/bt_trees/test_tree.xml
@@ -6,7 +6,7 @@
             <Action ID="EnvStarter" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"
                 server_name="/env_manager/start_env" server_timeout="1000" workspace="{workspace}" />
             <SubTreePlus ID="WorkspaceInspection" __autoremap="1" goal_pose="{workspace}"
-                robot_name="ur_manipulator" />
+                robot_name="rbs_arm" />
         </Sequence>
     </BehaviorTree>
     <!-- ////////// -->