interface node synchronized + readme for perception

rbs_bt_executor/scripts/rbs_interface.py

@@ -5,13 +5,13 @@
 
   @shalenikol release 0.1
   @shalenikol release 0.2 BT v.4
+  @shalenikol release 0.3 synchronize
 """
 import os
 import json
 
 import rclpy
 from rclpy.node import Node
-# from rclpy.action import ActionClient
 from rclpy.action import ActionServer
 from rclpy.callback_groups import ReentrantCallbackGroup
 from ament_index_python.packages import get_package_share_directory
@@ -19,14 +19,15 @@ from ament_index_python.packages import get_package_share_directory
 from rclpy.parameter import Parameter
 from rcl_interfaces.srv import SetParameters #, GetParameters
 from rcl_interfaces.msg import SetParametersResult #, ParameterEvent
-from lifecycle_msgs.srv import ChangeState, GetState
+from lifecycle_msgs.srv import ChangeState
-from lifecycle_msgs.msg import Transition, State
+from lifecycle_msgs.msg import Transition
 from rbs_skill_interfaces.srv import RbsBt
 from rbs_skill_interfaces.action import RbsBt as RbsBtAction
 
 # from rclpy.parameter_client import AsyncParameterClient # only Iron
 
 CONDITION_SRV_NAME = "/condition"
+COMPLETION_SRV_NAME = "/completion"
 BT_PARAM     = "bt_path"
 NODE_NAME    = "rbs_interface"
 SERVICE_NAME = "rbs_interface_s"
@@ -40,7 +41,6 @@ class rbsInterface(Node):
         """Construct the node."""
         self.bt_path = ""    # path to the current BehaviorTree
         self.cfg_data = None # config for current action
-        self._timer = None   # for BT v.4
         super().__init__(node_name)
         self.declare_parameter(BT_PARAM, rclpy.Parameter.Type.STRING)
         self.cb_group = ReentrantCallbackGroup()
@@ -164,18 +164,23 @@ class rbsInterface(Node):
                         ret = res.success
 
         elif oper_type == "action":
+            # cnt = 0
+            while not self.run_condition(COMPLETION_SRV_NAME, command_data["name"]):
+                pass
+                # cnt += 1
+                # self.get_logger().info(f"action + run_condition: {cnt}")
             ret = True
 
         return ret
 
-    def run_condition(self, command_data: dict) -> bool:
+    def run_condition(self, srv_name: str, command: str) -> bool:
-        node_name = self.cfg_data["Module"]["node_name"]
+        srv_topic = self.cfg_data["Module"]["node_name"] + srv_name
 
-        self.cli = self.create_client(RbsBt, node_name + CONDITION_SRV_NAME)
+        self.cli = self.create_client(RbsBt, srv_topic)
         while not self.cli.wait_for_service(timeout_sec=1.0):
-            self.get_logger().info("'" + node_name + "' service not available, waiting again...")
+            self.get_logger().info(f"'{srv_topic}' service not available, waiting again...")
         req = RbsBt.Request()
-        req.command = command_data["name"]
+        req.command = command
         future = self.cli.call_async(req)
 
         self.executor.spin_until_future_complete(future)
@@ -183,15 +188,14 @@ class rbsInterface(Node):
         return res.ok
 
     def _interface(self, sid: str, action: str, command: str, isCondition: bool) -> bool:
-        self.get_logger().info(f"Incoming request ({action}/{command})")
         self.cfg_data = self._load_config(sid)
         typeBTnode = "Condition" if isCondition else "Action"
-        self.get_logger().info(f'{typeBTnode}: Ok ({self.cfg_data["Module"]["description"]})')
+        self.get_logger().info(f'Incoming request ({action}/{command}) {typeBTnode} ({self.cfg_data["Module"]["description"]})')
 
         is_success = False
         for comm in self.cfg_data[KEY_BTPARAM]:
             if comm["name"] == command:
-                is_success = self.run_condition(comm) if isCondition else self.run_action(comm)
+                is_success = self.run_condition(CONDITION_SRV_NAME, command) if isCondition else self.run_action(comm)
 
         return is_success
 
@@ -213,8 +217,8 @@ class rbsInterface(Node):
 
 def main():
     rclpy.init()
-    # executor = rclpy.executors.SingleThreadedExecutor()
+    executor = rclpy.executors.SingleThreadedExecutor()
-    executor = rclpy.executors.MultiThreadedExecutor()
+    # executor = rclpy.executors.MultiThreadedExecutor() # can't be used
     i_node = rbsInterface(NODE_NAME)
     executor.add_node(i_node)
     try:

rbs_perception/README.md

@@ -18,23 +18,66 @@
 ### Этап 1. Создание датасета
 
 Для создания датасета используется модуль на Python для BlenderProc. Внешними параметрами для модуля являются:
-- сцена, подготовленная в Blender (файл *.blend)
+- файл, описывающий параметры рандомизиции, а также объекты сцены с подготовленными мешами (файл *.json)
-- каталог с файлами мешей в формате *.fbx  
+- выходной каталог.
 
-Сцена используется для случайного размещения в ней объектов из папки с мешами. Затем производится рендеринг полученной сцены с рандомизацией параметров освещения, текстур и размещением камеры.  
+Формируется сцена для случайного размещения в ней объектов из описания. Затем производится рендеринг полученной сцены с рандомизацией параметров освещения, текстур и размещением камеры. Имена объектов должны совпадать с именами ассетов в нашей базе данных.
-Имена файлов мешей должны совпадать с именами ассетов в нашей базе данных.
+В результате будет получен датасет в формате [BOP](https://github.com/thodan/bop_toolkit/blob/master/docs/bop_datasets_format.md)
 
 Пример запуска модуля генерации датасета:  
 ```bash
-blenderproc run objs2Yolov4dataset.py [scene] [obj_path] [output_dir] [vhacd_path] [--imgs 1]
+blenderproc run renderBOPdataset2.py --form description.json --path /home/user/path/to/dataset
+```
+Пример файла description.json:
+```json
+{"output":{
+    "datasetObjects":{
+        "details":[
+            {"name":"star",
+             "inertia":{
+                "ixx":0.1,"ixy":0,"ixz":0,"iyy":0.1,"iyz":0,"izz":0.1
+                },
+             "mass":"0",
+             "visual":"/assets/libs/objects/star.dae",
+             "collision":"/assets/libs/objects/star.stl",
+             "type":"env",
+             "material_path":"",
+             "part_path":"/libs/objects/star.stl",
+             "fbx":"/home/webservice/server/build/public/4c4f3909-74b0-4206-aec1-fc4acd3a1081/assets/libs/objects/star.fbx",
+             "solidType":"active",
+             "isSelect":true
+            }
+        ]
+    },
+    "typedataset":"ObjectDetection",
+    "models_randomization":{
+        "loc_range_low":[-1,-1,0],"loc_range_high":[1,1,2]
+    },
+    "scene":{
+        "objects":[
+            {"name":"floor","collision_shape":"BOX","loc_xyz":[0,0,0],"rot_euler":[0,0,0],"material_randomization":{"specular":[0,1],"roughness":[0,1],"metallic":[0,1],"base_color":[[0,0,0,1],[1,1,1,1]}}},
+            {"name":"star","collision_shape":"BOX","loc_xyz":[0,0,0.2],"rot_euler":[0,0,0],"material_randomization":{"specular":[0,1],"roughness":[0,1],"metallic":[0,1],"base_color":[[0,0,0,1],[1,1,1,1]}}
+        ],
+        "lights":[
+            {"id":1,"type":"SUN","loc_xyz":[5,5,5],"rot_euler":[-0.06,0.61,-0.19],"color_range_low":[0.5,0.5,0.5],"color_range_high":[1,1,1],"energy_range":[2,9]}
+        ]
+    },
+    "camera_position":{
+        "center_shell":[0,0,0],
+        "radius_range":[0.3,0.65],
+        "elevation_range":[10,90]
+    },
+    "generation":{
+        "n_cam_pose":3,
+        "n_sample_on_pose":3,
+        "n_series":222,
+        "image_format":"JPEG",
+        "image_size_wh":[640,480]
+    }
+}
 ```
-- scene: путь к файлу описания сцены (*.blend)
-- obj_path: путь к каталогу с файлами описания детектируемых объектов *.obj
-- output_dir: выходной каталог
-- vhacd_path: каталог, в котором должен быть установлен или уже установлен vhacd (по умолчанию blenderproc_resources/vhacd)
-- --imgs 2: количество серий рендеринга (по 15 изображений каждая) на выходе (рекомендуется imgs=200, будет получено 3000 изображений)  
 
-В результате работы модуля в папке output_dir будет создана файловая структура с датасетом.
+В результате работы модуля в папке '/home/user/path/to/dataset' будет создана файловая структура с датасетом.
 
 ### Этап 2. Обучение модели Yolov8.
 
@@ -43,157 +86,238 @@ blenderproc run objs2Yolov4dataset.py [scene] [obj_path] [output_dir] [vhacd_pat
 
 Пример запуска модуля обучения:
 ```bash
-python rbs_yolov8_train.py [dataset_path] [n_epoch]
+python train_Yolo.py --path /home/user/path/to/dataset --epoch 11 --outpath /home/user/path/to/weights
 ```
-- dataset_path: путь к каталогу с датасетом
+- path: путь к каталогу с датасетом
-- --n_epoch 3: количество эпох обучения (пока рекомендуем 60-100)  
+- epoch 11: количество эпох обучения (пока рекомендуем 30-50)
+- outpath: выходной каталог  
 В результате работы создается файл весов нейросети с лучшими характеристиками обнаружения best.pt
 
-Замечание. Пока этот модуль отсутствует. Подготовка параметров запуска обучения и само обучение проводились в тестовом (ручном) режиме.
-
 ### Этап 3. Использование навыка в ROS2 для обнаружения объекта на изображении (runtime).
 
-1. Запуск lifecycle-узла ROS2, реализующего алгоритм навыка обнаружения объектов.
+1. Подготовить папку с файлами BT v.4
-
+* Папка /path/to/bt/
-Запуск узла производится командой:  
+* bt.xml
-```bash
-ros2 run rbs_perception pose_estimation_lifecycle.py
-```
-Запускается узел с именем `lc_detection` по умолчанию. Настроечные переменные задаются в файле конфигурации `rbs_perception/config/detection_config.json`. Пример:  
-```json
-{
-    "nodeName":"object_detection",
-    "cameraLink":"inner_rgbd_camera",
-    "topicImage":"/inner_rgbd_camera/image",
-    "publishDelay": 0.5
-}
-```
-- `"nodeName"`- это поле указывает на имя узла в контексте ROS;  
-- `"cameraLink"`- это поле определяет наименование RGB-камеры;
-- `"topicImage"`- это поле определяет топик, по которому публикуются изображения, полученные с RGB-камеры;
-- `"topicSrv"`- это поле определяет выходной топик (публикуется сообщение BoundBox);  
-- `"publishDelay"`- это поле, указывает задержку (в секундах) перед публикацией сообщений в выходной топик.  
-
-2. Запуск процесса обнаружения заданного объекта.
-
-Процесс стартует после выполнения операций конфигурирования lifecycle-узла, запущенного в пункте 1. Операции конфигурирования реализуются в дереве поведения `rbs_bt_executor/bt_trees/test_objdet_run.xml`, которое выполняется командой:
-```bash
-ros2 launch rbs_bt_executor rbs_executor.launch.py bt_file:=test_objdet_run.xml
-```
-В файле дерева поведения test_objdet_run.xml используется внешний параметр - `SettingPath`, для указания пути к json-файлу с настройками:
 ```xml
-<?xml version="1.0"?>
+<root BTCPP_format="4">
-<root main_tree_to_execute="ObjDetection">
+    <BehaviorTree ID="Main">
-    <BehaviorTree ID="ObjDetection">
         <Sequence>
-            <Action ID="ObjectDetection" 
+            <Action ID="RbsAction" do="ObjectDetection" command="odConfigure" sid="a"></Action>
-            SettingPath="!/home/shalenikol/0yolov8/test.json"
-            ReqKind = "configure"
-            server_name="/object_detection/change_state" 
-            server_timeout="1000"/>
-            <Action ID="ObjectDetection" 
-            SettingPath=""
-            ReqKind = "activate"
-            server_name="/object_detection/change_state" 
-            server_timeout="1000"/>
         </Sequence>
     </BehaviorTree>
 </root>
 ```
-Пример json-файла с настройками навыка обнаружения объекта:
+* skills.json
 ```json
-{
+{"skills": [
- "yolov8_weights_file":"/home/shalenikol/0yolov8/yolov8n.pt",
+    {
- "objName":"king",
+        "sid": "a",
- "mode":"image_res",
+        "SkillPackage": {
-}
+            "name": "Robossembler", "version": "1", "format": "1.0"
+        },
+        "Module": {
+            "node_name": "lc_yolo", "name": "ObjectDetection", "description": "Object detection skill with YOLOv8"
+        },
+        "BTAction": [
+            {
+                "name": "odConfigure",
+                "type": "run",
+                "param": [
+                    {
+                        "type": "weights",
+                        "dependency": {"object_name": "board", "weights_file": "/home/shalenikol/0_rbs/w_od_board.pt"}
+                    },
+                    {
+                        "type": "topic",
+                        "dependency": {
+                            "type": "topic",
+                            "topicType": "sensor_msgs/msg/Image",
+                            "sid": "7b832b17-3030-4758-aab5-96a5046797f7",
+                            "topicOut": "/robot_camera/image"
+                        },
+                        "isFilled": true
+                    }
+                ],
+                "result": [],
+                "typeAction": "ACTION"
+            }
+        ],
+        "topicsOut": [
+            {
+                "name": "lc_yolo/object_detection",
+                "type": "rbs_skill_interfaces/msg/BoundBox"
+            }
+        ],
+        "Launch": {
+            "executable": "od_yolo_lc.py",
+            "package": "rbss_objectdetection"
+        }
+    }
+]}
 ```
-- `"yolov8_weights_file"`- это путь к файлу весов нейросети;  
-- `"objName"`- это наименование объекта для обнаружения;
-- `"mode"`- это режим работы навыка (по умолчанию - в топик `"topicSrv"` публикуется сообщение BoundBox, если `"image_res"` - то публикуется изображение с наименованными рамками обнаруженных объектов).
 
+2. Запуск интерфейсной ноды с сервером навыка, реализующего алгоритм обнаружения объектов.
+
+```bash
+ros2 launch rbs_bt_executor interface.launch.py bt_path:=/path/to/bt
+```
+
+3. Запуск процесса обнаружения заданного объекта через дерево поведения.  
+Выполняется командой:
+```bash
+ros2 launch rbs_bt_executor rbs_executor.launch.py bt_path:=/path/to/bt
+```
 После этого узел начинает публиковать в выходной топик информацию об обнаружении объекта на каждом полученном с камеры изображении.
 
-3. Прекращение процесса обнаружения объекта.
+4. Прекращение процесса обнаружения объекта.
 
-Для завершения навыка нужно выполнить действия из дерева поведения `rbs_bt_executor/bt_trees/test_objdet_cleanup.xml`:
+Для завершения навыка нужно выполнить дерево поведения:
 ```xml
-<?xml version="1.0"?>
+<root BTCPP_format="4">
-<root main_tree_to_execute="ObjDetection">
+    <BehaviorTree ID="Main">
-    <BehaviorTree ID="ObjDetection">
         <Sequence>
-            <Action ID="ObjectDetection" 
+            <Action ID="RbsAction" do="ObjectDetection" command="odStop" sid="b"></Action>
-            SettingPath=""
-            ReqKind = "deactivate"
-            server_name="/object_detection/change_state" 
-            server_timeout="1000"/>
-            <Action ID="ObjectDetection" 
-            SettingPath=""
-            ReqKind = "cleanup"
-            server_name="/object_detection/change_state" 
-            server_timeout="1000"/>
         </Sequence>
     </BehaviorTree>
 </root>
 ```
-
+Файл skills.json
-Команда запуска этого дерева:
-```bash
-ros2 launch rbs_bt_executor rbs_executor.launch.py bt_file:=test_objdet_cleanup.xml
-```
-После выполнения этих действий lifecycle-узел перейдёт в начальное состояние и можно, повторив пункт 2, вновь запустить процесс обнаружения уже с другим объектом.
-
-## Оценка 6D положения объекта
-
-Есть два варианта работы узла по оценке 6D позы объекта. Первый предполагает использование ROS-узла как сервиса, второй - как [lifecycle-узла](https://design.ros2.org/articles/node_lifecycle.html).
-Узел с управляемым жизненным циклом (lifecycle) позволяет лучше контролировать состояние системы ROS. Такой подход позволит системе убедиться, что все компоненты были созданы правильно, прежде чем любой компонент сможет начать выполнение своей задачи. Это также позволит перезапускать или заменять узел в режиме онлайн.  
-Так как задача оценки позиции объекта требует использования больших вычислительных ресурсов, то реализация lifecycle-узла позволяет управлять загрузкой ресурсов при этом.
-
-### Вариант 1. Сервис по оценке позы объекта
-
-Запуск узла производится командой:  
-```bash
-ros2 run rbs_perception pose_estimation.py
-```
-Запускается узел с именем `image_sub2` по умолчанию. В нём создаётся сервис для распознавания позиции с именем `detect6Dpose`, который ожидает клиентский запрос.  
-Для получения позиции заданного объекта клиент посылает сервису запрос с параметром ObjectInfo на входе  
-- `id` - идентификатор,
-- `name` - имя объекта,
-- `mesh_path` - путь к mesh-файлу в формате *.ply.  
-
-При получении запроса сервис `detect6Dpose` подписывается на Image-сообщения с камеры `/ground_true/camera_node`, которые использует для запуска алгоритма 6D оценки позиции Megapose. После получения результата от Megapose сервис публикует сообщение с позицией (Quaternion) в `pose6D_[obj]` topic.  
-
-### Вариант 2. Lifecycle-узел ROS для получения 6D-позы
-
-Запуск узла производится командой:  
-```bash
-ros2 run rbs_perception pose_estimation_lifecycle.py
-```
-Запускается узел с именем `image_sub2` по умолчанию. Настроечные переменные задаются в файле конфигурации `config/pose_estimation_config.json`. Пример:  
 ```json
-{
+{"skills": [
- "nodeName":"image_sub2",
+    {
- "topicImage":"/outer_rgbd_camera/image",
+        "sid": "b",
- "topicCameraInfo":"/outer_rgbd_camera/camera_info",
+        "SkillPackage": { "name": "Robossembler", "version": "1", "format": "1.0" },
- "topicDepth":"/outer_rgbd_camera/depth_image",
+        "Module": {"node_name": "lc_yolo", "name": "ObjectDetection", "description": "Object detection skill with YOLOv8"},
- "topicSrv":"/image_sub2/detect6Dpose",
+        "BTAction": [
- "publishDelay": 3.3,
+            {
- "tf2_send_pose": 1
+                "name": "odStop",
-}
+                "type": "stop",
+                "param": [],
+                "result": [],
+                "typeAction": "ACTION"
+            }
+        ],
+        "topicsOut": [
+            {
+                "name": "lc_yolo/object_detection",
+                "type": "rbs_skill_interfaces/msg/BoundBox"
+            }
+        ],
+        "Launch": {
+            "executable": "od_yolo_lc.py",
+            "package": "rbss_objectdetection"
+        }
+    }
+]}
 ```
-- `"nodeName"`- это поле указывает на имя узла в контексте ROS;  
+Команда запуска этого дерева та же, что и в пункте 3.
-- `"topicImage"`- это поле определяет топик, по которому публикуются изображения, полученные с RGBD-камеры;
+После выполнения этих действий lifecycle-узел навыка перейдёт в начальное состояние и можно, повторив пункт 1-3, вновь запустить процесс обнаружения уже с другим объектом.
-- `"topicCameraInfo"`- это поле указывает на топик, по которому публикуется информация о камере, такая как параметры калибровки и настройки;  
-- `"topicDepth"`- это поле определяет топик для изображений глубины, получаемых от RGBD-камеры. Изображения глубины предоставляют информацию о расстоянии до объектов в сцене;  
-- `"topicSrv"`- это поле определяет топик, по которому публикуется 6D-поза объекта после оценки;  
-- `"publishDelay"`- это поле, указывает задержку (в секундах) перед публикацией сообщений в топики;  
-- `"tf2_send_pose"`- это поле связано с отправкой данных о позе (положении и ориентации) в системе tf2, которая используется в ROS для управления координатными преобразованиями. Значение 1 означает включение или активацию данной функции (0 - отключение).
 
-Первым этапом работы узла будет являться его настройка вызовом `on_configure()`, перед которым необходимо установить параметр узла `"mesh_path"`. Этот строковый параметр должен содержать полный путь к файлу сетчатой модели объекта в формате *.ply.  
+## Оценка 6D положения объекта (Pose Estimation). API навыка.
-Если конфигурирование завершено успешно, узел перейдёт в состояние `'inactive'`. Затем нужно узел сделать активным, вызвав `on_activate()`. При этом активируется подписчик на изображения с камеры (`"topicImage"`), в обратном вызове которого будет происходить распознавание позиции объекта (megapose) и его публикация в топике `"topicSrv"`.  
-Чтобы отключить распознавание нужно вызвать событие `on_deactivate()`.
-Для новой настройки узла при необходимости оценки позы другого объекта нужно вызвать событие `on_cleanup()`, а затем повторить процедуру конфигурирования и активации.
 
-### Важное замечание
+### Этап 1. Создание датасета
----
+
-Для правильной работы алгоритма Megapose нужно передавать модель объекта с размерами в мм (mesh-файл *.ply).
+Этот этап точно такой же, как и в случае с Object Detection. Так как синтетический датасет формата [BOP](https://github.com/thodan/bop_toolkit/blob/master/docs/bop_datasets_format.md) содержит в аннотации истинные позиции заданных объектов в сцене (ground true pose), поэтому его можно использовать также и при обучения модели [DOPE](https://github.com/NVlabs/Deep_Object_Pose) для оценки 6D положения объекта.
+
+
+### Этап 2. [Обучение модели DOPE.](https://github.com/NVlabs/Deep_Object_Pose/tree/master/train)
+
+Для обучения модели используется скрипт на Python. Аргументом для скрипта является:
+- каталог с датасетом, сгенерированный на первом этапе.
+В ходе работы скрипта исходный датасет предварительно конвертируется в формат, который используется непосредственно при обучении модели DOPE.
+
+Пример запуска модуля обучения:
+```bash
+python train_Dope.py --path /home/user/path/to/dataset --epoch 44 --outpath /home/user/path/to/weights --name weightsX
+```
+- path: путь к каталогу с датасетом
+- epoch: количество эпох обучения
+- name: наименование файла весов модели на выходе
+- outpath: выходной каталог  
+В результате работы создается файл весов модели с лучшими характеристиками обнаружения weightsX.pth
+
+### Этап 3. Использование навыка в ROS2 для оценки 6D-положения объекта на изображении (runtime).
+
+Этот процесс аналогичен такому же при обнаружении объекта (YoloV8), здесь приведу лишь дерево (bt.xml) и файл описания скилов в дереве (skills.json).  
+* Папка /path/to/bt/
+* bt.xml
+```xml
+<root BTCPP_format="4">
+    <BehaviorTree ID="Main">
+        <Sequence>
+            <Action ID="RbsAction" do="PoseEstimation" command="peConfigure" sid="a"></Action>
+        </Sequence>
+    </BehaviorTree>
+</root>
+```
+* skills.json
+```json
+{"skills": [
+    {
+        "sid": "a",
+        "SkillPackage": {"name": "Robossembler","version": "1.0","format": "1"},
+        "Module": {"node_name": "lc_dope","name": "PoseEstimation","description": "Pose Estimation skill with DOPE"},
+        "BTAction": [
+            {
+                "name": "peConfigure",
+                "type": "run",
+                "param": [
+                    {
+                        "type": "weights",
+                        "dependency": { "object_name": "knight", "weights_file": "/home/shalenikol/0_rbs/w_knight.pth", "dimensions": [0.03, 0.026, 0.065] }
+                    },
+                    {
+                        "type": "topic",
+                        "dependency": {
+                            "type": "topic",
+                            "topicType": "sensor_msgs/msg/Image",
+                            "topicOut": "/rgbd_camera/image"
+                        },
+                        "isFilled": true
+                    },
+                    {
+                        "type": "topic",
+                        "dependency": {
+                            "type": "topic",
+                            "topicType": "sensor_msgs/msg/CameraInfo",
+                            "topicOut": "/rgbd_camera/camera_info"
+                        },
+                        "isFilled": true
+                    }
+                ],
+                "result": [],
+                "typeAction": "ACTION"
+            }
+        ],
+        "topicsOut": [
+            {
+                "name": "lc_dope/pose_estimation",
+                "type": "geometry_msgs/msg/Pose"
+            }
+        ],
+        "Launch": {
+            "executable": "pe_dope_lc.py",
+            "package": "rbss_poseestimation"
+        },
+        "Settings": {
+            "output": {
+                "params": [
+                    {
+                        "name": "publishDelay",
+                        "value": "0.5"
+                    },
+                    {
+                        "name": "tf2_send_pose",
+                        "value": "1"
+                    },
+                    {
+                        "name": "mesh_scale",
+                        "value": "0.001"
+                    }
+                ]
+            },
+            "type": "formBuilder"
+        }
+    }
+]}
+```

rbss_movetopose/scripts/mtp_cartesian.py

@@ -3,30 +3,23 @@
   Move_to_pose_cartesian_node_via_interface_node
   ROS 2 program for Move to Pose skill
 
-  @shalenikol release 0.2
+  @shalenikol release 0.3
 """
 import json
