Keyboard shortcuts

Press ← or → to navigate between chapters

Press S or / to search in the book

Press ? to show this help

Press Esc to hide this help

Домашнее задание

В итоговом задании курса вы объедините все ключевые приёмы работы на роботе TurtleBro: обмен с Raspberry Pi через порт Serial1, предварительную регистрацию кнопок ros.advertise(), управление массивом из четырёх светодиодов D48–D51 (как в уроках 2 и 5) и обработку данных со штатного лазерного дальномера (лидара).

Вы соберёте световой парктроник с кнопочным управлением движением:

  • Python-узел на Raspberry Pi читает штатный топик лидара /scan, находит минимальное расстояние до препятствий и передаёт его в плату Arduino;
  • скетч на плате зажигает от 0 до 4 светодиодов D48–D51 по мере приближения препятствия;
  • кнопки «Старт» (D28) и «Стоп» (D15) включают и останавливают движение робота вперёд.

Поставьте робота на подставку при первой проверке

В этом задании движение включается и выключается только по нажатию кнопок (без автоматического торможения). Для первой проверки поставьте робота на коробку или подставку так, чтобы колёса свободно вращались в воздухе, а при заезде по полу держите руку наготове у кнопки «Стоп» (D15). Шкала парктроника работает постоянно — её удобно сначала испытать ладонью на стоящем роботе, не нажимая «Старт».

Как читать данные лидара из топика /scan

Лидар на верхней площадке TurtleBro непрерывно измеряет расстояния вокруг робота и публикует их в штатный топик /scan с типом sensor_msgs/msg/LaserScan.

В каждом сообщении LaserScan передаются:

  • msg.ranges — массив измеренных расстояний в метрах (0.30 соответствует 30 см, 1.0 — 1 м);
  • msg.range_min и msg.range_max — минимальная и максимальная рабочая дальность прибора.

Если луч не отразился от слишком далёкого или тёмного предмета, в msg.ranges попадают нули (0.0), бесконечность (inf) или nan. Поэтому перед поиском ближайшего объекта оставляют только конечные значения строго между msg.range_min и msg.range_max. Для подписки на поток лидара используют стандартный профиль качества обслуживания датчиков qos_profile_sensor_data, совместимый с любым драйвером лидара в ROS 2.

Пример узла на Raspberry Pi, который читает /scan и публикует минимальное расстояние (в метрах) в топик /panel/distance (std_msgs/msg/Float32):

#!/usr/bin/env python3
import math
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Float32


class LidarMinExample(Node):
    def __init__(self):
        super().__init__('lidar_min_example')
        self.create_subscription(
            LaserScan,
            '/scan',
            self.on_scan,
            qos_profile_sensor_data
        )
        self.dist_pub = self.create_publisher(Float32, '/panel/distance', 10)

    def on_scan(self, msg: LaserScan):
        valid = [
            r for r in msg.ranges
            if math.isfinite(r) and msg.range_min < r < msg.range_max
        ]
        if not valid:
            return

        dist_msg = Float32()
        dist_msg.data = float(min(valid))
        self.dist_pub.publish(dist_msg)


def main(args=None):
    rclpy.init(args=args)
    node = LidarMinExample()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

Если запустить такой скрипт на роботе и выполнить ros2 topic echo /panel/distance, при приближении ладони к лидару число в метрах будет плавно уменьшаться (например, с 1.20 до 0.20).

Что должно получиться

