ROS — Урок 12 — Написание простого publisher и subscriber (C++)



Содержание

1. Написание простого publisher и subscriber (C++)

Описание: На этом уроке рассказывается, как написать узлы издателя(publisher) и подписчика(subscriber) на C++.

1.1 Написание Узла Издателя (Publisher Node)

В ROS, узел (node) — это термин для обозначения исполняемого файла, который подключён к сети ROS. Попробуем создать узел издателя («болтун» («talker»)), который будет постоянно транслировать сообщения.

Перейдите в каталог в пакета beginner_tutorials, созданного в начале этого курса:

roscd beginner_tutorials

1.1.1 Код

Создайте внутри пакета beginner_tutorials файл src/talker.cpp и вставьте в него следующий код:
https://code.ros.org/svn/ros/stacks/ros_tutorials/trunk/roscpp_tutorials/talker/talker.cpp

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>

/**
 * Этот пример демонстрирует простую отправку сообщений через систему ROS.
 */
int main(int argc, char **argv)
{
  /**
   * Функция ros::init() требуется для проверки аргументов argc и argv, чтобы выполнить
   * преобразования или переопределение аргументов ROS, задаваемых через командную строку.
   * Для программного переопределения вы можете использовать другую версию init(), которая
   * принимает переопределение напрямую, но для большинства программ командной сроки, обработка
   * argc и argv - простейший путь реализовать это. Третий аргумент init() - это название узла.
   *
   * Вы должны вызвать одну из версий ros::init() перед использованием любых других
   * частей системы ROS.
   */
  ros::init(argc, argv, "talker");

  /**
   * NodeHandle - главная точка доступа для взаимодействия с системой ROS.
   * Конструктор NodeHandle полностью инициализирует этот узел, а в конце,
   * деструктор NodeHandle завершит работу узла.
   */
  ros::NodeHandle n;

  /**
   * Функция advertise() определяет для ROS что вы будете публиковать в задаваемую Тему.
   * Это включает в себя вызов Мастер-узла ROS, который сохраняет регистрационные данные
   * кто является издателем, а кто является подписчиком.
   * После завершения advertise(), Мастер-узел ROS извещает всех, кто пытается подписаться
   * на заданную Тему и они должны устанавливать прямое соединение
   * (peer-to-peer connection)  между собой и этим узлом.
   * Функция advertise() возвращает объект Издатель (Publisher), который позволяет Вам
   * публиковать сообщения в Тему, с помощью метода publish(). После того, как все
   * копии возвращённого объекта Издателя (Publisher) будут разрушены, Тема будет автоматически
   * отписана (unadvertised).
   *
   * Второй параметр метода advertise() - это размер очереди сообщений, используемый для
   * публикуемых сообщений. Если сообщения публикуются быстрее, чем они могут быть отправлены,
   * этот параметр задаёт сколько сообщений необходимо сохранять в буфере прежде чем
   * бросать исключение.
   */
  ros::Publisher chatter_pub = n.advertise("chatter", 1000);

  ros::Rate loop_rate(10);

  /**
   * count - счётчик, в котором сохраняется число отправленных сообщений. Используется
   * при создании уникальной строки для каждого сообщения.
   */
  int count = 0;
  while (ros::ok())
  {
    /**
     * Это объект сообщения. Вы сохраняете в него данные, а затем публикуете их.
     */
    std_msgs::String msg;

    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();

    ROS_INFO("%s", msg.data.c_str());

    /**
     * Метод publish() для отправки сообщений. Параметр - это объект сообщения.
     * Тип объекта должен совпадать с типом, который задавался параметром шаблона
     * при вызове advertise<>(), как это сделано в конструкторе выше.
     *
     */
    chatter_pub.publish(msg);

    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }


  return 0;
}

1.1.2 Разъяснение кода

Теперь, давайте разберём, что же делает код, который мы используем.

#include "ros/ros.h"

ros/ros.h — этот заголовочный файл содержит все требумые заголовочные файлы, которые используются для работы с системой ROS.

#include "std_msgs/String.h"

Этот заголовочный файл содержит объявление сообщения типа std_msgs/String, который находится в пакете std_msgs. Этот заголовочный файл генерируется автоматически из файла String.msg в этом пакете. Для более подробной информации об объявлении сообщений, см. страницу про msg.

ros::init(argc, argv, "talker");

Инициализация ROS. Это позволяет ROS осуществлять переопределение аргументов ROS, задаваемых через командную строку — теперь не важно. Это так же задаёт имя нашего узла. Имя узла должно быть уникальным в работающей системе.

Имя, используемое здесь, должно быть базовым, т.е. не может содержать символ косой черты «/».

ros::NodeHandle n;

Создаёт дескриптор (handle) для данного узла. Первый созданный NodeHandle автоматически выполняет инициализацию узла, а последний разрушенный дескриптор освобождает все ресурсы, который использовались узлом.

ros::Publisher chatter_pub = n.advertise("chatter", 1000);

Сообщает Мастеру, что мы собираемся публиковать сообщения типа std_msgs/String на Тему chatter. Это позволяет Мастеру сообщить другим узлам, слушающих chatter, что мы собираемся публиковать сообщения на данную Тему. Второй аргумент — это размер нашей публикуемой очереди. В случае, если мы публикуем сообщения слишком быстро, то в буфере накопится максимум 1000 сообщений прежде чем старые сообщения будут выбрасываться.

NodeHandle::advertise() возвращает объект ros::Publisher, который служит двум целям: 1) он содержит метод publish(), который позволяет вам публиковать сообщения в Тему, и 2) когда он выходит за область видимости, он автоматически отписывается (unadvertise).

ros::Rate loop_rate(10);

Объект ros::Rate позволяет вам задавать частоту, с которой должен работать цикл. Он будет отслеживать, сколько прошло с последнего вызова Rate::sleep(), и спать заданный промежуток времени.