-import math
-import numpy as np
-from scipy.spatial.transform import Slerp, Rotation as R
 
 import rclpy
-from rclpy.action import ActionClient, ActionServer
+from rclpy.action import ActionClient
 from rclpy.node import Node
-from rclpy.callback_groups import ReentrantCallbackGroup
 
 from rcl_interfaces.msg import SetParametersResult
-from geometry_msgs.msg import Pose, PoseStamped
 
 from rbs_skill_interfaces.action import MoveitSendPose
+from rbs_skill_interfaces.srv import RbsBt
 
 NODE_NAME_DEFAULT = "mtp_cartesian"   # this name must match the name in the description (["Module"]["node_name"])
 PARAM_SKILL_CFG = "mtp_cartesian_cfg"
+COMPLETION_SRV_NAME = "/completion"
 SERVER_NAME = "cartesian_movetopose"
-TOPIC_CURRENT_POSE = "/cartesian_motion_controller/current_pose"
-TOPIC_TARGET_FRAME = "/cartesian_motion_controller/target_frame"
-SERVER_PAR1_BASE_LINK = "base_link"
-SERVER_PAR2_ROBOT_NAME = "robot_name"
 
 class MoveToPoseCartesianSkill(Node):
     """ <Move to pose> skill node """
@@ -47,6 +40,8 @@ class MoveToPoseCartesianSkill(Node):
         #         self.topicSrv = prop["name"]
 
     def __init__(self, **kwargs):
