diff --git a/.gitignore b/.gitignore deleted file mode 100644 index 49165bf..0000000 --- a/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -*.blend1 -*.vscode -**/__pycache__/** diff --git a/docs/bt_example_with_tasks.md b/docs/bt_example_with_tasks.md deleted file mode 100644 index 4c7bf22..0000000 --- a/docs/bt_example_with_tasks.md +++ /dev/null @@ -1,165 +0,0 @@ -Для реализации обработки очереди задач в сценарии, описанном вами, можно использовать **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 deleted file mode 100644 index d32b612..0000000 --- a/docs/objects.json +++ /dev/null @@ -1,38 +0,0 @@ -[ - { - "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 deleted file mode 100644 index e0f3e5b..0000000 --- a/docs/shildik_templates/README.md +++ /dev/null @@ -1,16 +0,0 @@ -# Шаблоны изображений шильдиков - -Директория содержит шаблоны шильдиков в двух вариантах: -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 deleted file mode 100644 index 89dd0c7..0000000 --- a/docs/shildik_templates/curves/Табличка_пустая_Ex.svg +++ /dev/null @@ -1,899 +0,0 @@ - -image/svg+xml diff --git a/docs/shildik_templates/curves/Табличка_пустая_Насосная_установка.svg b/docs/shildik_templates/curves/Табличка_пустая_Насосная_установка.svg deleted file mode 100644 index 260e10f..0000000 --- a/docs/shildik_templates/curves/Табличка_пустая_Насосная_установка.svg +++ /dev/null @@ -1,934 +0,0 @@ - -image/svg+xml diff --git a/docs/shildik_templates/curves/Табличка_пустая_Общепром.svg b/docs/shildik_templates/curves/Табличка_пустая_Общепром.svg deleted file mode 100644 index 9567994..0000000 --- a/docs/shildik_templates/curves/Табличка_пустая_Общепром.svg +++ /dev/null @@ -1,412 +0,0 @@ - -image/svg+xml diff --git a/docs/shildik_templates/curves/Табличка_пустая_Шкаф.svg b/docs/shildik_templates/curves/Табличка_пустая_Шкаф.svg deleted file mode 100644 index efce9f3..0000000 --- a/docs/shildik_templates/curves/Табличка_пустая_Шкаф.svg +++ /dev/null @@ -1,899 +0,0 @@ - -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 deleted file mode 100644 index 436f92a..0000000 Binary files a/docs/shildik_templates/fonts/GOST 2.304 type A.ttf and /dev/null 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 deleted file mode 100644 index 884b2fe..0000000 Binary files a/docs/shildik_templates/fonts/GOST 2.304 type B.ttf and /dev/null differ diff --git a/docs/shildik_templates/test_templates/Табличка_пустая_Ex.svg b/docs/shildik_templates/test_templates/Табличка_пустая_Ex.svg deleted file mode 100644 index 6c72a8a..0000000 --- a/docs/shildik_templates/test_templates/Табличка_пустая_Ex.svg +++ /dev/null @@ -1,855 +0,0 @@ - -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 deleted file mode 100644 index 859107f..0000000 --- a/docs/shildik_templates/test_templates/Табличка_пустая_Ex_plain.svg +++ /dev/null @@ -1,932 +0,0 @@ - -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 deleted file mode 100644 index ab959dd..0000000 --- a/docs/shildik_templates/text/Табличка_пустая_Ex.svg +++ /dev/null @@ -1,657 +0,0 @@ - -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 deleted file mode 100644 index aa8a06f..0000000 --- a/docs/shildik_templates/text/Табличка_пустая_Насосная_установка.svg +++ /dev/null @@ -1,912 +0,0 @@ - -image/svg+xml diff --git a/docs/shildik_templates/text/Табличка_пустая_Общепром.svg b/docs/shildik_templates/text/Табличка_пустая_Общепром.svg deleted file mode 100644 index ff439e9..0000000 --- a/docs/shildik_templates/text/Табличка_пустая_Общепром.svg +++ /dev/null @@ -1,578 +0,0 @@ - -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 deleted file mode 100644 index a29c61c..0000000 --- a/docs/shildik_templates/text/Табличка_пустая_Шкаф.svg +++ /dev/null @@ -1,877 +0,0 @@ - -image/svg+xml diff --git a/docs/tasks.csv b/docs/tasks.csv deleted file mode 100644 index 1c7ad06..0000000 --- a/docs/tasks.csv +++ /dev/null @@ -1,6 +0,0 @@ -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 deleted file mode 100644 index a376bab..0000000 --- a/docs/to_start_BT_main.md +++ /dev/null @@ -1,25 +0,0 @@ -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 ce3bc50..84deb0d 100644 --- a/rbs_mill_assist/CMakeLists.txt +++ b/rbs_mill_assist/CMakeLists.txt @@ -7,16 +7,53 @@ 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) -install(DIRECTORY world urdf meshes moveit srdf launch config assets bt/xmls bt/config +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 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 deleted file mode 100644 index 8096159..0000000 Binary files a/rbs_mill_assist/assets/aruco_1/meshes/aruco_marker_21.png and /dev/null 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 deleted file mode 100644 index b129471..0000000 --- a/rbs_mill_assist/assets/aruco_1/meshes/aruco_template.mtl +++ /dev/null @@ -1,12 +0,0 @@ -# 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 deleted file mode 100644 index 8b8dc1a..0000000 --- a/rbs_mill_assist/assets/aruco_1/meshes/aruco_template.obj +++ /dev/null @@ -1,48 +0,0 @@ -# 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 deleted file mode 100644 index a8f06d1..0000000 --- a/rbs_mill_assist/assets/aruco_1/model.config +++ /dev/null @@ -1,17 +0,0 @@ - - - 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 deleted file mode 100644 index 0409e46..0000000 --- a/rbs_mill_assist/assets/aruco_1/model.sdf +++ /dev/null @@ -1,16 +0,0 @@ - - - - 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 deleted file mode 100644 index b6a5e40..0000000 Binary files a/rbs_mill_assist/assets/aruco_2/meshes/aruco_marker_22.png and /dev/null 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 deleted file mode 100644 index a048f46..0000000 --- a/rbs_mill_assist/assets/aruco_2/meshes/aruco_template.mtl +++ /dev/null @@ -1,13 +0,0 @@ -# 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 deleted file mode 100644 index 8b8dc1a..0000000 --- a/rbs_mill_assist/assets/aruco_2/meshes/aruco_template.obj +++ /dev/null @@ -1,48 +0,0 @@ -# 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 deleted file mode 100644 index c9e79b1..0000000 --- a/rbs_mill_assist/assets/aruco_2/model.config +++ /dev/null @@ -1,17 +0,0 @@ - - - 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 deleted file mode 100644 index 24b1028..0000000 --- a/rbs_mill_assist/assets/aruco_2/model.sdf +++ /dev/null @@ -1,16 +0,0 @@ - - - - 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 deleted file mode 100644 index 377fd6d..0000000 Binary files a/rbs_mill_assist/assets/aruco_3/meshes/aruco_marker_23.png and /dev/null 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 deleted file mode 100644 index 065c8da..0000000 --- a/rbs_mill_assist/assets/aruco_3/meshes/aruco_template.mtl +++ /dev/null @@ -1,12 +0,0 @@ -# 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 deleted file mode 100644 index 8b8dc1a..0000000 --- a/rbs_mill_assist/assets/aruco_3/meshes/aruco_template.obj +++ /dev/null @@ -1,48 +0,0 @@ -# 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 deleted file mode 100644 index f8d0193..0000000 --- a/rbs_mill_assist/assets/aruco_3/model.config +++ /dev/null @@ -1,17 +0,0 @@ - - - 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 deleted file mode 100644 index 3389aa2..0000000 --- a/rbs_mill_assist/assets/aruco_3/model.sdf +++ /dev/null @@ -1,16 +0,0 @@ - - - - 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 deleted file mode 100644 index 0654fae..0000000 Binary files a/rbs_mill_assist/assets/aruco_4/meshes/aruco_marker_24.png and /dev/null 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 deleted file mode 100644 index 0803561..0000000 --- a/rbs_mill_assist/assets/aruco_4/meshes/aruco_template.mtl +++ /dev/null @@ -1,12 +0,0 @@ -# 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 deleted file mode 100644 index 8b8dc1a..0000000 --- a/rbs_mill_assist/assets/aruco_4/meshes/aruco_template.obj +++ /dev/null @@ -1,48 +0,0 @@ -# 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 deleted file mode 100644 index e1c5682..0000000 --- a/rbs_mill_assist/assets/aruco_4/model.config +++ /dev/null @@ -1,17 +0,0 @@ - - - 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 deleted file mode 100644 index e2dde7b..0000000 --- a/rbs_mill_assist/assets/aruco_4/model.sdf +++ /dev/null @@ -1,16 +0,0 @@ - - - - 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 deleted file mode 100644 index 629701f..0000000 Binary files a/rbs_mill_assist/assets/aruco_c/meshes/aruco_marker_25.png and /dev/null 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 deleted file mode 100644 index 9f5cb72..0000000 --- a/rbs_mill_assist/assets/aruco_c/meshes/aruco_template.mtl +++ /dev/null @@ -1,12 +0,0 @@ -# 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 deleted file mode 100644 index 8b8dc1a..0000000 --- a/rbs_mill_assist/assets/aruco_c/meshes/aruco_template.obj +++ /dev/null @@ -1,48 +0,0 @@ -# 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 deleted file mode 100644 index 82276f5..0000000 --- a/rbs_mill_assist/assets/aruco_c/model.config +++ /dev/null @@ -1,17 +0,0 @@ - - - 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 deleted file mode 100644 index 9979e2e..0000000 --- a/rbs_mill_assist/assets/aruco_c/model.sdf +++ /dev/null @@ -1,16 +0,0 @@ - - - - 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 deleted file mode 100644 index f8e2518..0000000 Binary files a/rbs_mill_assist/assets/bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL and /dev/null differ diff --git a/rbs_mill_assist/assets/bunker/meshes/bunker.stl b/rbs_mill_assist/assets/bunker/meshes/bunker.stl new file mode 100644 index 0000000..d4ef85c Binary files /dev/null and b/rbs_mill_assist/assets/bunker/meshes/bunker.stl differ diff --git a/rbs_mill_assist/assets/bunker/model.sdf b/rbs_mill_assist/assets/bunker/model.sdf index a395451..ede8a4e 100644 --- a/rbs_mill_assist/assets/bunker/model.sdf +++ b/rbs_mill_assist/assets/bunker/model.sdf @@ -6,48 +6,43 @@ - model://bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL + model://bunker/meshes/bunker.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_50mm_height_for_labels_60х40.STL + model://bunker/meshes/bunker.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 deleted file mode 100644 index 2609b85..0000000 Binary files a/rbs_mill_assist/assets/bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL and /dev/null differ diff --git a/rbs_mill_assist/assets/bunker_4_slots/model.config b/rbs_mill_assist/assets/bunker_4_slots/model.config deleted file mode 100644 index 5db6d5c..0000000 --- a/rbs_mill_assist/assets/bunker_4_slots/model.config +++ /dev/null @@ -1,16 +0,0 @@ - - - - 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 deleted file mode 100644 index 0b6ccb4..0000000 --- a/rbs_mill_assist/assets/bunker_4_slots/model.sdf +++ /dev/null @@ -1,54 +0,0 @@ - - - - - - - - - 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 deleted file mode 100644 index 7696eb8..0000000 Binary files a/rbs_mill_assist/assets/conductor/meshes/conductor.stl and /dev/null differ diff --git a/rbs_mill_assist/assets/conductor/model.config b/rbs_mill_assist/assets/conductor/model.config deleted file mode 100644 index 4d85e18..0000000 --- a/rbs_mill_assist/assets/conductor/model.config +++ /dev/null @@ -1,16 +0,0 @@ - - - - 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 deleted file mode 100644 index 79e9cd3..0000000 --- a/rbs_mill_assist/assets/conductor/model.sdf +++ /dev/null @@ -1,61 +0,0 @@ - - - - - - - - - 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 deleted file mode 100644 index d380252..0000000 --- a/rbs_mill_assist/assets/laser/laser.material +++ /dev/null @@ -1,11 +0,0 @@ -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 deleted file mode 100644 index b3f6ca0..0000000 --- a/rbs_mill_assist/assets/laser/meshes/laser.dae +++ /dev/null @@ -1,207 +0,0 @@ - - - - - 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 deleted file mode 100644 index 24c0d6a..0000000 --- a/rbs_mill_assist/assets/laser/meshes/laser.mtl +++ /dev/null @@ -1,2 +0,0 @@ -# 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 deleted file mode 100644 index e3c99c5..0000000 --- a/rbs_mill_assist/assets/laser/meshes/laser.obj +++ /dev/null @@ -1,103 +0,0 @@ -# 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 383c7ab..a15df0e 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 574c6c9..0713e03 100644 --- a/rbs_mill_assist/assets/laser/model.sdf +++ b/rbs_mill_assist/assets/laser/model.sdf @@ -7,33 +7,32 @@ - model://laser/meshes/laser.obj + model://laser/meshes/laser.stl - - 1 1 1 1 - 1 1 1 1 - 0.5 0.5 0.5 1 - 0 0 0 1 - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - 1000 - model://laser/meshes/laser.obj + model://laser/meshes/laser.stl diff --git a/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae b/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae index e7108de..3106369 100644 --- a/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae +++ b/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae @@ -3,66 +3,13 @@ Blender User - Blender 4.3.2 commit date:2024-12-16, commit time:21:10, hash:32f5fdce0a0a + Blender 4.1.1 commit date:2024-04-15, commit time:15:11, hash:e1743a0317bc - 2025-03-03T12:34:37 - 2025-03-03T12:34:37 + 2025-02-12T10:55:51 + 2025-02-12T10:55:51 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 - - - - @@ -92,7 +39,7 @@ - -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 + -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 @@ -140,20 +87,12 @@ - + - - 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 fc16e63..1674d34 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,21 +29,20 @@ - 2700 - 0.06 0.04 0.0005 + 0.06 0.04 0.001 - - - - - - - - - + + + + + + + + + diff --git a/rbs_mill_assist/bt/CMakeLists.txt b/rbs_mill_assist/bt/CMakeLists.txt index 6b5fd57..70f2b16 100644 --- a/rbs_mill_assist/bt/CMakeLists.txt +++ b/rbs_mill_assist/bt/CMakeLists.txt @@ -2,10 +2,8 @@ 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(rbs_mill_interfaces REQUIRED) +# find_package(std_srvs REQUIRED) # Behaviortree interfaces set(dependencies @@ -13,34 +11,14 @@ 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_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) +add_library(get_grasp_place_pose SHARED plugins/get_grasp_place_pose.cpp) +list(APPEND plugin_libs get_grasp_place_pose) 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 069aafc..6feace9 100644 --- a/rbs_mill_assist/bt/config/bt_executor.yaml +++ b/rbs_mill_assist/bt/config/bt_executor.yaml @@ -10,5 +10,4 @@ bt_action_server: - rbs_bt_executor/bt_plugins behavior_trees: - - rbs_mill_assist/xmls/moveit - - rbs_mill_assist/xmls/no_moveit + - rbs_mill_assist/xmls diff --git a/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp b/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp new file mode 100644 index 0000000..7eeb671 --- /dev/null +++ b/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp @@ -0,0 +1,118 @@ +#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 deleted file mode 100644 index 9e0451a..0000000 --- a/rbs_mill_assist/bt/plugins/get_grasp_pose.cpp +++ /dev/null @@ -1,89 +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 "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 deleted file mode 100644 index 518782c..0000000 --- a/rbs_mill_assist/bt/plugins/get_named_pose.cpp +++ /dev/null @@ -1,78 +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 "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 deleted file mode 100644 index 777e201..0000000 --- a/rbs_mill_assist/bt/plugins/get_place_pose.cpp +++ /dev/null @@ -1,90 +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 "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 deleted file mode 100644 index dda3714..0000000 --- a/rbs_mill_assist/bt/plugins/is_in_pose.cpp +++ /dev/null @@ -1,48 +0,0 @@ -#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 deleted file mode 100644 index 844ee92..0000000 --- a/rbs_mill_assist/bt/plugins/takeon_task.cpp +++ /dev/null @@ -1,54 +0,0 @@ -#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 deleted file mode 100644 index d94c7b6..0000000 --- a/rbs_mill_assist/bt/plugins/task_completed.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#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 deleted file mode 100644 index 7688e56..0000000 --- a/rbs_mill_assist/bt/plugins/twist_cmd.cpp +++ /dev/null @@ -1,85 +0,0 @@ -#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 5bb5c04..a7a2fb1 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(), "[%s] Starting send request", name().c_str()); + RCLCPP_INFO(this->logger(), "Starting send request"); if (!getInput("enable", request->data)) { - RCLCPP_ERROR(this->logger(), - "[%s] Failed to get sending data from input port", name().c_str()); + RCLCPP_ERROR(this->node_.lock()->get_logger(), + "Failed to get sending data from input port"); return false; } return true; @@ -35,7 +35,7 @@ public: NodeStatus onResponseReceived(const Response::SharedPtr &response) override { if (!response->success) { - RCLCPP_ERROR(this->logger(), "[%s] Response indicates failure.", name().c_str()); + RCLCPP_ERROR(this->logger(), "Response indicates failure."); return NodeStatus::FAILURE; } diff --git a/rbs_mill_assist/bt/xmls/Grasp.xml b/rbs_mill_assist/bt/xmls/Grasp.xml new file mode 100644 index 0000000..4d1bde0 --- /dev/null +++ b/rbs_mill_assist/bt/xmls/Grasp.xml @@ -0,0 +1,21 @@ + + + + + +