robossembler/cnc-graver-assist
7
0
Fork 0
Code Issues 5 Pull requests 1 Projects Releases Packages Wiki Activity Actions

Дерево поведения с имитацией полного цикла производства #24

Open
solid-sinusoid wants to merge 58 commits from dev into main
pull from: dev
merge into: robossembler:main
Conversation 36 Commits 58 Files changed 160 +17276 -739
160 changed files with 17276 additions and 739 deletions
Download patch file Download diff file Expand all files Collapse all files

3
.gitignore vendored Normal file
View file

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

165
docs/bt_example_with_tasks.md Normal file
View file

@ -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**: Повторение действий до определенного условия.
Этот подход обеспечивает четкое разделение логики и позволяет легко масштабировать систему.

38
docs/objects.json Normal file
View file

@ -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", "..."]
}
]

16
docs/shildik_templates/README.md Normal file
View file

@ -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, Кол-во электродвигателей, Дата производства, Заводской номер |

899
docs/shildik_templates/curves/Табличка_пустая_Ex.svg Normal file
View file

File diff suppressed because one or more lines are too long
Side by side

After

Width:  |  Height:  |  Size: 212 KiB

934
docs/shildik_templates/curves/Табличка_пустая_Насосная_установка.svg Normal file
View file

File diff suppressed because one or more lines are too long
Side by side

After

Width:  |  Height:  |  Size: 218 KiB

412
docs/shildik_templates/curves/Табличка_пустая_Общепром.svg Normal file
View file

File diff suppressed because one or more lines are too long
Side by side

After

Width:  |  Height:  |  Size: 146 KiB

899
docs/shildik_templates/curves/Табличка_пустая_Шкаф.svg Normal file
View file

File diff suppressed because one or more lines are too long
Side by side

After

Width:  |  Height:  |  Size: 212 KiB

BIN
docs/shildik_templates/fonts/GOST 2.304 type A.ttf Normal file
View file

Binary file not shown.

BIN
docs/shildik_templates/fonts/GOST 2.304 type B.ttf Normal file
View file

Binary file not shown.

855
docs/shildik_templates/test_templates/Табличка_пустая_Ex.svg Normal file
View file

File diff suppressed because one or more lines are too long
Side by side

After

Width:  |  Height:  |  Size: 177 KiB

932
docs/shildik_templates/test_templates/Табличка_пустая_Ex_plain.svg Normal file
View file

File diff suppressed because one or more lines are too long
Side by side

After

Width:  |  Height:  |  Size: 214 KiB

657
docs/shildik_templates/text/Табличка_пустая_Ex.svg Normal file
View file

File diff suppressed because one or more lines are too long
Side by side

After

Width:  |  Height:  |  Size: 138 KiB

912
docs/shildik_templates/text/Табличка_пустая_Насосная_установка.svg Normal file
View file

File diff suppressed because one or more lines are too long
Side by side

After

Width:  |  Height:  |  Size: 217 KiB

578
docs/shildik_templates/text/Табличка_пустая_Общепром.svg Normal file
View file

File diff suppressed because one or more lines are too long
Side by side

After

Width:  |  Height:  |  Size: 92 KiB

877
docs/shildik_templates/text/Табличка_пустая_Шкаф.svg Normal file
View file

File diff suppressed because one or more lines are too long
Side by side

After

Width:  |  Height:  |  Size: 211 KiB

6
docs/tasks.csv Normal file
View file

@ -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
Can't render this file because it has a wrong number of fields in line 5.

25
docs/to_start_BT_main.md Normal file
View file

