Содержание

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++)