+        self._tmode = 0
+        self._completion = False
         self.end_effector_velocity = 1.0
         self.end_effector_acceleration = 1.0
         # for other nodes
@@ -62,6 +57,7 @@ class MoveToPoseCartesianSkill(Node):
         self._Settings()
 
         self._action_client = ActionClient(self, MoveitSendPose, self.server_name) # "/"+ robot_name + 
+        self._srv_completion = self.create_service(RbsBt, NODE_NAME_DEFAULT + COMPLETION_SRV_NAME, self.completion_callback)
 
         self._cnt = 0
         self.add_on_set_parameters_callback(self._on_set_param)
@@ -74,22 +70,54 @@ class MoveToPoseCartesianSkill(Node):
                 self.skill_cfg = json.loads(parameter.value)
 
                 self._Settings()
-                dependency = {}
+                self.dependency = {}
                 for comm in self.skill_cfg["BTAction"]:
                     for par in comm["param"]:
                         if par["type"] == "move_to_pose":
-                            dependency = par["dependency"]
+                            self.dependency = par["dependency"]
-                assert dependency, "no dependency"
+                assert self.dependency, "no dependency"
 
-                self.send_goal(dependency)
+                self._completion = False # run new action
-                self.get_logger().info(f"{self._cnt}) dependency = {dependency}")
+                self.act_timer = self.create_timer(0.01, self.t_goal)
                 break
         return SetParametersResult(successful=True)
 
