ПР08. Локализация и цепочка map→odom

НЕДЕЛЯ 08понять→собрать→сломать→доказать→проверить себя

Работа продолжает проектную систему. Рассчитывайте на одну лабораторную пару и примерно 2,5 часа самостоятельной доработки.

Применение AMCL к дифференциальному командному роботу описано в маршруте «симуляция → реальный робот». В проекте сверяем его frames и карту; в этой ПР используем опубликованные входы и критерии без замены модели.

На лекции мы увидим и локализацию, и движение к цели через Nav2. В этой работе вы самостоятельно собираете запуск AMCL и измерение результата. Планировщик, контроллер движения и собственная SLAM-карта для зачёта ПР08 не требуются.

Входные данные

Скачайте недельный course kit. Для ПР08 используются:

• .course-kit/v1/worlds/mcity-minimal.sdf — тот же ограниченный коридор, в котором записывались данные ПР07;
• .course-kit/v1/fixtures/pr08/map.yaml и map.pgm — карта collision-геометрии этого мира;
• .course-kit/v1/fixtures/pr08/amcl.yaml — обязательный профиль AMCL;
• .course-kit/v1/fixtures/pr08/contract.json — топики, frames, геометрия и численные пороги;
• .course-kit/v1/fixtures/pr08/localization-result-v1.schema.json — точный формат итогового localization.json.

Используйте исправленную запись fixtures/pr07-sensors из ПР07 продолжительностью не менее 30 секунд. Она должна быть сделана в mcity-minimal.sdf той же ревизии и содержать:

Топик	Тип	Обязательные поля
/vehicle/scan	sensor_msgs/msg/LaserScan	header.frame_id=lidar_link
/vehicle/odom	nav_msgs/msg/Odometry	header.frame_id=odom, child_frame_id=base_link
/tf	tf2_msgs/msg/TFMessage	динамическое odom→base_link
/tf_static	tf2_msgs/msg/TFMessage	статическое base_link→lidar_link
/clock	rosgraph_msgs/msg/Clock	время симуляции

Исходный bringup Gazebo не создаёт /tf_static. Во время записи запустите robot_state_publisher с URDF/Xacro из ПР06: он является единственным владельцем base_link→lidar_link. Преобразование odom→base_link публикует один адаптер одометрии ПР07, который снял префикс course_ackermann/ с frames Gazebo и сохранил исходные stamp, pose и twist. Если ros2 bag info не показывает все пять топиков, исправьте и повторите запись до начала ПР08. Проверьте также, что запись содержит движение: AMCL обновляет оценку после перемещения, поэтому неподвижный робот может не дать десяти сообщений /amcl_pose. Во время replay используйте записанные преобразования и не запускайте их вторых владельцев.

Цель

Запустить AMCL на согласованных карте и записи, измерить его внутреннюю сходимость и доказать, что отсутствие map→odom вызвано неверным base_frame_id, а не картой или переустановкой Nav2. После 10 секунд разгона доступность map→odom должна быть не ниже 95%, а медиана covariance[0] + covariance[7] сообщений /amcl_pose — не выше 0,25 м².

Это критерии сходимости и уверенности оценки. В ПР08 нет независимой ground truth-траектории, поэтому ковариация не называется абсолютной ошибкой позиции.

Критерии зачтено

Работа зачтена, когда выполнены все три стадии и прошла автопроверка:

Стадия	Что требуется
1 · Собрать	map_server + AMCL + lifecycle manager запускаются одной командой; фиксированы карта, запись, начальная поза и владельцы трёх преобразований
2 · Сломать	в копии amcl.yaml задан отсутствующий base_frame_id: chassis; сохранены ошибка tf2 и недоступность map→odom
3 · Доказать	возвращён base_link; та же карта и запись дают доступность ≥ 95% и медиану ковариации ≤ 0,25 м²
Автопроверка	check_practice.py PR08 проверит src/**/package.xml, src/**/localization.launch.py, src/**/amcl.yaml, evidence/pr08/{tf.md,localization.json,schema-check.txt,map.*} и заявления public_tests_passed, defect_reproduced, defect_fixed

