Урок 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();
}
Как устроено решение
- Тип сообщения
std_msgs::Int32: в отличие от логическогоBool, сообщениеInt32передаёт знаковое целое число в полеmsg.data. Поэтому оно может содержать как корректные углы (0..180), так и ошибочные отрицательные или слишком большие значения (-45,250). - Проверка границ (
||— логическое ИЛИ): условиеrequestedAngle < MIN_ANGLE || requestedAngle > MAX_ANGLEсрабатывает, если нарушена хотя бы одна из двух границ. В этом случае зажигается светодиод ошибкиD48. Как только в следующем сообщении приходит допустимый угол, веткаelseгасит светодиод. - Защита привода (
constrain): функцияconstrain()обрезает любое значение меньше0до0, а больше180— до180. Благодаря этому на сервопривод никогда не подаётся команда за пределами его механического хода.
Как проверить решение
- Если
rosduino_agentзапущен в Терминале 1, остановите его клавишамиCtrl+C. - Загрузите скетч в плату TurtleBro через Arduino IDE 2. Вал сервопривода встанет в среднее положение
90°, светодиодD48погашен. - Запустите агент в Терминале 1:
ros2 run rosduino_agent rosduino_agent --port /dev/ttyACM0 - В Терминале 2 проверьте узел и тип топика:
ros2 node list # Вывод: /servo_driver_node ros2 topic info /servo_angle # Вывод: # Type: std_msgs/msg/Int32 # Publisher count: 0 # Subscription count: 1 - Протестируйте допустимые и выходящие за границы значения в Терминале 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гаснет.
Ответы на вопросы для самопроверки
-
Какие две части входят в систему
RosDuinoи какая из них работает на микроконтроллере, а какая — на компьютере с Ubuntu?
На микроконтроллере работает библиотекаrosduino_arduino(#include <RosDuino.h>), а на компьютере с Ubuntu — узел-агентrosduino_agentиз пакетаrosduino_ros. -
Почему в скетче с
RosDuinoнельзя одновременно использоватьSerial.println()и держать открытой утилиту мониторинга порта в Arduino IDE?
Потому что последовательный порт уже занят двоичным протоколом обмена междуRosDuinoиrosduino_agent. Посторонний текст изSerial.println()повредит пакеты данных, а открытая утилита мониторинга порта заблокирует доступ к порту для агента. -
Что нужно сделать в терминале с
rosduino_agentперед тем, как нажать кнопку «Загрузить» в Arduino IDE?
Нужно остановитьrosduino_agentсочетанием клавишCtrl+C, чтобы освободить последовательный порт для загрузчика Arduino IDE. -
Как агент
rosduino_agentузнаёт, какое имя узла и какой тип топика нужно создать в ROS 2?
При установлении соединения плата сама передаёт агенту служебное сообщение с именем узла изros.begin()и списком всех зарегистрированных топиков и сервисов, а тип топика в скетче автоматически определяется по типу аргумента функции-обработчика вros.subscribe().
Назад: Домашнее задание.
Дальше: Урок 5. Топики в RosDuino.