После успешной поездки к Пруду в журнале осталось 25 предупреждений: обработчик препятствий не дождался преобразования координат. Облако точек публиковалось раньше соответствующей позы и TF. Исправлять пришлось готовую ARM64-библиотеку без исходников.
Я Лев Ларин, инженер Unibot. Мы подключаем Alfa Robotics SpeedyBot к своей платформе UNIBOT Fleet, сохраняя штатную навигацию робота-доставщика. Это четвёртая часть серии.
Начало серии · ← Часть 3: 25 FPS с видеокамеры по 3G. Модем, извини · Часть 5: Гражданин куст, уступите дорогу →
Поиск причины в бинарнике
На RK3588 работают Linux/AArch64 и ROS1. Лидар RoboSense Airy Lite публикует /rslidar_points; laserLocation совмещает LiDAR и IMU и выдаёт /cloud_registered. Затем obstacle_cloud_classifier обрабатывает препятствия с учётом преобразований координат TF.
Для точки p преобразование имеет вид p_base=R(t)×p_sensor+T(t), где t — время измерения. Поза из соседнего момента сместит препятствие относительно поворачивающегося робота.
Записывали облако, одометрию и /tf вместе:
rostopic echo -n 1 /cloud_registered/header rostopic hz /cloud_registered rosbag record -O timing.bag /cloud_registered /tf /tf_static
Этого достаточно для разбора выходных сообщений. Для повторного запуска LIO нужны также исходные LiDAR/IMU, сохранённые в наших полных записях.
Классификатор ждал TF до 0,1 с и иногда выдавал TF sync failed. Подходящее преобразование приходило после предупреждения. По временным меткам, затем по машинному коду установили порядок: публикация облака, продолжение расчёта положения, публикация odometry/TF. В управляемом опыте с одним входом поздний TF менял выход обработчика.
Для ELF использовали Python и pyelftools, для инструкций AArch64 — Capstone, для отдельных функций — Unicorn, для Linux/ROS — QEMU. Вставку на C и ассемблере собирали Zig cc для aarch64-linux-gnu.
Первый осмотр binutils; liblio.so — условное имя файла:
file liblio.so readelf -h -l -Ws liblio.so aarch64-linux-gnu-objdump -d -C liblio.so > lio.asm
Адрес инструкции переводится в смещение ELF через подходящий PT_LOAD: file_offset=p_offset+(VA−p_vaddr). Читать и заменять четыре байта нужно по файловому смещению.
Стенд на ПК
На планировщике маршрутов мы уже исполняли настоящие ARM64-функции в Unicorn: задавали карту, препятствия и стоимость покрытия. ROS/C+±вызовы обслуживала подставная обвязка. Такой стенд проверяет конкретную функцию; время эмуляции не характеризует бортовой процессор.
Схема запуска по Unicorn API; загрузка сегментов и обработчики внешних вызовов опущены:
from unicorn import Uc, UC_ARCH_ARM64, UC_MODE_ARM from unicorn.arm64_const import UC_ARM64_REG_SP, UC_ARM64_REG_X0 uc = Uc(UC_ARCH_ARM64, UC_MODE_ARM) # map_elf_segments(uc, elf); map_stack_and_heap(uc) uc.reg_write(UC_ARM64_REG_SP, stack_top) uc.reg_write(UC_ARM64_REG_X0, native_object_address) # this uc.emu_start(function_address, return_trap)
Загрузчик задаёт стек, объект и адрес возврата. Для метода C++ нужны корректные объекты, аллокатор и ответы ROS. Полный процесс LIO поэтому проверяли отдельно в ARM64-VM.
Почему нельзя просто перенести вызов
Между формированием облака и публикацией позы меняются данные, используемые для расчёта точек. Поздний вызов мог пересчитать координаты. Кроме того, ранние выходы обходят новое место: на них облако перестало бы публиковаться.
Оставили формирование на прежнем месте и отложили только отправку. Добавленный исполняемый код переносит готовое сообщение во временный объект на стеке текущего вызова, после odometry/TF публикует его и возвращается в штатную программу.
Вставка не пересчитывает координаты, время и систему отсчёта. Временный объект принадлежит одному шагу; отдельной очереди и потока нет.
Владение сообщением
В этой сборке C+±объект PointCloud2 занимает 128 байт, массив точек хранится отдельно. Frame содержит объект и указатель на publisher. Перенос в frame.c:
typedef unsigned char Byte; typedef struct { Byte message[128]; void *publisher; } Frame; void move_cloud(void *publisher, void *message, Frame *frame) { Byte *source = message; for (unsigned i = 0; i < 128; ++i) frame->message[i] = source[i]; if (*(void **)(source + 16) == source + 32) *(void **)(frame->message + 16) = frame->message + 32; frame->publisher = publisher; for (unsigned i = 0; i < 128; ++i) source[i] = 0; *(void **)(source + 16) = source + 32; }
Поправка source+32 нужна для small string optimization в используемой libstdc++: короткий frame_id хранится внутри объекта строки. Простая копия оставила бы указатель в исходном сообщении. Для длинной строки отдельный буфер сохраняет свой адрес. Исходный объект переводим в пустое состояние, чтобы деструктор не освободил переданную память.
Смещения относятся к исследованным версиям ABI, ROS и libstdc++; для другой сборки раскладку нужно проверить заново.
Выпуск сообщения:
void flush_frame(Frame *frame) { if (frame->publisher) { native_publish(frame->publisher, frame->message); discard_frame(frame); } }
discard_frame вызывает родной деструктор и обнуляет publisher. Владение сохраняется до возврата из native_publish, чтобы при исключении память можно было освободить.
Переходы и ранние выходы
Инструкции AArch64 имеют размер четыре байта. Патчер заменяет проверенные B/BL переходами в добавленный сегмент ELF:
def branch(address, target, link=False): delta = target - address if delta % 4 or not -(1 << 27) <= delta < (1 << 27): raise ValueError('native branch displacement outside AArch64 ABI') opcode = 0x94000000 if link else 0x14000000 return opcode | ((delta // 4) & 0x3ffffff)
BL сохраняет адрес возврата, B передаёт управление. Проверяем выравнивание, диапазон смещения, исходную инструкцию и версию библиотеки.
Обёртка резервирует 160 байт стека, Frame размещается со смещения 16. После штатной PublishOdometry, выпускающей также TF, вызывается flush_frame. На обычном раннем выходе без odometry сохранённое сообщение выпускается перед возвратом, как в исходной ветке. При исключении освобождаем память и передаём ошибку дальше.
Первая версия проходила нормальный путь, но аварийно завершалась на проверке исключения. Причина — неполный диапазон unwind-описания. В ассемблере нужны DWARF CFI, LSDA, .cfi_personality, .cfi_lsda и cleanup с discard_frame, затем _Unwind_Resume. Добавили отсутствующий диапазон без обработчика.
Существенные флаги сборки:
zig cc -target aarch64-linux-gnu -O2 -ffreestanding \ -fno-stack-protector -fno-builtin -fPIC -fvisibility=hidden \ -fno-lto -fasynchronous-unwind-tables -nostdlib -static \ -Wl,--no-gc-sections -Wl,-T,payload.ld -Wl,--eh-frame-hdr \ -Wl,-e,run_entry frame.c abi.S -o payload.elf
payload.ld связывает вставку с родными функциями библиотеки. При -nostdlib её unwind-описание всё равно должно согласовываться с исключениями окружающего C+±кода.
Проверка на записях
Отдельная ARM64-программа проверила десять случаев: обычный ход, ранний выход, пустой результат и ошибки, с короткими и длинными строками.
Затем в QEMU запустили настоящий процесс LIO с ROS: сначала исходную библиотеку, затем исправленную, на одной записи LiDAR/IMU. В парном прогоне обе версии выдали по 69 облаков. Сравнили число, структуру и размеры сообщений, позы и TF. В дополнительном прогоне сравнили XYZ у 29 пар облаков.
Полного побайтового совпадения всех выходов двух запусков не было. Максимальное расхождение координат в отдельном сравнении — меньше 0,1 мм. Это различие replay-запусков, не точность локализации. Отдельно проверили, что патч сам готовые точки не пересчитывает.
Проверка на роботе
С резервной копией установили библиотеку, перезапустили компонент локализации с текущими картой и позой и проверили загруженный файл в работающем процессе.
Через Fleet выполнили три команды подряд: Площадка, Пруд, док. Джойстик, ручной вывод и повторные попытки не использовались; карты, скорости и защиты не менялись.
Команда |
Время до успешного завершения |
|---|---|
Площадка |
25,25 с |
Пруд |
37,22 с |
Возврат на док со стыковкой |
79,34 с |
Все команды завершились успешно. Шасси подтвердило контакт зарядки, штатная стыковка — завершение.
За проверенное окно не было TF sync failed и вызовов реплики «Проезд перекрыт, пожалуйста, уступите дорогу» в журналах озвучки. Контроль блокировки оставался включён. В установленном комплекте были и другие исправления обработки препятствий, поэтому весь результат поездки нельзя приписать только TF-вставке.
Все 10 историй SpeedyBot
Робот доставщик дороже автомобиля, мокрая трава и открытый JDWP
CRISPR для ARM: чиним ДНК робота — вы здесь.