Одновременная публикация и подписка
В реальных задачах плата микроконтроллера работает в обе стороны одновременно: отправляет в ROS 2 полученные значения датчиков и тут же принимает от компьютера управляющие команды. В этой части урока мы соберём двусторонний узел и познакомимся со стандартными типами сообщений ROS 2, которые входят в комплект RosDuino.
Двусторонний узел: датчик + команда светодиоду
Объединим в одном скетче публикацию и подписку. Пусть плата:
- Каждые
100 мссчитывает значение на разъёме GPIOA8, пересчитывает его в диапазон0–100(в процентах) и публикует в топик/sensor_percent. - Слушает топик
/alarm_led(std_msgs/msg/Bool) и по команде из ROS 2 включает или выключает светодиодD48.
#include <RosDuino.h>
#include <std_msgs/msg/Bool.h>
#include <std_msgs/msg/Int32.h>
const int SENSOR_PIN = A8;
const int ALARM_LED_PIN = 48;
std_msgs::Int32 percentMsg;
rosduino::Timer sensorTimer(100);
// Обработчик входящих команд из топика "/alarm_led".
void onAlarmLed(std_msgs::Bool msg) {
digitalWrite(ALARM_LED_PIN, msg.data ? HIGH : LOW);
}
void setup() {
pinMode(ALARM_LED_PIN, OUTPUT);
digitalWrite(ALARM_LED_PIN, LOW);
ros.begin("io_board");
ros.subscribe("alarm_led", onAlarmLed);
}
void loop() {
ros.update();
if (sensorTimer.check()) {
int rawValue = analogRead(SENSOR_PIN);
percentMsg.data = map(rawValue, 0, 1023, 0, 100);
ros.publish("sensor_percent", percentMsg);
}
}
Проверка двустороннего обмена
- Остановите
rosduino_agentв Терминале 1 (Ctrl+C), загрузите скетч в плату и снова запустите агент. - В Терминале 2 проверьте, что данные от платы поступают:
Терминал 2:
ros2 topic echo /sensor_percent --once
- Не останавливая поток публикации, отправьте команду на включение светодиода
D48:
Терминал 2:
ros2 topic pub --once /alarm_led std_msgs/msg/Bool "{data: true}"
Светодиод D48 сразу загорится, а плата продолжит публиковать /sensor_percent с частотой 10 Гц.
Какие типы сообщений ROS 2 есть в RosDuino
До сих пор мы использовали простые типы Bool и Int32 из пакета std_msgs. Но в ROS 2 для датчиков и движения робота приняты специальные стандартные сообщения, чтобы с вашей платой могли без переделок работать штатные программы визуализации (rviz2) и навигации.
В библиотеку RosDuino уже встроены заголовочные файлы для самых востребованных пакетов:
| Пакет ROS 2 | Основные типы сообщений в RosDuino | Где применяются |
|---|---|---|
std_msgs | Bool, Int8…Int64, UInt8…UInt64, Float32, Float64, String, Empty | Одиночные флаги, счётчики, простые численные значения и текстовые статусы |
sensor_msgs | Range, Imu, Temperature, BatteryState, Joy | Показания дальномеров, инерциальных датчиков, температуры и состояния аккумулятора |
geometry_msgs | Twist, Pose, Point, Vector3, Quaternion | Команды линейной и угловой скорости робота (cmd_vel), координаты и ориентация |
nav_msgs | Odometry | Одометрия колёсного робота |
Все эти типы подключаются точно так же, как в обычных C++-узлах ROS 2. Например, для публикации показаний ультразвукового или инфракрасного дальномера в стандартном формате sensor_msgs/msg/Range:
#include <RosDuino.h>
#include <sensor_msgs/msg/Range.h>
sensor_msgs::Range rangeMsg;
void setup() {
rangeMsg.radiation_type = sensor_msgs::msg::Range::ULTRASOUND;
rangeMsg.min_range = 0.05f; // минимальная дистанция, м
rangeMsg.max_range = 2.00f; // максимальная дистанция, м
ros.begin("range_node");
}
Вопросы для самопроверки
- Какую задачу решает класс
rosduino::Timerи почему с ним не нужно вручную хранить переменнуюlastTimeдляmillis()? - Что происходит при самом первом вызове
ros.publish()без предварительной регистрации и зачем для кнопки вsetup()вызываютros.advertise()? - Какой командой в терминале можно измерить фактическую частоту публикации сообщений в топике
/sensor_percent? - Почему для события нажатия кнопки лучше вызывать
ros.publish()только при смене состояния пина, а не отправлять поток сообщений при удержании или в каждом циклеloop()?
Справочный материал: расширенный (полный) API и профили QoS
Этот раздел можно пропустить при первом прохождении. Во всех уроках курса мы используем простой API через глобальный объект ros (ros.begin(), ros.advertise(), ros.publish(), ros.subscribe(), ros.update()).
Помимо него, в библиотеке есть расширенный (полный) API (examples/Full/), в котором узел, издатели и подписчики объявляются как отдельные объекты rosduino::Node<>, rosduino::Publisher<T> и rosduino::Subscriber<T>. Он полезен в двух случаях:
- Когда нужно явно задать профиль качества обслуживания QoS — например,
rosduino::QoS::sensorData()(BestEffort) для высокочастотных потоков датчиков вместо стандартного надёжного профиляrosduino::QoS::defaults()(Reliable). - Когда на младших платах (Arduino Nano / Uno) нужно хранить имена топиков во Flash-памяти с помощью макроса
F("имя_топика"), чтобы сэкономить оперативную память.
Пример того же двустороннего узла на полном API:
#include <RosDuino.h>
#include <std_msgs/msg/Int32.h>
rosduino::Node<> node;
rosduino::Publisher<std_msgs::msg::Int32> pub;
rosduino::Subscriber<std_msgs::msg::Int32> sub;
void onIn(const std_msgs::msg::Int32& msg) {
std_msgs::msg::Int32 out;
out.data = msg.data + 1;
pub.publish(out);
}
void setup() {
node.begin(Serial, 115200, F("arduino_pubsub"));
pub = rosduino::Publisher<std_msgs::msg::Int32>(F("echo_out"));
sub = rosduino::Subscriber<std_msgs::msg::Int32>(F("echo_in"), onIn);
node.advertise(pub, rosduino::QoS::sensorData());
node.subscribe(sub);
}
void loop() {
node.spinOnce();
}
Назад: Публикация данных и таймер rosduino::Timer.
Дальше: Домашнее задание.