Код для оценки визуальной, лидарной и колесной одометрии, комплексирования навигационных данных, а также локализации по заранее сохраненной карте размещен в папке odometry
Код для построения и обновления карты проходимости по данным лидара размещен в папке occupancy_grid_mapping
Код для инстанс-сегментации объектов и оценке их 6Dof позы размещен в папке 3D_objects_detection
Код для планирования пути мобильной платформы размещен в папке planning
Код для управления движением мобильной платформы размещен в папке control
Код для планирования и управления движением манипулятором размещен в папке manipulator
В рамках сценария робот подъезжал к панели лифта, вызывал лифт, заезжал в него и выбирал нужный этаж.
Схема координации ROS-узлов при выполнении сценария приведена на схеме ниже. Для наглядности процессов координации узлов эта схема организована несколько иначе, чем схема общей архитектуры системы и на ней опущены модули, отвечающие за реактивный уровень архитектуры (кроме модуля управления платформой). Стратегический узел-координатор (lift_scenario_node), ответственный за выполнение сценария, определяет, решение каких подзадач требуется при выполнении сценария. Эта информация передается на узел, ответственный за распределение ресурсов между процессами (launcher_killer), который предоставляет вычислительные ресурсы либо модулям локализации и построения карты (Odometry, Navigation) и планирования движения (planner_cds) колесной платформы, либо модулю распознавания объектов (solo_node). При заезде в лифт модуль управления платформой (сontrol) отрабатывает предпрописанную траекторию, не используя данные навигации. Модуль планирования и управления манипулятором (press_button) не требует доступа к ресурсам, поскольку выполняется на выделенном аппаратном обеспечении манипулятора.
Структура фреймворка с описанием ROS-узлов в pdf
В данном документе подробно описаны различные системы координат робота и методы перехода между ними.
Occupancy grid в rtabmap представлена стандартными сообщениями ros. Подробнее про систему координат occupancy grid можно почитать в google doc’е в разделе “Работа с Occupancy Grid”. Также в папке occupancy_grid_mapping есть папка rtabmap_example, в которой находится скрипт occupancy_grid_demo.py, реализующий работу с occupancy grid. Про этот скрипт написано в том же google doc'е в разделе "Реализация работы с occupancy grid".
2. Переход из системы координат карты в систему координат одометрии (в систему координат baselink робота)
(Андрей Криштопик + Линар Абдразаков + Владислав Головин)
Чтобы получить трансформацию между различными системами координат, можно использовать функцию lookupTransform для C++ или lookup_transform для Python. Пример получения координат робота в мировой системе координат можно посмотреть в скрипте occupancy_grid_demo.py в папке occupancy_grid_mapping/rtabmap_example/scripts в функции get_robot_world_pose.
Есть три основных систем координат:
- map - система координат карты;
- odom - система координат, относительно которой считается одометрия (совпадает с системой координат base_link при запуске одометрии);
- base_link - система координат робота.
В сообщениях одометрии хранится трансформация между двумя системами координат (поля frame_id и child_frame_id). Каждый метод одометрии имеет свои системы координат frame_id и child_frame_id, информация о которых не публикуется в топик /tf. Поэтому их нужно привести к общим системам координат так, чтобы frame_id = odom и child_frame_id = base_link. Перевод одометрии в систему координат base_link происходит с использованием пакета [tf_transformer](odometry/odometry_fusion/tf_transformer) , который автоматически запускается вместе с robot_localization.
В папке occupancy_grid_mapping есть папка rtabmap_example, в которой находится launch файл для запуска rtabmap и вспомогательные скрипты. Про запуск rtabmap с помощью launch файла из rtabmap_example написано в google doc'е. В этом же google doc'е описана работа в режиме multi-session.
Запуск методов одометрии и их комплексирования описан здесь.
Комплексирование происходит трех видов одометрии: визуальная, лидарная и колесная.
Перед комплексированием каждая одометрия приводится к системе координат base_link (frame_id=odom, child_frame_id=base_link).
Изменить конфигурацию комплексирования можно в данном файле. Описание параметров конфигурации можно найти здесь.
Планирование движения осуществляется с помощью узлов, находящихся в папке planning. Для постановки задачи планировщику необходимо подать на вход карту проходимости типа nav_msgs/OccupancyGrid, целевую точку geometry_msgs/PoseStamped и локализацию робота - трансформацию geometry_msgs/TransformStamped из tf дерева. На выходе узел выдает траекторию типа geometry_msgs/PoseArray и визуализацию этой траектории visualization_msgs/Marker. Подробнее о работе узла планирования написано здесь.
Модуль управления обеспечивает высокоуровневый контроллер (high-level control) для мобильного робота с дифференциальным приводом. Он содержит два метода управления роботом - отслеживание пути (Path Following) и отслеживание траектории (Trajectory Tracking). Модуль управления реализован в ROS. Пакеты управления принимают одометрие робота и путь планировщика в качестве входных данных и выдает закон управления линейной и угловой скоростями в качестве выходных данных. Подробнее о узле управления написано здесь.
Инструкция по запуску манипулятора и установке соединения с помощью RTDE и ROS driver: https://drive.google.com/file/d/16oZ_x8Eod7x_Ixsb_DDcYgvkqMCB1I2m/view?usp=sharing Также там описание скриптов set_freedrive и fold_the_robot, которые можно применить, чтобы изменить положение манипулятора вручную (например, если он мешает работе сенсоров), а потом автоматически вернуть манипулятор в исходную позицию. Код, задействующий манипулятор, на Husky лежит в папке Repos/manipulator.
Набор данных для инстанс-сегментации кнопок лифта ElevatorButtons: https://github.com/linukc/elevator_buttons
Сегментация кнопок моделью SOLOv2. Используется 1 нода solo (распознавание на rgb) и данные с карты глубины. Команды для запуска на роботе Husky находятся в docs. Публикуется PoseStamped msg с координатами центра и оринтацией в пространстве. Ориентация в пространстве получена с помощью 3D координат точек вокруг цели.
Основная информация о состоянии робота публикуется в топик /status.
Пример вывода топика /status при питании робота от сети
header:
seq: 28287
stamp:
secs: 1614184608
nsecs: 173392658
frame_id: ''
uptime: 28330379
ros_control_loop_freq: 9.9974492508
mcu_and_user_port_current: 0.82
left_driver_current: 0.0
right_driver_current: 0.0
battery_voltage: 26.88
left_driver_voltage: 0.0
right_driver_voltage: 0.0
left_driver_temp: 0.0
right_driver_temp: 0.0
left_motor_temp: 0.0
right_motor_temp: 0.0
capacity_estimate: 480
charge_estimate: 1.0
timeout: False
lockout: True
e_stop: Trueна рисунке \ref{fig:architecture}
ros_pause: False
no_battery: True
current_limit: False