Что получится

Пакет localization_case будет запускать карту и AMCL для сохранённой записи, а отчёт покажет доступность преобразования и ковариацию на одной временной шкале. Дефект и исправление воспроизводятся без изменения входных данных.

Карта и системы координат

Карта имеет разрешение 0,25 м/ячейку, размер 52×24 и начало координат (-2,5; -3,0; 0). Стены ограничивают область x=-2…10; асимметричные выступы вдоль стен дают лидару признаки для оценки координаты x. Маршрут от (0, 0) до (8, 0) и полоса |y|≤0,8 свободны.

Каждое преобразование публикует ровно один участник:

Преобразование	Владелец	Топик
map→odom	AMCL во время локализации	/tf, dynamic
odom→base_link	адаптер одометрии из записи ПР07	/tf, dynamic
base_link→lidar_link	robot_state_publisher из записи ПР07	/tf_static, static

AMCL получает /vehicle/scan, карту /map, начальную позу (x=0, y=0, yaw=0) и общее время /clock. Он не должен повторно публиковать odom→base_link или статическую связь лидара.

Перед началом

Продолжайте в той же ROS-среде, в которой выполняли ПР07. Nav2 может отсутствовать даже в desktop-full. После настройки apt-репозитория ROS по инструкции установки установите пакеты для своего дистрибутива:

# Сначала source /opt/ros/<ваш-дистрибутив>/setup.bash
sudo apt update
sudo apt install "ros-${ROS_DISTRO}-nav2-amcl" \
  "ros-${ROS_DISTRO}-nav2-map-server" \
  "ros-${ROS_DISTRO}-nav2-lifecycle-manager" \
  "ros-${ROS_DISTRO}-tf2-ros-py" "ros-${ROS_DISTRO}-tf2-tools"
ros2 pkg prefix nav2_amcl
ros2 pkg prefix nav2_map_server
ros2 pkg prefix nav2_lifecycle_manager

В Docker установите их при сборке образа, без sudo. Workspace собирайте в этой же среде. Для replay Gazebo, мост и управляющая нода уже не нужны: остановите их, чтобы они не добавляли второй /clock и вторую одометрию.

Что сделать

На паре сначала откройте карту и получите связное дерево frames. Затем разберите отказ из-за base_frame_id. Измерение и оформление отчёта можно завершить самостоятельно.

1. Проверьте хэши карты и состав записи:

   sha256sum .course-kit/v1/fixtures/pr08/map.yaml \
     .course-kit/v1/fixtures/pr08/map.pgm
   ros2 bag info fixtures/pr07-sensors

2. Создайте пакет localization_case и скопируйте профиль:

   ros2 pkg create localization_case --destination-directory src \
     --build-type ament_cmake --license Apache-2.0
   mkdir -p src/localization_case/config
   cp .course-kit/v1/fixtures/pr08/amcl.yaml \
     src/localization_case/config/amcl.yaml

   В package.xml объявите зависимости времени исполнения (exec_depend): ament_index_python, launch, launch_ros, nav2_amcl, nav2_map_server, nav2_lifecycle_manager, rclpy, tf2_ros_py, geometry_msgs, rosgraph_msgs. Пакет устанавливает launch, конфигурацию и Python-скрипт, поэтому в CMake достаточно find_package(ament_cmake REQUIRED); find_package(ament_index_python ...) и аналогичные поиски Python-пакетов добавлять не нужно.

   Реализуйте localization.launch.py с аргументами params_file и map. Одной командой он запускает ноды map_server, amcl и lifecycle_manager_localization; абсолютный путь аргумента map передаётся map server как yaml_filename. Все ноды используют use_sim_time=true. Добавьте в CMakeLists.txt установку каталогов launch и config в share/${PROJECT_NAME}, иначе ros2 launch не найдёт эти файлы после сборки. Имена нод должны совпадать с ключами YAML: map_server, amcl, lifecycle_manager_localization. В профиле уже указаны autostart: true и node_names: [map_server, amcl]. Менеджер переводит эти две ноды из unconfigured через inactive в active; наличие процесса само по себе ещё не означает, что карта и AMCL работают.

