Узел ROS2 для генерации точек вблизи поверхности мэша

Узел содержит сервисы для создания и удаления точек вблизи поверхности указанного мэша.

Мэш должен состоять только из треугольных граней; требований по непрерывности и др. хорошим свойствам нет, но обрабатываются только грани с уклоном не более 0.1 рад (5.7 градусов), чтобы не заставлять робота ездить по крутым склонам.

Если в составе мэша есть несколько отдельных несвязных областей с почти горизонтальными мэшами, используется только та, в которой больше граней (независимо от площади и т.п.).

По возможности точки располагаются друг от друга на расстоянии не меньше 0.01 (в единицах, в которых заданы координаты мэша); от поверхности они удалены на 0.1. Вероятность генерации на каждом треугольнике одинакова

Установка

Для работы требуется нестандартный модуль Python trimesh. Чтобы поставить его, можно использовать rosdep:

export PIP_BREAK_SYSTEM_PACKAGES=1

rosdep install --from-paths src -y --ignore-src

Собирать всё потом можно, как обычно

colcon build --symlink-install

Запуск

source install/local_setup.bash

ros2 launch gen_locations_on_mesh mesh_loc_generator.py

После первого запуска генерируется кэшированный результат предобработки мэша в течение нескольких секунд (10-20); об окончании этого говорит появившееся сообщение

Cache for gen_locations_on_mesh saved successfully

Если нужно использовать другой мэш, то кэшированную версию надо удалить вручную из директории install/gen_locations_on_mesh/share/gen_locations_on_mesh/share/cache.pkl

Топики

Состояние постоянно спамится в топик locations типа gen_locations_on_mesh_msgs/msg/LocationsOnMesh. Каждое из его полей: location_id, x, y, z --- список, содержащий параметры точек. Все списки имеют одну длину. Координаты даны в системе координат, приемлемой для Unity

Маркеры всех точек (и актуальных, и потухших) постоянно спамятся в топик locations_markers типа visualization_msgs/MarkerArray. Активные точки показаны красным, потушенные - синим.

Сервисы

Генерация точки выполняется вызовом сервиса generate_location без параметров:

ros2 service call /generate_location gen_locations_on_mesh_msgs/srv/GenerateLocation {}

В ответе содержатся номер точки и её координаты: gen_locations_on_mesh_msgs.srv.GenerateLocation_Response(location_id=1, x=434.56764747196445, y=-969.690370275587, z=73.794516038155)

Для уничтожения ранее созданной точки вызывается сервис remove_location с параметром location_id --- номером точки.

ros2 service call /remove_location gen_locations_on_mesh_msgs/srv/RemoveLocation '{location_id: 2}'

Возвращаемое значение False говорит о том, что такая точка неизвестна; True --- что точка успешно удалена.

Параметры

• mesh_filename --- имя файла мэша;
• cache_filename --- файл для кэширования результатов предобработки мэша, ускоряющий загрузку. При использовании нового мэша следует удалить старый вручную;
• init_number_of_points --- начальное число активных точек, по умолчанию 10;
• max_z_coord --- максимальная высота точки (координата z), по умолчанию 2.;
• min_distance_between_points --- минимальная дистанция между точками, по умолчанию 2.;
• height_upon_surface --- высота точки над поверхностью мэша, по умолчанию 0.1;
• num_generation_trials --- число попыток создать точку, удовлетворяющую условиям; по умолчанию 1000.