Урок 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()
Как устроено решение
- Скользящее среднее (
deque(maxlen=5)): когда в очередьself.windowдобавляется шестой элемент, самый старый первый элемент автоматически удаляется. Поэтомуsum(self.window) / len(self.window)всегда возвращает среднее арифметическое за последние полсекунды (5 измерений по 100 мс), убирая резкие рывки сервопривода. - Экономия команд для светодиода (
self.last_alarm): угол сервопривода обновляется плавно при каждом измерении, а команда включения или выключения светодиода/ledи запись в журнал отправляются только тогда, когда статус тревоги изменился.
Ответы на вопросы для самопроверки
-
Какими пятью функциями объекта
rosможно отправить текстовое сообщение с платы в системный журнал/rosout?
Функциямиros.logDebug(),ros.logInfo(),ros.logWarn(),ros.logError()иros.logFatal(). -
Зачем нужна функция
ros.now()и чем возвращаемое ею время отличается отmillis()?
Функцияmillis()отсчитывает миллисекунды с момента включения самой платы, тогда какros.now()возвращает структуруbuiltin_interfaces::msg::Time(секунды и наносекунды), синхронизированную черезrosduino_agentс системными часами компьютера ROS 2. Единое время позволяет узлам робота отбрасывать устаревшие сообщения, которые при задержке связи могли бы привести к аварии. -
Какой топик ROS 2 позволяет проверить связь платы с
rosduino_agentи что в нём означает статусWARN? Стандартный диагностический топик/diagnostics. СтатусWARNозначает, что связь с платой временно потеряна (например, плата перезагружается или отключён кабель) либо одна из объявленных в скетче сущностей не зарегистрировалась. -
В чём преимущество размещения пороговой логики и фильтрации сигнала в Python-узле на компьютере вместо скетча на плате? Скетч на микроконтроллере остаётся простым и прошивается всего один раз, а все пороги, фильтры и алгоритмы принятия решений на Python можно менять, отлаживать и перезапускать за секунду без остановки агента и без перепрошивки платы.
-
Зачем нужен launch-файл и какие два процесса запускает
bringup.launch.pyиз этого урока? Launch-файл позволяет одной командой запустить связанные компоненты системы с согласованными параметрами.bringup.launch.pyзапускает узелrosduino_agentс выбранным последовательным портом и Python-скриптlight_controller.py.
Назад: Домашнее задание.
Дальше: Урок 8. Работа на роботе TurtleBro.