Домашнее задание
В реальных роботах приводы часто снабжают программной блокировкой безопасности (предохранителем): пока привод заблокирован, он игнорирует случайные команды движения. В этом задании вы создадите узел безопасного управления сервоприводом, в котором разрешение на движение включается и выключается через сервис ROS 2 /set_drive_enable, целевой угол приходит из топика /target_angle, а текущий угол публикуется в /current_angle.
Перед подключением сервопривода к разъёму D2 отключите USB-кабель от платы и сверьте контакты −, +, S с цветами проводов.
Что должно получиться
Напишите скетч узла safe_servo_node, в котором хранится флаг разрешения работы bool driveEnabled = false;:
- Начальное состояние после включения:
- привод заблокирован (
driveEnabled = false), светодиодD48погашен; - вал сервопривода на разъёме
D2установлен в начальное положение90°.
- привод заблокирован (
- Сервис блокировки
/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".
- при вызове с
- Приём целевого угла из топика
/target_angle(std_msgs/msg/Int32):- если
driveEnabled == false— команда игнорируется, сервопривод остаётся на прежнем месте; - если
driveEnabled == true— программа ограничивает угол диапазоном0°–180°черезconstrain(msg.data, 0, 180)и поворачивает сервопривод.
- если
- Публикация текущего угла (
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).
Как составить алгоритм
- Объявите константы
SERVO_PIN = 2,STATUS_LED_PIN = 48, переменныеbool driveEnabled = false;иint currentAngle = 90;, объектServo servo;и таймерrosduino::Timer telemetryTimer(1000);. - Напишите обработчик сервиса
void onSetDriveEnable(std_srvs::SetBool& srv):- сохраните
driveEnabled = srv.request.data;; - включите или выключите светодиод
D48в зависимости отdriveEnabled; - заполните
srv.response.success = true;и текстовое полеsrv.response.message.
- сохраните
- Напишите обработчик подписки
void onTargetAngle(std_msgs::Int32 msg):- если
!driveEnabled, сразу выйдите из функции (return;); - иначе ограничьте угол
currentAngle = constrain(msg.data, 0, 180);и вызовитеservo.write(currentAngle);.
- если
- В
setup()настройте пин светодиода, привяжите сервопривод, установите начальный угол90°, вызовитеros.begin("safe_servo_node"), зарегистрируйте подпискуros.subscribe("target_angle", onTargetAngle)и сервисros.service("set_drive_enable", onSetDriveEnable). - В
loop()вызывайтеros.update()и по срабатываниюtelemetryTimer.check()публикуйтеcurrentAngleв топик"current_angle".
Как проверить результат из терминала ROS 2
- Загрузите скетч в плату и запустите
rosduino_agentв Терминале 1. СветодиодD48погашен (привод заблокирован). - Попробуйте отправить команду поворота на
45°в Терминале 2:
Сервопривод останется на месте (ros2 topic pub --once /target_angle std_msgs/msg/Int32 "{data: 45}"90°), так как разрешение ещё не включено. - Разблокируйте привод вызовом сервиса в Терминале 2:
Загорится светодиодros2 service call /set_drive_enable std_srvs/srv/SetBool "{data: true}"D48, а в терминале вернётся ответsuccess=True, message='Drive enabled'. - Снова отправьте угол
45°:
Теперь сервопривод повернётся наros2 topic pub --once /target_angle std_msgs/msg/Int32 "{data: 45}"45°, а проверкаros2 topic echo /current_angle --onceпокажетdata: 45.
Назад: Создание сообщений для сервисов.
Дальше: Урок 7. Диагностика, время и узлы на Python.