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

Урок 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,
    ])

Как устроено решение

  1. Сжатие данных лидара для микроконтроллера (/scan → /panel/distance): сообщение sensor_msgs/msg/LaserScan содержит сотни чисел с плавающей точкой, которые избыточны для индикации на плате. Python-узел на Raspberry Pi отбрасывает некорректные лучи за пределами (msg.range_min, msg.range_max), находит одно минимальное число min(valid) и передаёт его в компактном сообщении std_msgs/msg/Float32 (всего 4 байта).
  2. Шкала парктроника на стороне Arduino (onDistance): плата принимает расстояние в метрах и через функцию showLitCount() включает от 0 до 4 светодиодов D48–D51. Индикация работает непрерывно даже тогда, когда робот стоит на месте.
  3. Надёжный старт и стоп по кнопкам: благодаря вызовам ros.advertise() в setup() топики /panel/btn_start и /panel/btn_stop создаются сразу при запуске агента, поэтому и старт по кнопке D28, и остановка по кнопке D15 срабатывают с первого нажатия. При получении сигнала «Стоп» (а также при остановке скрипта по Ctrl+C) в /cmd_vel сразу отправляется пустое сообщение Twist() с нулевой скоростью.

Как проверить решение

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

Ответы на вопросы для самопроверки

  1. Какие два микроконтроллера установлены на плате робота TurtleBro и почему загрузка собственного скетча в Arduino Due не может нарушить работу приводов шасси?
    На плате установлены системный микроконтроллер STM32F4 (отвечает за колёса, энкодеры, IMU и базовые топики движения) и пользовательский микроконтроллер Arduino Due (отвечает за кнопки, светодиоды, сервоприводы и разъёмы GPIO). Они работают независимо и подключены к разным USB-портам, поэтому прошивка Arduino Due никак не затрагивает программу управления шасси.

  2. Через какой порт микроконтроллера (Serial или Serial1) плата общается с Raspberry Pi на роботе и почему при прошивке с компьютера через разъём DUE не нужно переключать провода?
    На роботе связь микроконтроллера с Raspberry Pi разведена на плате через второй аппаратный порт Serial1 (ros.begin(Serial1, "имя_узла")), а разъём USB-C с надписью DUE (Serial, Programming Port) используется только для загрузки скетчей с компьютера. Поскольку это два разных независимых порта, переключать провода или останавливать агент на роботе во время прошивки не нужно.

  3. В какой штатный топик робота TurtleBro нужно публиковать сообщения geometry_msgs/msg/Twist, чтобы управлять его движением?
    В топик /cmd_vel (twist.linear.x задаёт линейную скорость вперёд/назад в м/с, а twist.angular.z — угловую скорость поворота в рад/с).

  4. Какой командой с помощью rosduino-gen можно создать заголовочные файлы для Arduino из собственного пакета сообщений my_robot_msgs?
    Командой ros2 run rosduino_gen rosduino-gen --package my_robot_msgs --out "$HOME/Arduino/libraries/RosDuino/src".

Назад: Домашнее задание.