После успешной поездки к Пруду в журнале осталось 25 предупреждений: обработчик препятствий не дождался преобразования координат. Облако точек публиковалось раньше соответствующей позы и TF. Исправлять пришлось готовую ARM64-библиотеку без исходников.

Я Лев Ларин, инженер Unibot. Мы подключаем Alfa Robotics SpeedyBot к своей платформе UNIBOT Fleet, сохраняя штатную навигацию робота-доставщика. Это четвёртая часть серии.

Начало серии · ← Часть 3: 25 FPS с видеокамеры по 3G. Модем, извини · Часть 5: Гражданин куст, уступите дорогу →

Владелец и SpeedyBot: точечная замена звена в условной ДНК программы

Поиск причины в бинарнике

На 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 публикует его и возвращается в штатную программу.

Раньше облако отправляли до позы и 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 с

Все команды завершились успешно. Шасси подтвердило контакт зарядки, штатная стыковка — завершение.

Реальный экран Fleet после контрольного маршрута Площадка — Пруд — док

За проверенное окно не было TF sync failed и вызовов реплики «Проезд перекрыт, пожалуйста, уступите дорогу» в журналах озвучки. Контроль блокировки оставался включён. В установленном комплекте были и другие исправления обработки препятствий, поэтому весь результат поездки нельзя приписать только TF-вставке.

Все 10 историй SpeedyBot

CRISPR для ARM: чиним ДНК робота