-    def send_goal(self, dep):
+    def completion_callback(self, request, response):
+        # if request.command == "isCompletion":
+        response.ok = self._completion
+        return response
+
+    def t_goal(self):
+        if self._tmode == 0:
+
+            self.get_logger().info(f"{self._cnt}) dependency = {self.dependency}")
+
+            goal_msg = self.set_goal_msg(self.dependency)
+
+            self._action_client.wait_for_server()
+            self.goal_fut = self._action_client.send_goal_async(goal_msg)
+
+            self.get_logger().info(f"goal {self._cnt}): waiting for completion...")
+            self._tmode = 1
+
+        elif self._tmode == 1:
+            if self.goal_fut.result():
+                result_future = self.goal_fut.result() # ClientGoalHandle
+                # stop timer
+                self.act_timer.cancel()
+                self._tmode = 0
+
+                result_exe = result_future.get_result_async()
+                result_exe.add_done_callback(self.get_result_callback)
+    
+    def get_result_callback(self, future):
+        self._completion = True
+        result = future.result().result
+        self.get_logger().info(f"result_callback: goal - {result.success}")
+
+    def set_goal_msg(self, dep):
         goal_msg = MoveitSendPose.Goal()
         pose = dep["pose"]
-        # goal_msg.target_pose
         goal_msg.target_pose.position.x = pose["position"]["x"]
         goal_msg.target_pose.position.y = pose["position"]["y"]
         goal_msg.target_pose.position.z = pose["position"]["z"]