В данном примере, мы устанавливаем частоту в 10Гц.

  int count = 0;
  while (ros::ok())
  {

По-умолчанию, roscpp устанавливает дескриптор SIGINT который обеспечивает обработку нажатия Ctrl-C, что приведёт к тому, что ros::ok() вернёт false.

ros::ok() вернёт false если:

* получен сигнал SIGINT (Ctrl-C)
* мы запустили в сети другой узел с тем же именем
* другим приложением был вызван метод ros::shutdown()

После того, как ros::ok() возвращает false, все методы ROS будут возвращать ошибку.

    std_msgs::String msg;

    std::stringstream ss;
    ss << "hello world " << count;
    msg.data = ss.str();

В ROS, мы передаём сообщение, используя адаптированный класс, который обычно генерируется из msg -файлов. Возможны и более сложные типы данных, но сейчас мы используем стандартное сообщение типа String, который является одним из базовых типов данных.

chatter_pub.publish(msg);

Теперь мы производим трансляцию сообщения для всех, кто подключен.

	ROS_INFO("%s", msg.data.c_str());

ROS_INFO и другие подобные макросы — это замена стандартным printf / cout. Смотрите документацию rosconsole для получения дополнительной информации.

	ros::spinOnce();

Вызов ros::spinOnce() не является необходимым для этой простой программы, потому что мы не получаем каких-либо обратных вызовов. Однако, если бы вы должны были добавить подписку на это приложение, и не выполнили ros::spinOnce(), то ваши обратные вызовы никогда не выполнятся. Таким образом, его добавление является хорошей манерой.

	loop_rate.sleep();

Теперь мы используем объект ros::Rate , чтобы спать в течение оставшегося времени, чтобы публиковать наше сообщение с частотой 10Гц.

Вот сокращенный вариант того, что происходит:
* Инициализация системы ROS
* Сообщение мастеру, что мы собираемся публиковать сообщения std_msgs/String на тему chatter
* Цикл публикации сообщений в chatter 10 раз в секунду

Теперь нам нужно написать узел-получатель наших сообщений.

1.2 Написание Подписчика узла (Subscriber Node)

1.2.1 Код

Создайте файл src/listener.cpp в пакете beginner_tutorials и вставьте в него следующий код:
https://code.ros.org/svn/ros/stacks/ros_tutorials/trunk/roscpp_tutorials/listener/listener.cpp

#include "ros/ros.h"
#include "std_msgs/String.h"

/**
 * Этот пример демонстрирует простое получение сообщений в системе ROS.
 */
void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
  /**
   * Функция ros::init() требуется для проверки аргументов argc и argv, чтобы выполнить
   * преобразования или переопределение аргументов ROS, задаваемых через командную строку.
   * Для программного переопределения вы можете использовать другую версию init(), которая
   * принимает переопределение напрямую, но для большинства программ командной сроки, обработка
   * argc и argv - простейший путь реализовать это. Третий аргумент init() - это название узла.
   *
   * Вы должны вызвать одну из версий ros::init() перед использованием любых других
   * частей системы ROS.
   */
  ros::init(argc, argv, "listener");

  /**
   * NodeHandle - главная точка доступа для взаимодействия с системой ROS.
   * Конструктор NodeHandle полностью инициализирует этот узел, а в конце,
   * деструктор NodeHandle завершит работу узла.
   */
  ros::NodeHandle n;

  /**
   * subscribe() сообщает ROS, что вы хотите получать сообщения
   * на заданную тему. Это приводит к запросу Мастер-узла ROS,
   * который содержит регистрационные данные о том, кто публикуте и
   * кто получает сообщения. Сообщения передаются в функцию обратного вызова, здесь
   * она называется chatterCallback. subscribe() возвращает объект подписчика(Subscriber), который вы
   * должны держать, пока вы не захотите отказаться от подписки. Когда все копии объекта подписки
   * выходят из области видимости, то обратный вызов будет автоматически отписан от
   * этой темы.
   *
   * Второй параметр subscribe() указывает размер очереди сообщений.
   * Если сообщения прибывают быстрее, чем они обрабатываются, это
   * число указывает количество сообщений, которые будут сохраняться
   * в буфере прежде чем удалять самые старые.
   */
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

  /**
   * ros::spin() будет входить в цикл, прокачки обратных вызовов. В этой версии, все
   * обратные вызовы будут вызвана из этой нити (основноой).
   * ros::spin() будет завершён после нажатия Ctrl-C, или отключения узла от мастера.
   */
  ros::spin();

  return 0;
}

1.2.2 Разъяснение кода

Теперь, давайте разобьём код по частям, игнорируя некоторые части, которые уже были описаны выше.

	void chatterCallback(const std_msgs::String::ConstPtr& msg)
	{
		ROS_INFO("I heard: [%s]", msg->data.c_str());
	}

Это функция обратного вызова, которая будет вызываться при получении нового сообщения на тему chatter. Сообщение передаётся в boost shared_ptr, что означает, что вы можете сохранить его, если вы хотите, не беспокоясь о его получении к вам, и без копирования исходных данных.

	ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);

Подписаться на тему chatter с мастером. ROS будет вызывать функцию chatterCallback() когда будет получено новое сообщение. Второй аргумент указывает размер очереди, если мы не в состоянии обрабатывать сообщения, достаточно быстро. В этом случае, если очередь достигнет 1000 сообщений, мы начнем выбрасывать старые сообщения по мере поступления новых.

NodeHandle::subscribe() возвращает объект ros::Subscriber, который вы должны держать, пока вы не захотите отказаться от подписки. Когда объект Subscriber разрушается, мы автоматически отписываемся от темы chatter.

Существуют версии NodeHandle::subscribe(), которые позволяют указать метод класса, или даже что-нибудь типа Boost.Function объекта. Обзор roscpp содержит больше информации.

	ros::spin();

