diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..49165bf --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +*.blend1 +*.vscode +**/__pycache__/** diff --git a/docs/bt_example_with_tasks.md b/docs/bt_example_with_tasks.md new file mode 100644 index 0000000..4c7bf22 --- /dev/null +++ b/docs/bt_example_with_tasks.md @@ -0,0 +1,165 @@ +Для реализации обработки очереди задач в сценарии, описанном вами, можно использовать **Behavior Tree (BT)** — дерево поведения. Это мощный инструмент для управления сложными последовательностями действий, особенно в робототехнике и автоматизированных системах. Давайте разберем, как это можно сделать. + +--- + +### **Основные компоненты Behavior Tree** +1. **Action Node**: + - Выполняет конкретное действие. + - Например: "Добавить задачу в очередь", "Выполнить гравировку". + +2. **Control Node**: + - Управляет выполнением дочерних узлов. + - Примеры: + - **Sequence**: Выполняет дочерние узлы по порядку. Если один узел завершается неудачно, вся последовательность останавливается. + - **Selector**: Пытается выполнить дочерние узлы по порядку. Если один узел завершается успешно, остальные игнорируются. + - **Parallel**: Выполняет несколько узлов одновременно. + +3. **Condition Node**: + - Проверяет условие (например: "Есть ли задачи в очереди?"). + - Возвращает успех или неудачу в зависимости от результата проверки. + +4. **Decorator Node**: + - Модифицирует поведение дочернего узла. + - Например: "Повторять действие, пока условие истинно". + +--- + +### **Алгоритм работы системы** +1. Оператор вводит данные в веб-интерфейс, создавая спецификацию задачи. +2. Спецификация добавляется в очередь задач. +3. Управляющая программа РТК (роботизированный технологический комплекс) обрабатывает задачи из очереди. +4. Для каждой задачи выполняется алгоритм гравировки: + - Подготовка оборудования. + - Гравировка шильдика. + - Проверка качества выполнения. + +--- + +### **Реализация в Behavior Tree** + +#### 1. **Основная структура дерева** +Мы можем разделить процесс на две основные части: +- **Обработка очереди задач**. +- **Выполнение гравировки**. + +#### 2. **Control Nodes** +- **Selector**: Используется для выбора между "обработкой очереди" и "выполнением гравировки". +- **Sequence**: Используется для выполнения последовательных шагов внутри каждой задачи. +- **Parallel**: Может быть использован, если нужно одновременно мониторить состояние очереди и выполнять текущую задачу. + +#### 3. **Action Nodes** +- **Добавление задачи в очередь**: Получает данные из веб-интерфейса и добавляет их в очередь. +- **Проверка очереди**: Проверяет, есть ли задачи в очереди. +- **Подготовка оборудования**: Настройка оборудования для гравировки. +- **Гравировка**: Выполнение гравировки на основе данных задачи. +- **Проверка качества**: Проверка результатов гравировки. + +#### 4. **Condition Nodes** +- **Есть ли задачи в очереди?**: Возвращает `true`, если очередь не пуста. +- **Задача выполнена успешно?**: Проверяет, завершилась ли гравировка без ошибок. + +#### 5. **Decorator Nodes** +- **Repeat Until Fail**: Повторяет проверку очереди, пока она не станет пустой. +- **Retry**: Повторяет выполнение гравировки в случае ошибки. + +--- + +### **Пример Behavior Tree** + +```plaintext +Root +│ +├── Selector (Выбор между обработкой очереди и выполнением гравировки) +│ ├── Sequence (Обработка очереди задач) +│ │ ├── Condition: Есть ли задачи в очереди? +│ │ └── Action: Добавить задачу в очередь +│ │ +│ └── Sequence (Выполнение гравировки) +│ ├── Action: Подготовка оборудования +│ ├── Action: Гравировка +│ ├── Condition: Задача выполнена успешно? +│ └── Action: Проверка качества +│ +└── Decorator: Repeat Until Fail (Повторять, пока очередь не пуста) +``` + +--- + +### **Пошаговое объяснение работы дерева** + +1. **Selector**: + - Проверяет, есть ли задачи в очереди. + - Если очередь пуста, ничего не делает. + - Если очередь не пуста, переходит к выполнению гравировки. + +2. **Sequence (Обработка очереди задач)**: + - Убедиться, что задачи есть в очереди. + - Добавить новую задачу в очередь (если оператор ввел данные). + +3. **Sequence (Выполнение гравировки)**: + - Подготовить оборудование. + - Выполнить гравировку. + - Проверить качество выполнения. + +4. **Decorator: Repeat Until Fail**: + - Повторяет весь процесс, пока очередь задач не станет пустой. + +--- + +### **Преимущества такого подхода** +1. **Модульность**: Каждый узел отвечает за конкретную задачу, что упрощает отладку и расширение. +2. **Гибкость**: Можно легко добавить новые условия или действия. +3. **Надежность**: Дерево может обрабатывать ошибки и повторять действия при необходимости. + +--- + +### **Пример кода (псевдокод)** + +```python +class BehaviorTree: + def __init__(self): + self.queue = [] + + def check_queue(self): + return len(self.queue) > 0 + + def add_task_to_queue(self, task): + self.queue.append(task) + + def prepare_equipment(self): + print("Equipment prepared.") + + def engrave(self, task): + print(f"Engraving task: {task}") + + def check_quality(self): + print("Quality check passed.") + + def run(self): + while True: + if not self.check_queue(): + print("Queue is empty. Waiting for tasks...") + break + + task = self.queue.pop(0) + self.prepare_equipment() + self.engrave(task) + self.check_quality() + +# Пример использования +bt = BehaviorTree() +bt.add_task_to_queue("Task 1") +bt.add_task_to_queue("Task 2") +bt.run() +``` + +--- + +### **Итог** +Для реализации обработки очереди задач в вашем сценарии можно использовать следующие узлы: +- **Control Nodes**: `Selector`, `Sequence`, `Parallel`. +- **Action Nodes**: Добавление задачи, подготовка оборудования, гравировка, проверка качества. +- **Condition Nodes**: Проверка наличия задач в очереди, проверка успешности выполнения. +- **Decorator Nodes**: Повторение действий до определенного условия. + +Этот подход обеспечивает четкое разделение логики и позволяет легко масштабировать систему. \ No newline at end of file diff --git a/docs/objects.json b/docs/objects.json new file mode 100644 index 0000000..d32b612 --- /dev/null +++ b/docs/objects.json @@ -0,0 +1,38 @@ +[ + { + "obj_id": 1, + "place_name": "bunker_1", + "place_aruco": 21, + "place_pos": "{pose}", + "graver_template": "{file}", + "dimensions": "{X,Y,Z}", + "skills": ["sid", "..."] + }, + { + "obj_id": 2, + "place_name": "bunker_2", + "place_aruco": 22, + "place_pos": "{pose}", + "graver_template": "{file}", + "dimensions": "{X,Y,Z}", + "skills": ["sid", "..."] + }, + { + "obj_id": 3, + "place_name": "bunker_3", + "place_aruco": 23, + "place_pos": "{pose}", + "graver_template": "{file}", + "dimensions": "{X,Y,Z}", + "skills": ["sid", "..."] + }, + { + "obj_id": 4, + "place_name": "bunker_4", + "place_aruco": 24, + "place_pos": "{pose}", + "graver_template": "{file}", + "dimensions": "{X,Y,Z}", + "skills": ["sid", "..."] + } +] \ No newline at end of file diff --git a/docs/shildik_templates/README.md b/docs/shildik_templates/README.md new file mode 100644 index 0000000..e0f3e5b --- /dev/null +++ b/docs/shildik_templates/README.md @@ -0,0 +1,16 @@ +# Шаблоны изображений шильдиков + +Директория содержит шаблоны шильдиков в двух вариантах: +1. С надписями в виде редактируемого текста в директории `text`. Для корректного отображения должны быть установлены в системе или в редакторе шрифты. +2. С надписями в виде кривых в директории `curves`. + +Открывать рекомендуется в свободном редакторе векторной графики [Inkscape](https://inkscape.org) + +## Типы шильдиков и наборы полей данных + +| Тип устройства | Перечень параметров | +| - | - | +| Насос общепромышленный | Наименование, Q - подача, n - частота оборотов, H - напор, m - масса, P2 - мощность, T - диапазон температур, Дата производства, Заводской номер | +| Насос взрывозащищённый (Ex) | Наименование, Q - подача, n - частота оборотов, H - напор, m - масса, P2 - мощность, T - диапазон температур, Дата производства, Заводской номер, Диапазон ta в градусах цельсия | +| Насосная установка| Наименование, Q - подача, H - напор, m - масса, P - мощность, Кол-во насосов, Дата производства, Заводской номер | +| Шкаф управления | Наименование, Iоб, Iсум, IP, Кол-во электродвигателей, Дата производства, Заводской номер | diff --git a/docs/shildik_templates/curves/Табличка_пустая_Ex.svg b/docs/shildik_templates/curves/Табличка_пустая_Ex.svg new file mode 100644 index 0000000..89dd0c7 --- /dev/null +++ b/docs/shildik_templates/curves/Табличка_пустая_Ex.svg @@ -0,0 +1,899 @@ + +image/svg+xml diff --git a/docs/shildik_templates/curves/Табличка_пустая_Насосная_установка.svg b/docs/shildik_templates/curves/Табличка_пустая_Насосная_установка.svg new file mode 100644 index 0000000..260e10f --- /dev/null +++ b/docs/shildik_templates/curves/Табличка_пустая_Насосная_установка.svg @@ -0,0 +1,934 @@ + +image/svg+xml diff --git a/docs/shildik_templates/curves/Табличка_пустая_Общепром.svg b/docs/shildik_templates/curves/Табличка_пустая_Общепром.svg new file mode 100644 index 0000000..9567994 --- /dev/null +++ b/docs/shildik_templates/curves/Табличка_пустая_Общепром.svg @@ -0,0 +1,412 @@ + +image/svg+xml diff --git a/docs/shildik_templates/curves/Табличка_пустая_Шкаф.svg b/docs/shildik_templates/curves/Табличка_пустая_Шкаф.svg new file mode 100644 index 0000000..efce9f3 --- /dev/null +++ b/docs/shildik_templates/curves/Табличка_пустая_Шкаф.svg @@ -0,0 +1,899 @@ + +image/svg+xml diff --git a/docs/shildik_templates/fonts/GOST 2.304 type A.ttf b/docs/shildik_templates/fonts/GOST 2.304 type A.ttf new file mode 100644 index 0000000..436f92a Binary files /dev/null and b/docs/shildik_templates/fonts/GOST 2.304 type A.ttf differ diff --git a/docs/shildik_templates/fonts/GOST 2.304 type B.ttf b/docs/shildik_templates/fonts/GOST 2.304 type B.ttf new file mode 100644 index 0000000..884b2fe Binary files /dev/null and b/docs/shildik_templates/fonts/GOST 2.304 type B.ttf differ diff --git a/docs/shildik_templates/test_templates/Табличка_пустая_Ex.svg b/docs/shildik_templates/test_templates/Табличка_пустая_Ex.svg new file mode 100644 index 0000000..6c72a8a --- /dev/null +++ b/docs/shildik_templates/test_templates/Табличка_пустая_Ex.svg @@ -0,0 +1,855 @@ + +image/svg+xml1КММ-ХА 80-50-200б/2/18,5-E-55T/BBQV-PC-У32,3SN21216121624.12.21251,2954 diff --git a/docs/shildik_templates/test_templates/Табличка_пустая_Ex_plain.svg b/docs/shildik_templates/test_templates/Табличка_пустая_Ex_plain.svg new file mode 100644 index 0000000..859107f --- /dev/null +++ b/docs/shildik_templates/test_templates/Табличка_пустая_Ex_plain.svg @@ -0,0 +1,932 @@ + +image/svg+xmlКММ-ХА 80-50-200б/2/18,5-E-55T/BBQV-PC-У32 diff --git a/docs/shildik_templates/text/Табличка_пустая_Ex.svg b/docs/shildik_templates/text/Табличка_пустая_Ex.svg new file mode 100644 index 0000000..ab959dd --- /dev/null +++ b/docs/shildik_templates/text/Табличка_пустая_Ex.svg @@ -0,0 +1,657 @@ + +image/svg+xmlг.Долгопрудный,Лихачевский пр-д, 26тел. (495) 642-72-10ООО "ПРОММАШ ТЕСТ Инжиниринг" № ЕАЭС RU C-RU.АЖ58.В.02981/22Сделано в РоссииQм3/чnоб/минT°C°CДатагод HмmкгЗав.№P2кВтТУ 28.13.14-001-23519199-2018 II Gb c IIB Т3/Т4 X≤ ta ≤ diff --git a/docs/shildik_templates/text/Табличка_пустая_Насосная_установка.svg b/docs/shildik_templates/text/Табличка_пустая_Насосная_установка.svg new file mode 100644 index 0000000..aa8a06f --- /dev/null +++ b/docs/shildik_templates/text/Табличка_пустая_Насосная_установка.svg @@ -0,0 +1,912 @@ + +image/svg+xml diff --git a/docs/shildik_templates/text/Табличка_пустая_Общепром.svg b/docs/shildik_templates/text/Табличка_пустая_Общепром.svg new file mode 100644 index 0000000..ff439e9 --- /dev/null +++ b/docs/shildik_templates/text/Табличка_пустая_Общепром.svg @@ -0,0 +1,578 @@ + +image/svg+xmlг.Долгопрудный,Лихачевский пр-д, 26тел. (495) 642-72-10Qм3/чnоб/минT°CДатагод HмmкгЗав.№P2кВтТУ 28.13.14-001-23519199-2018 Сделано в России diff --git a/docs/shildik_templates/text/Табличка_пустая_Шкаф.svg b/docs/shildik_templates/text/Табличка_пустая_Шкаф.svg new file mode 100644 index 0000000..a29c61c --- /dev/null +++ b/docs/shildik_templates/text/Табличка_пустая_Шкаф.svg @@ -0,0 +1,877 @@ + +image/svg+xml diff --git a/docs/tasks.csv b/docs/tasks.csv new file mode 100644 index 0000000..1c7ad06 --- /dev/null +++ b/docs/tasks.csv @@ -0,0 +1,6 @@ +obj_id,device_type,device_name,supply,freq,napor,mass,power,temp_range,date,serial_num,ta_range,pump_num,i_ob,i_sum,ip,engine_num +Object ID,Тип устройства,Наименование,"Q - подача, м³/ч",n - частота оборотов,"H - напор, м",m - масса,P2 - мощность,T - диапазон температур,Дата производства,Заводской номер,Диапазон ta в градусах цельсия,Кол-во насосов,Iоб,Iсум,IP,Кол-во электродвигателей +1,Насос общепромышленный,"КММ-ХА 80-50-200б/2/18,5-Е-55Т/BBQV-HC-У3",50,1450 об/мин,30,200.0 кг,15.0 кВт,-10...+80°C,01-15-23,SN12345,,,,,, +2,Насос взрывозащищённый (Ex),"КММ-ХА 80-50-200б/2/18,5-Е-55Т/BBQV-HC-У3",30 м³/ч,1450 об/мин,25,180.0 кг,10.0 кВт,-20...+60°C,02-10-23,SN12346,-40...+85°C,,,,, +3,Насосная установка,,100 м³/ч,,50,500.0 кг,30.0 кВт,,03-01-23,SN12347,2,,55,4, +4,Шкаф управления,,,,,,,,04-05-23,SN12348,,,30.0A,50.0A,IP54,3 diff --git a/docs/to_start_BT_main.md b/docs/to_start_BT_main.md new file mode 100644 index 0000000..a376bab --- /dev/null +++ b/docs/to_start_BT_main.md @@ -0,0 +1,25 @@ +1) runtime CNC-Graver Assist +```sh +ros2 launch rbs_mill_assist simulation.launch.py +``` +2) интерфейсная нода (bt_path: путь к skills.json) +```sh +ros2 launch rbs_bt_executor interface.launch.py bt_path:=/home//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//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}" +``` \ No newline at end of file diff --git a/rbs_mill_assist/CMakeLists.txt b/rbs_mill_assist/CMakeLists.txt index 84deb0d..ce3bc50 100644 --- a/rbs_mill_assist/CMakeLists.txt +++ b/rbs_mill_assist/CMakeLists.txt @@ -7,53 +7,16 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_srvs REQUIRED) -find_package(gz-cmake3 REQUIRED) -find_package(gz-plugin2 REQUIRED COMPONENTS register) -set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) -find_package(gz-common5 REQUIRED COMPONENTS profiler) -set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) - -# Harmonic -if("$ENV{GZ_VERSION}" STREQUAL "harmonic") - find_package(gz-sim8 REQUIRED) - set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) - message(STATUS "Compiling against Gazebo Harmonic") -# Default to Garden -else() - find_package(gz-sim7 REQUIRED) - set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR}) - message(STATUS "Compiling against Gazebo Garden") -endif() - -add_library(VacuumGripper - SHARED - src/VacuumGripper.cpp -) include_directories(include) -target_link_libraries(VacuumGripper - 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) diff --git a/rbs_mill_assist/assets/aruco_1/meshes/aruco_marker_21.png b/rbs_mill_assist/assets/aruco_1/meshes/aruco_marker_21.png new file mode 100644 index 0000000..8096159 Binary files /dev/null and b/rbs_mill_assist/assets/aruco_1/meshes/aruco_marker_21.png differ diff --git a/rbs_mill_assist/assets/aruco_1/meshes/aruco_template.mtl b/rbs_mill_assist/assets/aruco_1/meshes/aruco_template.mtl new file mode 100644 index 0000000..b129471 --- /dev/null +++ b/rbs_mill_assist/assets/aruco_1/meshes/aruco_template.mtl @@ -0,0 +1,12 @@ +# Blender 4.2.1 LTS MTL File: 'aruco_template.blend' +# www.blender.org + +newmtl aruco_tag_124 +Ns 0.000000 +Ka 1.000000 1.000000 1.000000 +Ks 0.000000 0.000000 0.000000 +Ke 0.000000 0.000000 0.000000 +Ni 1.500000 +d 1.000000 +illum 1 +map_Kd aruco_marker_21.png diff --git a/rbs_mill_assist/assets/aruco_1/meshes/aruco_template.obj b/rbs_mill_assist/assets/aruco_1/meshes/aruco_template.obj new file mode 100644 index 0000000..8b8dc1a --- /dev/null +++ b/rbs_mill_assist/assets/aruco_1/meshes/aruco_template.obj @@ -0,0 +1,48 @@ +# Blender 4.2.1 LTS +# www.blender.org +mtllib aruco_template.mtl +o aruco_tag_124 +v -0.000464 0.000314 0.039912 +v 0.039536 0.000314 0.039912 +v 0.039536 0.000314 -0.000088 +v -0.000464 0.000314 -0.000088 +v -0.000464 0.001314 0.039912 +v 0.039536 0.001314 0.039912 +v 0.039536 0.001314 -0.000088 +v -0.000464 0.001314 -0.000088 +vn -0.0000 -1.0000 -0.0000 +vn -0.0000 1.0000 -0.0000 +vn 0.9999 0.0125 -0.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.9997 -0.0250 -0.0000 +vn -1.0000 -0.0080 -0.0000 +vn 1.0000 -0.0000 -0.0000 +vn 1.0000 0.0080 -0.0000 +vn 0.9997 0.0250 -0.0000 +vn -0.9999 -0.0125 -0.0000 +vn -0.0000 0.0125 -0.9999 +vn -0.0000 -0.0000 1.0000 +vn -0.0000 -0.0250 0.9997 +vn -0.0000 -0.0080 1.0000 +vn -0.0000 -0.0000 -1.0000 +vn -0.0000 0.0080 -1.0000 +vn -0.0000 0.0250 -0.9997 +vn -0.0000 -0.0125 0.9999 +vt 0.000000 0.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +s 0 +usemtl aruco_tag_124 +f 1/1/1 2/2/1 3/3/1 +f 1/1/1 3/3/1 4/4/1 +f 5/1/2 6/2/2 7/3/2 +f 5/1/2 7/3/2 8/4/2 +f 1/1/3 2/2/4 6/3/5 +f 1/1/6 6/3/4 5/4/4 +f 4/1/7 3/2/7 7/3/8 +f 4/1/9 7/3/10 8/4/7 +f 1/1/11 4/2/12 8/3/13 +f 1/1/14 8/3/12 5/4/12 +f 2/1/15 3/2/15 7/3/16 +f 2/1/17 7/3/18 6/4/15 diff --git a/rbs_mill_assist/assets/aruco_1/model.config b/rbs_mill_assist/assets/aruco_1/model.config new file mode 100644 index 0000000..a8f06d1 --- /dev/null +++ b/rbs_mill_assist/assets/aruco_1/model.config @@ -0,0 +1,17 @@ + + + aruco_1 + 1.0 + model.sdf + + 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. + + + Yoan Mollard + opensource@aubrune.eu + + http://www.apache.org/licenses/LICENSE-2.0 + Other + ARUco, tag, marker, Gazebo + diff --git a/rbs_mill_assist/assets/aruco_1/model.sdf b/rbs_mill_assist/assets/aruco_1/model.sdf new file mode 100644 index 0000000..0409e46 --- /dev/null +++ b/rbs_mill_assist/assets/aruco_1/model.sdf @@ -0,0 +1,16 @@ + + + + true + 0 0 0 0 0 0 + + + + + model://aruco_1/meshes/aruco_template.obj + + + + + + diff --git a/rbs_mill_assist/assets/aruco_2/meshes/aruco_marker_22.png b/rbs_mill_assist/assets/aruco_2/meshes/aruco_marker_22.png new file mode 100644 index 0000000..b6a5e40 Binary files /dev/null and b/rbs_mill_assist/assets/aruco_2/meshes/aruco_marker_22.png differ diff --git a/rbs_mill_assist/assets/aruco_2/meshes/aruco_template.mtl b/rbs_mill_assist/assets/aruco_2/meshes/aruco_template.mtl new file mode 100644 index 0000000..a048f46 --- /dev/null +++ b/rbs_mill_assist/assets/aruco_2/meshes/aruco_template.mtl @@ -0,0 +1,13 @@ +# Blender 4.2.1 LTS MTL File: 'aruco_template.blend' +# www.blender.org + +newmtl aruco_tag_124 +Ns 0.000000 +Ka 1.000000 1.000000 1.000000 +Ks 0.000000 0.000000 0.000000 +Ke 0.000000 0.000000 0.000000 +Ni 1.500000 +d 1.000000 +illum 1 +map_Kd aruco_marker_22.png +map_Ka aruco_marker_22.png diff --git a/rbs_mill_assist/assets/aruco_2/meshes/aruco_template.obj b/rbs_mill_assist/assets/aruco_2/meshes/aruco_template.obj new file mode 100644 index 0000000..8b8dc1a --- /dev/null +++ b/rbs_mill_assist/assets/aruco_2/meshes/aruco_template.obj @@ -0,0 +1,48 @@ +# Blender 4.2.1 LTS +# www.blender.org +mtllib aruco_template.mtl +o aruco_tag_124 +v -0.000464 0.000314 0.039912 +v 0.039536 0.000314 0.039912 +v 0.039536 0.000314 -0.000088 +v -0.000464 0.000314 -0.000088 +v -0.000464 0.001314 0.039912 +v 0.039536 0.001314 0.039912 +v 0.039536 0.001314 -0.000088 +v -0.000464 0.001314 -0.000088 +vn -0.0000 -1.0000 -0.0000 +vn -0.0000 1.0000 -0.0000 +vn 0.9999 0.0125 -0.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.9997 -0.0250 -0.0000 +vn -1.0000 -0.0080 -0.0000 +vn 1.0000 -0.0000 -0.0000 +vn 1.0000 0.0080 -0.0000 +vn 0.9997 0.0250 -0.0000 +vn -0.9999 -0.0125 -0.0000 +vn -0.0000 0.0125 -0.9999 +vn -0.0000 -0.0000 1.0000 +vn -0.0000 -0.0250 0.9997 +vn -0.0000 -0.0080 1.0000 +vn -0.0000 -0.0000 -1.0000 +vn -0.0000 0.0080 -1.0000 +vn -0.0000 0.0250 -0.9997 +vn -0.0000 -0.0125 0.9999 +vt 0.000000 0.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +s 0 +usemtl aruco_tag_124 +f 1/1/1 2/2/1 3/3/1 +f 1/1/1 3/3/1 4/4/1 +f 5/1/2 6/2/2 7/3/2 +f 5/1/2 7/3/2 8/4/2 +f 1/1/3 2/2/4 6/3/5 +f 1/1/6 6/3/4 5/4/4 +f 4/1/7 3/2/7 7/3/8 +f 4/1/9 7/3/10 8/4/7 +f 1/1/11 4/2/12 8/3/13 +f 1/1/14 8/3/12 5/4/12 +f 2/1/15 3/2/15 7/3/16 +f 2/1/17 7/3/18 6/4/15 diff --git a/rbs_mill_assist/assets/aruco_2/model.config b/rbs_mill_assist/assets/aruco_2/model.config new file mode 100644 index 0000000..c9e79b1 --- /dev/null +++ b/rbs_mill_assist/assets/aruco_2/model.config @@ -0,0 +1,17 @@ + + + aruco_2 + 1.0 + model.sdf + + 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. + + + Yoan Mollard + opensource@aubrune.eu + + http://www.apache.org/licenses/LICENSE-2.0 + Other + ARUco, tag, marker, Gazebo + diff --git a/rbs_mill_assist/assets/aruco_2/model.sdf b/rbs_mill_assist/assets/aruco_2/model.sdf new file mode 100644 index 0000000..24b1028 --- /dev/null +++ b/rbs_mill_assist/assets/aruco_2/model.sdf @@ -0,0 +1,16 @@ + + + + true + 0 0 0 0 0 0 + + + + + model://aruco_2/meshes/aruco_template.obj + + + + + + diff --git a/rbs_mill_assist/assets/aruco_3/meshes/aruco_marker_23.png b/rbs_mill_assist/assets/aruco_3/meshes/aruco_marker_23.png new file mode 100644 index 0000000..377fd6d Binary files /dev/null and b/rbs_mill_assist/assets/aruco_3/meshes/aruco_marker_23.png differ diff --git a/rbs_mill_assist/assets/aruco_3/meshes/aruco_template.mtl b/rbs_mill_assist/assets/aruco_3/meshes/aruco_template.mtl new file mode 100644 index 0000000..065c8da --- /dev/null +++ b/rbs_mill_assist/assets/aruco_3/meshes/aruco_template.mtl @@ -0,0 +1,12 @@ +# Blender 4.2.1 LTS MTL File: 'aruco_template.blend' +# www.blender.org + +newmtl aruco_tag_124 +Ns 0.000000 +Ka 1.000000 1.000000 1.000000 +Ks 0.000000 0.000000 0.000000 +Ke 0.000000 0.000000 0.000000 +Ni 1.500000 +d 1.000000 +illum 1 +map_Kd aruco_marker_23.png diff --git a/rbs_mill_assist/assets/aruco_3/meshes/aruco_template.obj b/rbs_mill_assist/assets/aruco_3/meshes/aruco_template.obj new file mode 100644 index 0000000..8b8dc1a --- /dev/null +++ b/rbs_mill_assist/assets/aruco_3/meshes/aruco_template.obj @@ -0,0 +1,48 @@ +# Blender 4.2.1 LTS +# www.blender.org +mtllib aruco_template.mtl +o aruco_tag_124 +v -0.000464 0.000314 0.039912 +v 0.039536 0.000314 0.039912 +v 0.039536 0.000314 -0.000088 +v -0.000464 0.000314 -0.000088 +v -0.000464 0.001314 0.039912 +v 0.039536 0.001314 0.039912 +v 0.039536 0.001314 -0.000088 +v -0.000464 0.001314 -0.000088 +vn -0.0000 -1.0000 -0.0000 +vn -0.0000 1.0000 -0.0000 +vn 0.9999 0.0125 -0.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.9997 -0.0250 -0.0000 +vn -1.0000 -0.0080 -0.0000 +vn 1.0000 -0.0000 -0.0000 +vn 1.0000 0.0080 -0.0000 +vn 0.9997 0.0250 -0.0000 +vn -0.9999 -0.0125 -0.0000 +vn -0.0000 0.0125 -0.9999 +vn -0.0000 -0.0000 1.0000 +vn -0.0000 -0.0250 0.9997 +vn -0.0000 -0.0080 1.0000 +vn -0.0000 -0.0000 -1.0000 +vn -0.0000 0.0080 -1.0000 +vn -0.0000 0.0250 -0.9997 +vn -0.0000 -0.0125 0.9999 +vt 0.000000 0.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +s 0 +usemtl aruco_tag_124 +f 1/1/1 2/2/1 3/3/1 +f 1/1/1 3/3/1 4/4/1 +f 5/1/2 6/2/2 7/3/2 +f 5/1/2 7/3/2 8/4/2 +f 1/1/3 2/2/4 6/3/5 +f 1/1/6 6/3/4 5/4/4 +f 4/1/7 3/2/7 7/3/8 +f 4/1/9 7/3/10 8/4/7 +f 1/1/11 4/2/12 8/3/13 +f 1/1/14 8/3/12 5/4/12 +f 2/1/15 3/2/15 7/3/16 +f 2/1/17 7/3/18 6/4/15 diff --git a/rbs_mill_assist/assets/aruco_3/model.config b/rbs_mill_assist/assets/aruco_3/model.config new file mode 100644 index 0000000..f8d0193 --- /dev/null +++ b/rbs_mill_assist/assets/aruco_3/model.config @@ -0,0 +1,17 @@ + + + aruco_3 + 1.0 + model.sdf + + 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. + + + Yoan Mollard + opensource@aubrune.eu + + http://www.apache.org/licenses/LICENSE-2.0 + Other + ARUco, tag, marker, Gazebo + diff --git a/rbs_mill_assist/assets/aruco_3/model.sdf b/rbs_mill_assist/assets/aruco_3/model.sdf new file mode 100644 index 0000000..3389aa2 --- /dev/null +++ b/rbs_mill_assist/assets/aruco_3/model.sdf @@ -0,0 +1,16 @@ + + + + true + 0 0 0 0 0 0 + + + + + model://aruco_3/meshes/aruco_template.obj + + + + + + diff --git a/rbs_mill_assist/assets/aruco_4/meshes/aruco_marker_24.png b/rbs_mill_assist/assets/aruco_4/meshes/aruco_marker_24.png new file mode 100644 index 0000000..0654fae Binary files /dev/null and b/rbs_mill_assist/assets/aruco_4/meshes/aruco_marker_24.png differ diff --git a/rbs_mill_assist/assets/aruco_4/meshes/aruco_template.mtl b/rbs_mill_assist/assets/aruco_4/meshes/aruco_template.mtl new file mode 100644 index 0000000..0803561 --- /dev/null +++ b/rbs_mill_assist/assets/aruco_4/meshes/aruco_template.mtl @@ -0,0 +1,12 @@ +# Blender 4.2.1 LTS MTL File: 'aruco_template.blend' +# www.blender.org + +newmtl aruco_tag_124 +Ns 0.000000 +Ka 1.000000 1.000000 1.000000 +Ks 0.000000 0.000000 0.000000 +Ke 0.000000 0.000000 0.000000 +Ni 1.500000 +d 1.000000 +illum 1 +map_Kd aruco_marker_24.png diff --git a/rbs_mill_assist/assets/aruco_4/meshes/aruco_template.obj b/rbs_mill_assist/assets/aruco_4/meshes/aruco_template.obj new file mode 100644 index 0000000..8b8dc1a --- /dev/null +++ b/rbs_mill_assist/assets/aruco_4/meshes/aruco_template.obj @@ -0,0 +1,48 @@ +# Blender 4.2.1 LTS +# www.blender.org +mtllib aruco_template.mtl +o aruco_tag_124 +v -0.000464 0.000314 0.039912 +v 0.039536 0.000314 0.039912 +v 0.039536 0.000314 -0.000088 +v -0.000464 0.000314 -0.000088 +v -0.000464 0.001314 0.039912 +v 0.039536 0.001314 0.039912 +v 0.039536 0.001314 -0.000088 +v -0.000464 0.001314 -0.000088 +vn -0.0000 -1.0000 -0.0000 +vn -0.0000 1.0000 -0.0000 +vn 0.9999 0.0125 -0.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.9997 -0.0250 -0.0000 +vn -1.0000 -0.0080 -0.0000 +vn 1.0000 -0.0000 -0.0000 +vn 1.0000 0.0080 -0.0000 +vn 0.9997 0.0250 -0.0000 +vn -0.9999 -0.0125 -0.0000 +vn -0.0000 0.0125 -0.9999 +vn -0.0000 -0.0000 1.0000 +vn -0.0000 -0.0250 0.9997 +vn -0.0000 -0.0080 1.0000 +vn -0.0000 -0.0000 -1.0000 +vn -0.0000 0.0080 -1.0000 +vn -0.0000 0.0250 -0.9997 +vn -0.0000 -0.0125 0.9999 +vt 0.000000 0.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +s 0 +usemtl aruco_tag_124 +f 1/1/1 2/2/1 3/3/1 +f 1/1/1 3/3/1 4/4/1 +f 5/1/2 6/2/2 7/3/2 +f 5/1/2 7/3/2 8/4/2 +f 1/1/3 2/2/4 6/3/5 +f 1/1/6 6/3/4 5/4/4 +f 4/1/7 3/2/7 7/3/8 +f 4/1/9 7/3/10 8/4/7 +f 1/1/11 4/2/12 8/3/13 +f 1/1/14 8/3/12 5/4/12 +f 2/1/15 3/2/15 7/3/16 +f 2/1/17 7/3/18 6/4/15 diff --git a/rbs_mill_assist/assets/aruco_4/model.config b/rbs_mill_assist/assets/aruco_4/model.config new file mode 100644 index 0000000..e1c5682 --- /dev/null +++ b/rbs_mill_assist/assets/aruco_4/model.config @@ -0,0 +1,17 @@ + + + aruco_4 + 1.0 + model.sdf + + 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. + + + Yoan Mollard + opensource@aubrune.eu + + http://www.apache.org/licenses/LICENSE-2.0 + Other + ARUco, tag, marker, Gazebo + diff --git a/rbs_mill_assist/assets/aruco_4/model.sdf b/rbs_mill_assist/assets/aruco_4/model.sdf new file mode 100644 index 0000000..e2dde7b --- /dev/null +++ b/rbs_mill_assist/assets/aruco_4/model.sdf @@ -0,0 +1,16 @@ + + + + true + 0 0 0 0 0 0 + + + + + model://aruco_4/meshes/aruco_template.obj + + + + + + diff --git a/rbs_mill_assist/assets/aruco_c/meshes/aruco_marker_25.png b/rbs_mill_assist/assets/aruco_c/meshes/aruco_marker_25.png new file mode 100644 index 0000000..629701f Binary files /dev/null and b/rbs_mill_assist/assets/aruco_c/meshes/aruco_marker_25.png differ diff --git a/rbs_mill_assist/assets/aruco_c/meshes/aruco_template.mtl b/rbs_mill_assist/assets/aruco_c/meshes/aruco_template.mtl new file mode 100644 index 0000000..9f5cb72 --- /dev/null +++ b/rbs_mill_assist/assets/aruco_c/meshes/aruco_template.mtl @@ -0,0 +1,12 @@ +# Blender 4.2.1 LTS MTL File: 'aruco_template.blend' +# www.blender.org + +newmtl aruco_tag_124 +Ns 0.000000 +Ka 1.000000 1.000000 1.000000 +Ks 0.000000 0.000000 0.000000 +Ke 0.000000 0.000000 0.000000 +Ni 1.500000 +d 1.000000 +illum 1 +map_Kd aruco_marker_25.png diff --git a/rbs_mill_assist/assets/aruco_c/meshes/aruco_template.obj b/rbs_mill_assist/assets/aruco_c/meshes/aruco_template.obj new file mode 100644 index 0000000..8b8dc1a --- /dev/null +++ b/rbs_mill_assist/assets/aruco_c/meshes/aruco_template.obj @@ -0,0 +1,48 @@ +# Blender 4.2.1 LTS +# www.blender.org +mtllib aruco_template.mtl +o aruco_tag_124 +v -0.000464 0.000314 0.039912 +v 0.039536 0.000314 0.039912 +v 0.039536 0.000314 -0.000088 +v -0.000464 0.000314 -0.000088 +v -0.000464 0.001314 0.039912 +v 0.039536 0.001314 0.039912 +v 0.039536 0.001314 -0.000088 +v -0.000464 0.001314 -0.000088 +vn -0.0000 -1.0000 -0.0000 +vn -0.0000 1.0000 -0.0000 +vn 0.9999 0.0125 -0.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.9997 -0.0250 -0.0000 +vn -1.0000 -0.0080 -0.0000 +vn 1.0000 -0.0000 -0.0000 +vn 1.0000 0.0080 -0.0000 +vn 0.9997 0.0250 -0.0000 +vn -0.9999 -0.0125 -0.0000 +vn -0.0000 0.0125 -0.9999 +vn -0.0000 -0.0000 1.0000 +vn -0.0000 -0.0250 0.9997 +vn -0.0000 -0.0080 1.0000 +vn -0.0000 -0.0000 -1.0000 +vn -0.0000 0.0080 -1.0000 +vn -0.0000 0.0250 -0.9997 +vn -0.0000 -0.0125 0.9999 +vt 0.000000 0.000000 +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +s 0 +usemtl aruco_tag_124 +f 1/1/1 2/2/1 3/3/1 +f 1/1/1 3/3/1 4/4/1 +f 5/1/2 6/2/2 7/3/2 +f 5/1/2 7/3/2 8/4/2 +f 1/1/3 2/2/4 6/3/5 +f 1/1/6 6/3/4 5/4/4 +f 4/1/7 3/2/7 7/3/8 +f 4/1/9 7/3/10 8/4/7 +f 1/1/11 4/2/12 8/3/13 +f 1/1/14 8/3/12 5/4/12 +f 2/1/15 3/2/15 7/3/16 +f 2/1/17 7/3/18 6/4/15 diff --git a/rbs_mill_assist/assets/aruco_c/model.config b/rbs_mill_assist/assets/aruco_c/model.config new file mode 100644 index 0000000..82276f5 --- /dev/null +++ b/rbs_mill_assist/assets/aruco_c/model.config @@ -0,0 +1,17 @@ + + + aruco_c + 1.0 + model.sdf + + 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. + + + Yoan Mollard + opensource@aubrune.eu + + http://www.apache.org/licenses/LICENSE-2.0 + Other + ARUco, tag, marker, Gazebo + diff --git a/rbs_mill_assist/assets/aruco_c/model.sdf b/rbs_mill_assist/assets/aruco_c/model.sdf new file mode 100644 index 0000000..9979e2e --- /dev/null +++ b/rbs_mill_assist/assets/aruco_c/model.sdf @@ -0,0 +1,16 @@ + + + + true + 0 0 0 0 0 0 + + + + + model://aruco_c/meshes/aruco_template.obj + + + + + + diff --git a/rbs_mill_assist/assets/bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL b/rbs_mill_assist/assets/bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL new file mode 100644 index 0000000..f8e2518 Binary files /dev/null and b/rbs_mill_assist/assets/bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL differ diff --git a/rbs_mill_assist/assets/bunker/meshes/bunker.stl b/rbs_mill_assist/assets/bunker/meshes/bunker.stl deleted file mode 100644 index d4ef85c..0000000 Binary files a/rbs_mill_assist/assets/bunker/meshes/bunker.stl and /dev/null differ diff --git a/rbs_mill_assist/assets/bunker/model.sdf b/rbs_mill_assist/assets/bunker/model.sdf index ede8a4e..a395451 100644 --- a/rbs_mill_assist/assets/bunker/model.sdf +++ b/rbs_mill_assist/assets/bunker/model.sdf @@ -6,43 +6,48 @@ - model://bunker/meshes/bunker.stl + model://bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL - - - - - - - - - - - - - - - - - - + + 1 1 1 1 + 1 1 1 1 + 0.5 0.5 0.5 1 + 0 0 0 1 + + + + + - + + 7850 - model://bunker/meshes/bunker.stl + model://bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + diff --git a/rbs_mill_assist/assets/bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL b/rbs_mill_assist/assets/bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL new file mode 100644 index 0000000..2609b85 Binary files /dev/null and b/rbs_mill_assist/assets/bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL differ diff --git a/rbs_mill_assist/assets/bunker_4_slots/model.config b/rbs_mill_assist/assets/bunker_4_slots/model.config new file mode 100644 index 0000000..5db6d5c --- /dev/null +++ b/rbs_mill_assist/assets/bunker_4_slots/model.config @@ -0,0 +1,16 @@ + + + + bunker_4_slots + 0.1 + model.sdf + + + Bill Finger + ur.narmak@gmail.com + + + + Empty + + diff --git a/rbs_mill_assist/assets/bunker_4_slots/model.sdf b/rbs_mill_assist/assets/bunker_4_slots/model.sdf new file mode 100644 index 0000000..0b6ccb4 --- /dev/null +++ b/rbs_mill_assist/assets/bunker_4_slots/model.sdf @@ -0,0 +1,54 @@ + + + + + + + + + model://bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL + + + + 1 1 1 1 + 1 1 1 1 + 0.5 0.5 0.5 1 + 0 0 0 1 + + + + + + + + 7850 + + + model://bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rbs_mill_assist/assets/conductor/meshes/conductor.stl b/rbs_mill_assist/assets/conductor/meshes/conductor.stl new file mode 100644 index 0000000..7696eb8 Binary files /dev/null and b/rbs_mill_assist/assets/conductor/meshes/conductor.stl differ diff --git a/rbs_mill_assist/assets/conductor/model.config b/rbs_mill_assist/assets/conductor/model.config new file mode 100644 index 0000000..4d85e18 --- /dev/null +++ b/rbs_mill_assist/assets/conductor/model.config @@ -0,0 +1,16 @@ + + + + conductor + 0.1 + model.sdf + + + Bill Finger + ur.narmak@gmail.com + + + + Empty + + diff --git a/rbs_mill_assist/assets/conductor/model.sdf b/rbs_mill_assist/assets/conductor/model.sdf new file mode 100644 index 0000000..79e9cd3 --- /dev/null +++ b/rbs_mill_assist/assets/conductor/model.sdf @@ -0,0 +1,61 @@ + + + + + + + + + model://conductor/meshes/conductor.stl + + + + 0.8 0.8 0.8 1 + 0.3 0.3 0.3 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + + + + + + + + + + 1000 + + + model://conductor/meshes/conductor.stl + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rbs_mill_assist/assets/laser/laser.material b/rbs_mill_assist/assets/laser/laser.material new file mode 100644 index 0000000..d380252 --- /dev/null +++ b/rbs_mill_assist/assets/laser/laser.material @@ -0,0 +1,11 @@ +material LaserMaterial +{ + technique + { + pass + { + ambient 0.5 0.5 0.5 1.0 + diffuse 0.5 0.5 0.5 1.0 + } + } +} diff --git a/rbs_mill_assist/assets/laser/meshes/laser.dae b/rbs_mill_assist/assets/laser/meshes/laser.dae new file mode 100644 index 0000000..b3f6ca0 --- /dev/null +++ b/rbs_mill_assist/assets/laser/meshes/laser.dae @@ -0,0 +1,207 @@ + + + + + Blender User + Blender 4.3.2 commit date:2024-12-16, commit time:21:10, hash:32f5fdce0a0a + + 2025-03-05T17:38:05 + 2025-03-05T17:38:05 + + Z_UP + + + + + + + 39.59775 + 1.777778 + 0.1 + 100 + + + + + + 0 + 0 + 10 + + + + + + + + 39.59775 + 1.777778 + 0.1 + 100 + + + + + + 0 + 0 + 10 + + + + + + + + + 1000 1000 1000 + 1 + 0 + 0 + + + + + 0 + 0 + 2097153 + 1 + 1 + 1 + 1000 + 75 + 0.15 + 0.04999995 + 40 + 0.1 + 1 + 0.1 + 0.1 + 1 + + + + + + + 1000 1000 1000 + 1 + 0 + 0 + + + + + 0 + 0 + 2097153 + 1 + 1 + 1 + 1000 + 75 + 0.15 + 0.04999995 + 40 + 0.1 + 1 + 0.1 + 0.1 + 1 + + + + + + + + + + + 0 0 0 1 + + + 0.1584888 0.3396751 0.8005451 1 + + + 1.5 + + + + + + + + + + + + + + + + + -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 + + + + + + + + + + 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 + + + + + + + + + + + + + + +

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

+ + + + + + + + -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 + + + + 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 + + + + 1 0 0 0 0 7.54979e-8 -1 0 0 1 7.54979e-8 0 0 0 0 1 + + + + + + + + + + 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 + + + + -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 + + + + + + + + \ No newline at end of file diff --git a/rbs_mill_assist/assets/laser/meshes/laser.mtl b/rbs_mill_assist/assets/laser/meshes/laser.mtl new file mode 100644 index 0000000..24c0d6a --- /dev/null +++ b/rbs_mill_assist/assets/laser/meshes/laser.mtl @@ -0,0 +1,2 @@ +# Blender 4.3.2 MTL File: 'None' +# www.blender.org diff --git a/rbs_mill_assist/assets/laser/meshes/laser.obj b/rbs_mill_assist/assets/laser/meshes/laser.obj new file mode 100644 index 0000000..e3c99c5 --- /dev/null +++ b/rbs_mill_assist/assets/laser/meshes/laser.obj @@ -0,0 +1,103 @@ +# Blender 4.3.2 +# www.blender.org +mtllib laser.mtl +o laser +v -0.095000 0.005000 0.040000 +v -0.095000 0.005000 -0.040000 +v -0.095000 0.305000 0.040000 +v -0.095000 0.305000 -0.040000 +v -0.175000 0.005000 -0.040000 +v -0.175000 0.305000 -0.040000 +v -0.175000 0.005000 0.040000 +v -0.175000 0.305000 0.040000 +v -0.225000 0.005000 -0.155000 +v -0.225000 0.005000 0.155000 +v 0.225000 0.005000 0.155000 +v 0.225000 0.005000 -0.155000 +v 0.225000 0.000000 -0.155000 +v 0.225000 -0.000000 0.155000 +v -0.225000 0.000000 -0.155000 +v -0.225000 -0.000000 0.155000 +v -0.225000 0.455000 -0.072500 +v -0.175000 0.455000 -0.040000 +v -0.095000 0.455000 -0.040000 +v -0.175000 0.455000 0.040000 +v -0.225000 0.455000 0.072500 +v -0.095000 0.455000 0.040000 +v 0.175000 0.455000 0.072500 +v 0.175000 0.455000 -0.072500 +v 0.175000 0.305000 0.072500 +v -0.225000 0.305000 0.072500 +v 0.175000 0.305000 -0.072500 +v -0.225000 0.305000 -0.072500 +v -0.175000 0.605000 -0.040000 +v -0.175000 0.605000 0.040000 +v -0.095000 0.605000 -0.040000 +v -0.095000 0.605000 0.040000 +vn 1.0000 -0.0000 -0.0000 +vn -0.0000 -0.0000 -1.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.0000 -0.0000 1.0000 +vn -0.0000 1.0000 -0.0000 +vn -0.0000 -1.0000 -0.0000 +s 0 +f 1//1 2//1 3//1 +f 3//1 2//1 4//1 +f 2//2 5//2 4//2 +f 4//2 5//2 6//2 +f 5//3 7//3 6//3 +f 6//3 7//3 8//3 +f 7//4 1//4 8//4 +f 8//4 1//4 3//4 +f 5//5 9//5 7//5 +f 7//5 9//5 10//5 +f 7//5 10//5 1//5 +f 1//5 10//5 11//5 +f 1//5 11//5 2//5 +f 2//5 11//5 12//5 +f 2//5 12//5 5//5 +f 5//5 12//5 9//5 +f 12//1 11//1 13//1 +f 13//1 11//1 14//1 +f 9//2 12//2 15//2 +f 15//2 12//2 13//2 +f 10//3 9//3 16//3 +f 16//3 9//3 15//3 +f 11//4 10//4 14//4 +f 14//4 10//4 16//4 +f 16//6 15//6 14//6 +f 14//6 15//6 13//6 +f 17//5 18//5 19//5 +f 18//5 17//5 20//5 +f 20//5 17//5 21//5 +f 20//5 21//5 22//5 +f 22//5 21//5 23//5 +f 22//5 23//5 19//5 +f 19//5 23//5 24//5 +f 19//5 24//5 17//5 +f 3//6 25//6 26//6 +f 25//6 3//6 27//6 +f 27//6 3//6 4//6 +f 27//6 4//6 28//6 +f 28//6 4//6 6//6 +f 28//6 6//6 26//6 +f 26//6 6//6 8//6 +f 26//6 8//6 3//6 +f 24//1 23//1 27//1 +f 27//1 23//1 25//1 +f 17//2 24//2 28//2 +f 28//2 24//2 27//2 +f 21//3 17//3 26//3 +f 26//3 17//3 28//3 +f 23//4 21//4 25//4 +f 25//4 21//4 26//4 +f 29//5 30//5 31//5 +f 31//5 30//5 32//5 +f 22//4 32//4 20//4 +f 20//4 32//4 30//4 +f 20//3 30//3 18//3 +f 18//3 30//3 29//3 +f 18//2 29//2 19//2 +f 19//2 29//2 31//2 +f 19//1 31//1 22//1 +f 22//1 31//1 32//1 diff --git a/rbs_mill_assist/assets/laser/meshes/laser.stl b/rbs_mill_assist/assets/laser/meshes/laser.stl index a15df0e..383c7ab 100644 Binary files a/rbs_mill_assist/assets/laser/meshes/laser.stl and b/rbs_mill_assist/assets/laser/meshes/laser.stl differ diff --git a/rbs_mill_assist/assets/laser/model.sdf b/rbs_mill_assist/assets/laser/model.sdf index 0713e03..574c6c9 100644 --- a/rbs_mill_assist/assets/laser/model.sdf +++ b/rbs_mill_assist/assets/laser/model.sdf @@ -7,32 +7,33 @@ - model://laser/meshes/laser.stl + model://laser/meshes/laser.obj - - - - - - - - - - - - - - - - - - + + 1 1 1 1 + 1 1 1 1 + 0.5 0.5 0.5 1 + 0 0 0 1 + + + + + + + + + + + + + + 1000 - model://laser/meshes/laser.stl + model://laser/meshes/laser.obj diff --git a/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae b/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae index 3106369..e7108de 100644 --- a/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae +++ b/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae @@ -3,13 +3,66 @@ Blender User - Blender 4.1.1 commit date:2024-04-15, commit time:15:11, hash:e1743a0317bc + Blender 4.3.2 commit date:2024-12-16, commit time:21:10, hash:32f5fdce0a0a - 2025-02-12T10:55:51 - 2025-02-12T10:55:51 + 2025-03-03T12:34:37 + 2025-03-03T12:34:37 Z_UP + + + + + + 39.59775 + 1.777778 + 0.1 + 100 + + + + + + 0 + 0 + 10 + + + + + + + + + 1000 1000 1000 + 1 + 0 + 0 + + + + + 0 + 0 + 2097153 + 1 + 1 + 1 + 1000 + 75 + 0.15 + 0.04999995 + 40 + 0.1 + 1 + 0.1 + 0.1 + 1 + + + + @@ -39,7 +92,7 @@ - -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 + -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 @@ -87,12 +140,20 @@ - + + + 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 + + + + -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 + + diff --git a/rbs_mill_assist/assets/shildik/model.sdf b/rbs_mill_assist/assets/shildik/model.sdf index 1674d34..fc16e63 100644 --- a/rbs_mill_assist/assets/shildik/model.sdf +++ b/rbs_mill_assist/assets/shildik/model.sdf @@ -14,9 +14,9 @@ 1 1 1 1 0.5 0.5 0.5 1 0 0 0 1 - - model://shildik/textures/shildik_sh.png - + + + model://shildik/textures/shildik_sh_d.png @@ -29,20 +29,21 @@ + 2700 - 0.06 0.04 0.001 + 0.06 0.04 0.0005 - - - - - - - - - + + + + + + + + + diff --git a/rbs_mill_assist/bt/CMakeLists.txt b/rbs_mill_assist/bt/CMakeLists.txt index 70f2b16..6b5fd57 100644 --- a/rbs_mill_assist/bt/CMakeLists.txt +++ b/rbs_mill_assist/bt/CMakeLists.txt @@ -2,8 +2,10 @@ find_package(behaviortree_ros2 REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(rbs_skill_interfaces REQUIRED) +find_package(rbs_utils_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) -# find_package(std_srvs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(rbs_mill_interfaces REQUIRED) # Behaviortree interfaces set(dependencies @@ -11,14 +13,34 @@ set(dependencies rbs_skill_interfaces geometry_msgs behaviortree_ros2 + rbs_utils_interfaces std_srvs + rbs_mill_interfaces ) add_library(vacuum_gripper_toggle SHARED plugins/vacuum_gripper_toggle.cpp) list(APPEND plugin_libs vacuum_gripper_toggle) -add_library(get_grasp_place_pose SHARED plugins/get_grasp_place_pose.cpp) -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}) diff --git a/rbs_mill_assist/bt/config/bt_executor.yaml b/rbs_mill_assist/bt/config/bt_executor.yaml index 6feace9..069aafc 100644 --- a/rbs_mill_assist/bt/config/bt_executor.yaml +++ b/rbs_mill_assist/bt/config/bt_executor.yaml @@ -10,4 +10,5 @@ bt_action_server: - rbs_bt_executor/bt_plugins behavior_trees: - - rbs_mill_assist/xmls + - rbs_mill_assist/xmls/moveit + - rbs_mill_assist/xmls/no_moveit diff --git a/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp b/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp deleted file mode 100644 index 7eeb671..0000000 --- a/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#include "behaviortree_ros2/bt_service_node.hpp" - -#include "behaviortree_ros2/plugins.hpp" -#include -// #include -#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" -#include -#include -#include -#include -// #include -#include -#include - -using GraspingService = rbs_skill_interfaces::srv::GetPickPlacePoses; -using namespace BT; - -class Grasping : public RosServiceNode { -public: - Grasping(const std::string &name, const NodeConfig &conf, - const RosNodeParams ¶ms) - : RosServiceNode(name, conf, params) { - - RCLCPP_INFO(this->logger(), "Starting GetGraspPose"); - } - - static PortsList providedPorts() { - return providedBasicPorts( - {InputPort("object_name"), - InputPort("action_name"), - OutputPort>("pose"), - OutputPort>("pre_pose"), - OutputPort>("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 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(); - auto pregrasp_pose = std::make_shared(); - auto postgrasp_pose = std::make_shared(); - - *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 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(); - auto preplace_pose = std::make_shared(); - auto postplace_pose = std::make_shared(); - - *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"); diff --git a/rbs_mill_assist/bt/plugins/get_grasp_pose.cpp b/rbs_mill_assist/bt/plugins/get_grasp_pose.cpp new file mode 100644 index 0000000..9e0451a --- /dev/null +++ b/rbs_mill_assist/bt/plugins/get_grasp_pose.cpp @@ -0,0 +1,89 @@ +#include "behaviortree_ros2/bt_service_node.hpp" + +#include "behaviortree_ros2/plugins.hpp" +#include +// #include +#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 +#include +#include +#include +// #include +#include +#include + +using GraspingService = rbs_mill_interfaces::srv::GetGraspingPose; +using namespace BT; + +class GetGraspPose : public RosServiceNode { +public: + GetGraspPose(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosServiceNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting GetSlotGraspingPose"); + } + + static PortsList providedPorts() { + return providedBasicPorts( + {InputPort("from_pose"), + InputPort("relative_to"), + OutputPort>("grasp"), + OutputPort>("pregrasp"), + OutputPort>("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(response->grasp_poses.pose); + auto pregrasp = std::make_shared( + response->grasp_poses.pre_pose); + auto postgrasp = std::make_shared( + 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"); diff --git a/rbs_mill_assist/bt/plugins/get_named_pose.cpp b/rbs_mill_assist/bt/plugins/get_named_pose.cpp new file mode 100644 index 0000000..518782c --- /dev/null +++ b/rbs_mill_assist/bt/plugins/get_named_pose.cpp @@ -0,0 +1,78 @@ +#include "behaviortree_ros2/bt_service_node.hpp" + +#include "behaviortree_ros2/plugins.hpp" +#include +// #include +#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" +#include "rbs_utils_interfaces/srv/get_relative_named_pose.hpp" +#include +#include +#include +#include +#include +#include + +using GetRelativeNamedPose = rbs_utils_interfaces::srv::GetRelativeNamedPose; +using namespace BT; + +class GetNamedPose : public RosServiceNode { +public: + GetNamedPose(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosServiceNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting GetNamedPose"); + } + + static PortsList providedPorts() { + return providedBasicPorts( + {InputPort("pose_name"), + InputPort("relative_to"), + OutputPort>("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(); + *pose = response->named_pose.pose; + setOutput("output_pose", pose); + + return NodeStatus::SUCCESS; + } +}; + +CreateRosNodePlugin(GetNamedPose, "GetNamedPose"); diff --git a/rbs_mill_assist/bt/plugins/get_place_pose.cpp b/rbs_mill_assist/bt/plugins/get_place_pose.cpp new file mode 100644 index 0000000..777e201 --- /dev/null +++ b/rbs_mill_assist/bt/plugins/get_place_pose.cpp @@ -0,0 +1,90 @@ +#include "behaviortree_ros2/bt_service_node.hpp" + +#include "behaviortree_ros2/plugins.hpp" +#include +// #include +#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 +#include +#include +#include +// #include +#include +#include + +using GraspingService = rbs_mill_interfaces::srv::GetGraspingPose; +using namespace BT; + +class GetPlacePoses : public RosServiceNode { +public: + GetPlacePoses(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosServiceNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting GetPlacePose"); + } + + static PortsList providedPorts() { + return providedBasicPorts( + {InputPort("to_pose"), + InputPort("relative_to"), + OutputPort>("place"), + OutputPort>("preplace"), + OutputPort>("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(response->place_poses.pose); + auto preplace = std::make_shared( + response->place_poses.pre_pose); + auto postplace = std::make_shared( + 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"); diff --git a/rbs_mill_assist/bt/plugins/is_in_pose.cpp b/rbs_mill_assist/bt/plugins/is_in_pose.cpp new file mode 100644 index 0000000..dda3714 --- /dev/null +++ b/rbs_mill_assist/bt/plugins/is_in_pose.cpp @@ -0,0 +1,48 @@ +#include "behaviortree_ros2/bt_topic_sub_node.hpp" + +#include "behaviortree_ros2/plugins.hpp" +#include +// #include +#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" +#include "rbs_utils_interfaces/srv/get_named_pose.hpp" +#include +#include +#include +#include +#include +#include + +using MSG = geometry_msgs::msg::Pose; +using namespace BT; + +class IsInPose : public RosTopicSubNode { +public: + IsInPose(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosTopicSubNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting IsInPose"); + } + // + // static PortsList providedPorts() { + // return {}; + // } + + NodeStatus onTick(const std::shared_ptr &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"); diff --git a/rbs_mill_assist/bt/plugins/takeon_task.cpp b/rbs_mill_assist/bt/plugins/takeon_task.cpp new file mode 100644 index 0000000..844ee92 --- /dev/null +++ b/rbs_mill_assist/bt/plugins/takeon_task.cpp @@ -0,0 +1,54 @@ +#include "nlohmann/json.hpp" +#include "behaviortree_ros2/bt_service_node.hpp" +#include "behaviortree_ros2/plugins.hpp" +#include "rbs_utils_interfaces/srv/task_from_queue.hpp" +#include +#include +#include + +using TaskFromQueue = rbs_utils_interfaces::srv::TaskFromQueue; +using namespace BT; + +class TakeonTask : public RosServiceNode { +public: + TakeonTask(const std::string &name, const NodeConfig &conf, const RosNodeParams ¶ms) + : RosServiceNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting TakeonTask"); + } + + static PortsList providedPorts() { + return providedBasicPorts( + {OutputPort("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"); diff --git a/rbs_mill_assist/bt/plugins/task_completed.cpp b/rbs_mill_assist/bt/plugins/task_completed.cpp new file mode 100644 index 0000000..d94c7b6 --- /dev/null +++ b/rbs_mill_assist/bt/plugins/task_completed.cpp @@ -0,0 +1,40 @@ +#include "behaviortree_ros2/bt_service_node.hpp" +#include "behaviortree_ros2/plugins.hpp" +#include "rbs_utils_interfaces/srv/task_from_queue.hpp" + +using TaskFromQueue = rbs_utils_interfaces::srv::TaskFromQueue; +using namespace BT; + +class TaskCompleted : public RosServiceNode { +public: +TaskCompleted(const std::string &name, const NodeConfig &conf, const RosNodeParams ¶ms) + : RosServiceNode(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"); diff --git a/rbs_mill_assist/bt/plugins/twist_cmd.cpp b/rbs_mill_assist/bt/plugins/twist_cmd.cpp new file mode 100644 index 0000000..7688e56 --- /dev/null +++ b/rbs_mill_assist/bt/plugins/twist_cmd.cpp @@ -0,0 +1,85 @@ +#include "behaviortree_ros2/bt_action_node.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "rbs_skill_interfaces/action/twist_control_with_condition.hpp" +#include +#include +#include +#include + +using namespace BT; +using TwistCmdWithCondAction = + rbs_skill_interfaces::action::TwistControlWithCondition; + +class TwistCmdWithCondClient : public RosActionNode { +public: + TwistCmdWithCondClient(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosActionNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting MoveToPose"); + } + + static BT::PortsList providedPorts() { + return providedBasicPorts({BT::InputPort("robot_name"), + BT::InputPort>("twist"), + // BT::InputPort("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 m_target_pose; + std::vector twist_cmd_vec; +}; + +CreateRosNodePlugin(TwistCmdWithCondClient, "TwistCmdWithCond"); diff --git a/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp b/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp index a7a2fb1..5bb5c04 100644 --- a/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp +++ b/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp @@ -24,10 +24,10 @@ public: } bool setRequest(Request::SharedPtr &request) override { - RCLCPP_INFO(this->logger(), "Starting send request"); + RCLCPP_INFO(this->logger(), "[%s] Starting send request", name().c_str()); if (!getInput("enable", request->data)) { - RCLCPP_ERROR(this->node_.lock()->get_logger(), - "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; } diff --git a/rbs_mill_assist/bt/xmls/Grasp.xml b/rbs_mill_assist/bt/xmls/Grasp.xml deleted file mode 100644 index 4d1bde0..0000000 --- a/rbs_mill_assist/bt/xmls/Grasp.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - -