8、ROS使用C++编写一个简单的Publisher和Subscriber

一、Publisher节点

“节点” 是一个连接着ROS框架的可执行文件，这里我们用C++写一个publisher节点 (命名为”talker”) ，不断地发布一条信息。

首先进入包目录的src文件夹：

roscd beginner_tutorials
cd src

然后创建一个名为

'talker.cpp'

的文件，内容为：

/*
"ros/ros.h"里面包含了ROS系统内最常用的一些头文件，包含此文件，便可以使用ROS的核心功能。
*/
#include "ros/ros.h"

/*
"std_msgs/String"是由std_msgs包自动生成的头文件，定义了String信息类型，包含此文件，我们就可以使用String类型
*/
#include "std_msgs/String.h"

#include <sstream>

/**
* This tutorial demonstrates simple sending of messages over the ROS system.
*/
int main(int argc, char **argv)
{
/**
* ros::init()函数需要两个系统命令行参数argc和argv，
* 由此可以执行命令行传来的任何ROS参数和节点的重命名

* 第三个参数是节点的名字，
* 注意这里只能使用基本命名，
* 即名字里不能含有'/'

* 在使用ROS的其他部分之前，你必须调用ros::init()
**/
ros::init(argc, argv, "talker");

/**
* NodeHandle 是节点同ROS系统交流的主要接口
* NodeHandle 在构造的时候会完整地初始化本节点
* NodeHandle 析构的时候会关闭此节点
*/
ros::NodeHandle n;

/**
* 我们通过advertise() 函数指定我们如何在给定的topic上发布信息
* 它会触发对ROS master的调用，master会记录话题发布者和订阅者
* 在advertise()函数执行之后，master会通知每一个订阅此话题的节点
* 两节点间由此可以建立直接的联系

* advertise()会返回一个Publisher对象，使用这个对象的publish方法我们就可以在此话题上发布信息
* 当返回的Publisher对象的所有引用都被销毁的时候，本节点将不再是该话题的发布者

* 此函数是一个带模板的函数，需要传入具体的类型进行实例化
* 传入的类型就是要发布的信息的类型，在这里是String

* 第一个参数是话题名称

* 第二个参数是信息队列的长度，相当于信息的一个缓冲区
* 在我们发布信息的速度大于处理信息的速度时
* 信息会被缓存在先进先出的信息队列里
*/
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);

/**
* Rate loop_rate()构造了一个Rate类的对象
* 用来指定我们发布信息的频率，单位为hz，即每秒多少次
* 在我们调用Rate对象的sleep（）方法之前，信息发布的频率不会发生变化
**/
ros::Rate loop_rate(10);

/**
* 一个记录我们发布的信息数量的计数器
* 它用来为每条信息产生不一样的字符串
* 如'1 message','2 message'这样
*/
int count = 0;

/**
* roscpp默认会构造一个咱SIGINT的处理器来处理系统信号
* 当出现以下情况之一的时候ros:ok()会返回false：
*   1.接受到了一个SIGINT信号(Ctrl-C)
*   2.在程序中调用了ros::shutdown()
*   3.所有的ros::NodeHandle对象及引用都被销毁
**/
while (ros::ok())
{
/**
* 这是一个message对象，我们向其中填入数据，然后可以发布它
*/
std_msgs::String msg;

/**
* 我们发布的信息的格式为"hello world 1/2/3..."
*/
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();

/**
* ROS_INFO是对ROS系统对printf/cout的替代
*/
ROS_INFO("%s", msg.data.c_str());

/**
* publish()函数用来发布信息
* 信息类型必须为前一步实例化advertised()时使用的模板参数的类型
* 这里为String
*/
chatter_pub.publish(msg);

/**
* 在这个简单的应用中，我们没有使用任何回调函数
* 所以ros::spinOnce()的调用不是必须的
* 但是一直在代码里调用ros::spinOnce()是个好习惯
* 它可以保证你指定的回调函数会被调用
*/
ros::spinOnce();

/**
* 调用Rate对象的sleep方法来使我们前面指定的信息发布频率10Hz生效
*/
loop_rate.sleep();
++count;
}
return 0;
}

各语句的功能如注释所示，不再讲解。

二、Subscriber节点

Publisher节点写完，我们有了信息的发布者，接下来我们写一个Subcriber节点来接收信息。

同样在beginner_tutorials包目录的src文件夹下，建立一个名为

'listener.cpp

的文件，内容为：

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

/**
* 传给NodeHandle.subscribe()的回调函数
* 它的参数是一个share_ptr类型的只能指针，功能这里不细讲
*/
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, "listener");

ros::NodeHandle n;

/**
* 参数1：话题名称
* 参数2：信息队列长度
* 参数3：回调函数，每当一个信息到来的时候，这个函数会被调用
* 返回一个ros::Subscriber类的对象，当此对象的所有引用都被销毁是，本节点将不再是该话题的订阅者
*/
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
/**
* 调用ros::spin()函数，进入一个循环
* 不断地接受信息，然后执行回调函数，知道ros::ok()返回false
*/
ros::spin();

return 0;
}

代码比较简单，功能如注释所示。

三、构建项目

1、修改CMakeList.txt

继续使用我们上一节使用的自己创建的beginner_tutorial包。

此包的CMakeLists.txt文件应该如下所示（移除不必要的注释之后）：

cmake_minimum_required(VERSION 2.8.3)
project(beginner_tutorials)

## Find catkin and any catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)

## Declare ROS messages and services
add_message_files(DIRECTORY msg FILES Num.msg)
add_service_files(DIRECTORY srv FILES AddTwoInts.srv)

## Generate added messages and services
generate_messages(DEPENDENCIES std_msgs)

## Declare a catkin package
catkin_package()

我们需要添加以下内容：

include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker beginner_tutorials_generate_messages_cpp)

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener beginner_tutorials_generate_messages_cpp)

最终的CMakeLists.txt应该像这样：

cmake_minimum_required(VERSION 2.8.3)
project(beginner_tutorials)

## Find catkin and any catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)

## Declare ROS messages and services
add_message_files(FILES Num.msg)
add_service_files(FILES AddTwoInts.srv)

## Generate added messages and services
generate_messages(DEPENDENCIES std_msgs)

## Declare a catkin package
catkin_package()

## Build talker and listener
include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker beginner_tutorials_generate_messages_cpp)

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener beginner_tutorials_generate_messages_cpp)

这将创建两个可执行文件

talker

和

listenner

，默认存放于

~/catkin_ws/devel/lib/<package name>

。

2、解释

add_dependencies(talker
beginner_tutorials_generate_messages_cpp
)

add_dependencies声明某CMake target对另外的CMake target的依赖。

这确保了在我们使用某个包里定义的信息类型之前生成它们的头文件。在我们使用其他包的信息类型的时候，也要添加类似这样的依赖。例如再添加一条：

add_dependencies(talker
beginner_tutorials_generate_messages_cpp
roscpp_generate_messages_cpp
)

3、构建

进入我们工作空间的顶层目录执行：

catkin_make

等待一会儿项目即可构建成功。

四、运行节点

首先要启动ROS：

roscore

然后进入工作目录将包导出到全局：

cd ~/catkin_ws
source ./devel/setup.bash

运行talker节点：

rosrun beginner_tutorials talker

将看到类似于：

[ INFO] [1461049103.046522470]: hello world 0
[ INFO] [1461049103.146694944]: hello world 1
[ INFO] [1461049103.246582083]: hello world 2
[ INFO] [1461049103.346603146]: hello world 3
[ INFO] [1461049103.446525742]: hello world 4
[ INFO] [1461049103.546518768]: hello world 5

运行listener节点：

rosrun beginner_tutorials listener

将看到类似于：

[ INFO] [1461049221.914537834]: I heard: [hello world 3]
[ INFO] [1461049222.014228659]: I heard: [hello world 4]
[ INFO] [1461049222.114193464]: I heard: [hello world 5]
[ INFO] [1461049222.214291045]: I heard: [hello world 6]
[ INFO] [1461049222.314210593]: I heard: [hello world 7]
[ INFO] [1461049222.414272158]: I heard: [hello world 8]

节点运行成功。