3. Запустите исходный профиль и запись с начальной позой из amcl.yaml. Убедитесь, что /amcl_pose появляется, а цепочка map→odom→base_link→lidar_link связна.

4. Напишите небольшую ноду измерения с use_sim_time=true. Запустите её до воспроизведения записи, чтобы она приняла первый /clock. Обозначьте его время как t0; оно не обязано быть нулевым. Пропустите первые 10 секунд и измеряйте в интервале [t0+10; t0+30) по правилам ниже. Программа должна записать измеренные значения, а не заранее подготовленные числа. Установите исполняемый Python-скрипт через install(PROGRAMS ... DESTINATION lib/${PROJECT_NAME}), чтобы запускать его командой ros2 run localization_case <имя_скрипта> после сборки.

5. Скопируйте профиль в amcl-defect.yaml и замените только base_frame_id: base_link на base_frame_id: chassis. На той же записи сохраните сообщение о недоступном frame и отсутствие map→odom. Для дефекта достаточно нескольких секунд replay: после появления ошибки сначала остановите launch локализации, пока player ещё работает, и только затем player. В Jazzy ожидание отсутствующего transform может зависнуть, если /clock уже остановлен. Полное окно метрик нужно исправленному запуску, а не дефектному. Перед каждым опытом заново запускайте локализацию, player и измеритель. Так AMCL и буфер tf2 не сохранят преобразование из предыдущего успешного опыта. Не используйте --loop: при возврате времени назад вы получите ещё один источник ошибок. Верните base_link и повторите без изменения карты, записи, начальной позы и окна измерения.

6. Скопируйте карту в evidence и заполните отчёт по опубликованной схеме. json.tool проверяет только синтаксис, поэтому итог обязательно пропустите через недельный валидатор: он проверяет типы, диапазоны, лишние поля, отношение tf_queries_successful / tf_queries_total, число запросов при 10 Гц, длительность записи и реальные SHA-256 двух файлов карты.

   mkdir -p evidence/pr08
   cp .course-kit/v1/fixtures/pr08/map.* evidence/pr08/
   python3 .course-kit/v1/tools/validate_localization.py \
     evidence/pr08/localization.json \
     --schema .course-kit/v1/fixtures/pr08/localization-result-v1.schema.json \
     --map-yaml evidence/pr08/map.yaml \
     --map-pgm evidence/pr08/map.pgm \
     > evidence/pr08/schema-check.txt 2>&1
   cat evidence/pr08/schema-check.txt

evidence/pr08/localization.json содержит SHA-256 обоих файлов карты, путь и длительность записи по первому и последнему /clock, начальную позу, длительность окна, число всех и успешных tf2-запросов, их долю, число /amcl_pose, медиану ковариации и значения base_frame_id до/после. evidence/pr08/tf.md фиксирует команду, ошибку до исправления, владельцев связей и результат той же команды после исправления.

Как измерять

Все интервалы отсчитываются по записанному /clock, а не по настенным часам и не от абсолютной отметки 10.0. Если запись начинается с 42.0, разгон закончится на 52.0, а двадцатисекундное окно — на 72.0.

• На каждом шаге 0,1 с в окне делайте один запрос lookup_transform('map', 'odom', rclpy.time.Time()). Нулевой аргумент времени означает последнее доступное преобразование. Для окна 20 с ожидаются 200 запросов. Исключение tf2 считается неуспешным запросом; доля успешных должна быть не ниже 0.95.
• Считайте только /amcl_pose, чей header.stamp попал в то же окно. Требуются минимум 10 сообщений. Для каждого сложите элементы 0 и 7 ковариационной матрицы, затем найдите медиану этих сумм.
• Если запись закончилась до конца окна, время пошло назад или не удалось выдержать частоту запросов, сообщите об этом и повторите опыт. При нагрузке можно снизить скорость player (--rate 0.5): окно всё равно измеряется в секундах симуляции. Не заменяйте пропущенные запросы серией запросов к одному и тому же состоянию буфера.

