Arduino в качестве подписчика ROS

В прошлый раз, когдя я рассказывал о совместном использовании ROS и Arduino, речь шла об использовании Arduino как издателе ROS. Сегодня создадим подписчика ROS. После того, как мы установили библиотеку ros_lib (подробнее в статье Arduino и ROS — первый шаг), можно воспользоваться примером, выбрав пункт меню Файл → Образцы → ros_lib → Blink в Arduino IDE.

Перед нами предстанет такой код:

/*
 * rosserial Subscriber Example
 * Blinks an LED on callback
 */

#include <ros.h>
#include <std_msgs/Empty.h>

ros::NodeHandle nh;

void messageCb( const std_msgs::Empty& toggle_msg){
  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
}

ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );

void setup()
{
  pinMode(13, OUTPUT);
  nh.initNode();
  nh.subscribe(sub);
}
void loop()
{
  nh.spinOnce();
  delay(1);
}

Разберем этот пример более подробно.

#include <ros.h>
#include <std_msgs/Empty.h>

Сначала подключается заголовочный файл ros.h как и для для любой программы для Arduino, использующей ROS. Затем подключается заголовочный файл для сообщений, в этом случае используется сообщение Empty.

ros::NodeHandle nh;

Далее нам необходим экземпляр обработчика узла, который позволит нашей программе создавать подписчиков и издателей. Обработчик узла также принимает участие в коммуникациях через Serial Port.

void messageCb(const std_msgs::Empty& toggle_msg){
  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
}

Затем мы создаем функцию обратного вызова (callback-функцию) для нашего подписчика. Эта функция должна принимать константу в виде сообщения в качестве своего аргумента. В данном примере, функция обратного вызова называется messageCb,  тип сообщения std_msgs::Empty и сообщение должно называться toggle_msg.

Внутри нашего обратного вызова мы можем ссылаться при помощи toggle_msg, но так как оно пустое, в этом нет необходимости. Мы просто включаем и выключаем светодиод на Arduino, каждый раз, когда получаем сообщение.

ros::Subscriber<std_msgs::Empty> sub("toggle_led", &messageCb );

Мы должны создать экземпляры издателей и подписчиков, которыми мы будем пользоваться. Здесь мы создаем Subscriber на тему под названием toggle_led, которая имеет тип std_msgs::Empty. Для Subscriber, мы должны запомнить, шаблон подписчика на сообщения. Его аргументы представляют тему, на которую он подписан и функцию обратного вызова, которая будет использоваться.

void setup()
{
 pinMode(13, OUTPUT);
 nh.initNode();
 nh.subscribe(sub);
}

В функции setup нам нужно инициализировать наш обработчик узла ROS, оповещающего о любых опубликованных темах и подписывающегося на любые необходимые темы.

void loop()
{
 nh.spinOnce();
 delay(1);
}

Наконец, в функции loop, мы вызываем ros::spinOnce, которая обрабатывает все коммуникационные обратные вызовы ROS. У нас нет необходимости в какой-то дополнительной обработке внутри loop, поскольку ros::spinOnce обработает переданные сообщения при обратном вызове подписчика.

Загрузим этот код в Arduino.

После этого откроем Терминал и запустим ROS, выполнив

$ roscore

Далее, в новой вкладке Терминала запустим клиентское приложение rosserial, которое отправляет сообщения Arduino в ROS. Удостоверьтесь, что указали правильный serial port.

$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0

Теперь мы можем включать и выключать светодиод при помощи rostopic:

$ rostopic pub toggle_led std_msgs/Empty --once

[add_ratings]