From addfd4016096e46046a6493125428b675090b657 Mon Sep 17 00:00:00 2001
From: shalenikol <shaniks77s@gmail.com>
Date: Thu, 18 Apr 2024 16:56:01 +0300
Subject: [PATCH] add MoveToPose skill config (through Interface Node)

---
 rbs_bt_executor/CMakeLists.txt             |  3 +++
 rbs_bt_executor/bt_trees/bt_movetopose.xml | 17 +++++++++++++++
 rbs_bt_executor/bt_trees/bt_pe_stop.xml    | 17 +++++++++++++++
 rbs_perception/config/MoveToPose.json      | 24 ++++++++++++++++++++++
 rbs_perception/config/move.yaml            |  3 +++
 5 files changed, 64 insertions(+)
 create mode 100644 rbs_bt_executor/bt_trees/bt_movetopose.xml
 create mode 100644 rbs_bt_executor/bt_trees/bt_pe_stop.xml
 create mode 100644 rbs_perception/config/MoveToPose.json
 create mode 100644 rbs_perception/config/move.yaml

diff --git a/rbs_bt_executor/CMakeLists.txt b/rbs_bt_executor/CMakeLists.txt
index 1f6cac1..6036924 100644
--- a/rbs_bt_executor/CMakeLists.txt
+++ b/rbs_bt_executor/CMakeLists.txt
@@ -80,6 +80,9 @@ list(APPEND plugin_libs rbs_env_manager_starter)
 add_library(rbs_skill_move_topose_array_bt_action_client SHARED src/MoveToPoseArray.cpp)
 list(APPEND plugin_libs rbs_skill_move_topose_array_bt_action_client)
 
+add_library(rbs_interface SHARED src/rbsBTAction.cpp)
+list(APPEND plugin_libs rbs_interface)
+
 foreach(bt_plugin ${plugin_libs})
     ament_target_dependencies(${bt_plugin} ${dependencies})
     target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
diff --git a/rbs_bt_executor/bt_trees/bt_movetopose.xml b/rbs_bt_executor/bt_trees/bt_movetopose.xml
new file mode 100644
index 0000000..68be359
--- /dev/null
+++ b/rbs_bt_executor/bt_trees/bt_movetopose.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root main_tree_to_execute="Main">
+  <BehaviorTree ID="Main">
+    <Sequence>
+      <Action ID="RbsBtAction" do="MoveToPose" command="move" server_name="rbs_interface" server_timeout="1000" />
+    </Sequence>
+  </BehaviorTree>
+
+  <TreeNodesModel>
+    <Action ID="RbsBtAction">
+      <input_port name="do" />
+      <input_port name="command" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+    </Action>
+  </TreeNodesModel>
+</root>
diff --git a/rbs_bt_executor/bt_trees/bt_pe_stop.xml b/rbs_bt_executor/bt_trees/bt_pe_stop.xml
new file mode 100644
index 0000000..d493571
--- /dev/null
+++ b/rbs_bt_executor/bt_trees/bt_pe_stop.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root main_tree_to_execute="Main">
+  <BehaviorTree ID="Main">
+    <Sequence>
+      <Action ID="RbsBtAction" do="PoseEstimation" command="peStop" server_name="rbs_interface" server_timeout="1000" />
+    </Sequence>
+  </BehaviorTree>
+
+  <TreeNodesModel>
+    <Action ID="RbsBtAction">
+      <input_port name="do" />
+      <input_port name="command" />
+      <input_port name="server_name" />
+      <input_port name="server_timeout" />
+    </Action>
+  </TreeNodesModel>
+</root>
diff --git a/rbs_perception/config/MoveToPose.json b/rbs_perception/config/MoveToPose.json
new file mode 100644
index 0000000..0508827
--- /dev/null
+++ b/rbs_perception/config/MoveToPose.json
@@ -0,0 +1,24 @@
+{
+    "SkillPackage": {
+        "name": "Robossembler", "version": "1.0", "format": "1"
+    },
+    "Module": {
+        "name": "MoveToPose", "description": "Move to Pose skill with dekart controllers"
+    },
+    "Launch": {
+        "executable": "movetopose_lc.py"
+    },
+    "ROS2": {
+        "node_name": "lc_dc"
+    },
+    "BTAction": [
+        {
+            "name": "move",
+            "format": "yaml",
+            "type": "run",
+            "param":["robot_name","pose"],
+            "result":[]
+        }
+    ],
+    "Settings": []
+}
diff --git a/rbs_perception/config/move.yaml b/rbs_perception/config/move.yaml
new file mode 100644
index 0000000..b8958b3
--- /dev/null
+++ b/rbs_perception/config/move.yaml
@@ -0,0 +1,3 @@
+robot_name: arm1
+# loc_xyz, rot_euler
+pose: [0.137, 0.165, 0.202, 0.0, 0.0, 3.14]