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

Урок 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);
  }
}

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

  1. Сервис как переключатель режима (onSetDriveEnable): команда включения или блокировки привода подаётся редко, но требует стопроцентной уверенности, что плата её приняла. Поэтому для неё выбран сервис std_srvs/srv/SetBool, который сразу меняет флаг driveEnabled, зажигает или гасит светодиод D48 и возвращает текстовое подтверждение.
  2. Топик для команд угла (onTargetAngle): целевой угол приходит через топик /target_angle. Если driveEnabled == false, функция сразу завершается (return;), не меняя currentAngle и не трогая сервопривод.
  3. Периодическая телеметрия (telemetryTimer): раз в секунду узел публикует фактический угол currentAngle в топик /current_angle, чтобы другие узлы ROS 2 всегда знали текущее положение механизма.

Как проверить решение

  1. Остановите rosduino_agent в Терминале 1 (Ctrl+C), загрузите скетч в плату и снова запустите агент:
    ros2 run rosduino_agent rosduino_agent --port /dev/ttyACM0
    
  2. Во втором окне терминала (Терминал 2) отправьте команду угла при заблокированном приводе:
    ros2 topic pub --once /target_angle std_msgs/msg/Int32 "{data: 30}"
    
    Вал не повернётся, а ros2 topic echo /current_angle --once по-прежнему покажет data: 90.
  3. Разблокируйте привод через сервис:
    ros2 service call /set_drive_enable std_srvs/srv/SetBool "{data: true}"
    
    Светодиод D48 загорится, а сервис вернёт success=True, message='Drive enabled'.
  4. Повторите отправку угла 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.

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

  1. Зачем в ROS 2 нужны сервисы, если в системе уже есть топики, и как один сервис с входным параметром помогает избежать создания множества топиков?
    Сервис работает как удалённый вызов функции: клиент передаёт входные аргументы в request (например, номер кнопки, пина или числа для расчёта) и получает вычисленный результат в response. Благодаря передаче параметров один сервис может обслуживать сразу все кнопки или режимы платы, избавляя от необходимости создавать отдельный топик под каждый запрос и каждый ответ.

  2. Почему в обработчике сервера сервиса void onSetLed(std_srvs::SetBool& srv) параметр принимается по ссылке (&) и почему внутри этой функции нельзя делать долгие паузы?
    Передача по ссылке (&) позволяет функции записать результат прямо в поля srv.response, которые библиотека сразу после выхода из функции отправит клиенту. Долгие паузы внутри обработчика остановят вызов ros.update(), из-за чего плата перестанет принимать пакеты и сервис завершится ошибкой по тайм-ауту.

  3. Чем отличается роль платы при вызове ros.service() от её роли при вызове ros.call()?
    При использовании ros.service() плата выступает сервером сервиса (ждёт запросов от компьютера и выполняет их), а при вызове ros.call() плата выступает клиентом сервиса (сама отправляет запрос узлу ROS 2 на компьютере и получает от него ответ).

  4. Почему вызов ros.call("add_two_ints", adder, onSumAnswer) не останавливает работу loop() во время ожидания ответа от компьютера?
    Потому что ros.call() работает асинхронно: он лишь отправляет пакет запроса в порт и сразу возвращает управление в loop(). Когда ответ от компьютера приходит позже, библиотека сама обнаруживает его внутри ros.update() и вызывает функцию-обработчик onSumAnswer().

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