Система позиционирования с помощью модулей DWM1000

Пакет dwm1000_pose состоит из двух нод: map публикует карту маяков, трансформации TF, и маркеры визуализации, pose публикует позицию коптера относительно карты маяков.

Для расчета позиции коптера используется алгоритм трилатерации в виде системы нелинейных уравнений. Решение данной системы производится с помощью алгоритма Левенберга-Марквардта.

Быстрый старт

Чтобы запустить систему позиционирования:

roslaunch dwm1000_pose sample.launch

Нода pose

pose публикует рассчитанную позицию на основе расстояний от коптера до маяков с помощью алгоритма трилатерации.

Топики

Подписан

• dwm1000/beacon_data (dwm1000_msgs/BeaconDataArray) – список расстояний от маяков до коптера
• rangefinder/range (sensor_msgs/Range) – расстояние от коптера до пола

Публикует

• mavros/vision_pose/pose (geometry_msgs/PoseStamped) – рассчитанная позиция коптера

Нода map

map публикует карту маяков

Параметры

• map – путь к текстовому файлу со списком маяков

Файл карты имеет один маркер на строку со следующим форматом строки:

marker_id x y z yaw pitch roll

Топики

Публикует

• /dwm1000/visualization_anchor (visualization_msgs/MarkerArray) – визуализация карты маркеров для rviz

Опубликованные преобразования

• <map> => <anchor_map> – положение карты маяков

• <map> => <anchor><id> – положение маяков