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

Домашнее задание

В реальных роботах приводы часто снабжают программной блокировкой безопасности (предохранителем): пока привод заблокирован, он игнорирует случайные команды движения. В этом задании вы создадите узел безопасного управления сервоприводом, в котором разрешение на движение включается и выключается через сервис ROS 2 /set_drive_enable, целевой угол приходит из топика /target_angle, а текущий угол публикуется в /current_angle.

Перед подключением сервопривода к разъёму D2 отключите USB-кабель от платы и сверьте контакты −, +, S с цветами проводов.

Что должно получиться

Напишите скетч узла safe_servo_node, в котором хранится флаг разрешения работы bool driveEnabled = false;:

  1. Начальное состояние после включения:
    • привод заблокирован (driveEnabled = false), светодиод D48 погашен;
    • вал сервопривода на разъёме D2 установлен в начальное положение 90°.
  2. Сервис блокировки /set_drive_enable (std_srvs/srv/SetBool):
    • при вызове с data: true включает разрешение (driveEnabled = true), зажигает светодиод D48 и возвращает success = true, message = "Drive enabled";
    • при вызове с data: false выключает разрешение (driveEnabled = false), гасит светодиод D48 и возвращает success = true, message = "Drive disabled".
  3. Приём целевого угла из топика /target_angle (std_msgs/msg/Int32):
    • если driveEnabled == false — команда игнорируется, сервопривод остаётся на прежнем месте;
    • если driveEnabled == true — программа ограничивает угол диапазоном 0°–180° через constrain(msg.data, 0, 180) и поворачивает сервопривод.
  4. Публикация текущего угла (1 Гц — каждые 1000 мс):
    • по таймеру rosduino::Timer(1000) публикует текущий установленный угол сервопривода в топик /current_angle (std_msgs/msg/Int32).

Требования к программе

  • Подключены <Servo.h>, <RosDuino.h>, <std_msgs/msg/Int32.h> и <std_srvs/srv/SetBool.h>.
  • В функции loop() вызывается ros.update() и не используется delay().
  • Для публикации текущего угла раз в секунду используется объект rosduino::Timer(1000).

Как составить алгоритм

  1. Объявите константы SERVO_PIN = 2, STATUS_LED_PIN = 48, переменные bool driveEnabled = false; и int currentAngle = 90;, объект Servo servo; и таймер rosduino::Timer telemetryTimer(1000);.
  2. Напишите обработчик сервиса void onSetDriveEnable(std_srvs::SetBool& srv):
    • сохраните driveEnabled = srv.request.data;;
    • включите или выключите светодиод D48 в зависимости от driveEnabled;
    • заполните srv.response.success = true; и текстовое поле srv.response.message.
  3. Напишите обработчик подписки void onTargetAngle(std_msgs::Int32 msg):
    • если !driveEnabled, сразу выйдите из функции (return;);
    • иначе ограничьте угол currentAngle = constrain(msg.data, 0, 180); и вызовите servo.write(currentAngle);.
  4. В setup() настройте пин светодиода, привяжите сервопривод, установите начальный угол 90°, вызовите ros.begin("safe_servo_node"), зарегистрируйте подписку ros.subscribe("target_angle", onTargetAngle) и сервис ros.service("set_drive_enable", onSetDriveEnable).
  5. В loop() вызывайте ros.update() и по срабатыванию telemetryTimer.check() публикуйте currentAngle в топик "current_angle".

Как проверить результат из терминала ROS 2

  1. Загрузите скетч в плату и запустите rosduino_agent в Терминале 1. Светодиод D48 погашен (привод заблокирован).
  2. Попробуйте отправить команду поворота на 45° в Терминале 2:
    ros2 topic pub --once /target_angle std_msgs/msg/Int32 "{data: 45}"
    
    Сервопривод останется на месте (90°), так как разрешение ещё не включено.
  3. Разблокируйте привод вызовом сервиса в Терминале 2:
    ros2 service call /set_drive_enable std_srvs/srv/SetBool "{data: true}"
    
    Загорится светодиод D48, а в терминале вернётся ответ success=True, message='Drive enabled'.
  4. Снова отправьте угол 45°:
    ros2 topic pub --once /target_angle std_msgs/msg/Int32 "{data: 45}"
    
    Теперь сервопривод повернётся на 45°, а проверка ros2 topic echo /current_angle --once покажет data: 45.

Посмотреть решение и разбор

Назад: Создание сообщений для сервисов.
Дальше: Урок 7. Диагностика, время и узлы на Python.