Домашнее задание
В итоговом задании курса вы объедините все ключевые приёмы работы на роботе 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-файл:
-
Скетч на плате 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.01D480.5 <= d < 0.72D48,D490.3 <= d < 0.53D48,D49,D50d < 0.3(ближе30 см)4(вплотную)D48,D49,D50,D51 - открывает связь с Raspberry Pi на порту
-
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м/с) и не едет до нового нажатия «Старт».
- по нажатию кнопки «Старт» (
- читает топик лидара
-
Файл запуска (
~/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
- Возьмите за основу скетч
turtlebro_panel_nodeиз раздела «Управление роботом кнопками и launch-файл» и подключите заголовок<std_msgs/msg/Float32.h>. - Замените одиночный пин светодиода на массив
const int LED_PINS[4] = {48, 49, 50, 51};и перенесите из урока 5 вспомогательную функциюvoid showLitCount(int count). - Напишите обработчик подписки
void onDistance(std_msgs::Float32 msg):- по цепочке
if / else if / elseпереведите значениеmsg.data(в метрах) в число светодиодовlevelот0до4согласно таблице выше; - вызовите
showLitCount(level);.
- по цепочке
- В
setup()настройте пины кнопок и четырёх светодиодов, погасите шкалуshowLitCount(0), вызовитеros.begin(Serial1, "turtlebro_parktronic_node"), зарегистрируйтеros.advertise()для обеих кнопок и подпискуros.subscribe("panel/distance", onDistance). - Загрузите скетч со своего компьютера через разъём DUE.
Шаг 2. Напишите Python-узел и launch-файл на Raspberry Pi
- Возьмите за основу скрипт
~/ros2_ws/panel_drive_node.pyиз урока (убрав из него публикацию в старый топик/panel/led). - Добавьте в конструктор подписку на
/scan(LaserScan, профильqos_profile_sensor_data) и издателя/panel/distance(Float32) из опорного примера выше. - Оставьте управление флагом
self.movingи публикацией скорости в/cmd_velтолько в обработчиках кнопокon_startиon_stop. - Создайте
~/ros2_ws/parktronic.launch.pyпо образцуturtlebro_rosduino.launch.py, указав в нём путь~/ros2_ws/parktronic_drive_node.py.
Как проверить результат
- Запустите систему на Raspberry Pi робота по SSH:
ros2 launch ~/ros2_ws/parktronic.launch.py - Проверка парктроника без движения: не нажимая кнопок, плавно приближайте ладонь к лидару с расстояния
1,2 мдо20 см. На плате последовательно загорятся один (D48), два (D48–D49), три (D48–D50) и все четыре светодиода (D48–D51). Уберите руку — светодиоды погаснут в обратном порядке. - Проверка кнопок Старт и Стоп: поставьте робота на подставку (или на свободный пол) и нажмите кнопку
D28— колёса начнут вращаться вперёд (linear.x = 0.10м/с), при этом шкала светодиодов продолжит в реальном времени показывать расстояние до препятствий. - Нажмите кнопку
D15— колёса сразу остановятся (linear.x = 0.0м/с), и робот останется на месте до следующего нажатияD28.
Назад: Генерация собственных сообщений (rosduino_gen).
Дальше: Урок 8. Решение задания.