Урок 6. Решение задания
На этой странице разобрано решение домашнего задания урока 6. Узел /safe_servo_node управляет сервоприводом на разъёме D2 с защитной блокировкой: разрешение на движение включается и выключается через сервис /set_drive_enable, светодиод D48 показывает состояние блокировки, а текущий угол раз в секунду публикуется в топик /current_angle.
Полный код скетча
#include <Servo.h>
#include <RosDuino.h>
#include <std_msgs/msg/Int32.h>
#include <std_srvs/srv/SetBool.h>
const int SERVO_PIN = 2;
const int STATUS_LED_PIN = 48;
const int MIN_ANGLE = 0;
const int MAX_ANGLE = 180;
Servo servo;
std_msgs::Int32 angleMsg;
rosduino::Timer telemetryTimer(1000); // публикация угла (1 Гц)
bool driveEnabled = false;
int currentAngle = 90;
// Обработчик сервиса "/set_drive_enable".
void onSetDriveEnable(std_srvs::SetBool& srv) {
driveEnabled = srv.request.data;
digitalWrite(STATUS_LED_PIN, driveEnabled ? HIGH : LOW);
srv.response.success = true;
if (driveEnabled) {
srv.response.message = "Drive enabled";
} else {
srv.response.message = "Drive disabled";
}
}
// Обработчик целевого угла из топика "/target_angle".
void onTargetAngle(std_msgs::Int32 msg) {
if (!driveEnabled) {
return;
}
currentAngle = constrain(msg.data, MIN_ANGLE, MAX_ANGLE);
servo.write(currentAngle);
}
void setup() {
pinMode(STATUS_LED_PIN, OUTPUT);
digitalWrite(STATUS_LED_PIN, LOW);
servo.attach(SERVO_PIN);
servo.write(currentAngle);
ros.begin("safe_servo_node");
ros.subscribe("target_angle", onTargetAngle);
ros.service("set_drive_enable", onSetDriveEnable);
}
void loop() {
ros.update();
// Публикация текущего угла раз в секунду (1 Гц).
if (telemetryTimer.check()) {
angleMsg.data = currentAngle;
ros.publish("current_angle", angleMsg);
}
}
Как устроено решение
- Сервис как переключатель режима (
onSetDriveEnable): команда включения или блокировки привода подаётся редко, но требует стопроцентной уверенности, что плата её приняла. Поэтому для неё выбран сервисstd_srvs/srv/SetBool, который сразу меняет флагdriveEnabled, зажигает или гасит светодиодD48и возвращает текстовое подтверждение. - Топик для команд угла (
onTargetAngle): целевой угол приходит через топик/target_angle. ЕслиdriveEnabled == false, функция сразу завершается (return;), не меняяcurrentAngleи не трогая сервопривод. - Периодическая телеметрия (
telemetryTimer): раз в секунду узел публикует фактический уголcurrentAngleв топик/current_angle, чтобы другие узлы ROS 2 всегда знали текущее положение механизма.
Как проверить решение
- Остановите
rosduino_agentв Терминале 1 (Ctrl+C), загрузите скетч в плату и снова запустите агент:ros2 run rosduino_agent rosduino_agent --port /dev/ttyACM0 - Во втором окне терминала (Терминал 2) отправьте команду угла при заблокированном приводе:
Вал не повернётся, аros2 topic pub --once /target_angle std_msgs/msg/Int32 "{data: 30}"ros2 topic echo /current_angle --onceпо-прежнему покажетdata: 90. - Разблокируйте привод через сервис:
Светодиодros2 service call /set_drive_enable std_srvs/srv/SetBool "{data: true}"D48загорится, а сервис вернётsuccess=True, message='Drive enabled'. - Повторите отправку угла
30, а затем150:
Сервопривод повернётся в заданное положение, а топикros2 topic pub --once /target_angle std_msgs/msg/Int32 "{data: 30}" ros2 topic pub --once /target_angle std_msgs/msg/Int32 "{data: 150}"/current_angleпокажетdata: 150.
Ответы на вопросы для самопроверки
-
Зачем в ROS 2 нужны сервисы, если в системе уже есть топики, и как один сервис с входным параметром помогает избежать создания множества топиков?
Сервис работает как удалённый вызов функции: клиент передаёт входные аргументы вrequest(например, номер кнопки, пина или числа для расчёта) и получает вычисленный результат вresponse. Благодаря передаче параметров один сервис может обслуживать сразу все кнопки или режимы платы, избавляя от необходимости создавать отдельный топик под каждый запрос и каждый ответ. -
Почему в обработчике сервера сервиса
void onSetLed(std_srvs::SetBool& srv)параметр принимается по ссылке (&) и почему внутри этой функции нельзя делать долгие паузы?
Передача по ссылке (&) позволяет функции записать результат прямо в поляsrv.response, которые библиотека сразу после выхода из функции отправит клиенту. Долгие паузы внутри обработчика остановят вызовros.update(), из-за чего плата перестанет принимать пакеты и сервис завершится ошибкой по тайм-ауту. -
Чем отличается роль платы при вызове
ros.service()от её роли при вызовеros.call()?
При использованииros.service()плата выступает сервером сервиса (ждёт запросов от компьютера и выполняет их), а при вызовеros.call()плата выступает клиентом сервиса (сама отправляет запрос узлу ROS 2 на компьютере и получает от него ответ). -
Почему вызов
ros.call("add_two_ints", adder, onSumAnswer)не останавливает работуloop()во время ожидания ответа от компьютера?
Потому чтоros.call()работает асинхронно: он лишь отправляет пакет запроса в порт и сразу возвращает управление вloop(). Когда ответ от компьютера приходит позже, библиотека сама обнаруживает его внутриros.update()и вызывает функцию-обработчикonSumAnswer().
Назад: Домашнее задание.
Дальше: Урок 7. Диагностика, время и узлы на Python.