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

Урок 4. Решение задания

На этой странице разобрано решение задания «Управление сервоприводом через топик». Узел /servo_driver_node принимает желаемый угол поворота из топика /servo_angle (std_msgs/msg/Int32), защищает сервопривод на разъёме D2 от выхода за пределы 0°–180° и сигнализирует о некорректных командах светодиодом D48.

Полный код скетча

#include <Servo.h>
#include <RosDuino.h>
#include <std_msgs/msg/Int32.h>

const int SERVO_PIN = 2;
const int ERROR_LED_PIN = 48;

const int MIN_ANGLE = 0;
const int MAX_ANGLE = 180;
const int START_ANGLE = 90;

Servo servo;

// Вызывается при получении нового угла из топика "servo_angle".
void onServoAngle(std_msgs::Int32 msg) {
  int requestedAngle = msg.data;

  if (requestedAngle < MIN_ANGLE || requestedAngle > MAX_ANGLE) {
    digitalWrite(ERROR_LED_PIN, HIGH);
  } else {
    digitalWrite(ERROR_LED_PIN, LOW);
  }

  int safeAngle = constrain(requestedAngle, MIN_ANGLE, MAX_ANGLE);
  servo.write(safeAngle);
}

void setup() {
  pinMode(ERROR_LED_PIN, OUTPUT);
  digitalWrite(ERROR_LED_PIN, LOW);

  servo.attach(SERVO_PIN);
  servo.write(START_ANGLE);

  ros.begin("servo_driver_node");
  ros.subscribe("servo_angle", onServoAngle);
}

void loop() {
  ros.update();
}

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

  1. Тип сообщения std_msgs::Int32: в отличие от логического Bool, сообщение Int32 передаёт знаковое целое число в поле msg.data. Поэтому оно может содержать как корректные углы (0..180), так и ошибочные отрицательные или слишком большие значения (-45, 250).
  2. Проверка границ (|| — логическое ИЛИ): условие requestedAngle < MIN_ANGLE || requestedAngle > MAX_ANGLE срабатывает, если нарушена хотя бы одна из двух границ. В этом случае зажигается светодиод ошибки D48. Как только в следующем сообщении приходит допустимый угол, ветка else гасит светодиод.
  3. Защита привода (constrain): функция constrain() обрезает любое значение меньше 0 до 0, а больше 180 — до 180. Благодаря этому на сервопривод никогда не подаётся команда за пределами его механического хода.

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

  1. Если rosduino_agent запущен в Терминале 1, остановите его клавишами Ctrl+C.
  2. Загрузите скетч в плату TurtleBro через Arduino IDE 2. Вал сервопривода встанет в среднее положение 90°, светодиод D48 погашен.
  3. Запустите агент в Терминале 1:
    ros2 run rosduino_agent rosduino_agent --port /dev/ttyACM0
    
  4. В Терминале 2 проверьте узел и тип топика:
    ros2 node list
    # Вывод: /servo_driver_node
    
    ros2 topic info /servo_angle
    # Вывод:
    # Type: std_msgs/msg/Int32
    # Publisher count: 0
    # Subscription count: 1
    
  5. Протестируйте допустимые и выходящие за границы значения в Терминале 2:
    ros2 topic pub --once /servo_angle std_msgs/msg/Int32 "{data: 45}"
    ros2 topic pub --once /servo_angle std_msgs/msg/Int32 "{data: 220}"
    ros2 topic pub --once /servo_angle std_msgs/msg/Int32 "{data: -30}"
    ros2 topic pub --once /servo_angle std_msgs/msg/Int32 "{data: 90}"
    
    При отправке 220 и -30 светодиод D48 загорается, а вал останавливается на 180° и 0°. При отправке 45 и 90 светодиод D48 гаснет.

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

  1. Какие две части входят в систему RosDuino и какая из них работает на микроконтроллере, а какая — на компьютере с Ubuntu?
    На микроконтроллере работает библиотека rosduino_arduino (#include <RosDuino.h>), а на компьютере с Ubuntu — узел-агент rosduino_agent из пакета rosduino_ros.

  2. Почему в скетче с RosDuino нельзя одновременно использовать Serial.println() и держать открытой утилиту мониторинга порта в Arduino IDE?
    Потому что последовательный порт уже занят двоичным протоколом обмена между RosDuino и rosduino_agent. Посторонний текст из Serial.println() повредит пакеты данных, а открытая утилита мониторинга порта заблокирует доступ к порту для агента.

  3. Что нужно сделать в терминале с rosduino_agent перед тем, как нажать кнопку «Загрузить» в Arduino IDE?
    Нужно остановить rosduino_agent сочетанием клавиш Ctrl+C, чтобы освободить последовательный порт для загрузчика Arduino IDE.

  4. Как агент rosduino_agent узнаёт, какое имя узла и какой тип топика нужно создать в ROS 2?
    При установлении соединения плата сама передаёт агенту служебное сообщение с именем узла из ros.begin() и списком всех зарегистрированных топиков и сервисов, а тип топика в скетче автоматически определяется по типу аргумента функции-обработчика в ros.subscribe().

Назад: Домашнее задание.
Дальше: Урок 5. Топики в RosDuino.