@ -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}"
```

43
rbs_mill_assist/CMakeLists.txt
View file

@ -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
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
install(DIRECTORY world urdf meshes moveit srdf 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)

BIN
rbs_mill_assist/assets/aruco_1/meshes/aruco_marker_21.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 3 KiB

12
rbs_mill_assist/assets/aruco_1/meshes/aruco_template.mtl Normal file
View file

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

48
rbs_mill_assist/assets/aruco_1/meshes/aruco_template.obj Normal file
View file

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

17
rbs_mill_assist/assets/aruco_1/model.config Normal file
View file

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

16
rbs_mill_assist/assets/aruco_1/model.sdf Normal file
View file

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

BIN
rbs_mill_assist/assets/aruco_2/meshes/aruco_marker_22.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 3 KiB

13
rbs_mill_assist/assets/aruco_2/meshes/aruco_template.mtl Normal file
View file

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

48
rbs_mill_assist/assets/aruco_2/meshes/aruco_template.obj Normal file
View file

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

17
rbs_mill_assist/assets/aruco_2/model.config Normal file
View file

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

16
rbs_mill_assist/assets/aruco_2/model.sdf Normal file
View file

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

BIN
rbs_mill_assist/assets/aruco_3/meshes/aruco_marker_23.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 3.2 KiB

12
rbs_mill_assist/assets/aruco_3/meshes/aruco_template.mtl Normal file
View file

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

48
rbs_mill_assist/assets/aruco_3/meshes/aruco_template.obj Normal file
View file

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

17
rbs_mill_assist/assets/aruco_3/model.config Normal file
View file

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

16
rbs_mill_assist/assets/aruco_3/model.sdf Normal file
View file

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

BIN
rbs_mill_assist/assets/aruco_4/meshes/aruco_marker_24.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 2.8 KiB

12
rbs_mill_assist/assets/aruco_4/meshes/aruco_template.mtl Normal file
View file

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

48
rbs_mill_assist/assets/aruco_4/meshes/aruco_template.obj Normal file
View file

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

17
rbs_mill_assist/assets/aruco_4/model.config Normal file
View file

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

16
rbs_mill_assist/assets/aruco_4/model.sdf Normal file
View file

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

BIN
rbs_mill_assist/assets/aruco_c/meshes/aruco_marker_25.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 3.3 KiB

12
rbs_mill_assist/assets/aruco_c/meshes/aruco_template.mtl Normal file
View file

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

48
rbs_mill_assist/assets/aruco_c/meshes/aruco_template.obj Normal file
View file

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

17
rbs_mill_assist/assets/aruco_c/model.config Normal file
View file

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

16
rbs_mill_assist/assets/aruco_c/model.sdf Normal file
View file

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

BIN
rbs_mill_assist/assets/bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL Normal file
View file

Binary file not shown.

BIN
rbs_mill_assist/assets/bunker/meshes/bunker.stl
View file

Binary file not shown.

65
rbs_mill_assist/assets/bunker/model.sdf
View file

@ -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> -->
<!-- <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://bunker/textures/shildik_sh.png</diffuse_map> -->
<!-- </texture> -->
<!-- <pbr> -->
<!-- <metal> -->
<!-- <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> -->
<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="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>
<contact>
<ode />
</contact>
<bounce />
<friction>
<ode />
</friction>
</surface>
<!-- <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>

BIN
rbs_mill_assist/assets/bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL Normal file
View file

Binary file not shown.

16
rbs_mill_assist/assets/bunker_4_slots/model.config Normal file
View file

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

54
rbs_mill_assist/assets/bunker_4_slots/model.sdf Normal file
View file

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

BIN
rbs_mill_assist/assets/conductor/meshes/conductor.stl Normal file
View file

Binary file not shown.

16
rbs_mill_assist/assets/conductor/model.config Normal file
View file

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

61
rbs_mill_assist/assets/conductor/model.sdf Normal file
View file

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

11
rbs_mill_assist/assets/laser/laser.material Normal file
View file

@ -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
}
}
}

207
rbs_mill_assist/assets/laser/meshes/laser.dae Normal file
View file

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

2
rbs_mill_assist/assets/laser/meshes/laser.mtl Normal file
View file

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

103
rbs_mill_assist/assets/laser/meshes/laser.obj Normal file
View file

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

BIN
rbs_mill_assist/assets/laser/meshes/laser.stl
View file

Binary file not shown.

41
rbs_mill_assist/assets/laser/model.sdf
View file

@ -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> -->
<!-- <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://laser/textures/shildik_sh.png</diffuse_map> -->
<!-- </texture> -->
<!-- <pbr> -->
<!-- <metal> -->
<!-- <albedo_map>model://laser/textures/shildik_sh_d.png</albedo_map> -->
<!-- <normal_map>model://laser/textures/shildik_sh_n.png</normal_map> -->
<!-- <roughness_map>model://laser/textures/shildik_sh_r.png</roughness_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> -->
<!-- </metal> -->
<!-- </pbr> -->
<!-- </material> -->
<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://laser/textures/shildik_sh.png</diffuse_map> -->
<!-- </texture> -->
<!-- <pbr> -->
<!-- <metal> -->
<!-- <albedo_map>model://laser/textures/shildik_sh_d.png</albedo_map> -->
<!-- <normal_map>model://laser/textures/shildik_sh_n.png</normal_map> -->
<!-- <roughness_map>model://laser/textures/shildik_sh_r.png</roughness_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> -->
<!-- </metal> -->
<!-- </pbr> -->
</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>

71
rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae
View file

@ -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>
<modified>2025-02-12T10:55:51</modified>
<created>2025-03-03T12:34:37</created>
<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>

27
rbs_mill_assist/assets/shildik/model.sdf
View file

@ -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>
<diffuse_map>model://shildik/textures/shildik_sh.png</diffuse_map>
</texture>
<!-- <texture> -->
<!-- <diffuse_map>model://shildik/textures/shildik_sh.png</diffuse_map> -->
<!-- </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>
<contact>
<ode />
</contact>
<bounce />
<friction>
<ode />
</friction>
</surface>
<!-- <surface> -->
<!-- <contact> -->
<!-- <ode /> -->
<!-- </contact> -->
<!-- <bounce /> -->
<!-- <friction> -->
<!-- <ode /> -->
<!-- </friction> -->
<!-- </surface> -->
</collision>
</link>
</model>

28
rbs_mill_assist/bt/CMakeLists.txt
View file

@ -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)
list(APPEND plugin_libs get_grasp_place_pose)
add_library(get_grasp_poses SHARED plugins/get_grasp_pose.cpp)
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})

3
rbs_mill_assist/bt/config/bt_executor.yaml
View file

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

118
rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp
View file

@ -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");

89
rbs_mill_assist/bt/plugins/get_grasp_pose.cpp Normal file
View file

@ -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");

78
rbs_mill_assist/bt/plugins/get_named_pose.cpp Normal file
View file

@ -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");

90
rbs_mill_assist/bt/plugins/get_place_pose.cpp Normal file
View file

@ -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");

48
rbs_mill_assist/bt/plugins/is_in_pose.cpp Normal file
View file

@ -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");

54
rbs_mill_assist/bt/plugins/takeon_task.cpp Normal file
View file

@ -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");

40
rbs_mill_assist/bt/plugins/task_completed.cpp Normal file
View file

@ -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");

85
rbs_mill_assist/bt/plugins/twist_cmd.cpp Normal file
View file

@ -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");

8
rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp
View file

@ -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(),
"Failed to get sending data from input port");
RCLCPP_ERROR(this->logger(),
"[%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;
}

21
rbs_mill_assist/bt/xmls/Grasp.xml
View file

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

9
rbs_mill_assist/bt/xmls/MainTree.xml
View file

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

21
rbs_mill_assist/bt/xmls/Place.xml
View file

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

37
rbs_mill_assist/bt/xmls/moveit/Grasp.xml Normal file
View file

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

136
rbs_mill_assist/bt/xmls/moveit/MainTree.xml Normal file
View file

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

9
rbs_mill_assist/bt/xmls/moveit/PickAndPlace.xml Normal file
View file

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

27
rbs_mill_assist/bt/xmls/moveit/Place.xml Normal file
View file

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

10
rbs_mill_assist/bt/xmls/moveit/btmill.btproj Normal file
View file

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

34
rbs_mill_assist/bt/xmls/no_moveit/FromGraver.xml Normal file
View file

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

36
rbs_mill_assist/bt/xmls/no_moveit/GraspNoMoveit.xml Normal file
View file

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

116
rbs_mill_assist/bt/xmls/no_moveit/MainTree.xml Normal file
View file

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

28
rbs_mill_assist/bt/xmls/no_moveit/PlaceNoMoveit.xml Normal file
View file

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

28
rbs_mill_assist/bt/xmls/no_moveit/ToGraver.xml Normal file
View file

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

60
rbs_mill_assist/config/grasping.yaml
View file

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

18
rbs_mill_assist/config/gz_bridge.yaml
View file

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

38
rbs_mill_assist/config/obj_cfg.json Normal file
View file

@ -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", "..."]
}
]

93
rbs_mill_assist/config/skills.json Normal file
View file

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

855
rbs_mill_assist/config/template_obj_1.svg Normal file
View file

File diff suppressed because one or more lines are too long
Side by side

After

Width:  |  Height:  |  Size: 177 KiB

855
rbs_mill_assist/config/template_obj_2.svg Normal file
View file

File diff suppressed because one or more lines are too long
Side by side

After

Width:  |  Height:  |  Size: 177 KiB

9
rbs_mill_assist/include/rbs_mill_assist/VacuumGripper.hpp
View file

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

176
rbs_mill_assist/include/rbs_mill_assist/places_config.hpp Normal file
View file

@ -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_;
};

82
rbs_mill_assist/include/rbs_mill_assist/ref_frames_config.hpp Normal file
View file

@ -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");
}
}
};

201
rbs_mill_assist/launch/moveit.launch.py Normal file
View file

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

114
rbs_mill_assist/launch/simulation.launch.py
View file

@ -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(
get_package_share_directory(moveit_config_package.perform(context)),
"srdf",
"xacro_args.yaml",
package_dir = get_package_share_directory(
moveit_config_package.perform(context)
)
srdf_file = os.path.join(
get_package_share_directory(moveit_config_package.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=" ")
package_dir, "srdf", moveit_config_file.perform(context)
)
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(
[os.path.join(get_package_share_directory('ros_gz_sim'),
'launch', 'gz_sim.launch.py')]),
launch_arguments=[('gz_args', [' -r -v 4 ', gazebo_world])],
PythonLaunchDescriptionSource(
[
os.path.join(
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",
),
)

BIN
rbs_mill_assist/meshes/Link6.STL
View file

Binary file not shown.

BIN
rbs_mill_assist/meshes/Link7.STL
View file

Binary file not shown.

BIN
rbs_mill_assist/meshes/Link8.STL
View file

Binary file not shown.

9
rbs_mill_assist/moveit/initial_positions.yaml Normal file
View file

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

Some files were not shown because too many files have changed in this diff Show more