Full BT(moveit) with condition for checking Aruco

docs/to_start_BT_main.md

@@ -0,0 +1,25 @@
+1) runtime CNC-Graver Assist
+```sh
+ros2 launch rbs_mill_assist simulation.launch.py
+```
+2) интерфейсная нода (bt_path: путь к skills.json)
+```sh
+ros2 launch rbs_bt_executor interface.launch.py bt_path:=/home/<user>/robossembler-ws/install/rbs_mill_assist/share/rbs_mill_assist/config/
+```
+3) BT
+```sh
+ros2 launch rbs_mill_assist bt_executor.launch.py
+```
+4) очередь
+```sh
+source ~/robossembler-ws/.venv/bin/activate
+ros2 run rbs_mill_assist queue_srv.py
+```
+5) добавить задания в очередь (если их нет)
+```sh
+ros2 service call queue/add_tasks rbs_utils_interfaces/srv/AddTasks "{tasks_csv: /home/<user>/robossembler-ws/src/robossembler-cnc-graver-assist/docs/tasks.csv}"
+```
+6) запуск дерева поведения
+```sh
+ros2 action send_goal /behavior_server btcpp_ros2_interfaces/action/ExecuteTree "{target_tree: Main}"
+```

rbs_mill_assist/bt/xmls/moveit/MainTree.xml

@@ -10,6 +10,8 @@
           <Sequence>
             <!-- Preparing -->
 
+            <Action ID="RbssAction" do="ArucoPE" command="arucoConfig" sid="ArucoPE-example" action_name="rbs_interface_a"></Action>
+
             <Action ID="TakeonTask" from_pose="{from_pose}" service_name="queue/takeon_task"/>
             <!-- <ReactiveFallback>
               <Action ID="TakeonTask" service_name="queue/takeon_task" from_pose="{from_pose}"/>
@@ -48,6 +50,10 @@
               service_name="/get_tf_frame_pose" />
             <Action ID="MoveToPose" pose="{named_pose}" robot_name="arm" duration="3"
               action_name="/mtp_jtc" />
+            
+            <Inverter>
+              <Condition ID="RbssCondition" do="ArucoPE" command="isAruco" sid="ArucoPE-example" service_name="rbs_interface_s"/>
+            </Inverter>
 
             <!-- Send task and wait Graver -->
 
@@ -82,6 +88,8 @@
 
             <Action ID="TaskCompleted" service_name="queue/task_completed"/>
 
+            <Action ID="RbssAction" do="ArucoPE" command="arucoStop" sid="ArucoPE-example" action_name="rbs_interface_a"></Action>
+
             <!-- Ending -->
           </Sequence>
         </Repeat>

rbs_mill_assist/config/skills.json

@@ -0,0 +1,93 @@
+{
+  "skills": [
+    {
+      "sid": "ArucoPE-example",
+      "SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" },
+      "Module": { "name": "ArucoPE", "node_name": "aruco_pe", "description": "Aruco Marker Tracking and Pose Estimation wrapper for ROS2, using RGB cameras" },
+      "Launch": { "package": "rbss_aruco_pe", "executable": "aruco_node_lc.py" },
+      "BTAction": [
+        {
+          "name": "arucoConfig",
+          "type": "run",
+          "param": [
+            {
+              "type": "topic",
+              "dependency": {
+                  "type": "topic",
+                  "topicType": "sensor_msgs/msg/Image",
+                  "topicOut": "/rgbd_camera/image/image"
+              }
+            },
+            {
+              "type": "topic",
+              "dependency": {
+                  "type": "topic",
+                  "topicType": "sensor_msgs/msg/Image",
+                  "topicOut": "/rgbd_camera/image/depth_image"
+              }
+            },
+            {
+              "type": "topic",
+              "dependency": {
+                  "type": "topic",
+                  "topicType": "sensor_msgs/msg/CameraInfo",
+                  "topicOut": "/rgbd_camera/image/camera_info"
+              }
+            }
+          ],
+          "typeAction": "ACTION"
+        },
+        { "name": "isAruco", "type": "if", "typeAction": "CONDITION",
+          "param": [
+            {
+              "type": "formBuilder",
+              "dependency": {
+                  "output": {
+                      "aruco_id": "25"
+                  }
+              }
+            }
+          ]
+        },
+        { "name": "arucoStop", "type": "stop", "param": [], "typeAction": "ACTION" }
+      ],
+      "topicsOut": [
+          {
+            "name": "/aruco/markers",
+            "type": "rbs_skill_interfaces/msg/ArucoMarkers"
+          },
+          {
+            "name": "/aruco/poses",
+            "type": "geometry_msgs/msg/PoseArray"
+          },
+          {
+            "name": "/aruco/image",
+            "type": "sensor_msgs/msg/Image"
+          }
+      ],
+      "Settings": {
+        "output": {
+            "params": [
+                {
+                  "name": "is_image_mode",
+                  "value": "True"
+                },
+                {
+                  "name": "aruco_dictionary_id",
+                  "value": "DICT_6X6_50"
+                },
+                {
+                  "name": "marker_size",
+                  "value": "0.04"
+                },
+                {
+                  "name": "camera_frame",
+                  "value": ""
+                }
+            ]
+        },
+        "type": "formBuilder"
+      }
+    }
+  ]
+}