Вам нужно доработать и скетч на плате Arduino, и управляющий Python-узел на Raspberry Pi, а также собрать для них единый launch-файл:

  1. Скетч на плате Arduino (turtlebro_parktronic_node):

    • открывает связь с Raspberry Pi на порту Serial1: ros.begin(Serial1, "turtlebro_parktronic_node");
    • заранее регистрирует в setup() топики кнопок /panel/btn_start и /panel/btn_stop (std_msgs/msg/Bool) через ros.advertise() и публикует в них события смены состояния кнопок D28 и D15 с периодом опроса 30 мс;
    • подписывается на топик /panel/distance (std_msgs/msg/Float32) и зажигает шкалу из четырёх светодиодов {48, 49, 50, 51} в зависимости от расстояния d (в метрах):
    Расстояние d в /panel/distanceКоличество горящих светодиодовВключённые пины на плате
    d >= 1.00 (свободно)все погашены
    0.7 <= d < 1.01D48
    0.5 <= d < 0.72D48, D49
    0.3 <= d < 0.53D48, D49, D50
    d < 0.3 (ближе 30 см)4 (вплотную)D48, D49, D50, D51
  2. Python-узел на Raspberry Pi (~/ros2_ws/parktronic_drive_node.py):

    • читает топик лидара /scan (sensor_msgs/msg/LaserScan), вычисляет минимальное корректное расстояние в метрах и отправляет его в плату через топик /panel/distance (std_msgs/msg/Float32);
    • подписывается на кнопки /panel/btn_start и /panel/btn_stop (std_msgs/msg/Bool) и управляет движением в топике /cmd_vel (geometry_msgs/msg/Twist) только по кнопкам:
      • по нажатию кнопки «Старт» (D28) робот начинает плавно ехать вперёд (linear.x = 0.10 м/с с частотой 10 Гц);
      • по нажатию кнопки «Стоп» (D15) робот останавливается (linear.x = 0.0 м/с) и не едет до нового нажатия «Старт».
  3. Файл запуска (~/ros2_ws/parktronic.launch.py):

    • запускает одной командой rosduino_agent (на порту моста Serial1 с параметром keep_on_disconnect: True) и узел parktronic_drive_node.py.

Требования к программе

  • В скетче Arduino не используется delay(), в loop() вызывается ros.update(), а опрос кнопок выполняется через rosduino::Timer(30).
  • Для зажигания линейки светодиодов используется массив const int LED_PINS[4] = {48, 49, 50, 51}; и цикл for.
  • Топики кнопок предварительно регистрируются в setup() после ros.begin(Serial1, ...) с помощью ros.advertise().

Как составить алгоритм

Шаг 1. Доработайте скетч для платы Arduino

  1. Возьмите за основу скетч turtlebro_panel_node из раздела «Управление роботом кнопками и launch-файл» и подключите заголовок <std_msgs/msg/Float32.h>.
  2. Замените одиночный пин светодиода на массив const int LED_PINS[4] = {48, 49, 50, 51}; и перенесите из урока 5 вспомогательную функцию void showLitCount(int count).
  3. Напишите обработчик подписки void onDistance(std_msgs::Float32 msg):
    • по цепочке if / else if / else переведите значение msg.data (в метрах) в число светодиодов level от 0 до 4 согласно таблице выше;
    • вызовите showLitCount(level);.
  4. В setup() настройте пины кнопок и четырёх светодиодов, погасите шкалу showLitCount(0), вызовите ros.begin(Serial1, "turtlebro_parktronic_node"), зарегистрируйте ros.advertise() для обеих кнопок и подписку ros.subscribe("panel/distance", onDistance).
  5. Загрузите скетч со своего компьютера через разъём DUE.

Шаг 2. Напишите Python-узел и launch-файл на Raspberry Pi

  1. Возьмите за основу скрипт ~/ros2_ws/panel_drive_node.py из урока (убрав из него публикацию в старый топик /panel/led).
  2. Добавьте в конструктор подписку на /scan (LaserScan, профиль qos_profile_sensor_data) и издателя /panel/distance (Float32) из опорного примера выше.
  3. Оставьте управление флагом self.moving и публикацией скорости в /cmd_vel только в обработчиках кнопок on_start и on_stop.
  4. Создайте ~/ros2_ws/parktronic.launch.py по образцу turtlebro_rosduino.launch.py, указав в нём путь ~/ros2_ws/parktronic_drive_node.py.

Как проверить результат

  1. Запустите систему на Raspberry Pi робота по SSH:
    ros2 launch ~/ros2_ws/parktronic.launch.py
    
  2. Проверка парктроника без движения: не нажимая кнопок, плавно приближайте ладонь к лидару с расстояния 1,2 м до 20 см. На плате последовательно загорятся один (D48), два (D48–D49), три (D48–D50) и все четыре светодиода (D48–D51). Уберите руку — светодиоды погаснут в обратном порядке.
  3. Проверка кнопок Старт и Стоп: поставьте робота на подставку (или на свободный пол) и нажмите кнопку D28 — колёса начнут вращаться вперёд (linear.x = 0.10 м/с), при этом шкала светодиодов продолжит в реальном времени показывать расстояние до препятствий.
  4. Нажмите кнопку D15 — колёса сразу остановятся (linear.x = 0.0 м/с), и робот останется на месте до следующего нажатия D28.

Посмотреть решение и разбор

Назад: Генерация собственных сообщений (rosduino_gen).
Дальше: Урок 8. Решение задания.