Урок 8. Решение задания
На этой странице приведено полное решение итогового задания курса: скетч светового парктроника с кнопками на плате TurtleBro, управляющий Python-узел parktronic_drive_node.py для чтения лидара /scan и управления движением /cmd_vel, а также файл единого запуска parktronic.launch.py.
1. Скетч на плате TurtleBro (turtlebro_parktronic_node.ino)
Загрузите этот скетч в плату робота со своего компьютера через разъём DUE:
#include <RosDuino.h>
#include <std_msgs/msg/Bool.h>
#include <std_msgs/msg/Float32.h>
const int START_BTN_PIN = 28;
const int STOP_BTN_PIN = 15;
const int LED_COUNT = 4;
const int LED_PINS[LED_COUNT] = {48, 49, 50, 51};
std_msgs::Bool btnMsg;
rosduino::Timer buttonTimer(30); // 30 мс для защиты кнопок от дребезга
int lastStartState = LOW;
int lastStopState = LOW;
// Зажигает первые count светодиодов шкалы и гасит остальные.
void showLitCount(int count) {
for (int i = 0; i < LED_COUNT; i++) {
digitalWrite(LED_PINS[i], (i < count) ? HIGH : LOW);
}
}
// Переводит минимальную дистанцию лидара (в метрах) в число светодиодов 0..4.
void onDistance(std_msgs::Float32 msg) {
float d = msg.data;
int level = 0;
if (d < 0.30f) {
level = 4;
} else if (d < 0.50f) {
level = 3;
} else if (d < 0.70f) {
level = 2;
} else if (d < 1.00f) {
level = 1;
} else {
level = 0;
}
showLitCount(level);
}
void setup() {
pinMode(START_BTN_PIN, INPUT);
pinMode(STOP_BTN_PIN, INPUT);
for (int i = 0; i < LED_COUNT; i++) {
pinMode(LED_PINS[i], OUTPUT);
}
showLitCount(0);
lastStartState = digitalRead(START_BTN_PIN);
lastStopState = digitalRead(STOP_BTN_PIN);
// На роботе связь с Raspberry Pi идёт через аппаратный порт Serial1.
ros.begin(Serial1, "turtlebro_parktronic_node");
ros.advertise("panel/btn_start", btnMsg);
ros.advertise("panel/btn_stop", btnMsg);
ros.subscribe("panel/distance", onDistance);
}
void loop() {
ros.update();
if (buttonTimer.check()) {
int startReading = digitalRead(START_BTN_PIN);
if (startReading != lastStartState) {
lastStartState = startReading;
btnMsg.data = (lastStartState == HIGH);
ros.publish("panel/btn_start", btnMsg);
}
int stopReading = digitalRead(STOP_BTN_PIN);
if (stopReading != lastStopState) {
lastStopState = stopReading;
btnMsg.data = (lastStopState == HIGH);
ros.publish("panel/btn_stop", btnMsg);
}
}
}
2. Управляющий Python-узел (~/ros2_ws/parktronic_drive_node.py)
Создайте этот файл на Raspberry Pi робота:
#!/usr/bin/env python3
import math
import rclpy
from geometry_msgs.msg import Twist
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import LaserScan
from std_msgs.msg import Bool, Float32
FORWARD_SPEED = 0.10 # скорость движения вперёд, м/с
class ParktronicDriveNode(Node):
def __init__(self):
super().__init__('parktronic_drive_node')
self.moving = False
# Подписки на кнопки платы и штатный топик лидара /scan
self.create_subscription(Bool, '/panel/btn_start', self.on_start, 10)
self.create_subscription(Bool, '/panel/btn_stop', self.on_stop, 10)
self.create_subscription(
LaserScan,
'/scan',
self.on_scan,
qos_profile_sensor_data
)
# Издатели: минимальная дистанция для платы и скорость робота /cmd_vel
self.dist_pub = self.create_publisher(Float32, '/panel/distance', 10)
self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
# Таймер 10 Гц для регулярной публикации скорости во время движения
self.create_timer(0.1, self.publish_velocity)
self.get_logger().info(
'Узел parktronic_drive_node запущен. D28 — старт, D15 — стоп.'
)
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 on_start(self, msg: Bool):
if msg.data and not self.moving:
self.moving = True
self.get_logger().info('Кнопка Старт (D28): начинаем движение вперёд')
def on_stop(self, msg: Bool):
if msg.data and self.moving:
self.moving = False
self.cmd_vel_pub.publish(Twist())
self.get_logger().warn('Кнопка Стоп (D15): остановка робота!')
def publish_velocity(self):
if self.moving:
cmd = Twist()
cmd.linear.x = FORWARD_SPEED
self.cmd_vel_pub.publish(cmd)
def main(args=None):
rclpy.init(args=args)
node = ParktronicDriveNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.cmd_vel_pub.publish(Twist())
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
3. Launch-файл (~/ros2_ws/parktronic.launch.py)
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
port_arg = DeclareLaunchArgument(
'port',
default_value='/dev/serial/by-id/usb-Silicon_Labs_CP2102N_USB_to_UART_Bridge_Controller_0010-if00-port0',
description='Порт моста Serial1 пользовательского МК Arduino на роботе TurtleBro'
)
agent_node = Node(
package='rosduino_agent',
executable='rosduino_agent',
name='rosduino_agent',
output='screen',
parameters=[{
'port': LaunchConfiguration('port'),
'baud': 115200,
'keep_on_disconnect': True,
}]
)
script_path = os.path.expanduser('~/ros2_ws/parktronic_drive_node.py')
drive_controller = ExecuteProcess(
cmd=['python3', script_path],
output='screen'
)
return LaunchDescription([
port_arg,
agent_node,
drive_controller,
])
Как устроено решение
- Сжатие данных лидара для микроконтроллера (
/scan→/panel/distance): сообщениеsensor_msgs/msg/LaserScanсодержит сотни чисел с плавающей точкой, которые избыточны для индикации на плате. Python-узел на Raspberry Pi отбрасывает некорректные лучи за пределами(msg.range_min, msg.range_max), находит одно минимальное числоmin(valid)и передаёт его в компактном сообщенииstd_msgs/msg/Float32(всего4 байта). - Шкала парктроника на стороне Arduino (
onDistance): плата принимает расстояние в метрах и через функциюshowLitCount()включает от0до4светодиодовD48–D51. Индикация работает непрерывно даже тогда, когда робот стоит на месте. - Надёжный старт и стоп по кнопкам: благодаря вызовам
ros.advertise()вsetup()топики/panel/btn_startи/panel/btn_stopсоздаются сразу при запуске агента, поэтому и старт по кнопкеD28, и остановка по кнопкеD15срабатывают с первого нажатия. При получении сигнала «Стоп» (а также при остановке скрипта поCtrl+C) в/cmd_velсразу отправляется пустое сообщениеTwist()с нулевой скоростью.
Как проверить решение
- Загрузите скетч в плату через разъём DUE, затем по SSH запустите связку на Raspberry Pi:
ros2 launch ~/ros2_ws/parktronic.launch.py - Поднесите ладонь к лидару и плавно меняйте расстояние от
1 мдо20 см— светодиодыD48–D51на плате будут загораться и гаснуть ступенями от0до4. - Поставьте робота на подставку и нажмите кнопку
D28— колёса начнут плавно вращаться вперёд (linear.x = 0.10), а светодиоды продолжат показывать дистанцию по лидару. - Нажмите кнопку
D15— колёса мгновенно остановятся (linear.x = 0.0).
Ответы на вопросы для самопроверки
-
Какие два микроконтроллера установлены на плате робота TurtleBro и почему загрузка собственного скетча в Arduino Due не может нарушить работу приводов шасси?
На плате установлены системный микроконтроллерSTM32F4(отвечает за колёса, энкодеры, IMU и базовые топики движения) и пользовательский микроконтроллерArduino Due(отвечает за кнопки, светодиоды, сервоприводы и разъёмы GPIO). Они работают независимо и подключены к разным USB-портам, поэтому прошивкаArduino Dueникак не затрагивает программу управления шасси. -
Через какой порт микроконтроллера (
SerialилиSerial1) плата общается с Raspberry Pi на роботе и почему при прошивке с компьютера через разъём DUE не нужно переключать провода?
На роботе связь микроконтроллера с Raspberry Pi разведена на плате через второй аппаратный портSerial1(ros.begin(Serial1, "имя_узла")), а разъём USB-C с надписью DUE (Serial, Programming Port) используется только для загрузки скетчей с компьютера. Поскольку это два разных независимых порта, переключать провода или останавливать агент на роботе во время прошивки не нужно. -
В какой штатный топик робота TurtleBro нужно публиковать сообщения
geometry_msgs/msg/Twist, чтобы управлять его движением?
В топик/cmd_vel(twist.linear.xзадаёт линейную скорость вперёд/назад в м/с, аtwist.angular.z— угловую скорость поворота в рад/с). -
Какой командой с помощью
rosduino-genможно создать заголовочные файлы для Arduino из собственного пакета сообщенийmy_robot_msgs?
Командойros2 run rosduino_gen rosduino-gen --package my_robot_msgs --out "$HOME/Arduino/libraries/RosDuino/src".
Назад: Домашнее задание.