ros::spin() входит в цикл, обработки сообщений как можно быстрой обработки обратного вызова. Не волнуйтесь, хотя, если нет ничего для выполнения этот метод не будет использовать много ресурсов CPU. ros::spin() завершит работу если ros::ok() возвращает false, что означает, что был вызван ros::shutdown() , либо сработал обработчик Ctrl-C, т.е. мастер говорит нам о завершении работы, или это завершение было вызвано вручную.

Есть и другие способы обработки обратных вызовов, но пока мы не будем о них беспокоиться. Пакет roscpp_tutorials содержит некоторые демо-приложения, которые демонстрируют это. Обзор roscpp также содержит дополнительную информацию.

И снова, вот сокращенный вариант того, что происходит:
* Инициализация системы ROS
* Подписаться на тему chatter
* Spin, ожидает прибытия сообщений
* При поступлении сообщения, вызывается функция chatterCallback()

1.3 Сборка ваших узлов

roscreate-pkg создаст Makefile по-умолчанию и CMakeLists.txt для вашего пакета.

$ rosed beginner_tutorials CMakeLists.txt 

Он должен выглядеть примерно так:

	cmake_minimum_required(VERSION 2.4.6)
	include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

	# Set the build type.  Options are:
	#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
	#  Debug          : w/ debug symbols, w/o optimization
	#  Release        : w/o debug symbols, w/ optimization
	#  RelWithDebInfo : w/ debug symbols, w/ optimization
	#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
	#set(ROS_BUILD_TYPE RelWithDebInfo)

	rosbuild_init()

	#set the default path for built executables to the "bin" directory
	set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
	#set the default path for built libraries to the "lib" directory
	set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

	#uncomment if you have defined messages
	#rosbuild_genmsg()
	#uncomment if you have defined services
	#rosbuild_gensrv()

	#common commands for building c++ executables and libraries
	#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
	#target_link_libraries(${PROJECT_NAME} another_library)
	#rosbuild_add_boost_directories()
	#rosbuild_link_boost(${PROJECT_NAME} thread)
	#rosbuild_add_executable(example examples/example.cpp)
	#target_link_libraries(example ${PROJECT_NAME})

Добавьте в конец файла следующее:

	rosbuild_add_executable(talker src/talker.cpp)
	rosbuild_add_executable(listener src/listener.cpp)

Это создаст два исполняемых файла, talker и listener, которые по-умолчанию сохранятся в каталоге «bin».

Для получения дополнительной информации об использовании CMake с ROS см. CMakeLists. Теперь запустите make:

$ make

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

Ссылки
http://www.ros.org/wiki/ROS/Tutorials/WritingPublisherSubscriber(c++)


Добавить комментарий

Arduino

Что такое Arduino?
Зачем мне Arduino?
Начало работы с Arduino
Для начинающих ардуинщиков
Радиодетали (точка входа для начинающих ардуинщиков)
Первые шаги с Arduino

Разделы

  1. Преимуществ нет, за исключением читабельности: тип bool обычно имеет размер 1 байт, как и uint8_t. Думаю, компилятор в обоих случаях…

  2. Добрый день! Я недавно начал изучать программирование под STM32 и ваши уроки просто бесценны! Хотел узнать зачем использовать переменную типа…

3D-печать AI Android Arduino Bluetooth CraftDuino DIY IDE iRobot Kinect LEGO OpenCV Open Source Python Raspberry Pi RoboCraft ROS swarm ИК автоматизация андроид балансировать бионика версия видео военный датчик дрон интерфейс камера кибервесна манипулятор машинное обучение наше нейронная сеть подводный пылесос работа распознавание робот робототехника светодиод сервомашинка собака управление ходить шаг за шагом шаговый двигатель шилд юмор

OpenCV
Робототехника
Будущее за бионическими роботами?
Нейронная сеть - введение