.gitignore

@@ -0,0 +1,3 @@
+*.blend1
+*.vscode
+**/__pycache__/**

docs/bt_example_with_tasks.md

@@ -0,0 +1,165 @@
+Для реализации обработки очереди задач в сценарии, описанном вами, можно использовать **Behavior Tree (BT)** — дерево поведения. Это мощный инструмент для управления сложными последовательностями действий, особенно в робототехнике и автоматизированных системах. Давайте разберем, как это можно сделать.
+
+---
+
+### **Основные компоненты Behavior Tree**
+1. **Action Node**:
+   - Выполняет конкретное действие.
+   - Например: "Добавить задачу в очередь", "Выполнить гравировку".
+
+2. **Control Node**:
+   - Управляет выполнением дочерних узлов.
+   - Примеры:
+     - **Sequence**: Выполняет дочерние узлы по порядку. Если один узел завершается неудачно, вся последовательность останавливается.
+     - **Selector**: Пытается выполнить дочерние узлы по порядку. Если один узел завершается успешно, остальные игнорируются.
+     - **Parallel**: Выполняет несколько узлов одновременно.
+
+3. **Condition Node**:
+   - Проверяет условие (например: "Есть ли задачи в очереди?").
+   - Возвращает успех или неудачу в зависимости от результата проверки.
+
+4. **Decorator Node**:
+   - Модифицирует поведение дочернего узла.
+   - Например: "Повторять действие, пока условие истинно".
+
+---
+
+### **Алгоритм работы системы**
+1. Оператор вводит данные в веб-интерфейс, создавая спецификацию задачи.
+2. Спецификация добавляется в очередь задач.
+3. Управляющая программа РТК (роботизированный технологический комплекс) обрабатывает задачи из очереди.
+4. Для каждой задачи выполняется алгоритм гравировки:
+   - Подготовка оборудования.
+   - Гравировка шильдика.
+   - Проверка качества выполнения.
+
+---
+
+### **Реализация в Behavior Tree**
+
+#### 1. **Основная структура дерева**
+Мы можем разделить процесс на две основные части:
+- **Обработка очереди задач**.
+- **Выполнение гравировки**.
+
+#### 2. **Control Nodes**
+- **Selector**: Используется для выбора между "обработкой очереди" и "выполнением гравировки".
+- **Sequence**: Используется для выполнения последовательных шагов внутри каждой задачи.
+- **Parallel**: Может быть использован, если нужно одновременно мониторить состояние очереди и выполнять текущую задачу.
+
+#### 3. **Action Nodes**
+- **Добавление задачи в очередь**: Получает данные из веб-интерфейса и добавляет их в очередь.
+- **Проверка очереди**: Проверяет, есть ли задачи в очереди.
+- **Подготовка оборудования**: Настройка оборудования для гравировки.
+- **Гравировка**: Выполнение гравировки на основе данных задачи.
+- **Проверка качества**: Проверка результатов гравировки.
+
+#### 4. **Condition Nodes**
+- **Есть ли задачи в очереди?**: Возвращает `true`, если очередь не пуста.
+- **Задача выполнена успешно?**: Проверяет, завершилась ли гравировка без ошибок.
+
+#### 5. **Decorator Nodes**
+- **Repeat Until Fail**: Повторяет проверку очереди, пока она не станет пустой.
+- **Retry**: Повторяет выполнение гравировки в случае ошибки.
+
+---
+
+### **Пример Behavior Tree**
+
+```plaintext
+Root
+│
+├── Selector (Выбор между обработкой очереди и выполнением гравировки)
+│   ├── Sequence (Обработка очереди задач)
+│   │   ├── Condition: Есть ли задачи в очереди?
+│   │   └── Action: Добавить задачу в очередь
+│   │
+│   └── Sequence (Выполнение гравировки)
+│       ├── Action: Подготовка оборудования
+│       ├── Action: Гравировка
+│       ├── Condition: Задача выполнена успешно?
+│       └── Action: Проверка качества
+│
+└── Decorator: Repeat Until Fail (Повторять, пока очередь не пуста)
+```
+
+---
+
+### **Пошаговое объяснение работы дерева**
+
+1. **Selector**:
+   - Проверяет, есть ли задачи в очереди.
+   - Если очередь пуста, ничего не делает.
+   - Если очередь не пуста, переходит к выполнению гравировки.
+
+2. **Sequence (Обработка очереди задач)**:
+   - Убедиться, что задачи есть в очереди.
+   - Добавить новую задачу в очередь (если оператор ввел данные).
+
+3. **Sequence (Выполнение гравировки)**:
+   - Подготовить оборудование.
+   - Выполнить гравировку.
+   - Проверить качество выполнения.
+
+4. **Decorator: Repeat Until Fail**:
+   - Повторяет весь процесс, пока очередь задач не станет пустой.
+
+---
+
+### **Преимущества такого подхода**
+1. **Модульность**: Каждый узел отвечает за конкретную задачу, что упрощает отладку и расширение.
+2. **Гибкость**: Можно легко добавить новые условия или действия.
+3. **Надежность**: Дерево может обрабатывать ошибки и повторять действия при необходимости.
+
+---
+
+### **Пример кода (псевдокод)**
+
+```python
+class BehaviorTree:
+    def __init__(self):
+        self.queue = []
+
+    def check_queue(self):
+        return len(self.queue) > 0
+
+    def add_task_to_queue(self, task):
+        self.queue.append(task)
+
+    def prepare_equipment(self):
+        print("Equipment prepared.")
+
+    def engrave(self, task):
+        print(f"Engraving task: {task}")
+
+    def check_quality(self):
+        print("Quality check passed.")
+
+    def run(self):
+        while True:
+            if not self.check_queue():
+                print("Queue is empty. Waiting for tasks...")
+                break
+
+            task = self.queue.pop(0)
+            self.prepare_equipment()
+            self.engrave(task)
+            self.check_quality()
+
+# Пример использования
+bt = BehaviorTree()
+bt.add_task_to_queue("Task 1")
+bt.add_task_to_queue("Task 2")
+bt.run()
+```
+
+---
+
+### **Итог**
+Для реализации обработки очереди задач в вашем сценарии можно использовать следующие узлы:
+- **Control Nodes**: `Selector`, `Sequence`, `Parallel`.
+- **Action Nodes**: Добавление задачи, подготовка оборудования, гравировка, проверка качества.
+- **Condition Nodes**: Проверка наличия задач в очереди, проверка успешности выполнения.
+- **Decorator Nodes**: Повторение действий до определенного условия.
+
+Этот подход обеспечивает четкое разделение логики и позволяет легко масштабировать систему.

docs/objects.json

@@ -0,0 +1,38 @@
+[
+  {
+    "obj_id": 1,
+    "place_name": "bunker_1",
+    "place_aruco": 21,
+    "place_pos": "{pose}",
+    "graver_template": "{file}",
+    "dimensions": "{X,Y,Z}",
+    "skills": ["sid", "..."]
+  },
+  {
+    "obj_id": 2,
+    "place_name": "bunker_2",
+    "place_aruco": 22,
+    "place_pos": "{pose}",
+    "graver_template": "{file}",
+    "dimensions": "{X,Y,Z}",
+    "skills": ["sid", "..."]
+  },
+  {
+    "obj_id": 3,
+    "place_name": "bunker_3",
+    "place_aruco": 23,
+    "place_pos": "{pose}",
+    "graver_template": "{file}",
+    "dimensions": "{X,Y,Z}",
+    "skills": ["sid", "..."]
+  },
+  {
+    "obj_id": 4,
+    "place_name": "bunker_4",
+    "place_aruco": 24,
+    "place_pos": "{pose}",
+    "graver_template": "{file}",
+    "dimensions": "{X,Y,Z}",
+    "skills": ["sid", "..."]
+  }
+]

docs/shildik_templates/README.md

@@ -0,0 +1,16 @@
+# Шаблоны изображений шильдиков
+
+Директория содержит шаблоны шильдиков в двух вариантах:
+1. С надписями в виде редактируемого текста в директории `text`. Для корректного отображения должны быть установлены в системе или в редакторе шрифты.
+2. С надписями в виде кривых в директории `curves`.
+
+Открывать рекомендуется в свободном редакторе векторной графики [Inkscape](https://inkscape.org)
+
+## Типы шильдиков и наборы полей данных
+
+| Тип устройства | Перечень параметров |
+| - | - |
+| Насос общепромышленный | Наименование, Q - подача, n - частота оборотов, H - напор, m - масса, P2 - мощность, T - диапазон температур, Дата производства, Заводской номер |
+| Насос взрывозащищённый (Ex) | Наименование, Q - подача, n - частота оборотов, H - напор, m - масса, P2 - мощность, T - диапазон температур, Дата производства, Заводской номер, Диапазон ta в градусах цельсия |
+| Насосная установка| Наименование, Q - подача, H - напор, m - масса, P - мощность, Кол-во насосов, Дата производства, Заводской номер |
+| Шкаф управления   | Наименование, Iоб, Iсум, IP, Кол-во электродвигателей, Дата производства, Заводской номер |

docs/tasks.csv

@@ -0,0 +1,6 @@
+obj_id,device_type,device_name,supply,freq,napor,mass,power,temp_range,date,serial_num,ta_range,pump_num,i_ob,i_sum,ip,engine_num
+Object ID,Тип устройства,Наименование,"Q - подача, м³/ч",n - частота оборотов,"H - напор, м",m - масса,P2 - мощность,T - диапазон температур,Дата производства,Заводской номер,Диапазон ta в градусах цельсия,Кол-во насосов,Iоб,Iсум,IP,Кол-во электродвигателей
+1,Насос общепромышленный,"КММ-ХА 80-50-200б/2/18,5-Е-55Т/BBQV-HC-У3",50,1450 об/мин,30,200.0 кг,15.0 кВт,-10...+80°C,01-15-23,SN12345,,,,,,
+2,Насос взрывозащищённый (Ex),"КММ-ХА 80-50-200б/2/18,5-Е-55Т/BBQV-HC-У3",30 м³/ч,1450 об/мин,25,180.0 кг,10.0 кВт,-20...+60°C,02-10-23,SN12346,-40...+85°C,,,,,
+3,Насосная установка,,100 м³/ч,,50,500.0 кг,30.0 кВт,,03-01-23,SN12347,2,,55,4,
+4,Шкаф управления,,,,,,,,04-05-23,SN12348,,,30.0A,50.0A,IP54,3

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/CMakeLists.txt

@@ -7,53 +7,16 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(std_srvs REQUIRED)
-find_package(gz-cmake3 REQUIRED)
-find_package(gz-plugin2 REQUIRED COMPONENTS register)
-set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
-find_package(gz-common5 REQUIRED COMPONENTS profiler)
-set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
-
-# Harmonic
-if("$ENV{GZ_VERSION}" STREQUAL "harmonic")
-  find_package(gz-sim8 REQUIRED)
-  set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
-  message(STATUS "Compiling against Gazebo Harmonic")
-# Default to Garden
-else()
-  find_package(gz-sim7 REQUIRED)
-  set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})
-  message(STATUS "Compiling against Gazebo Garden")
-endif()
-
-add_library(VacuumGripper
-  SHARED
-  src/VacuumGripper.cpp
-)
 
 include_directories(include)
 
-target_link_libraries(VacuumGripper
+install(DIRECTORY world urdf meshes moveit srdf launch config assets bt/xmls bt/config
-  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
-)
-
-ament_target_dependencies(VacuumGripper
-  rclcpp
-  std_srvs
-)
-
-install(
-  TARGETS VacuumGripper
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-
-install(DIRECTORY world urdf meshes launch config assets bt/xmls bt/config
 DESTINATION share/${PROJECT_NAME})
 
+add_subdirectory(src)
 add_subdirectory(scripts)
 add_subdirectory(bt)
+add_subdirectory(utils)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

rbs_mill_assist/assets/aruco_1/meshes/aruco_template.mtl

@@ -0,0 +1,12 @@
+# Blender 4.2.1 LTS MTL File: 'aruco_template.blend'
+# www.blender.org
+
+newmtl aruco_tag_124
+Ns 0.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.000000 0.000000 0.000000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 1
+map_Kd aruco_marker_21.png

rbs_mill_assist/assets/aruco_1/meshes/aruco_template.obj

@@ -0,0 +1,48 @@
+# Blender 4.2.1 LTS
+# www.blender.org
+mtllib aruco_template.mtl
+o aruco_tag_124
+v -0.000464 0.000314 0.039912
+v 0.039536 0.000314 0.039912
+v 0.039536 0.000314 -0.000088
+v -0.000464 0.000314 -0.000088
+v -0.000464 0.001314 0.039912
+v 0.039536 0.001314 0.039912
+v 0.039536 0.001314 -0.000088
+v -0.000464 0.001314 -0.000088
+vn -0.0000 -1.0000 -0.0000
+vn -0.0000 1.0000 -0.0000
+vn 0.9999 0.0125 -0.0000
+vn -1.0000 -0.0000 -0.0000
+vn -0.9997 -0.0250 -0.0000
+vn -1.0000 -0.0080 -0.0000
+vn 1.0000 -0.0000 -0.0000
+vn 1.0000 0.0080 -0.0000
+vn 0.9997 0.0250 -0.0000
+vn -0.9999 -0.0125 -0.0000
+vn -0.0000 0.0125 -0.9999
+vn -0.0000 -0.0000 1.0000
+vn -0.0000 -0.0250 0.9997
+vn -0.0000 -0.0080 1.0000
+vn -0.0000 -0.0000 -1.0000
+vn -0.0000 0.0080 -1.0000
+vn -0.0000 0.0250 -0.9997
+vn -0.0000 -0.0125 0.9999
+vt 0.000000 0.000000
+vt 1.000000 0.000000
+vt 1.000000 1.000000
+vt 0.000000 1.000000
+s 0
+usemtl aruco_tag_124
+f 1/1/1 2/2/1 3/3/1
+f 1/1/1 3/3/1 4/4/1
+f 5/1/2 6/2/2 7/3/2
+f 5/1/2 7/3/2 8/4/2
+f 1/1/3 2/2/4 6/3/5
+f 1/1/6 6/3/4 5/4/4
+f 4/1/7 3/2/7 7/3/8
+f 4/1/9 7/3/10 8/4/7
+f 1/1/11 4/2/12 8/3/13
+f 1/1/14 8/3/12 5/4/12
+f 2/1/15 3/2/15 7/3/16
+f 2/1/17 7/3/18 6/4/15

rbs_mill_assist/assets/aruco_1/model.config

@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+<model>
+  <name>aruco_1</name>
+  <version>1.0</version>
+  <sdf version="1.7">model.sdf</sdf>
+  <description>
+    A flat ARUco tag with the ID 124, designed to be used in Gazebo Harmonic.
+    This model has a 1x1 m flat surface with a texture applied to it.
+  </description>
+  <author>
+    <name>Yoan Mollard</name>
+    <email>opensource@aubrune.eu</email>
+  </author>
+  <license>http://www.apache.org/licenses/LICENSE-2.0</license>
+  <category>Other</category>
+  <tags>ARUco, tag, marker, Gazebo</tags>
+</model>

rbs_mill_assist/assets/aruco_1/model.sdf

@@ -0,0 +1,16 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+    <model name="aruco_1">
+        <static>true</static>
+        <pose>0 0 0 0 0 0</pose>
+        <link name="link">
+            <visual name="visual">
+                <geometry>
+                    <mesh>
+                        <uri>model://aruco_1/meshes/aruco_template.obj</uri>
+                    </mesh>
+                </geometry>
+            </visual>
+        </link>
+    </model>
+</sdf>

rbs_mill_assist/assets/aruco_2/meshes/aruco_template.mtl

@@ -0,0 +1,13 @@
+# Blender 4.2.1 LTS MTL File: 'aruco_template.blend'
+# www.blender.org
+
+newmtl aruco_tag_124
+Ns 0.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.000000 0.000000 0.000000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 1
+map_Kd aruco_marker_22.png
+map_Ka aruco_marker_22.png

rbs_mill_assist/assets/aruco_2/meshes/aruco_template.obj

@@ -0,0 +1,48 @@
+# Blender 4.2.1 LTS
+# www.blender.org
+mtllib aruco_template.mtl
+o aruco_tag_124
+v -0.000464 0.000314 0.039912
+v 0.039536 0.000314 0.039912
+v 0.039536 0.000314 -0.000088
+v -0.000464 0.000314 -0.000088
+v -0.000464 0.001314 0.039912
+v 0.039536 0.001314 0.039912
+v 0.039536 0.001314 -0.000088
+v -0.000464 0.001314 -0.000088
+vn -0.0000 -1.0000 -0.0000
+vn -0.0000 1.0000 -0.0000
+vn 0.9999 0.0125 -0.0000
+vn -1.0000 -0.0000 -0.0000
+vn -0.9997 -0.0250 -0.0000
+vn -1.0000 -0.0080 -0.0000
+vn 1.0000 -0.0000 -0.0000
+vn 1.0000 0.0080 -0.0000
+vn 0.9997 0.0250 -0.0000
+vn -0.9999 -0.0125 -0.0000
+vn -0.0000 0.0125 -0.9999
+vn -0.0000 -0.0000 1.0000
+vn -0.0000 -0.0250 0.9997
+vn -0.0000 -0.0080 1.0000
+vn -0.0000 -0.0000 -1.0000
+vn -0.0000 0.0080 -1.0000
+vn -0.0000 0.0250 -0.9997
+vn -0.0000 -0.0125 0.9999
+vt 0.000000 0.000000
+vt 1.000000 0.000000
+vt 1.000000 1.000000
+vt 0.000000 1.000000
+s 0
+usemtl aruco_tag_124
+f 1/1/1 2/2/1 3/3/1
+f 1/1/1 3/3/1 4/4/1
+f 5/1/2 6/2/2 7/3/2
+f 5/1/2 7/3/2 8/4/2
+f 1/1/3 2/2/4 6/3/5
+f 1/1/6 6/3/4 5/4/4
+f 4/1/7 3/2/7 7/3/8
+f 4/1/9 7/3/10 8/4/7
+f 1/1/11 4/2/12 8/3/13
+f 1/1/14 8/3/12 5/4/12
+f 2/1/15 3/2/15 7/3/16
+f 2/1/17 7/3/18 6/4/15

rbs_mill_assist/assets/aruco_2/model.config

@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+<model>
+  <name>aruco_2</name>
+  <version>1.0</version>
+  <sdf version="1.7">model.sdf</sdf>
+  <description>
+    A flat ARUco tag with the ID 124, designed to be used in Gazebo Harmonic.
+    This model has a 1x1 m flat surface with a texture applied to it.
+  </description>
+  <author>
+    <name>Yoan Mollard</name>
+    <email>opensource@aubrune.eu</email>
+  </author>
+  <license>http://www.apache.org/licenses/LICENSE-2.0</license>
+  <category>Other</category>
+  <tags>ARUco, tag, marker, Gazebo</tags>
+</model>

rbs_mill_assist/assets/aruco_2/model.sdf

@@ -0,0 +1,16 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+    <model name="aruco_2">
+        <static>true</static>
+        <pose>0 0 0 0 0 0</pose>
+        <link name="link">
+            <visual name="visual">
+                <geometry>
+                    <mesh>
+                        <uri>model://aruco_2/meshes/aruco_template.obj</uri>
+                    </mesh>
+                </geometry>
+            </visual>
+        </link>
+    </model>
+</sdf>

rbs_mill_assist/assets/aruco_3/meshes/aruco_template.mtl

@@ -0,0 +1,12 @@
+# Blender 4.2.1 LTS MTL File: 'aruco_template.blend'
+# www.blender.org
+
+newmtl aruco_tag_124
+Ns 0.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.000000 0.000000 0.000000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 1
+map_Kd aruco_marker_23.png

rbs_mill_assist/assets/aruco_3/meshes/aruco_template.obj

@@ -0,0 +1,48 @@
+# Blender 4.2.1 LTS
+# www.blender.org
+mtllib aruco_template.mtl
+o aruco_tag_124
+v -0.000464 0.000314 0.039912
+v 0.039536 0.000314 0.039912
+v 0.039536 0.000314 -0.000088
+v -0.000464 0.000314 -0.000088
+v -0.000464 0.001314 0.039912
+v 0.039536 0.001314 0.039912
+v 0.039536 0.001314 -0.000088
+v -0.000464 0.001314 -0.000088
+vn -0.0000 -1.0000 -0.0000
+vn -0.0000 1.0000 -0.0000
+vn 0.9999 0.0125 -0.0000
+vn -1.0000 -0.0000 -0.0000
+vn -0.9997 -0.0250 -0.0000
+vn -1.0000 -0.0080 -0.0000
+vn 1.0000 -0.0000 -0.0000
+vn 1.0000 0.0080 -0.0000
+vn 0.9997 0.0250 -0.0000
+vn -0.9999 -0.0125 -0.0000
+vn -0.0000 0.0125 -0.9999
+vn -0.0000 -0.0000 1.0000
+vn -0.0000 -0.0250 0.9997
+vn -0.0000 -0.0080 1.0000
+vn -0.0000 -0.0000 -1.0000
+vn -0.0000 0.0080 -1.0000
+vn -0.0000 0.0250 -0.9997
+vn -0.0000 -0.0125 0.9999
+vt 0.000000 0.000000
+vt 1.000000 0.000000
+vt 1.000000 1.000000
+vt 0.000000 1.000000
+s 0
+usemtl aruco_tag_124
+f 1/1/1 2/2/1 3/3/1
+f 1/1/1 3/3/1 4/4/1
+f 5/1/2 6/2/2 7/3/2
+f 5/1/2 7/3/2 8/4/2
+f 1/1/3 2/2/4 6/3/5
+f 1/1/6 6/3/4 5/4/4
+f 4/1/7 3/2/7 7/3/8
+f 4/1/9 7/3/10 8/4/7
+f 1/1/11 4/2/12 8/3/13
+f 1/1/14 8/3/12 5/4/12
+f 2/1/15 3/2/15 7/3/16
+f 2/1/17 7/3/18 6/4/15

rbs_mill_assist/assets/aruco_3/model.config

@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+<model>
+  <name>aruco_3</name>
+  <version>1.0</version>
+  <sdf version="1.7">model.sdf</sdf>
+  <description>
+    A flat ARUco tag with the ID 124, designed to be used in Gazebo Harmonic.
+    This model has a 1x1 m flat surface with a texture applied to it.
+  </description>
+  <author>
+    <name>Yoan Mollard</name>
+    <email>opensource@aubrune.eu</email>
+  </author>
+  <license>http://www.apache.org/licenses/LICENSE-2.0</license>
+  <category>Other</category>
+  <tags>ARUco, tag, marker, Gazebo</tags>
+</model>

rbs_mill_assist/assets/aruco_3/model.sdf

@@ -0,0 +1,16 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+    <model name="aruco_3">
+        <static>true</static>
+        <pose>0 0 0 0 0 0</pose>
+        <link name="link">
+            <visual name="visual">
+                <geometry>
+                    <mesh>
+                        <uri>model://aruco_3/meshes/aruco_template.obj</uri>
+                    </mesh>
+                </geometry>
+            </visual>
+        </link>
+    </model>
+</sdf>

rbs_mill_assist/assets/aruco_4/meshes/aruco_template.mtl

@@ -0,0 +1,12 @@
+# Blender 4.2.1 LTS MTL File: 'aruco_template.blend'
+# www.blender.org
+
+newmtl aruco_tag_124
+Ns 0.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.000000 0.000000 0.000000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 1
+map_Kd aruco_marker_24.png

rbs_mill_assist/assets/aruco_4/meshes/aruco_template.obj

@@ -0,0 +1,48 @@
+# Blender 4.2.1 LTS
+# www.blender.org
+mtllib aruco_template.mtl
+o aruco_tag_124
+v -0.000464 0.000314 0.039912
+v 0.039536 0.000314 0.039912
+v 0.039536 0.000314 -0.000088
+v -0.000464 0.000314 -0.000088
+v -0.000464 0.001314 0.039912
+v 0.039536 0.001314 0.039912
+v 0.039536 0.001314 -0.000088
+v -0.000464 0.001314 -0.000088
+vn -0.0000 -1.0000 -0.0000
+vn -0.0000 1.0000 -0.0000
+vn 0.9999 0.0125 -0.0000
+vn -1.0000 -0.0000 -0.0000
+vn -0.9997 -0.0250 -0.0000
+vn -1.0000 -0.0080 -0.0000
+vn 1.0000 -0.0000 -0.0000
+vn 1.0000 0.0080 -0.0000
+vn 0.9997 0.0250 -0.0000
+vn -0.9999 -0.0125 -0.0000
+vn -0.0000 0.0125 -0.9999
+vn -0.0000 -0.0000 1.0000
+vn -0.0000 -0.0250 0.9997
+vn -0.0000 -0.0080 1.0000
+vn -0.0000 -0.0000 -1.0000
+vn -0.0000 0.0080 -1.0000
+vn -0.0000 0.0250 -0.9997
+vn -0.0000 -0.0125 0.9999
+vt 0.000000 0.000000
+vt 1.000000 0.000000
+vt 1.000000 1.000000
+vt 0.000000 1.000000
+s 0
+usemtl aruco_tag_124
+f 1/1/1 2/2/1 3/3/1
+f 1/1/1 3/3/1 4/4/1
+f 5/1/2 6/2/2 7/3/2
+f 5/1/2 7/3/2 8/4/2
+f 1/1/3 2/2/4 6/3/5
+f 1/1/6 6/3/4 5/4/4
+f 4/1/7 3/2/7 7/3/8
+f 4/1/9 7/3/10 8/4/7
+f 1/1/11 4/2/12 8/3/13
+f 1/1/14 8/3/12 5/4/12
+f 2/1/15 3/2/15 7/3/16
+f 2/1/17 7/3/18 6/4/15

rbs_mill_assist/assets/aruco_4/model.config

@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+<model>
+  <name>aruco_4</name>
+  <version>1.0</version>
+  <sdf version="1.7">model.sdf</sdf>
+  <description>
+    A flat ARUco tag with the ID 124, designed to be used in Gazebo Harmonic.
+    This model has a 1x1 m flat surface with a texture applied to it.
+  </description>
+  <author>
+    <name>Yoan Mollard</name>
+    <email>opensource@aubrune.eu</email>
+  </author>
+  <license>http://www.apache.org/licenses/LICENSE-2.0</license>
+  <category>Other</category>
+  <tags>ARUco, tag, marker, Gazebo</tags>
+</model>

rbs_mill_assist/assets/aruco_4/model.sdf

@@ -0,0 +1,16 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+    <model name="aruco_4">
+        <static>true</static>
+        <pose>0 0 0 0 0 0</pose>
+        <link name="link">
+            <visual name="visual">
+                <geometry>
+                    <mesh>
+                        <uri>model://aruco_4/meshes/aruco_template.obj</uri>
+                    </mesh>
+                </geometry>
+            </visual>
+        </link>
+    </model>
+</sdf>

rbs_mill_assist/assets/aruco_c/meshes/aruco_template.mtl

@@ -0,0 +1,12 @@
+# Blender 4.2.1 LTS MTL File: 'aruco_template.blend'
+# www.blender.org
+
+newmtl aruco_tag_124
+Ns 0.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.000000 0.000000 0.000000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 1
+map_Kd aruco_marker_25.png

rbs_mill_assist/assets/aruco_c/meshes/aruco_template.obj

@@ -0,0 +1,48 @@
+# Blender 4.2.1 LTS
+# www.blender.org
+mtllib aruco_template.mtl
+o aruco_tag_124
+v -0.000464 0.000314 0.039912
+v 0.039536 0.000314 0.039912
+v 0.039536 0.000314 -0.000088
+v -0.000464 0.000314 -0.000088
+v -0.000464 0.001314 0.039912
+v 0.039536 0.001314 0.039912
+v 0.039536 0.001314 -0.000088
+v -0.000464 0.001314 -0.000088
+vn -0.0000 -1.0000 -0.0000
+vn -0.0000 1.0000 -0.0000
+vn 0.9999 0.0125 -0.0000
+vn -1.0000 -0.0000 -0.0000
+vn -0.9997 -0.0250 -0.0000
+vn -1.0000 -0.0080 -0.0000
+vn 1.0000 -0.0000 -0.0000
+vn 1.0000 0.0080 -0.0000
+vn 0.9997 0.0250 -0.0000
+vn -0.9999 -0.0125 -0.0000
+vn -0.0000 0.0125 -0.9999
+vn -0.0000 -0.0000 1.0000
+vn -0.0000 -0.0250 0.9997
+vn -0.0000 -0.0080 1.0000
+vn -0.0000 -0.0000 -1.0000
+vn -0.0000 0.0080 -1.0000
+vn -0.0000 0.0250 -0.9997
+vn -0.0000 -0.0125 0.9999
+vt 0.000000 0.000000
+vt 1.000000 0.000000
+vt 1.000000 1.000000
+vt 0.000000 1.000000
+s 0
+usemtl aruco_tag_124
+f 1/1/1 2/2/1 3/3/1
+f 1/1/1 3/3/1 4/4/1
+f 5/1/2 6/2/2 7/3/2
+f 5/1/2 7/3/2 8/4/2
+f 1/1/3 2/2/4 6/3/5
+f 1/1/6 6/3/4 5/4/4
+f 4/1/7 3/2/7 7/3/8
+f 4/1/9 7/3/10 8/4/7
+f 1/1/11 4/2/12 8/3/13
+f 1/1/14 8/3/12 5/4/12
+f 2/1/15 3/2/15 7/3/16
+f 2/1/17 7/3/18 6/4/15

rbs_mill_assist/assets/aruco_c/model.config

@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+<model>
+  <name>aruco_c</name>
+  <version>1.0</version>
+  <sdf version="1.7">model.sdf</sdf>
+  <description>
+    A flat ARUco tag with the ID 124, designed to be used in Gazebo Harmonic.
+    This model has a 1x1 m flat surface with a texture applied to it.
+  </description>
+  <author>
+    <name>Yoan Mollard</name>
+    <email>opensource@aubrune.eu</email>
+  </author>
+  <license>http://www.apache.org/licenses/LICENSE-2.0</license>
+  <category>Other</category>
+  <tags>ARUco, tag, marker, Gazebo</tags>
+</model>

rbs_mill_assist/assets/aruco_c/model.sdf

@@ -0,0 +1,16 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+    <model name="aruco_c">
+        <static>true</static>
+        <pose>0 0 0 0 0 0</pose>
+        <link name="link">
+            <visual name="visual">
+                <geometry>
+                    <mesh>
+                        <uri>model://aruco_c/meshes/aruco_template.obj</uri>
+                    </mesh>
+                </geometry>
+            </visual>
+        </link>
+    </model>
+</sdf>

rbs_mill_assist/assets/bunker/model.sdf

@@ -6,43 +6,48 @@
       <visual name="bunker_visual">
         <geometry>
           <mesh>
-            <uri>model://bunker/meshes/bunker.stl</uri>
+            <uri>model://bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL</uri>
           </mesh>
         </geometry>
-        <!-- <material> -->
+        <material>
-        <!--   <diffuse>1 1 1 1</diffuse> -->
+          <diffuse>1 1 1 1</diffuse>
-        <!--   <ambient>1 1 1 1</ambient> -->
+          <ambient>1 1 1 1</ambient>
-        <!--   <specular>0.5 0.5 0.5 1</specular> -->
+          <specular>0.5 0.5 0.5 1</specular>
-        <!--   <emissive>0 0 0 1</emissive> -->
+          <emissive>0 0 0 1</emissive>
-        <!--   <texture> -->
+
-        <!--     <diffuse_map>model://bunker/textures/shildik_sh.png</diffuse_map> -->
+          <!-- <texture> -->
-        <!--   </texture> -->
+          <!--   <diffuse_map>model://shildik/textures/shildik_sh.png</diffuse_map> -->
-        <!--   <pbr> -->
+          <!-- </texture> -->
-        <!--     <metal> -->
+        </material>
-        <!--       <albedo_map>model://bunker/textures/shildik_sh_d.png</albedo_map> -->
-        <!--       <normal_map>model://bunker/textures/shildik_sh_n.png</normal_map> -->
-        <!--       <roughness_map>model://bunker/textures/shildik_sh_r.png</roughness_map> -->
-        <!--       <metalness_map>model://bunker/textures/shildik_sh_m.png</metalness_map> -->
-        <!--       <ambient_occlusion_map>model://bunker/textures/shildik_sh_o.png</ambient_occlusion_map> -->
-        <!--     </metal> -->
-        <!--   </pbr> -->
-        <!-- </material> -->
       </visual>
-      <collision name="laser_collision">
+      <collision name="bunker_collision">
+        <density>7850</density>
         <geometry>
           <mesh>
-            <uri>model://bunker/meshes/bunker.stl</uri>
+            <uri>model://bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL</uri>
           </mesh>
         </geometry>
-        <surface>
+        <!-- <surface> -->
-          <contact>
+        <!--   <contact> -->
-            <ode />
+        <!--     <ode> -->
-          </contact>
+        <!--       <kp>1e6</kp> -->
-          <bounce />
+        <!--       <kd>1e3</kd> -->
-          <friction>
+        <!--       <max_vel>0.1</max_vel> -->
-            <ode />
+        <!--       <min_depth>0.001</min_depth> -->
-          </friction>
+        <!--     </ode> -->
-        </surface>
+        <!--   </contact> -->
+        <!--   <bounce> -->
+        <!--     <restitution>0.3</restitution> -->
+        <!--   </bounce> -->
+        <!--   <friction> -->
+        <!--     <ode> -->
+        <!--       <mu>0.61</mu> -->
+        <!--       <mu2>0.47</mu2> -->
+        <!--       <slip1>0.0</slip1> -->
+        <!--       <slip2>0.0</slip2> -->
+        <!--     </ode> -->
+        <!--   </friction> -->
+        <!-- </surface> -->
       </collision>
     </link>
   </model>

rbs_mill_assist/assets/bunker_4_slots/model.config

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>bunker_4_slots</name>
+  <version>0.1</version>
+  <sdf version='1.10'>model.sdf</sdf>
+
+  <author>
+    <name>Bill Finger</name>
+    <email>ur.narmak@gmail.com</email>
+  </author>
+
+  <description>
+    Empty
+  </description>
+</model>

rbs_mill_assist/assets/bunker_4_slots/model.sdf

@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<sdf version="1.10">
+  <model name="bunker_4_slots">
+    <link name="bunker_4_slots_link">
+      <inertial auto="true" />
+      <visual name="bunker_4_slots_visual">
+        <geometry>
+          <mesh>
+            <uri>model://bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>1 1 1 1</diffuse>
+          <ambient>1 1 1 1</ambient>
+          <specular>0.5 0.5 0.5 1</specular>
+          <emissive>0 0 0 1</emissive>
+
+          <!-- <texture> -->
+          <!--   <diffuse_map>model://shildik/textures/shildik_sh.png</diffuse_map> -->
+          <!-- </texture> -->
+        </material>
+      </visual>
+      <collision name="bunker_4_slots_collision">
+        <density>7850</density>
+        <geometry>
+          <mesh>
+            <uri>model://bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL</uri>
+          </mesh>
+        </geometry>
+        <!-- <surface> -->
+        <!--   <contact> -->
+        <!--     <ode> -->
+        <!--       <kp>1e6</kp> -->
+        <!--       <kd>1e3</kd> -->
+        <!--       <max_vel>0.1</max_vel> -->
+        <!--       <min_depth>0.001</min_depth> -->
+        <!--     </ode> -->
+        <!--   </contact> -->
+        <!--   <bounce> -->
+        <!--     <restitution>0.3</restitution> -->
+        <!--   </bounce> -->
+        <!--   <friction> -->
+        <!--     <ode> -->
+        <!--       <mu>0.61</mu> -->
+        <!--       <mu2>0.47</mu2> -->
+        <!--       <slip1>0.0</slip1> -->
+        <!--       <slip2>0.0</slip2> -->
+        <!--     </ode> -->
+        <!--   </friction> -->
+        <!-- </surface> -->
+      </collision>
+    </link>
+  </model>
+</sdf>

rbs_mill_assist/assets/conductor/model.config

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>conductor</name>
+  <version>0.1</version>
+  <sdf version='1.10'>model.sdf</sdf>
+
+  <author>
+    <name>Bill Finger</name>
+    <email>ur.narmak@gmail.com</email>
+  </author>
+
+  <description>
+    Empty
+  </description>
+</model>

rbs_mill_assist/assets/conductor/model.sdf

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<sdf version="1.10">
+  <model name="conductor">
+    <link name="conductor_link">
+      <inertial auto="true" />
+      <visual name="conductor_visual">
+        <geometry>
+          <mesh>
+            <uri>model://conductor/meshes/conductor.stl</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.8 0.8 0.8 1</diffuse>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <specular>0.2 0.2 0.2 1</specular>
+          <emissive>0 0 0 1</emissive>
+          <!-- <texture> -->
+          <!--   <diffuse_map>model://conductor/textures/Metal055A_2K-PNG_Color.png</diffuse_map> -->
+          <!-- </texture> -->
+          <!-- <pbr> -->
+          <!--   <metal> -->
+          <!--     <albedo_map>model://conductor/textures/Metal055A_2K-PNG_Color.png</albedo_map> -->
+          <!--     <normal_map>model://conductor/textures/Metal055A_2K-PNG_NormalDX.png</normal_map> -->
+          <!--     <roughness_map>model://conductor/textures/Metal055A_2K-PNG_Roughness.png</roughness_map> -->
+          <!--     <metalness_map>model://conductor/textures/Metal055A_2K-PNG_Metalness.png</metalness_map> -->
+          <!--   </metal> -->
+          <!-- </pbr> -->
+        </material>
+      </visual>
+      <collision name="conductor_collision">
+        <density>1000</density>
+        <geometry>
+          <mesh>
+            <uri>model://conductor/meshes/conductor.stl</uri>
+          </mesh>
+        </geometry>
+        <!-- <surface> -->
+        <!--   <contact> -->
+        <!--     <ode> -->
+        <!--       <kp>1e6</kp> -->
+        <!--       <kd>1e3</kd> -->
+        <!--       <max_vel>0.1</max_vel> -->
+        <!--       <min_depth>0.001</min_depth> -->
+        <!--     </ode> -->
+        <!--   </contact> -->
+        <!--   <bounce> -->
+        <!--     <restitution>0.3</restitution> -->
+        <!--   </bounce> -->
+        <!--   <friction> -->
+        <!--     <ode> -->
+        <!--       <mu>0.61</mu> -->
+        <!--       <mu2>0.47</mu2> -->
+        <!--       <slip1>0.0</slip1> -->
+        <!--       <slip2>0.0</slip2> -->
+        <!--     </ode> -->
+        <!--   </friction> -->
+        <!-- </surface> -->
+      </collision>
+    </link>
+  </model>
+</sdf>

rbs_mill_assist/assets/laser/laser.material

@@ -0,0 +1,11 @@
+material LaserMaterial
+{
+    technique
+    {
+        pass
+        {
+            ambient 0.5 0.5 0.5 1.0
+            diffuse 0.5 0.5 0.5 1.0
+        }
+    }
+}

rbs_mill_assist/assets/laser/meshes/laser.dae

@@ -0,0 +1,207 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 4.3.2 commit date:2024-12-16, commit time:21:10, hash:32f5fdce0a0a</authoring_tool>
+    </contributor>
+    <created>2025-03-05T17:38:05</created>
+    <modified>2025-03-05T17:38:05</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_cameras>
+    <camera id="Camera_001-camera" name="Camera.001">
+      <optics>
+        <technique_common>
+          <perspective>
+            <xfov sid="xfov">39.59775</xfov>
+            <aspect_ratio>1.777778</aspect_ratio>
+            <znear sid="znear">0.1</znear>
+            <zfar sid="zfar">100</zfar>
+          </perspective>
+        </technique_common>
+      </optics>
+      <extra>
+        <technique profile="blender">
+          <shiftx sid="shiftx" type="float">0</shiftx>
+          <shifty sid="shifty" type="float">0</shifty>
+          <dof_distance sid="dof_distance" type="float">10</dof_distance>
+        </technique>
+      </extra>
+    </camera>
+    <camera id="Camera-camera" name="Camera">
+      <optics>
+        <technique_common>
+          <perspective>
+            <xfov sid="xfov">39.59775</xfov>
+            <aspect_ratio>1.777778</aspect_ratio>
+            <znear sid="znear">0.1</znear>
+            <zfar sid="zfar">100</zfar>
+          </perspective>
+        </technique_common>
+      </optics>
+      <extra>
+        <technique profile="blender">
+          <shiftx sid="shiftx" type="float">0</shiftx>
+          <shifty sid="shifty" type="float">0</shifty>
+          <dof_distance sid="dof_distance" type="float">10</dof_distance>
+        </technique>
+      </extra>
+    </camera>
+  </library_cameras>
+  <library_lights>
+    <light id="Light_001-light" name="Light.001">
+      <technique_common>
+        <point>
+          <color sid="color">1000 1000 1000</color>
+          <constant_attenuation>1</constant_attenuation>
+          <linear_attenuation>0</linear_attenuation>
+          <quadratic_attenuation>0</quadratic_attenuation>
+        </point>
+      </technique_common>
+      <extra>
+        <technique profile="blender">
+          <type sid="type" type="int">0</type>
+          <flag sid="flag" type="int">0</flag>
+          <mode sid="mode" type="int">2097153</mode>
+          <red sid="red" type="float">1</red>
+          <green sid="green" type="float">1</green>
+          <blue sid="blue" type="float">1</blue>
+          <energy sid="blender_energy" type="float">1000</energy>
+          <spotsize sid="spotsize" type="float">75</spotsize>
+          <spotblend sid="spotblend" type="float">0.15</spotblend>
+          <clipsta sid="clipsta" type="float">0.04999995</clipsta>
+          <clipend sid="clipend" type="float">40</clipend>
+          <radius sid="radius" type="float">0.1</radius>
+          <area_shape sid="area_shape" type="int">1</area_shape>
+          <area_size sid="area_size" type="float">0.1</area_size>
+          <area_sizey sid="area_sizey" type="float">0.1</area_sizey>
+          <area_sizez sid="area_sizez" type="float">1</area_sizez>
+        </technique>
+      </extra>
+    </light>
+    <light id="Light-light" name="Light">
+      <technique_common>
+        <point>
+          <color sid="color">1000 1000 1000</color>
+          <constant_attenuation>1</constant_attenuation>
+          <linear_attenuation>0</linear_attenuation>
+          <quadratic_attenuation>0</quadratic_attenuation>
+        </point>
+      </technique_common>
+      <extra>
+        <technique profile="blender">
+          <type sid="type" type="int">0</type>
+          <flag sid="flag" type="int">0</flag>
+          <mode sid="mode" type="int">2097153</mode>
+          <red sid="red" type="float">1</red>
+          <green sid="green" type="float">1</green>
+          <blue sid="blue" type="float">1</blue>
+          <energy sid="blender_energy" type="float">1000</energy>
+          <spotsize sid="spotsize" type="float">75</spotsize>
+          <spotblend sid="spotblend" type="float">0.15</spotblend>
+          <clipsta sid="clipsta" type="float">0.04999995</clipsta>
+          <clipend sid="clipend" type="float">40</clipend>
+          <radius sid="radius" type="float">0.1</radius>
+          <area_shape sid="area_shape" type="int">1</area_shape>
+          <area_size sid="area_size" type="float">0.1</area_size>
+          <area_sizey sid="area_sizey" type="float">0.1</area_sizey>
+          <area_sizez sid="area_sizez" type="float">1</area_sizez>
+        </technique>
+      </extra>
+    </light>
+  </library_lights>
+  <library_effects>
+    <effect id="Material_001-effect">
+      <profile_COMMON>
+        <technique sid="common">
+          <lambert>
+            <emission>
+              <color sid="emission">0 0 0 1</color>
+            </emission>
+            <diffuse>
+              <color sid="diffuse">0.1584888 0.3396751 0.8005451 1</color>
+            </diffuse>
+            <index_of_refraction>
+              <float sid="ior">1.5</float>
+            </index_of_refraction>
+          </lambert>
+        </technique>
+      </profile_COMMON>
+    </effect>
+  </library_effects>
+  <library_images/>
+  <library_materials>
+    <material id="Material_001-material" name="Material.001">
+      <instance_effect url="#Material_001-effect"/>
+    </material>
+  </library_materials>
+  <library_geometries>
+    <geometry id="laser-mesh" name="laser">
+      <mesh>
+        <source id="laser-mesh-positions">
+          <float_array id="laser-mesh-positions-array" count="96">-0.09499996 0.004999995 0.03999996 -0.09499996 0.004999995 -0.03999996 -0.09499996 0.305 0.03999996 -0.09499996 0.305 -0.03999996 -0.175 0.004999995 -0.03999996 -0.175 0.305 -0.03999996 -0.175 0.004999995 0.03999996 -0.175 0.305 0.03999996 -0.225 0.004999995 -0.155 -0.225 0.004999995 0.155 0.225 0.004999995 0.155 0.225 0.004999995 -0.155 0.225 0 -0.155 0.225 0 0.155 -0.225 0 -0.155 -0.225 0 0.155 -0.225 0.455 -0.07249999 -0.175 0.455 -0.03999996 -0.09499996 0.455 -0.03999996 -0.175 0.455 0.03999996 -0.225 0.455 0.07249999 -0.09499996 0.455 0.03999996 0.175 0.455 0.07249999 0.175 0.455 -0.07249999 0.175 0.305 0.07249999 -0.225 0.305 0.07249999 0.175 0.305 -0.07249999 -0.225 0.305 -0.07249999 -0.175 0.605 -0.03999996 -0.175 0.605 0.03999996 -0.09499996 0.605 -0.03999996 -0.09499996 0.605 0.03999996</float_array>
+          <technique_common>
+            <accessor source="#laser-mesh-positions-array" count="32" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="laser-mesh-normals">
+          <float_array id="laser-mesh-normals-array" count="42">1 0 0 0 0 -1 -1 0 0 0 0 1 0 1 0 0 1 0 0 -1 0 0 1 2.8656e-6 0 1 -2.29248e-6 0 1 2.29248e-6 0 -1 1.14624e-6 0 -1 -5.73121e-7 0 -1 -2.8656e-6 0 -1 2.8656e-6</float_array>
+          <technique_common>
+            <accessor source="#laser-mesh-normals-array" count="14" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="laser-mesh-vertices">
+          <input semantic="POSITION" source="#laser-mesh-positions"/>
+        </vertices>
+        <triangles material="Material_001-material" count="60">
+          <input semantic="VERTEX" source="#laser-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#laser-mesh-normals" offset="1"/>
+          <p>0 0 1 0 2 0 2 0 1 0 3 0 1 1 4 1 3 1 3 1 4 1 5 1 4 2 6 2 5 2 5 2 6 2 7 2 6 3 0 3 7 3 7 3 0 3 2 3 4 4 8 4 6 4 6 4 8 4 9 4 6 4 9 4 0 4 0 5 9 5 10 5 0 4 10 4 1 4 1 4 10 4 11 4 1 4 11 4 4 4 4 4 11 4 8 4 11 0 10 0 12 0 12 0 10 0 13 0 8 1 11 1 14 1 14 1 11 1 12 1 9 2 8 2 15 2 15 2 8 2 14 2 10 3 9 3 13 3 13 3 9 3 15 3 15 6 14 6 13 6 13 6 14 6 12 6 16 7 17 7 18 7 17 4 16 4 19 4 19 4 16 4 20 4 19 4 20 4 21 4 21 8 20 8 22 8 21 4 22 4 18 4 18 4 22 4 23 4 18 9 23 9 16 9 2 10 24 10 25 10 24 6 2 6 26 6 26 6 2 6 3 6 26 11 3 11 27 11 27 12 3 12 5 12 27 6 5 6 25 6 25 6 5 6 7 6 25 13 7 13 2 13 23 0 22 0 26 0 26 0 22 0 24 0 16 1 23 1 27 1 27 1 23 1 26 1 20 2 16 2 25 2 25 2 16 2 27 2 22 3 20 3 24 3 24 3 20 3 25 3 28 4 29 4 30 4 30 4 29 4 31 4 21 3 31 3 19 3 19 3 31 3 29 3 19 2 29 2 17 2 17 2 29 2 28 2 17 1 28 1 18 1 18 1 28 1 30 1 18 0 30 0 21 0 21 0 30 0 31 0</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="Light_001" name="Light.001" type="NODE">
+        <matrix sid="transform">-0.2908646 -0.7711008 0.5663933 4.076245 0.9551712 -0.1998833 0.2183913 1.005454 -0.05518912 0.6045247 0.7946723 5.903862 0 0 0 1</matrix>
+        <instance_light url="#Light_001-light"/>
+      </node>
+      <node id="Camera_001" name="Camera.001" type="NODE">
+        <matrix sid="transform">0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054209 -0.6141704 -6.925791 -4.01133e-9 0.8953956 0.4452714 4.958309 0 0 0 1</matrix>
+        <instance_camera url="#Camera_001-camera"/>
+      </node>
+      <node id="laser" name="laser" type="NODE">
+        <matrix sid="transform">1 0 0 0 0 7.54979e-8 -1 0 0 1 7.54979e-8 0 0 0 0 1</matrix>
+        <instance_geometry url="#laser-mesh" name="laser">
+          <bind_material>
+            <technique_common>
+              <instance_material symbol="Material_001-material" target="#Material_001-material"/>
+            </technique_common>
+          </bind_material>
+        </instance_geometry>
+      </node>
+      <node id="Camera" name="Camera" type="NODE">
+        <matrix sid="transform">0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1</matrix>
+        <instance_camera url="#Camera-camera"/>
+      </node>
+      <node id="Light" name="Light" type="NODE">
+        <matrix sid="transform">-0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1</matrix>
+        <instance_light url="#Light-light"/>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>

rbs_mill_assist/assets/laser/meshes/laser.mtl

@@ -0,0 +1,2 @@
+# Blender 4.3.2 MTL File: 'None'
+# www.blender.org

rbs_mill_assist/assets/laser/meshes/laser.obj

@@ -0,0 +1,103 @@
+# Blender 4.3.2
+# www.blender.org
+mtllib laser.mtl
+o laser
+v -0.095000 0.005000 0.040000
+v -0.095000 0.005000 -0.040000
+v -0.095000 0.305000 0.040000
+v -0.095000 0.305000 -0.040000
+v -0.175000 0.005000 -0.040000
+v -0.175000 0.305000 -0.040000
+v -0.175000 0.005000 0.040000
+v -0.175000 0.305000 0.040000
+v -0.225000 0.005000 -0.155000
+v -0.225000 0.005000 0.155000
+v 0.225000 0.005000 0.155000
+v 0.225000 0.005000 -0.155000
+v 0.225000 0.000000 -0.155000
+v 0.225000 -0.000000 0.155000
+v -0.225000 0.000000 -0.155000
+v -0.225000 -0.000000 0.155000
+v -0.225000 0.455000 -0.072500
+v -0.175000 0.455000 -0.040000
+v -0.095000 0.455000 -0.040000
+v -0.175000 0.455000 0.040000
+v -0.225000 0.455000 0.072500
+v -0.095000 0.455000 0.040000
+v 0.175000 0.455000 0.072500
+v 0.175000 0.455000 -0.072500
+v 0.175000 0.305000 0.072500
+v -0.225000 0.305000 0.072500
+v 0.175000 0.305000 -0.072500
+v -0.225000 0.305000 -0.072500
+v -0.175000 0.605000 -0.040000
+v -0.175000 0.605000 0.040000
+v -0.095000 0.605000 -0.040000
+v -0.095000 0.605000 0.040000
+vn 1.0000 -0.0000 -0.0000
+vn -0.0000 -0.0000 -1.0000
+vn -1.0000 -0.0000 -0.0000
+vn -0.0000 -0.0000 1.0000
+vn -0.0000 1.0000 -0.0000
+vn -0.0000 -1.0000 -0.0000
+s 0
+f 1//1 2//1 3//1
+f 3//1 2//1 4//1
+f 2//2 5//2 4//2
+f 4//2 5//2 6//2
+f 5//3 7//3 6//3
+f 6//3 7//3 8//3
+f 7//4 1//4 8//4
+f 8//4 1//4 3//4
+f 5//5 9//5 7//5
+f 7//5 9//5 10//5
+f 7//5 10//5 1//5
+f 1//5 10//5 11//5
+f 1//5 11//5 2//5
+f 2//5 11//5 12//5
+f 2//5 12//5 5//5
+f 5//5 12//5 9//5
+f 12//1 11//1 13//1
+f 13//1 11//1 14//1
+f 9//2 12//2 15//2
+f 15//2 12//2 13//2
+f 10//3 9//3 16//3
+f 16//3 9//3 15//3
+f 11//4 10//4 14//4
+f 14//4 10//4 16//4
+f 16//6 15//6 14//6
+f 14//6 15//6 13//6
+f 17//5 18//5 19//5
+f 18//5 17//5 20//5
+f 20//5 17//5 21//5
+f 20//5 21//5 22//5
+f 22//5 21//5 23//5
+f 22//5 23//5 19//5
+f 19//5 23//5 24//5
+f 19//5 24//5 17//5
+f 3//6 25//6 26//6
+f 25//6 3//6 27//6
+f 27//6 3//6 4//6
+f 27//6 4//6 28//6
+f 28//6 4//6 6//6
+f 28//6 6//6 26//6
+f 26//6 6//6 8//6
+f 26//6 8//6 3//6
+f 24//1 23//1 27//1
+f 27//1 23//1 25//1
+f 17//2 24//2 28//2
+f 28//2 24//2 27//2
+f 21//3 17//3 26//3
+f 26//3 17//3 28//3
+f 23//4 21//4 25//4
+f 25//4 21//4 26//4
+f 29//5 30//5 31//5
+f 31//5 30//5 32//5
+f 22//4 32//4 20//4
+f 20//4 32//4 30//4
+f 20//3 30//3 18//3
+f 18//3 30//3 29//3
+f 18//2 29//2 19//2
+f 19//2 29//2 31//2
+f 19//1 31//1 22//1
+f 22//1 31//1 32//1

rbs_mill_assist/assets/laser/model.sdf

@@ -7,32 +7,33 @@
       <visual name="laser_visual">
         <geometry>
           <mesh>
-            <uri>model://laser/meshes/laser.stl</uri>
+            <uri>model://laser/meshes/laser.obj</uri>
           </mesh>
         </geometry>
-        <!-- <material> -->
+        <material>
-        <!--   <diffuse>1 1 1 1</diffuse> -->
+          <diffuse>1 1 1 1</diffuse>
-        <!--   <ambient>1 1 1 1</ambient> -->
+          <ambient>1 1 1 1</ambient>
-        <!--   <specular>0.5 0.5 0.5 1</specular> -->
+          <specular>0.5 0.5 0.5 1</specular>
-        <!--   <emissive>0 0 0 1</emissive> -->
+          <emissive>0 0 0 1</emissive>
-        <!--   <texture> -->
+          <!-- <texture> -->
-        <!--     <diffuse_map>model://laser/textures/shildik_sh.png</diffuse_map> -->
+          <!--   <diffuse_map>model://laser/textures/shildik_sh.png</diffuse_map> -->
-        <!--   </texture> -->
+          <!-- </texture> -->
-        <!--   <pbr> -->
+          <!-- <pbr> -->
-        <!--     <metal> -->
+          <!--   <metal> -->
-        <!--       <albedo_map>model://laser/textures/shildik_sh_d.png</albedo_map> -->
+          <!--     <albedo_map>model://laser/textures/shildik_sh_d.png</albedo_map> -->
-        <!--       <normal_map>model://laser/textures/shildik_sh_n.png</normal_map> -->
+          <!--     <normal_map>model://laser/textures/shildik_sh_n.png</normal_map> -->
-        <!--       <roughness_map>model://laser/textures/shildik_sh_r.png</roughness_map> -->
+          <!--     <roughness_map>model://laser/textures/shildik_sh_r.png</roughness_map> -->
-        <!--       <metalness_map>model://laser/textures/shildik_sh_m.png</metalness_map> -->
+          <!--     <metalness_map>model://laser/textures/shildik_sh_m.png</metalness_map> -->
-        <!--       <ambient_occlusion_map>model://laser/textures/shildik_sh_o.png</ambient_occlusion_map> -->
+          <!--     <ambient_occlusion_map>model://laser/textures/shildik_sh_o.png</ambient_occlusion_map> -->
-        <!--     </metal> -->
+          <!--   </metal> -->
-        <!--   </pbr> -->
+          <!-- </pbr> -->
-        <!-- </material> -->
+        </material>
       </visual>
       <collision name="laser_collision">
+        <density>1000</density>
         <geometry>
           <mesh>
-            <uri>model://laser/meshes/laser.stl</uri>
+            <uri>model://laser/meshes/laser.obj</uri>
           </mesh>
         </geometry>
         <surface>

rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae

@@ -3,13 +3,66 @@
   <asset>
     <contributor>
       <author>Blender User</author>
-      <authoring_tool>Blender 4.1.1 commit date:2024-04-15, commit time:15:11, hash:e1743a0317bc</authoring_tool>
+      <authoring_tool>Blender 4.3.2 commit date:2024-12-16, commit time:21:10, hash:32f5fdce0a0a</authoring_tool>
     </contributor>
-    <created>2025-02-12T10:55:51</created>
+    <created>2025-03-03T12:34:37</created>
-    <modified>2025-02-12T10:55:51</modified>
+    <modified>2025-03-03T12:34:37</modified>
     <unit name="meter" meter="1"/>
     <up_axis>Z_UP</up_axis>
   </asset>
+  <library_cameras>
+    <camera id="Camera-camera" name="Camera">
+      <optics>
+        <technique_common>
+          <perspective>
+            <xfov sid="xfov">39.59775</xfov>
+            <aspect_ratio>1.777778</aspect_ratio>
+            <znear sid="znear">0.1</znear>
+            <zfar sid="zfar">100</zfar>
+          </perspective>
+        </technique_common>
+      </optics>
+      <extra>
+        <technique profile="blender">
+          <shiftx sid="shiftx" type="float">0</shiftx>
+          <shifty sid="shifty" type="float">0</shifty>
+          <dof_distance sid="dof_distance" type="float">10</dof_distance>
+        </technique>
+      </extra>
+    </camera>
+  </library_cameras>
+  <library_lights>
+    <light id="Light-light" name="Light">
+      <technique_common>
+        <point>
+          <color sid="color">1000 1000 1000</color>
+          <constant_attenuation>1</constant_attenuation>
+          <linear_attenuation>0</linear_attenuation>
+          <quadratic_attenuation>0</quadratic_attenuation>
+        </point>
+      </technique_common>
+      <extra>
+        <technique profile="blender">
+          <type sid="type" type="int">0</type>
+          <flag sid="flag" type="int">0</flag>
+          <mode sid="mode" type="int">2097153</mode>
+          <red sid="red" type="float">1</red>
+          <green sid="green" type="float">1</green>
+          <blue sid="blue" type="float">1</blue>
+          <energy sid="blender_energy" type="float">1000</energy>
+          <spotsize sid="spotsize" type="float">75</spotsize>
+          <spotblend sid="spotblend" type="float">0.15</spotblend>
+          <clipsta sid="clipsta" type="float">0.04999995</clipsta>
+          <clipend sid="clipend" type="float">40</clipend>
+          <radius sid="radius" type="float">0.1</radius>
+          <area_shape sid="area_shape" type="int">1</area_shape>
+          <area_size sid="area_size" type="float">0.1</area_size>
+          <area_sizey sid="area_sizey" type="float">0.1</area_sizey>
+          <area_sizez sid="area_sizez" type="float">1</area_sizez>
+        </technique>
+      </extra>
+    </light>
+  </library_lights>
   <library_effects>
     <effect id="nasosnaya_ustanovka_svg-effect">
       <profile_COMMON>
@@ -39,7 +92,7 @@
     <geometry id="shildik-mesh" name="shildik">
       <mesh>
         <source id="shildik-mesh-positions">
-          <float_array id="shildik-mesh-positions-array" count="54">-0.02999997 -0.01999998 0 -0.02999997 -0.01999998 5e-4 -0.02999997 0.01999998 0 -0.02999997 0.01999998 5e-4 0.02999997 -0.01999998 0 0.02999997 -0.01999998 5e-4 0.02999997 0.01999998 0 0.02999997 0.01999998 5e-4 -0.02999997 0 0 -0.02999997 0 5e-4 0 0.01999998 0 0 0.01999998 5e-4 0.02999997 0 0 0.02999997 0 5e-4 0 -0.01999998 0 0 -0.01999998 5e-4 0 0 5e-4 0 0 0</float_array>
+          <float_array id="shildik-mesh-positions-array" count="54">-0.02999997 -0.01999998 -2.5e-4 -0.02999997 -0.01999998 2.5e-4 -0.02999997 0.01999998 -2.5e-4 -0.02999997 0.01999998 2.5e-4 0.02999997 -0.01999998 -2.5e-4 0.02999997 -0.01999998 2.5e-4 0.02999997 0.01999998 -2.5e-4 0.02999997 0.01999998 2.5e-4 -0.02999997 0 -2.5e-4 -0.02999997 0 2.5e-4 0 0.01999998 -2.5e-4 0 0.01999998 2.5e-4 0.02999997 0 -2.5e-4 0.02999997 0 2.5e-4 0 -0.01999998 -2.5e-4 0 -0.01999998 2.5e-4 0 0 2.5e-4 0 0 -2.5e-4</float_array>
           <technique_common>
             <accessor source="#shildik-mesh-positions-array" count="18" stride="3">
               <param name="X" type="float"/>
@@ -87,12 +140,20 @@
           <bind_material>
             <technique_common>
               <instance_material symbol="nasosnaya_ustanovka_svg-material" target="#nasosnaya_ustanovka_svg-material">
-                <bind_vertex_input semantic="UVMap" input_semantic="TEXCOORD" input_set="0"/>
+                <bind_vertex_input semantic="shildik-mesh-map-0" input_semantic="TEXCOORD" input_set="0"/>
               </instance_material>
             </technique_common>
           </bind_material>
         </instance_geometry>
       </node>
+      <node id="Camera" name="Camera" type="NODE">
+        <matrix sid="transform">0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1</matrix>
+        <instance_camera url="#Camera-camera"/>
+      </node>
+      <node id="Light" name="Light" type="NODE">
+        <matrix sid="transform">-0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1</matrix>
+        <instance_light url="#Light-light"/>
+      </node>
     </visual_scene>
   </library_visual_scenes>
   <scene>

rbs_mill_assist/assets/shildik/model.sdf

@@ -14,9 +14,9 @@
           <ambient>1 1 1 1</ambient>
           <specular>0.5 0.5 0.5 1</specular>
           <emissive>0 0 0 1</emissive>
-          <texture>
+          <!-- <texture> -->
-            <diffuse_map>model://shildik/textures/shildik_sh.png</diffuse_map>
+          <!--   <diffuse_map>model://shildik/textures/shildik_sh.png</diffuse_map> -->
-          </texture>
+          <!-- </texture> -->
           <pbr>
             <metal>
               <albedo_map>model://shildik/textures/shildik_sh_d.png</albedo_map>
@@ -29,20 +29,21 @@
         </material>
       </visual>
       <collision name="shildik_collision">
+        <density>2700</density>
         <geometry>
           <box>
-            <size>0.06 0.04 0.001</size>
+            <size>0.06 0.04 0.0005</size>
           </box>
         </geometry>
-        <surface>
+        <!-- <surface> -->
-          <contact>
+        <!--   <contact> -->
-            <ode />
+        <!--     <ode /> -->
-          </contact>
+        <!--   </contact> -->
-          <bounce />
+        <!--   <bounce /> -->
-          <friction>
+        <!--   <friction> -->
-            <ode />
+        <!--     <ode /> -->
-          </friction>
+        <!--   </friction> -->
-        </surface>
+        <!-- </surface> -->
       </collision>
     </link>
   </model>

rbs_mill_assist/bt/CMakeLists.txt

@@ -2,8 +2,10 @@
 find_package(behaviortree_ros2 REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(rbs_skill_interfaces REQUIRED)
+find_package(rbs_utils_interfaces REQUIRED)
 find_package(geometry_msgs REQUIRED)
-# find_package(std_srvs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(rbs_mill_interfaces REQUIRED)
 
 # Behaviortree interfaces
 set(dependencies
@@ -11,14 +13,34 @@ set(dependencies
     rbs_skill_interfaces
     geometry_msgs
     behaviortree_ros2
+    rbs_utils_interfaces
     std_srvs
+    rbs_mill_interfaces
 )
 
 add_library(vacuum_gripper_toggle SHARED plugins/vacuum_gripper_toggle.cpp)
 list(APPEND plugin_libs vacuum_gripper_toggle)
 
-add_library(get_grasp_place_pose SHARED plugins/get_grasp_place_pose.cpp)
+add_library(get_grasp_poses SHARED plugins/get_grasp_pose.cpp)
-list(APPEND plugin_libs get_grasp_place_pose)
+list(APPEND plugin_libs get_grasp_poses)
+
+add_library(get_place_poses SHARED plugins/get_place_pose.cpp)
+list(APPEND plugin_libs get_place_poses)
+
+add_library(get_named_pose SHARED plugins/get_named_pose.cpp)
+list(APPEND plugin_libs get_named_pose)
+
+add_library(is_in_pose SHARED plugins/is_in_pose.cpp)
+list(APPEND plugin_libs is_in_pose)
+
+add_library(twist_command_with_condition SHARED plugins/twist_cmd.cpp)
+list(APPEND plugin_libs twist_command_with_condition)
+
+add_library(takeon_task SHARED plugins/takeon_task.cpp)
+list(APPEND plugin_libs takeon_task)
+
+add_library(task_completed SHARED plugins/task_completed.cpp)
+list(APPEND plugin_libs task_completed)
 
 foreach(bt_plugin ${plugin_libs})
     ament_target_dependencies(${bt_plugin} ${dependencies})

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

rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp

@@ -1,118 +0,0 @@
-#include "behaviortree_ros2/bt_service_node.hpp"
-
-#include "behaviortree_ros2/plugins.hpp"
-#include <behaviortree_cpp/basic_types.h>
-// #include <geometry_msgs/msg/detail/point__struct.hpp>
-#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
-#include <geometry_msgs/msg/detail/pose__struct.hpp>
-#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
-#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
-#include <memory>
-// #include <rbs_utils_interfaces/srv/detail/get_grasp_pose__struct.hpp>
-#include <rclcpp/logging.hpp>
-#include <string>
-
-using GraspingService = rbs_skill_interfaces::srv::GetPickPlacePoses;
-using namespace BT;
-
-class Grasping : public RosServiceNode<GraspingService> {
-public:
-  Grasping(const std::string &name, const NodeConfig &conf,
-           const RosNodeParams &params)
-      : RosServiceNode<GraspingService>(name, conf, params) {
-
-    RCLCPP_INFO(this->logger(), "Starting GetGraspPose");
-  }
-
-  static PortsList providedPorts() {
-    return providedBasicPorts(
-        {InputPort<std::string>("object_name"),
-         InputPort<std::string>("action_name"),
-         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
-         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pre_pose"),
-         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("post_pose")});
-  }
-
-  bool setRequest(Request::SharedPtr &request) override {
-    RCLCPP_INFO(this->logger(), "Starting send request for: [%s]",
-                this->service_name_.c_str());
-    if (!getInput("action_name", request->action_name)) {
-      RCLCPP_ERROR(this->node_.lock()->get_logger(),
-                   "Failed to get object_name from input port");
-      return false;
-    }
-    return true;
-  }
-
-  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
-    if (!response->ok) {
-      RCLCPP_ERROR(this->node_.lock()->get_logger(),
-                   "Response indicates failure.");
-      return NodeStatus::FAILURE;
-    }
-
-    RCLCPP_INFO(this->node_.lock()->get_logger(),
-                "Response received successfully.");
-
-    auto logPose = [this](const std::string &pose_name,
-                          const geometry_msgs::msg::Pose &pose) {
-      RCLCPP_INFO(this->node_.lock()->get_logger(),
-                  "%s:\n"
-                  "  Position: x = %.4f, y = %.4f, z = %.4f\n"
-                  "  Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f",
-                  pose_name.c_str(), pose.position.x, pose.position.y,
-                  pose.position.z, pose.orientation.x, pose.orientation.y,
-                  pose.orientation.z, pose.orientation.w);
-    };
-    if (!response->grasp.empty()) {
-      RCLCPP_INFO(this->logger(), "Got Pick Response");
-      std::vector<std::string> poses = {"Pregrasp Pose", "Grasp Pose",
-                                        "Postgrasp Pose"};
-
-      for (size_t n = 0; n < poses.size() && n < response->grasp.size(); ++n) {
-        logPose(poses[n], response->grasp.at(n));
-      }
-
-      auto grasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
-      auto pregrasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
-      auto postgrasp_pose = std::make_shared<geometry_msgs::msg::Pose>();
-
-      *pregrasp_pose = response->grasp.at(0);
-      *grasp_pose = response->grasp.at(1);
-      *postgrasp_pose = response->grasp.at(2);
-
-      setOutput("pre_pose", pregrasp_pose);
-      setOutput("pose", grasp_pose);
-      setOutput("post_pose", postgrasp_pose);
-      return NodeStatus::SUCCESS;
-    }
-    if (!response->place.empty()) {
-      RCLCPP_INFO(this->logger(), "Got Place Response");
-      std::vector<std::string> poses = {"Preplace Pose", "Place Pose",
-                                        "Postplace Pose"};
-
-      for (size_t n = 0; n < poses.size() && n < response->place.size(); ++n) {
-        logPose(poses[n], response->place.at(n));
-      }
-
-      auto place_pose = std::make_shared<geometry_msgs::msg::Pose>();
-      auto preplace_pose = std::make_shared<geometry_msgs::msg::Pose>();
-      auto postplace_pose = std::make_shared<geometry_msgs::msg::Pose>();
-
-      *preplace_pose = response->place.at(0);
-      *place_pose = response->place.at(1);
-      *postplace_pose = response->place.at(2);
-
-      setOutput("pre_pose", preplace_pose);
-      setOutput("pose", place_pose);
-      setOutput("post_pose", postplace_pose);
-      return NodeStatus::SUCCESS;
-    }
-
-    RCLCPP_FATAL(this->logger(), "Could not response grasp pose");
-    return NodeStatus::FAILURE;
-  }
-  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
-};
-
-CreateRosNodePlugin(Grasping, "GetGraspPlacePose");

rbs_mill_assist/bt/plugins/get_grasp_pose.cpp

@@ -0,0 +1,89 @@
+#include "behaviortree_ros2/bt_service_node.hpp"
+
+#include "behaviortree_ros2/plugins.hpp"
+#include <behaviortree_cpp/basic_types.h>
+// #include <geometry_msgs/msg/detail/point__struct.hpp>
+#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
+// #include "rbs_mill_interfaces"
+// #include "rbs_mill_interfaces/srv/get_slot_grasping_pose.hpp"
+#include "rbs_mill_interfaces/srv/get_grasping_pose.hpp"
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
+#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
+#include <memory>
+// #include <rbs_utils_interfaces/srv/detail/get_grasp_pose__struct.hpp>
+#include <rclcpp/logging.hpp>
+#include <string>
+
+using GraspingService = rbs_mill_interfaces::srv::GetGraspingPose;
+using namespace BT;
+
+class GetGraspPose : public RosServiceNode<GraspingService> {
+public:
+  GetGraspPose(const std::string &name, const NodeConfig &conf,
+               const RosNodeParams &params)
+      : RosServiceNode<GraspingService>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting GetSlotGraspingPose");
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts(
+        {InputPort<std::string>("from_pose"),
+         InputPort<std::string>("relative_to"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("grasp"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pregrasp"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("postgrasp")});
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
+    if (!getInput("from_pose", request->pose_name)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get slot_name from input port",
+                   name().c_str());
+      return false;
+    }
+    if (!getInput("relative_to", request->relative_to)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get relative_to from input port",
+                   name().c_str());
+      return false;
+    }
+    request->action_type = "pick";
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    if (!response->ok) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Response indicates failure.", name().c_str());
+      return NodeStatus::FAILURE;
+    }
+    RCLCPP_INFO(this->node_.lock()->get_logger(),
+                "[%s] Response received successfully.", name().c_str());
+
+    auto grasp =
+        std::make_shared<geometry_msgs::msg::Pose>(response->grasp_poses.pose);
+    auto pregrasp = std::make_shared<geometry_msgs::msg::Pose>(
+        response->grasp_poses.pre_pose);
+    auto postgrasp = std::make_shared<geometry_msgs::msg::Pose>(
+        response->grasp_poses.post_pose);
+
+    if (!grasp || !pregrasp || !postgrasp) {
+      RCLCPP_ERROR(this->logger(), "[%s] Response poses are empty",
+                   name().c_str());
+      return NodeStatus::FAILURE;
+    }
+
+    setOutput("grasp", grasp);
+    setOutput("pregrasp", pregrasp);
+    setOutput("postgrasp", postgrasp);
+
+    return NodeStatus::SUCCESS;
+  }
+  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
+};
+
+CreateRosNodePlugin(GetGraspPose, "GetSlotGraspPoses");

rbs_mill_assist/bt/plugins/get_named_pose.cpp

@@ -0,0 +1,78 @@
+#include "behaviortree_ros2/bt_service_node.hpp"
+
+#include "behaviortree_ros2/plugins.hpp"
+#include <behaviortree_cpp/basic_types.h>
+// #include <geometry_msgs/msg/detail/point__struct.hpp>
+#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
+#include "rbs_utils_interfaces/srv/get_relative_named_pose.hpp"
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
+#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
+#include <memory>
+#include <rclcpp/logging.hpp>
+#include <string>
+
+using GetRelativeNamedPose = rbs_utils_interfaces::srv::GetRelativeNamedPose;
+using namespace BT;
+
+class GetNamedPose : public RosServiceNode<GetRelativeNamedPose> {
+public:
+  GetNamedPose(const std::string &name, const NodeConfig &conf,
+               const RosNodeParams &params)
+      : RosServiceNode<GetRelativeNamedPose>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting GetNamedPose");
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts(
+        {InputPort<std::string>("pose_name"),
+         InputPort<std::string>("relative_to"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("output_pose")});
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
+    if (!getInput("pose_name", request->pose_name)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get pose_name from input port",
+                   name().c_str());
+      return false;
+    }
+    if (!getInput("pose_name", request->pose_name)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get pose_name from input port",
+                   name().c_str());
+      return false;
+    }
+
+    if (!getInput("relative_to", request->relative_to)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get pose_name from input port",
+                   name().c_str());
+      return false;
+    }
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    if (!response->ok) {
+      RCLCPP_ERROR(this->logger(), "[%s] Response indicates failure.",
+                   name().c_str());
+      return NodeStatus::FAILURE;
+    }
+
+    RCLCPP_INFO(this->logger(),
+                "[%s] Response received successfully with pose name [%s]",
+                name().c_str(), response->named_pose.name.c_str());
+
+    auto pose = std::make_shared<geometry_msgs::msg::Pose>();
+    *pose = response->named_pose.pose;
+    setOutput("output_pose", pose);
+
+    return NodeStatus::SUCCESS;
+  }
+};
+
+CreateRosNodePlugin(GetNamedPose, "GetNamedPose");

rbs_mill_assist/bt/plugins/get_place_pose.cpp

@@ -0,0 +1,90 @@
+#include "behaviortree_ros2/bt_service_node.hpp"
+
+#include "behaviortree_ros2/plugins.hpp"
+#include <behaviortree_cpp/basic_types.h>
+// #include <geometry_msgs/msg/detail/point__struct.hpp>
+#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
+// #include "rbs_mill_interfaces"
+// #include "rbs_mill_interfaces/srv/get_slot_grasping_pose.hpp"
+#include "rbs_mill_interfaces/srv/get_grasping_pose.hpp"
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
+#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
+#include <memory>
+// #include <rbs_utils_interfaces/srv/detail/get_grasp_pose__struct.hpp>
+#include <rclcpp/logging.hpp>
+#include <string>
+
+using GraspingService = rbs_mill_interfaces::srv::GetGraspingPose;
+using namespace BT;
+
+class GetPlacePoses : public RosServiceNode<GraspingService> {
+public:
+  GetPlacePoses(const std::string &name, const NodeConfig &conf,
+               const RosNodeParams &params)
+      : RosServiceNode<GraspingService>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting GetPlacePose");
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts(
+        {InputPort<std::string>("to_pose"),
+         InputPort<std::string>("relative_to"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("place"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("preplace"),
+         OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("postplace")});
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
+    if (!getInput("to_pose", request->pose_name)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get slot_name from input port",
+                   name().c_str());
+      return false;
+    }
+    if (!getInput("relative_to", request->relative_to)) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Failed to get relative_to from input port",
+                   name().c_str());
+      return false;
+    }
+    request->action_type = "place";
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    if (!response->ok) {
+      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+                   "[%s] Response indicates failure.", name().c_str());
+      return NodeStatus::FAILURE;
+    }
+
+    RCLCPP_INFO(this->node_.lock()->get_logger(),
+                "[%s] Response received successfully.", name().c_str());
+
+    auto place =
+        std::make_shared<geometry_msgs::msg::Pose>(response->place_poses.pose);
+    auto preplace = std::make_shared<geometry_msgs::msg::Pose>(
+        response->place_poses.pre_pose);
+    auto postplace = std::make_shared<geometry_msgs::msg::Pose>(
+        response->place_poses.post_pose);
+
+    if (!place || !preplace || !postplace) {
+      RCLCPP_ERROR(this->logger(), "[%s] Response poses are empty",
+                   name().c_str());
+      return NodeStatus::FAILURE;
+    }
+
+    setOutput("place", place);
+    setOutput("preplace", preplace);
+    setOutput("postplace", postplace);
+
+    return NodeStatus::SUCCESS;
+  }
+  // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
+};
+
+CreateRosNodePlugin(GetPlacePoses, "GetPlacePoses");

rbs_mill_assist/bt/plugins/is_in_pose.cpp

@@ -0,0 +1,48 @@
+#include "behaviortree_ros2/bt_topic_sub_node.hpp"
+
+#include "behaviortree_ros2/plugins.hpp"
+#include <behaviortree_cpp/basic_types.h>
+// #include <geometry_msgs/msg/detail/point__struct.hpp>
+#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
+#include "rbs_utils_interfaces/srv/get_named_pose.hpp"
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
+#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
+#include <memory>
+#include <rclcpp/logging.hpp>
+#include <string>
+
+using MSG = geometry_msgs::msg::Pose;
+using namespace BT;
+
+class IsInPose : public RosTopicSubNode<MSG> {
+public:
+  IsInPose(const std::string &name, const NodeConfig &conf,
+           const RosNodeParams &params)
+      : RosTopicSubNode<MSG>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting IsInPose");
+  }
+  //
+  // static PortsList providedPorts() {
+  //   return {};
+  // }
+
+  NodeStatus onTick(const std::shared_ptr<MSG> &t_last_msg) override {
+    if (t_last_msg) {
+      RCLCPP_INFO(this->logger(), "[%s] Got message Pose", name().c_str());
+      m_last_msg = t_last_msg;
+      return NodeStatus::SUCCESS;
+    }
+    RCLCPP_INFO(this->logger(), "[%s] NOT message received", name().c_str());
+    return NodeStatus::SUCCESS;
+  }
+  bool latchLastMessage() const override {
+    return true;
+  }
+
+private:
+  MSG::SharedPtr m_last_msg;
+};
+
+CreateRosNodePlugin(IsInPose, "IsInPose");

rbs_mill_assist/bt/plugins/takeon_task.cpp

@@ -0,0 +1,54 @@
+#include "nlohmann/json.hpp"
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "behaviortree_ros2/plugins.hpp"
+#include "rbs_utils_interfaces/srv/task_from_queue.hpp"
+#include <string>
+#include <sstream>
+#include <iostream>
+
+using TaskFromQueue = rbs_utils_interfaces::srv::TaskFromQueue;
+using namespace BT;
+
+class TakeonTask : public RosServiceNode<TaskFromQueue> {
+public:
+  TakeonTask(const std::string &name, const NodeConfig &conf, const RosNodeParams &params)
+      : RosServiceNode<TaskFromQueue>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting TakeonTask");
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts(
+        {OutputPort<std::string>("from_pose")});
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
+    request->mode = "takeon";
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    if (!response->ok) {
+      RCLCPP_WARN(this->node_.lock()->get_logger(),
+                   "[%s] No tasks in queue", name().c_str());
+      return NodeStatus::FAILURE;
+    }
+    auto json_task = nlohmann::json::parse(response->task);
+    int TaskId = json_task["TaskId"];
+    int slot = json_task["obj_id"];
+
+    std::ostringstream oss;
+    oss << "slot_" << slot;
+    const char *x = oss.str().c_str();
+
+    setOutput("from_pose", x);
+
+    RCLCPP_INFO(this->node_.lock()->get_logger(),
+                "[%s] Task takeon: Id = %d, slot: %d", name().c_str(), TaskId, slot);
+    return NodeStatus::SUCCESS;
+  }
+};
+
+CreateRosNodePlugin(TakeonTask, "TakeonTask");

rbs_mill_assist/bt/plugins/task_completed.cpp

@@ -0,0 +1,40 @@
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "behaviortree_ros2/plugins.hpp"
+#include "rbs_utils_interfaces/srv/task_from_queue.hpp"
+
+using TaskFromQueue = rbs_utils_interfaces::srv::TaskFromQueue;
+using namespace BT;
+
+class TaskCompleted : public RosServiceNode<TaskFromQueue> {
+public:
+TaskCompleted(const std::string &name, const NodeConfig &conf, const RosNodeParams &params)
+      : RosServiceNode<TaskFromQueue>(name, conf, params) {
+
+    // RCLCPP_INFO(this->logger(), "Starting TakeonTask");
+  }
+
+  static PortsList providedPorts() {
+    return providedBasicPorts({});
+  }
+
+  bool setRequest(Request::SharedPtr &request) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
+    request->mode = "completed";
+    return true;
+  }
+
+  NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+    if (!response->ok) {
+      RCLCPP_WARN(this->node_.lock()->get_logger(),
+                   "[%s] Not completed", name().c_str());
+      return NodeStatus::FAILURE;
+    }
+
+    RCLCPP_INFO(this->node_.lock()->get_logger(),
+                "[%s] Task completed", name().c_str());
+    return NodeStatus::SUCCESS;
+  }
+};
+
+CreateRosNodePlugin(TaskCompleted, "TaskCompleted");

rbs_mill_assist/bt/plugins/twist_cmd.cpp

@@ -0,0 +1,85 @@
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "rbs_skill_interfaces/action/twist_control_with_condition.hpp"
+#include <behaviortree_cpp/tree_node.h>
+#include <behaviortree_ros2/plugins.hpp>
+#include <behaviortree_ros2/ros_node_params.hpp>
+#include <rclcpp/logging.hpp>
+
+using namespace BT;
+using TwistCmdWithCondAction =
+    rbs_skill_interfaces::action::TwistControlWithCondition;
+
+class TwistCmdWithCondClient : public RosActionNode<TwistCmdWithCondAction> {
+public:
+  TwistCmdWithCondClient(const std::string &name, const NodeConfig &conf,
+                         const RosNodeParams &params)
+      : RosActionNode<TwistCmdWithCondAction>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting MoveToPose");
+  }
+
+  static BT::PortsList providedPorts() {
+    return providedBasicPorts({BT::InputPort<std::string>("robot_name"),
+                               BT::InputPort<std::vector<double>>("twist"),
+                               // BT::InputPort<std::string>("condition_topic")
+    });
+  }
+
+  bool setGoal(RosActionNode::Goal &goal) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send goal [%s]", name().c_str(),
+                this->action_name_.c_str());
+
+    if (!getInput("robot_name", goal.robot_name)) {
+      RCLCPP_FATAL(this->logger(), "[%s] Could not get robot name",
+                   name().c_str());
+      return false;
+    }
+
+    if (!getInput("twist", twist_cmd_vec)) {
+      RCLCPP_FATAL(this->logger(), "[%s] Could not get twist command",
+                   name().c_str());
+      return false;
+    }
+    if (!twist_cmd_vec.empty() && twist_cmd_vec.size() == 6) {
+      goal.twist_cmd.linear.x = twist_cmd_vec[0];
+      goal.twist_cmd.linear.y = twist_cmd_vec[1];
+      goal.twist_cmd.linear.z = twist_cmd_vec[2];
+      goal.twist_cmd.angular.x = twist_cmd_vec[3];
+      goal.twist_cmd.angular.y = twist_cmd_vec[4];
+      goal.twist_cmd.angular.z = twist_cmd_vec[5];
+    } else {
+      RCLCPP_FATAL(this->logger(),
+                   "[%s] Twist command is empty or should be 1x6 [x;y;z;r;p;y]",
+                   name().c_str());
+      return false;
+    }
+
+    // if (!getInput("condition_topic", goal.condition_topic)) {
+    //   RCLCPP_FATAL(this->logger(), "[%s] Could not get condition_topic",
+    //                name().c_str());
+    //   return false;
+    // } else {
+    //   RCLCPP_INFO(this->logger(), "[%s] Using condition_topic [%s]",
+    //               name().c_str(), goal.condition_topic.c_str());
+    // }
+
+    return true;
+  }
+
+  NodeStatus onResultReceived(const WrappedResult &wr) override {
+
+    RCLCPP_INFO(this->logger(), "[%s] Starting get response %s with status %b",
+                name().c_str(), this->action_name_.c_str(), wr.result->success);
+    if (!wr.result->success) {
+      return NodeStatus::FAILURE;
+    }
+    return NodeStatus::SUCCESS;
+  }
+
+private:
+  // std::shared_ptr<geometry_msgs::msg::Pose> m_target_pose;
+  std::vector<double> twist_cmd_vec;
+};
+
+CreateRosNodePlugin(TwistCmdWithCondClient, "TwistCmdWithCond");

rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp

@@ -24,10 +24,10 @@ public:
   }
 
   bool setRequest(Request::SharedPtr &request) override {
-    RCLCPP_INFO(this->logger(), "Starting send request");
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request", name().c_str());
     if (!getInput("enable", request->data)) {
-      RCLCPP_ERROR(this->node_.lock()->get_logger(),
+      RCLCPP_ERROR(this->logger(),
-                   "Failed to get sending data from input port");
+                   "[%s] Failed to get sending data from input port", name().c_str());
       return false;
     }
     return true;
@@ -35,7 +35,7 @@ public:
 
   NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
     if (!response->success) {
-      RCLCPP_ERROR(this->logger(), "Response indicates failure.");
+      RCLCPP_ERROR(this->logger(), "[%s] Response indicates failure.", name().c_str());
       return NodeStatus::FAILURE;
     }
 

rbs_mill_assist/bt/xmls/Grasp.xml

@@ -1,21 +0,0 @@
-<?xml version='1.0' encoding='utf-8'?>
-<root BTCPP_format="4">
-  <BehaviorTree ID="Grasp">
-    <Sequence>
-
-      <Script code="action:='/mtp_jtc'"/>
-      <Script code="get_workspace:='/get_workspace'"/>
-
-      <Action ID="GetGraspPlacePose" action_name="pick" pre_pose="{pregrasp}" pose="{grasp}" post_pose="{postgrasp}" service_name="/get_pick_place_poses"/>
-
-      <Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="2" action_name="{action}"  />
-      <Action ID="ToggleVacuumGrippper" enable="true" service_name="/gz_ros2_vacuum_gripper_plugin/toggle"  />
-      <Action ID="MoveToPose" pose="{grasp}" robot_name="{robot_name}" duration="2" action_name="{action}"  />
-      <Action ID="MoveToPose" pose="{postgrasp}" robot_name="{robot_name}" duration="2" action_name="{action}"  />
-
-    </Sequence>
-  </BehaviorTree>
-
-  <TreeNodesModel>
-  </TreeNodesModel>
-</root>

rbs_mill_assist/bt/xmls/MainTree.xml

@@ -1,9 +0,0 @@
-<?xml version='1.0' encoding='utf-8'?>
-<root BTCPP_format="4">
-  <BehaviorTree ID="Main">
-    <Sequence>
-      <SubTree ID="Grasp"/>
-      <SubTree ID="Place"/>
-    </Sequence>
-  </BehaviorTree>
-</root>

rbs_mill_assist/bt/xmls/Place.xml

@@ -1,21 +0,0 @@
-<?xml version='1.0' encoding='utf-8'?>
-<root BTCPP_format="4">
-  <BehaviorTree ID="Place">
-    <Sequence>
-
-      <Script code="action:='/mtp_jtc'"/>
-      <Script code="get_workspace:='/get_workspace'"/>
-
-      <Action ID="GetGraspPlacePose" action_name="place" pre_pose="{preplace}" pose="{place}" post_pose="{postplace}" service_name="/get_pick_place_poses"/>
-
-      <Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="2" action_name="{action}"  />
-      <Action ID="MoveToPose" pose="{place}" robot_name="{robot_name}" duration="2" action_name="{action}"  />
-      <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="{action}"  />
-
-    </Sequence>
-  </BehaviorTree>
-
-  <TreeNodesModel>
-  </TreeNodesModel>
-</root>

rbs_mill_assist/bt/xmls/moveit/Grasp.xml

@@ -0,0 +1,37 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="Grasp">
+    <Sequence>
+
+      <Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="4" try_plan_untill_success="true" action_name="/mtp_moveit" />
+      <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>

rbs_mill_assist/bt/xmls/moveit/MainTree.xml

@@ -0,0 +1,136 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="Main">
+
+    <Sequence>
+      <!-- <Action ID="AddTask" tasks_csv="..." service_name="queue/add_tasks"/> -->
+
+      <Fallback>
+        <Repeat num_cycles="1">
+          <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}"/>
+              <Delay delay_msec=1000>
+              </Delay>
+            </ReactiveFallback> -->
+
+            <Action ID="GetSlotGraspPoses" from_pose="{from_pose}" 
+              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="PickAndPlace"
+              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" />
+            
+            <Inverter>
+              <Condition ID="RbssCondition" do="ArucoPE" command="isAruco" sid="ArucoPE-example" service_name="rbs_interface_s"/>
+            </Inverter>
+
+            <!-- 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="PickAndPlace"
+              robot_name="arm"
+              preplace="{preplace}"
+              place="{place}"
+              postplace="{postplace}"
+              pregrasp="{pregrasp}"
+              grasp="{grasp}"
+              postgrasp="{postgrasp}"
+            />
+
+            <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>
+
+        <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="TakeonTask" editable="true">
+      <input_port name="from_pose"/> 
+      <input_port name="service_name"/>
+    </Action>
+
+    <Action ID="TaskCompleted" editable="true">
+      <input_port name="service_name"/>
+    </Action>
+
+    <Action ID="SendTask" editable="true" />
+  </TreeNodesModel>
+</root>
+
+

rbs_mill_assist/bt/xmls/moveit/PickAndPlace.xml

@@ -0,0 +1,9 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="PickAndPlace">
+    <Sequence>
+      <SubTree ID="Grasp" pregrasp="{pregrasp}" grasp="{grasp}" postgrasp="{postgrasp}" robot_name="{robot_name}"/>
+      <SubTree ID="Place" preplace="{preplace}" place="{place}" postplace="{postplace}" robot_name="{robot_name}"/>
+    </Sequence>
+  </BehaviorTree>
+</root>

rbs_mill_assist/bt/xmls/moveit/Place.xml

@@ -0,0 +1,27 @@
+<?xml version='1.0' encoding='utf-8'?>
+<root BTCPP_format="4">
+  <BehaviorTree ID="Place">
+    <Sequence>
+
+      <Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="4" try_plan_untill_success="true" allowed_planning_time="10.0" action_name="/mtp_moveit" />
+      <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>

rbs_mill_assist/bt/xmls/moveit/btmill.btproj

@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" project_name="Project">
+    <include path="Grasp.xml"/>
+    <include path="Place.xml"/>
+    <include path="MainTree.xml"/>
+    <include path="PickAndPlace.xml"/>
+    <!-- Description of Node Models (used by Groot) -->
+    <TreeNodesModel>
+    </TreeNodesModel>
+</root>

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>

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>

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>
+
+

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>

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>

rbs_mill_assist/config/grasping.yaml

@@ -1,60 +0,0 @@
-pregrasp_pose:
-  position:
-    x: 0.10395
-    y: -0.28
-    z: 0.1
-  orientation:
-    x: 0.0
-    y: 1.0
-    z: 0.0
-    w: 0.0
-grasp_pose:
-  position:
-    x: 0.10395
-    y: -0.28
-    z: 0.004
-  orientation:
-    x: 0.0
-    y: 1.0
-    z: 0.0
-    w: 0.04
-postgrasp_pose:
-  position:
-    x: 0.10395
-    y: -0.28
-    z: 0.1
-  orientation:
-    x: 0.0
-    y: 1.0
-    z: 0.0
-    w: 0.0
-preplace_pose:
-  position:
-    x: 0.360
-    y: -0.06
-    z: 0.1
-  orientation:
-    x: -0.707
-    y: 0.707
-    z: 0.0
-    w: 0.0
-place_pose:
-  position:
-    x: 0.360
-    y: -0.06
-    z: 0.05
-  orientation:
-    x: -0.707
-    y: 0.707
-    z: 0.0
-    w: 0.0
-postplace_pose:
-  position:
-    x: 0.360
-    y: -0.06
-    z: 0.1
-  orientation:
-    x: -0.707
-    y: 0.707
-    z: 0.0
-    w: 0.0

rbs_mill_assist/config/gz_bridge.yaml

@@ -3,7 +3,23 @@
   gz_type_name: "gz.msgs.Clock"
   direction: GZ_TO_ROS
 
-- topic_name: "rgbd_camera/image"
+- topic_name: "/rgbd_camera/image/image"
   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "gz.msgs.Image"
   direction: GZ_TO_ROS
+
+- topic_name: "/rgbd_camera/image/depth_image"
+  ros_type_name: "sensor_msgs/msg/Image"
+  gz_type_name: "gz.msgs.Image"
+  direction: GZ_TO_ROS
+
+- topic_name: "/rgbd_camera/image/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "gz.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
+# TODO: add static TF publishing of the camera frame
+# - topic_name: "/rgbd_camera/image/points"
+#   ros_type_name: "sensor_msgs/msg/PointCloud2"
+#   gz_type_name: "gz.msgs.PointCloudPacked"
+#   direction: GZ_TO_ROS

rbs_mill_assist/config/obj_cfg.json

@@ -0,0 +1,38 @@
+[
+  {
+    "obj_id": 1,
+    "place_name": "bunker_1",
+    "place_aruco": 21,
+    "place_pos": "{pose}",
+    "graver_template": "template_obj_1.svg",
+    "dimensions": "{X,Y,Z}",
+    "skills": ["sid", "..."]
+  },
+  {
+    "obj_id": 2,
+    "place_name": "bunker_2",
+    "place_aruco": 22,
+    "place_pos": "{pose}",
+    "graver_template": "template_obj_2.svg",
+    "dimensions": "{X,Y,Z}",
+    "skills": ["sid", "..."]
+  },
+  {
+    "obj_id": 3,
+    "place_name": "bunker_3",
+    "place_aruco": 23,
+    "place_pos": "{pose}",
+    "graver_template": "{file}",
+    "dimensions": "{X,Y,Z}",
+    "skills": ["sid", "..."]
+  },
+  {
+    "obj_id": 4,
+    "place_name": "bunker_4",
+    "place_aruco": 24,
+    "place_pos": "{pose}",
+    "graver_template": "{file}",
+    "dimensions": "{X,Y,Z}",
+    "skills": ["sid", "..."]
+  }
+]

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"
+      }
+    }
+  ]
+}

rbs_mill_assist/include/rbs_mill_assist/VacuumGripper.hpp

@@ -8,7 +8,9 @@
 #include <gz/sim/System.hh>
 
 #include "std_srvs/srv/set_bool.hpp"
+#include "std_msgs/msg/bool.hpp"
 #include <memory>
+#include <rclcpp/publisher.hpp>
 #include <rclcpp/service.hpp>
 
 namespace rbs_mill_assist {
@@ -17,7 +19,8 @@ class VacuumGripperPrivate;
 
 class VacuumGripper : public gz::sim::System,
                       public gz::sim::ISystemConfigure,
-                      public gz::sim::ISystemPreUpdate {
+                      public gz::sim::ISystemPreUpdate,
+                      public gz::sim::ISystemPostUpdate {
 public:
   VacuumGripper();
   ~VacuumGripper() override;
@@ -29,6 +32,9 @@ public:
   void PreUpdate(const gz::sim::UpdateInfo &_info,
                  gz::sim::EntityComponentManager &_ecm) override;
 
+  void PostUpdate(const gz::sim::UpdateInfo &_info,
+                  const gz::sim::EntityComponentManager &_ecm) override;
+
   bool AttachLink(gz::sim::EntityComponentManager &_ecm,
                   const gz::sim::Entity &gripperEntity,
                   const gz::sim::Entity &objectEntity);
@@ -41,6 +47,7 @@ public:
 private:
   std::unique_ptr<VacuumGripperPrivate> dataPtr;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_service_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr contact_publisher_;
 };
 } // namespace rbs_mill_assist
 #endif

rbs_mill_assist/include/rbs_mill_assist/places_config.hpp

@@ -0,0 +1,176 @@
+#include "geometry_msgs/msg/pose.hpp"
+#include <filesystem>
+#include <memory>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/node_interfaces/node_logging_interface.hpp>
+#include <string>
+#include <tf2/LinearMath/Quaternion.hpp>
+#include <vector>
+#include <yaml-cpp/node/node.h>
+#include <yaml-cpp/node/parse.h>
+#include <yaml-cpp/yaml.h>
+
+class PlacesConfig {
+public:
+  PlacesConfig(const std::string &places_filepath, const rclcpp::Logger& logger)
+      : _logger(logger) {
+
+    objects_to_spawn_ = std::make_shared<Places>();
+    if (!loadPlacesFromYaml(places_filepath)) {
+      RCLCPP_FATAL(_logger, "Failed to load objects from YAML file: %s",
+                   places_filepath.c_str());
+      rclcpp::shutdown();
+      return;
+    }
+  }
+
+  PlacesConfig(const std::string &places_filepath, rclcpp::Node::SharedPtr node)
+      : PlacesConfig(places_filepath, node->get_logger()) {}
+
+  struct PlaceSpec {
+    std::string name;
+    std::string type;
+    std::string frame_id = "world";
+    bool has_dimensions;
+    std::vector<double> dimensions;
+    geometry_msgs::msg::Pose pose;
+    std::optional<std::string> mesh_filepath;
+    std::tuple<double, double, double> scale_xyz = {1.0, 1.0, 1.0};
+  };
+
+  struct Places {
+    std::optional<std::string> model_dir;
+    std::vector<PlaceSpec> models;
+  };
+
+  inline std::shared_ptr<Places> get() const { return objects_to_spawn_; }
+
+private:
+  const rclcpp::Logger& _logger;
+
+  bool loadPlacesFromYaml(const std::string &filepath) {
+    auto places_config_node = YAML::LoadFile(filepath);
+
+    if (auto node = places_config_node["model_dir"]; node && node.IsScalar()) {
+      objects_to_spawn_->model_dir = node.as<std::string>();
+    }
+
+    if (!places_config_node["models"] ||
+        !places_config_node["models"].IsSequence()) {
+      RCLCPP_FATAL(_logger, "No valid 'models' section found in config_file");
+      return false;
+    }
+    for (const auto &model : places_config_node["models"]) {
+      PlaceSpec object;
+      if (model["ps_ignore"] && model["ps_ignore"].as<bool>()) {
+        continue;
+      }
+      object.name = model["name"].as<std::string>();
+      object.type = model["type"].as<std::string>();
+      const auto &type = object.type;
+      if (type == "box" || type == "sphere" || type == "cylinder") {
+        if (!model["dimensions"]) {
+          RCLCPP_ERROR(_logger, "Model '%s' is missing 'dimensions'",
+                       object.name.c_str());
+          return false;
+        }
+        auto d = model["dimensions"].as<std::vector<double>>();
+
+        if (type == "box" && d.size() == 3) {
+          object.dimensions = d;
+        } else if (type == "sphere" && d.size() == 1) {
+          object.dimensions = d;
+        } else if (type == "cylinder" && d.size() == 2) {
+          object.dimensions = d;
+        } else {
+          RCLCPP_ERROR(_logger, "Invalid dimensions count for type '%s'",
+                       type.c_str());
+          return false;
+        }
+
+      } else if (type == "mesh") {
+        if (!model["mesh_filename"]) {
+          RCLCPP_ERROR(_logger, "mesh object missing mesh_filename");
+          return false;
+        }
+        std::string mesh_filename = model["mesh_filename"].as<std::string>();
+        if (objects_to_spawn_->model_dir.has_value()) {
+          object.mesh_filepath =
+              std::filesystem::path(objects_to_spawn_->model_dir->c_str()) /
+              object.name / "meshes" / mesh_filename;
+          RCLCPP_WARN(_logger, "mesh filepath is: %s",
+                      object.mesh_filepath->c_str());
+        } else {
+          RCLCPP_ERROR(_logger, "mesh_filename in empty in config_file");
+          return false;
+        }
+
+      } else {
+        RCLCPP_ERROR(_logger, "Unrecognized object type: '%s'", type.c_str());
+        return false;
+      }
+      if (model["frame_id"]) {
+        object.frame_id = model["frame_id"].as<std::string>();
+      }
+      if (model["pose"]) {
+        auto pose = model["pose"];
+
+        if (pose["position"]) {
+          auto position = model["pose"]["position"];
+          object.pose.position.x = position["x"].as<double>();
+          object.pose.position.y = position["y"].as<double>();
+          object.pose.position.z = position["z"].as<double>();
+        }
+        if (pose["orientation"]) {
+          auto orientation = model["pose"]["orientation"];
+          if (!isQuat(orientation)) {
+            auto quat = rpyToQuat(orientation["r"].as<double>(),
+                                  orientation["p"].as<double>(),
+                                  orientation["y"].as<double>());
+
+            object.pose.orientation.x = quat.x;
+            object.pose.orientation.y = quat.y;
+            object.pose.orientation.z = quat.z;
+            object.pose.orientation.w = quat.w;
+          } else {
+            object.pose.orientation.x = orientation["x"].as<double>();
+            object.pose.orientation.y = orientation["y"].as<double>();
+            object.pose.orientation.z = orientation["z"].as<double>();
+            object.pose.orientation.w = orientation["w"].as<double>();
+          }
+        }
+      }
+
+      objects_to_spawn_->models.push_back(object);
+    }
+
+    return true;
+  }
+
+  bool isQuat(const YAML::Node &node) {
+    if (node["x"] && node["y"] && node["z"] && node["w"]) {
+      return true;
+    } else if (node["r"] && node["p"] && node["y"]) {
+      return false;
+    }
+    throw std::invalid_argument(
+        "Provided YAML Node is not quat or euler angles");
+  }
+
+  geometry_msgs::msg::Quaternion rpyToQuat(double roll, double pitch,
+                                           double yaw) {
+    tf2::Quaternion q;
+    q.setRPY(roll, pitch, yaw);
+    q.normalize();
+    geometry_msgs::msg::Quaternion q_msg;
+    q_msg.x = q.x();
+    q_msg.y = q.y();
+    q_msg.z = q.z();
+    q_msg.w = q.w();
+    return q_msg;
+  }
+
+  std::shared_ptr<Places> objects_to_spawn_;
+};

rbs_mill_assist/include/rbs_mill_assist/ref_frames_config.hpp

@@ -0,0 +1,82 @@
+#include "geometry_msgs/msg/pose.hpp"
+#include <filesystem>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/node.hpp>
+#include <stdexcept>
+#include <string>
+#include <tf2/LinearMath/Quaternion.hpp>
+#include <yaml-cpp/yaml.h>
+
+class RefFramesConfig {
+public:
+  RefFramesConfig(const std::string &ref_frames_filepath) {
+    loadConfig(ref_frames_filepath);
+  }
+
+  struct RefFrame {
+    std::string name;
+    std::string relative_to;
+    geometry_msgs::msg::Pose pose;
+  };
+
+  inline std::vector<RefFrame> get() const {
+    return _ref_frames;
+  }
+
+
+private:
+  std::vector<RefFrame> _ref_frames;
+
+  geometry_msgs::msg::Quaternion toQuat(const YAML::Node &node) {
+    geometry_msgs::msg::Quaternion quat;
+    if (node["x"] && node["y"] && node["z"] && node["w"]) {
+      quat.x = node["x"].as<double>();
+      quat.y = node["y"].as<double>();
+      quat.z = node["z"].as<double>();
+      quat.w = node["w"].as<double>();
+    } else if (node["r"] && node["p"] && node["y"]) {
+      tf2::Quaternion tf2_quat;
+      tf2_quat.setRPY(node["r"].as<double>(), node["p"].as<double>(),
+                      node["y"].as<double>());
+      quat.x = tf2_quat.x();
+      quat.y = tf2_quat.y();
+      quat.z = tf2_quat.z();
+      quat.w = tf2_quat.w();
+    }
+    return quat;
+  }
+
+  void parseRefFrames(const YAML::Node &node) {
+    for (const auto &frame : node) {
+      RefFrame ref_frame;
+      if (!frame["name"] || !frame["relative_to"] || !frame["pose"]) {
+        throw std::runtime_error(
+            "Missing required ref_frame parameters in YAML");
+      }
+      ref_frame.name = frame["name"].as<std::string>();
+      ref_frame.relative_to = frame["relative_to"].as<std::string>();
+
+      geometry_msgs::msg::Pose frame_pose;
+      frame_pose.position.x = frame["pose"]["position"]["x"].as<double>();
+      frame_pose.position.y = frame["pose"]["position"]["y"].as<double>();
+      frame_pose.position.z = frame["pose"]["position"]["z"].as<double>();
+
+      if (auto orientation = frame["pose"]["orientation"]; orientation) {
+        frame_pose.orientation = toQuat(orientation);
+      }
+      ref_frame.pose = frame_pose;
+      _ref_frames.push_back(ref_frame);
+    }
+  }
+
+  void loadConfig(const std::string &ref_frames_filepath) {
+    YAML::Node config = YAML::LoadFile(ref_frames_filepath);
+
+    if (config["ref_frames"] && config["ref_frames"].IsSequence()) {
+      parseRefFrames(config["ref_frames"]);
+    } else {
+      throw std::runtime_error("No ref_frames in config");
+    }
+
+  }
+};

rbs_mill_assist/launch/moveit.launch.py

@@ -0,0 +1,201 @@
+from launch.event_handlers import OnExecutionComplete, OnProcessExit, OnProcessStart
+from launch_param_builder import get_package_share_directory
+from launch_ros.actions import Node
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler, TimerAction
+from launch.substitutions import (
+    LaunchConfiguration,
+)
+from moveit_configs_utils import MoveItConfigsBuilder, moveit_configs_builder
+import os
+
+
+def generate_launch_description():
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_description",
+            default_value="''",
+            description="robot description param",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_description_semantic",
+            default_value="''",
+            description="robot description semantic param",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_description_kinematics",
+            default_value="''",
+            description="robot description kinematics param",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_package",
+            default_value="rbs_arm",
+            description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom moveit config.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_file",
+            default_value="rbs_arm.srdf.xacro",
+            description="MoveIt SRDF/XACRO description file with the robot.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_sim_time",
+            default_value="true",
+            description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tf_prefix",
+            default_value="''",
+            description="tf_prefix for robot links",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "namespace",
+            default_value="",
+            description="Namespace for move_group_node",
+        )
+    )
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )
+
+
+def launch_setup(context, *args, **kwargs):
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    robot_description_content = LaunchConfiguration("robot_description").perform(
+        context
+    )
+    robot_description_semantic_content = LaunchConfiguration(
+        "robot_description_semantic"
+    ).perform(context)
+    namespace = LaunchConfiguration("namespace").perform(context)
+
+    robot_description = {"robot_description": robot_description_content}
+    robot_description_semantic = {
+        "robot_description_semantic": robot_description_semantic_content
+    }
+    use_sim_time = {"use_sim_time": use_sim_time}
+
+    moveit_config = (
+        MoveItConfigsBuilder(robot_name="rbs_arm", package_name="rbs_mill_assist")
+        .trajectory_execution(file_path="moveit/moveit_controllers.yaml")
+        .joint_limits("moveit/joint_limits.yaml")
+        .pilz_cartesian_limits("moveit/pilz_cartesian_limits.yaml")
+        .robot_description_kinematics("moveit/kinematics.yaml")
+        .robot_description_semantic("srdf/rbs_arm.srdf", mappings={})
+        .planning_scene_monitor(
+            publish_robot_description=True, publish_robot_description_semantic=True
+        )
+        .planning_pipelines(
+            pipelines=["ompl", "stomp", "chomp", "pilz_industrial_motion_planner"], default_planning_pipeline="ompl"
+        )
+        .to_moveit_configs()
+    )
+    move_group_capabilities = {
+        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
+    }
+    planners_ids = [
+        "RRTstar",
+        "PRM",
+        "BiTRRT",
+        "SPARStwo",
+    ]
+    (
+        moveit_config.planning_pipelines["ompl"]
+        .setdefault("arm", {})
+        .update(
+            {"planner_configs":planners_ids}
+        )
+    )
+
+    move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        namespace=namespace,
+        parameters=[moveit_config.to_dict(), robot_description, use_sim_time],
+    )
+
+
+    places_config_filepath = os.path.join(get_package_share_directory("rbs_mill_assist"), "world/config", "places.yaml")
+
+    # planning_scene_publisher = Node(
+    #     package="rbs_mill_assist",
+    #     executable="planning_scene_publisher",
+    #     parameters=[moveit_config.to_dict(), {"use_sim_time": True}, {"places_config": places_config_filepath}],
+    # )
+
+    collision_spawner = Node(
+        package="rbs_mill_assist",
+        # prefix=['gdbserver localhost:1234'],
+        executable="collision_spawner",
+        parameters=[{"config_file": places_config_filepath}]
+    )
+
+
+    ps_manager = Node(
+        package="rbs_mill_assist",
+        executable="ps_manager",
+        output="screen",
+        parameters=[{"frame_id": "world"}]
+    )
+
+    ref_frames_config_filepath = os.path.join(
+        get_package_share_directory("rbs_mill_assist"),
+        "world/config",
+        "ref_frames.yaml",
+    )
+
+
+    places_frame_publisher = Node(
+        package="rbs_mill_assist",
+        executable="places_frame_publisher",
+        parameters=[
+            {"use_sim_time": use_sim_time},
+            {"places_config_filepath": places_config_filepath},
+            {"ref_frames_config_filepath": ref_frames_config_filepath},
+        ],
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        # arguments=["-d", rviz_full_config],
+        parameters=[
+            use_sim_time,
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.planning_pipelines,
+            moveit_config.planning_scene_monitor,
+            moveit_config.robot_description_kinematics,
+            moveit_config.joint_limits,
+        ],
+    )
+
+    nodes_to_start = [
+        move_group_node,
+        rviz_node,
+        places_frame_publisher,
+        # delay_planning_scene
+        # handler
+        collision_spawner,
+        ps_manager
+    ]
+
+    return nodes_to_start

rbs_mill_assist/launch/simulation.launch.py

@@ -1,5 +1,6 @@
 import os
 
+from launch.conditions import IfCondition, UnlessCondition
 import xacro
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
@@ -64,23 +65,28 @@ def launch_setup(context, *args, **kwargs):
     robot_description_semantic_content = ""
 
     if use_moveit.perform(context) == "true":
-        srdf_config_file = os.path.join(
+        package_dir = get_package_share_directory(
-            get_package_share_directory(moveit_config_package.perform(context)),
+            moveit_config_package.perform(context)
-            "srdf",
-            "xacro_args.yaml",
         )
+
         srdf_file = os.path.join(
-            get_package_share_directory(moveit_config_package.perform(context)),
+            package_dir, "srdf", moveit_config_file.perform(context)
-            "srdf",
-            moveit_config_file.perform(context),
-        )
-        srdf_mappings = load_xacro_args(srdf_config_file, locals())
-        robot_description_semantic_content = xacro.process_file(
-            srdf_file, mappings=srdf_mappings
-        )
-        robot_description_semantic_content = (
-            robot_description_semantic_content.toprettyxml(indent=" ")
         )
+
+        if srdf_file.endswith(".xacro"):
+            srdf_config_file = os.path.join(package_dir, "srdf", "xacro_args.yaml")
+            srdf_mappings = load_xacro_args(srdf_config_file, locals())
+            robot_description_semantic_content = xacro.process_file(
+                srdf_file, mappings=srdf_mappings
+            )
+            robot_description_semantic_content = (
+                robot_description_semantic_content.toprettyxml(indent=" ")
+            )
+
+        elif srdf_file.endswith(".srdf"):
+            with open(srdf_file, "r") as file:
+                robot_description_semantic_content = file.read()
+
         control_space = "joint"
         control_strategy = "position"
         interactive = "false"
@@ -128,22 +134,38 @@ def launch_setup(context, *args, **kwargs):
         }.items(),
     )
 
-    rviz_config_file = os.path.join(description_package_abs_path, "config", "config.rviz")
+    rviz_config_file = os.path.join(
+        description_package_abs_path, "config", "config.rviz"
+    )
 
     rviz_node = Node(
         package="rviz2",
         executable="rviz2",
         arguments=["-d", rviz_config_file],
-        parameters=[{"use_sim_time": True}]
+        parameters=[{"use_sim_time": True}],
     )
 
-    gazebo_world = os.path.join(description_package_abs_path, "world", "default.sdf")
+    gazebo_world_filepath = os.path.join(
+        description_package_abs_path, "world", "world.xacro"
+    )
+    gazebo_world = xacro.process_file(gazebo_world_filepath).toxml()
+    world_sdf_filepath = os.path.join(
+        description_package_abs_path, "world", "world.sdf"
+    )
+    with open(world_sdf_filepath, "w") as sdf_file:
+        sdf_file.write(gazebo_world)
 
     gazebo = IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
+        PythonLaunchDescriptionSource(
-                [os.path.join(get_package_share_directory('ros_gz_sim'),
+            [
-                            'launch', 'gz_sim.launch.py')]),
+                os.path.join(
-            launch_arguments=[('gz_args', [' -r -v 4 ', gazebo_world])],
+                    get_package_share_directory("ros_gz_sim"),
+                    "launch",
+                    "gz_sim.launch.py",
+                )
+            ]
+        ),
+        launch_arguments=[("gz_args", [" -r ", world_sdf_filepath])],
     )
 
     gazebo_spawn_robot = Node(
@@ -154,7 +176,8 @@ def launch_setup(context, *args, **kwargs):
             "-x", "0.0",
             "-z", "0.0",
             "-y", "0.0",
-            "-topic", "/robot_description",
+            # "-topic", "/robot_description",
+            "-string", robot_description_content
         ],
         output="screen",
         parameters=[{"use_sim_time": True}],
@@ -162,21 +185,50 @@ def launch_setup(context, *args, **kwargs):
 
     gz_bridge = RosGzBridge(
         bridge_name="ros_gz_bridge",
-        config_file=os.path.join(description_package_abs_path, "config", "gz_bridge.yaml")
+        config_file=os.path.join(
+            description_package_abs_path, "config", "gz_bridge.yaml"
+        ),
     )
 
-    grasping_service = Node(
+    grasping_service = Node(package="rbs_mill_assist", executable="grasping_service.py")
+
+    get_named_pose_service = Node(
         package="rbs_mill_assist",
-        executable="grasping_service.py"
+        executable="get_key_pose_frame.py",
+        parameters=[{"use_sim_time": use_sim_time}],
+    )
+
+    places_config_filepath = os.path.join(
+        get_package_share_directory("rbs_mill_assist"), "world/config", "places.yaml"
+    )
+
+    ref_frames_config_filepath = os.path.join(
+        get_package_share_directory("rbs_mill_assist"),
+        "world/config",
+        "ref_frames.yaml",
+    )
+    places_frame_publisher = Node(
+        package="rbs_mill_assist",
+        executable="places_frame_publisher",
+        parameters=[
+            {"use_sim_time": use_sim_time},
+            {"places_config_filepath": places_config_filepath},
+            {"ref_frames_config_filepath": ref_frames_config_filepath},
+        ],
+        condition=UnlessCondition(use_moveit)
     )
 
     nodes_to_start = [
         rbs_robot_setup,
-        rviz_node,
+        # rviz_node,
         gazebo,
         gazebo_spawn_robot,
         gz_bridge,
-        grasping_service
+        grasping_service,
+        get_named_pose_service,
+        places_frame_publisher,
+        # publish_ee_pose
+        # get_workspace
     ]
     return nodes_to_start
 
@@ -226,7 +278,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "moveit_config_package",
-            default_value="rbs_arm",
+            default_value="rbs_mill_assist",
             description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
         is not set, it enables use of a custom moveit config.",
         )
@@ -234,7 +286,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "moveit_config_file",
-            default_value="rbs_arm.srdf.xacro",
+            default_value="rbs_arm.srdf",
             description="MoveIt SRDF/XACRO description file with the robot.",
         )
     )
@@ -253,7 +305,7 @@ def generate_launch_description():
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "use_moveit", default_value="false", description="Launch moveit?"
+            "use_moveit", default_value="true", description="Launch moveit?"
         )
     )
     declared_arguments.append(
@@ -308,7 +360,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "interactive",
-            default_value="false",
+            default_value="true",
             description="Wheter to run the motion_control_handle controller",
         ),
     )

rbs_mill_assist/moveit/initial_positions.yaml

@@ -0,0 +1,9 @@
+# Default initial positions for rbs_arm's ros2_control fake system
+
+initial_positions:
+  ax1: 0
+  ax2: 0
+  ax3: 0
+  ax4: 0
+  ax5: 0
+  ax6: 0