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

Урок 7. Решение задания

На этой странице разобрано решение домашнего задания урока 7. Плата TurtleBro работает со стандартной прошивкой ввода-вывода io_board, а вся математика — сглаживание сигнала скользящим средним по 5 точкам, расчёт угла сервопривода и проверка порога тревоги — выполняется в Python-узле smooth_servo_controller.py на компьютере.

1. Скетч на плате TurtleBro (io_board.ino)

Этот скетч уже был загружен в плату в практической части урока:

#include <Servo.h>
#include <RosDuino.h>
#include <std_msgs/msg/Bool.h>
#include <std_msgs/msg/Int32.h>

const int SENSOR_PIN = A8;
const int LED_PIN = 48;
const int SERVO_PIN = 2;

Servo servo;
std_msgs::Int32 signalMsg;
rosduino::Timer sensorTimer(100);  // 10 Гц

void onLed(std_msgs::Bool msg) {
  digitalWrite(LED_PIN, msg.data ? HIGH : LOW);
}

void onServoAngle(std_msgs::Int32 msg) {
  int safeAngle = constrain(msg.data, 0, 180);
  servo.write(safeAngle);
}

void setup() {
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);

  servo.attach(SERVO_PIN);
  servo.write(90);

  ros.begin("io_board");
  ros.subscribe("led", onLed);
  ros.subscribe("servo_angle", onServoAngle);
}

void loop() {
  ros.update();

  if (sensorTimer.check()) {
    signalMsg.data = analogRead(SENSOR_PIN);
    ros.publish("analog_signal", signalMsg);
  }
}

2. Управляющий узел на Python (~/ros2_ws/smooth_servo_controller.py)

#!/usr/bin/env python3
from collections import deque

import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool, Int32

WINDOW_SIZE = 5
ALARM_THRESHOLD = 700


class SmoothServoController(Node):
    def __init__(self):
        super().__init__('smooth_servo_controller')

        # Очередь из последних 5 измерений для расчёта скользящего среднего
        self.window = deque(maxlen=WINDOW_SIZE)
        self.last_alarm = None

        self.sub = self.create_subscription(
            Int32,
            '/analog_signal',
            self.on_sensor,
            10
        )

        self.servo_pub = self.create_publisher(Int32, '/servo_angle', 10)
        self.led_pub = self.create_publisher(Bool, '/led', 10)
        self.get_logger().info('Узел smooth_servo_controller запущен')

    def on_sensor(self, msg: Int32):
        # 1. Добавляем новое значение в окно и считаем среднее
        self.window.append(msg.data)
        avg_val = sum(self.window) / len(self.window)

        # 2. Переводим среднее (0..1023) в угол сервопривода (0..180)
        angle = int(avg_val * 180 / 1023)
        angle = max(0, min(180, angle))

        angle_msg = Int32()
        angle_msg.data = angle
        self.servo_pub.publish(angle_msg)

        # 3. Включаем светодиод D48 при превышении порога 700
        alarm = avg_val > ALARM_THRESHOLD
        if alarm != self.last_alarm:
            self.last_alarm = alarm
            led_msg = Bool()
            led_msg.data = alarm
            self.led_pub.publish(led_msg)

            if alarm:
                self.get_logger().warn(f'Порог превышен! Среднее={avg_val:.1f}, угол={angle}')
            else:
                self.get_logger().info(f'Сигнал в норме. Среднее={avg_val:.1f}, угол={angle}')


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


if __name__ == '__main__':
    main()

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

  1. Скользящее среднее (deque(maxlen=5)): когда в очередь self.window добавляется шестой элемент, самый старый первый элемент автоматически удаляется. Поэтому sum(self.window) / len(self.window) всегда возвращает среднее арифметическое за последние полсекунды (5 измерений по 100 мс), убирая резкие рывки сервопривода.
  2. Экономия команд для светодиода (self.last_alarm): угол сервопривода обновляется плавно при каждом измерении, а команда включения или выключения светодиода /led и запись в журнал отправляются только тогда, когда статус тревоги изменился.

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

  1. Какими пятью функциями объекта ros можно отправить текстовое сообщение с платы в системный журнал /rosout?
    Функциями ros.logDebug(), ros.logInfo(), ros.logWarn(), ros.logError() и ros.logFatal().

  2. Зачем нужна функция ros.now() и чем возвращаемое ею время отличается от millis()?
    Функция millis() отсчитывает миллисекунды с момента включения самой платы, тогда как ros.now() возвращает структуру builtin_interfaces::msg::Time (секунды и наносекунды), синхронизированную через rosduino_agent с системными часами компьютера ROS 2. Единое время позволяет узлам робота отбрасывать устаревшие сообщения, которые при задержке связи могли бы привести к аварии.

  3. Какой топик ROS 2 позволяет проверить связь платы с rosduino_agent и что в нём означает статус WARN? Стандартный диагностический топик /diagnostics. Статус WARN означает, что связь с платой временно потеряна (например, плата перезагружается или отключён кабель) либо одна из объявленных в скетче сущностей не зарегистрировалась.

  4. В чём преимущество размещения пороговой логики и фильтрации сигнала в Python-узле на компьютере вместо скетча на плате? Скетч на микроконтроллере остаётся простым и прошивается всего один раз, а все пороги, фильтры и алгоритмы принятия решений на Python можно менять, отлаживать и перезапускать за секунду без остановки агента и без перепрошивки платы.

  5. Зачем нужен launch-файл и какие два процесса запускает bringup.launch.py из этого урока? Launch-файл позволяет одной командой запустить связанные компоненты системы с согласованными параметрами. bringup.launch.py запускает узел rosduino_agent с выбранным последовательным портом и Python-скрипт light_controller.py.

Назад: Домашнее задание.
Дальше: Урок 8. Работа на роботе TurtleBro.