@@ -102,166 +130,18 @@ class MoveToPoseCartesianSkill(Node):
         goal_msg.robot_name = dep["robot_name"]
         goal_msg.end_effector_velocity = self.end_effector_velocity
         goal_msg.end_effector_acceleration = self.end_effector_acceleration
-
+        return goal_msg
-        self._action_client.wait_for_server()
-
-        return self._action_client.send_goal_async(goal_msg)
-
-class CartesianMoveToPose(Node):
-    """ <Move to pose> action server """
-    def __init__(self, server_name:str = SERVER_NAME):
-        super().__init__("cartesian_server")
-
-        self.declare_parameter(SERVER_PAR1_BASE_LINK, "base_link")
-        self.declare_parameter(SERVER_PAR2_ROBOT_NAME, "")
-
-        self._callback_group = ReentrantCallbackGroup()
-        self._action_server = ActionServer(
-            self,
-            MoveitSendPose,
-            server_name,
-            self.execute_callback, callback_group=self._callback_group)
-
-        self.base_link: str = self.get_parameter(SERVER_PAR1_BASE_LINK).get_parameter_value().string_value
-        # for multirobot setup where each robot name is a namespace
-        self.robot_name: str = self.get_parameter(SERVER_PAR2_ROBOT_NAME).get_parameter_value().string_value
-        self.robot_name = self.robot_name.lstrip('/').rstrip('/')
-        self.robot_name = f"/{self.robot_name}" if self.robot_name else ""
-        self._pub = self.create_publisher(PoseStamped,
-                                          self.robot_name + TOPIC_TARGET_FRAME, 1,
-                                          callback_group=self._callback_group)
-        self.current_pose = None
-        self.goal_tolerance = 0.05
-        self.max_speed = 0.1
-        self.max_acceleration = 0.05
-
-    def on_pose_callback(self, msg: PoseStamped):
-        if isinstance(msg, PoseStamped):
-            self.current_pose = msg
-
-    def execute_callback(self, goal_handle):
-        self.get_logger().debug(f"Executing goal {goal_handle.request.target_pose}")
-        target_pose = goal_handle.request.target_pose
-        start_pose = self.current_pose.pose if self.current_pose else None
-
-        if start_pose is None:
-            self.get_logger().error("Current pose is not available")
-            goal_handle.abort()
-            return MoveitSendPose.Result()
-
-        trajectory = self.generate_trajectory(start_pose, target_pose)
-        for point in trajectory:
-            tp = PoseStamped()
-            tp.pose = point
-            tp.header.stamp = self.get_clock().now().to_msg()
-            tp.header.frame_id = self.base_link
-            self._pub.publish(tp)
-            rclpy.spin_once(self, timeout_sec=0.1)
-
-        goal_handle.succeed()
-
-        result = MoveitSendPose.Result()
-        result.success = True
-        return result
-
-    def generate_trajectory(self, start_pose, target_pose):
-        start_position = np.array([
-            start_pose.position.x,
-            start_pose.position.y,
-            start_pose.position.z
-        ])
-        target_position = np.array([
-            target_pose.position.x,
-            target_pose.position.y,
-            target_pose.position.z
-        ])
-
-        start_orientation = R.from_quat([
-            start_pose.orientation.x,
-            start_pose.orientation.y,
-            start_pose.orientation.z,
-            start_pose.orientation.w
-        ])
-        target_orientation = R.from_quat([
-            target_pose.orientation.x,
-            target_pose.orientation.y,
-            target_pose.orientation.z,
-            target_pose.orientation.w
-        ])
-
-        distance = np.linalg.norm(target_position - start_position)
-        max_speed = self.max_speed
-        max_acceleration = self.max_acceleration
-
-        t_acc = max_speed / max_acceleration
-        d_acc = 0.5 * max_acceleration * t_acc**2
-
-        if distance < 2 * d_acc:
-            t_acc = math.sqrt(distance / max_acceleration)
-            t_flat = 0
-        else:
-            t_flat = (distance - 2 * d_acc) / max_speed
-
-        total_time = 2 * t_acc + t_flat
-        num_points = int(total_time * 10)
-        trajectory = []
-
-        times = np.linspace(0, total_time, num_points + 1)
-        key_rots = R.from_quat([start_orientation.as_quat(), target_orientation.as_quat()])
-        slerp = Slerp([0, total_time], key_rots)
-
-        for t in times:
-            if t < t_acc:
-                fraction = 0.5 * max_acceleration * t**2 / distance
-            elif t < t_acc + t_flat:
-                fraction = (d_acc + max_speed * (t - t_acc)) / distance
-            else:
-                t_decel = t - t_acc - t_flat
-                fraction = (d_acc + max_speed * t_flat + 0.5 * max_acceleration * t_decel**2) / distance
-
-            intermediate_position = start_position + fraction * (target_position - start_position)
-            intermediate_orientation = slerp([t])[0]
-
-            intermediate_pose = Pose()
-            intermediate_pose.position.x = intermediate_position[0]
-            intermediate_pose.position.y = intermediate_position[1]
-            intermediate_pose.position.z = intermediate_position[2]
-            intermediate_orientation_quat = intermediate_orientation.as_quat()
-            intermediate_pose.orientation.x = intermediate_orientation_quat[0]
-            intermediate_pose.orientation.y = intermediate_orientation_quat[1]
-            intermediate_pose.orientation.z = intermediate_orientation_quat[2]
-            intermediate_pose.orientation.w = intermediate_orientation_quat[3]
-            trajectory.append(intermediate_pose)
-
-        return trajectory
-
-class PoseSubscriber(Node):
-    def __init__(self, parent: CartesianMoveToPose, robot_name: str):
-        super().__init__("pose_subscriber")
-        self.parent = parent
-        self._sub = self.create_subscription(PoseStamped,
-                                             robot_name + TOPIC_CURRENT_POSE,
-                                             self.parent.on_pose_callback, 1,
-                                             callback_group=self.parent._callback_group)
-        self.get_logger().info("PoseSubscriber node initialized")
 
 def main():
     rclpy.init()
 
     node = MoveToPoseCartesianSkill()
-    cartesian_mtp_node = CartesianMoveToPose(node.server_name)
-    pose_subscriber = PoseSubscriber(parent=cartesian_mtp_node, 
-                                     robot_name=cartesian_mtp_node.robot_name)
 
-    executor = rclpy.executors.MultiThreadedExecutor()
+    executor = rclpy.executors.SingleThreadedExecutor()
-    executor.add_node(cartesian_mtp_node)
-    executor.add_node(pose_subscriber)
     executor.add_node(node)
     try:
         executor.spin()
     except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
-        pose_subscriber.destroy_node()
-        cartesian_mtp_node.destroy_node()
         node.destroy_node()
 
 if __name__ == '__main__':