Это проверка доступности последней связи в tf2. Она сама по себе не доказывает свежесть каждого transform, точность положения или отсутствие пропусков сенсора. В tf.md отдельно сопоставьте stamp лидара, одометрии и /clock. Маленькая ковариация означает уверенность AMCL; уверенная, но неверная поза тоже возможна. Сравните наложение /vehicle/scan на стены /map в RViz.

Проверка

Обязательные файлы сдачи

• В корне репозитория — AI_USAGE.md, с отдельной записью для PR08: модель, инструмент, характер помощи и способ проверки. Если ИИ не использовался, напишите это явно и поставьте ai_used: false в отчёте; файл всё равно нужен. Предыдущие записи сохраняйте. Правила и пример.
• В evidence/pr08/report.json поле commit — SHA уже созданного коммита реализации. Отчёт фиксируется следующим коммитом; его SHA указывается при сдаче вместе со ссылкой на CI. Пошаговый пример двух коммитов.

Сначала соберите пакет:

colcon build --symlink-install --packages-select localization_case
source install/setup.bash
colcon test --packages-select localization_case

В первом терминале запустите локализацию из корня репозитория:

ros2 launch localization_case localization.launch.py \
  params_file:=$PWD/src/localization_case/config/amcl.yaml \
  map:=$PWD/.course-kit/v1/fixtures/pr08/map.yaml

Проверьте, что обе управляемые ноды активны:

ros2 lifecycle get /map_server
ros2 lifecycle get /amcl

Во втором терминале запустите свою ноду измерения и дождитесь её готовности принимать /clock. Только после этого в третьем терминале воспроизведите неизменную запись:

ros2 bag play fixtures/pr07-sensors

Не добавляйте --clock: источник времени уже есть внутри записи. В исправленном опыте дождитесь сохранения метрик и конца записи, затем завершите launch локализации через Ctrl+C. Дефектный опыт заканчивайте раньше, в порядке из шага 5. Перед повтором запустите все процессы заново.

В отдельном терминале можно проверить наблюдаемый результат:

ros2 topic echo /amcl_pose --once
ros2 run tf2_ros tf2_echo map odom --ros-args -p use_sim_time:=true
ros2 run tf2_tools view_frames --ros-args -p use_sim_time:=true

После записи результата выполните проверки:

python3 .course-kit/v1/tools/validate_localization.py \
  evidence/pr08/localization.json \
  --schema .course-kit/v1/fixtures/pr08/localization-result-v1.schema.json \
  --map-yaml evidence/pr08/map.yaml --map-pgm evidence/pr08/map.pgm
python3 .course-kit/v1/tools/check_practice.py PR08 --submission .

Готово, если map→odom имеет одного владельца, доступность после разгона составляет не менее 95%, медиана ковариации не превышает 0,25 м², а замена base_link на chassis воспроизводит и объясняет тот же отказ.

Для дополнительного опыта запустите отдельный replay с намеренно неверной начальной позой и наблюдайте восстановление оценки; обязательное измерение сохраните с исходной позой (0, 0, 0).

Как работаем на лабораторной

После проверки моста и одометрии сверим ros2 bag info с таблицей входов и владельцев tf2. Затем воспроизведите дефект chassis, сохраните первую ошибку AMCL и верните base_link, не меняя карту и запись.

Общий порядок: runbook · troubleshooting.

Декларация AI_USAGE.md оформляется по правилам курса, в том числе без ИИ.

Официальные справочники

• Nav2 concepts
• Настройка AMCL
• Nav2 Lifecycle Manager
• REP-105: map, odom, base_link
• tf2 и время