Создание Подписчика

Аналогичным образом мы можем создать подписчика на Arduino, который получает данные из ROS и обрабатывает их.

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

class NewHardware : public ArduinoHardware
{
  public:
  NewHardware():ArduinoHardware(&Serial1, 115200){};
};
ros::NodeHandle_<NewHardware>  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);
}

Приведенный выше скетч подписывается на топик toggle_led и при получения сообщения меняет уровень напряжения на ноге D13 к которой "подключен" светодиод.

Для проверки загрузим код на Arduino и начнем публиковать данные к топик.

rostopic pub /toggle_led std_msgs/Empty "{}" -r 2

Параметр -r 2 задает частоту публикации данных 2Гц. Это значит что наш светодиод должен переключаться из состояние "включен" в состояние "выключен" каждые 500мс.