ROS学习笔记（二）ROS消息发布（publish）与订阅(subscribe)（C++代码详解）

一、创建Publisher

Publisher的主要作用是针对指定话题发布特定数据类型的消息。
使用代码实现一个节点talker，节点中创建一个Publisher并发布字符串“Hello World”，代码详解如下

//头文件部分
#include <sstream> 
#include "ros/ros.h"
 /*"ros/ros.h 是一个实用的头文件，它引用了 ROS 系统中大部分常用的头文件。"
地址在 /opt/ros/noetic/include/ros/" */
#include "std_msgs/String.h" 
/* "std_msgs"是一个消息类型依赖包，此处要传输string类型数据，
需要包含该数据类型的头文件String.h，就在这个依赖包里 */

//初始化部分
int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");
    /* ROS节点初始化。初始化的init函数包含三个参数，
    前两个参数是命令行或launch文件输入的参数，可以用来命名重映射等功能；
    第三个参数定义了Publisher节点的名称“talker”，不允许重复，是一个base name。 */
    
    ros::NodeHandle n;
    /* 为这个进程的节点创建一个句柄。
    第一个创建的 NodeHandle 会为节点进行初始化，
    最后一个销毁的 NodeHandle 则会释放该节点所占用的所有资源。 */
    
    ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
    /* 告诉 master 将要在 chatter（话题名）上发布 std_msgs/String 消息类型的消息。
    这样 master 就会告诉所有订阅了 chatter 话题的节点，将要有数据发布。
    NodeHandle::advertise() 会建立一个topic。在ROS Master端注册一个Publisher，
    返回一个 ros::Publisher 对象,此处为chatter_pub，,它有两个作用： 
    1) 它有一个 publish()函数可以在topic上发布(pubish)消息； 
    2) <std_msgs::String>指定后面要发布的消息类型是std_msgs包中的string类型，如果消息类型不对,它会拒绝发布。 
    
    ("chatter", 1000)中两个参数，第一个参数指定以"chatter"为话题发布消息
    第二个参数是发布序列的大小。如果发布的消息的频率太高，缓冲区中的消息在大于 1000 个的时候就会开始丢弃先前发布的消息。*/
    
    ros::Rate loop_rate(10);
    /* ros::Rate 对象可以允许你指定自循环的频率。它会追踪记录自上一次调用 Rate::sleep() 后时间的流逝，并休眠直到一个频率周期的时间。在这个例子中，让它以 10Hz 的频率运行，即节点休眠时间为100ms。 */
   
    //循环部分
    int count = 0;
    while (ros::ok())
    {
    /*进入节点的主循环，如果下列条件之一发生，ros::ok() 返回false，跳出循环：

·SIGINT 被触发 (Ctrl+C)：roscpp 会默认生成一个 SIGINT 句柄，它负责·处理 Ctrl+C 键盘操作使ros::ok() 返回 false
·被另一同名节点踢出 ROS 网络
·关闭函数ros::shutdown() 被程序的另一部分调用
·节点中的所有 ros::NodeHandles 都已经被销毁
	一旦 ros::ok() 返回 false, 所有的 ROS 调用都会失效。
    */
    
        std_msgs::String msg;//建立暂存区，先将消息放入，在进行publish
        std::stringstream ss;
        ss << "hello world " << count;
        msg.data = ss.str();//将要输出的字符串消息存储到string消息类型中唯一成员data中
        ROS_INFO("%s", msg.data.c_str()); //类似C/C++的 printf/cout 等函数，打印日志信息。
        chatter_pub.publish(msg);//发布封装完毕的消息msg。Master会查找订阅该话题的节点，并完成两个节点的连接，传输消息
        
        ros::spinOnce();//处理订阅话题的所有回调函数callback()，
        loop_rate.sleep(); //休眠，休眠时间由loop_rate()设定
        ++count;

    }
    return 0;

}

总结

• 初始化 ROS节点
• 向ROS Master注册节点消息，包括发布的话题名和话题中的消息类型
• 以一定的频率循环在话题上发布消息

二、创建Subscribe

创建一个Subscriber以订阅Publisher节点发布的消息，此节点listener的内容如下

#include "ros/ros.h"
#include "std_msgs/String.h" //所要订阅的消息类型，此处是std)msgs包下的String。msg

//回调函数部分
void chatterCallback(const std_msgs::String::ConstPtr& msg)

{
    ROS_INFO("I heard: [%s]", msg->data.c_str()); //将接收到的消息打印出来
}
/*subscriber的回调函数，当接收到 chatter 话题的时候就会被调用。
参数是所接收的消息的常数指标（const pkg_name::msg_name::ConstPtr& msg）.
消息是以 boost shared_ptr 指针的形式传输，这就意味着你可以存储它而又不需要复制数据。
之后使用msg->field_name即可存取message的资料
*/

int main(int argc, char **argv){
    ros::init(argc, argv, "listener"); //初始化ROS节点
    ros::NodeHandle n; //创建句柄节点
    ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
/*告诉 master 要订阅 chatter 话题（第一个参数）上的消息。
当有消息发布到这个话题时，ROS 就会调用 chatterCallback() 函数（第三个参数）。
第二个参数是队列大小，当缓存达到 1000 条消息后，自动舍弃时间戳最早的消息。
NodeHandle::subscribe() 返回 ros::Subscriber 对象,此处为sub。
当这个对象销毁时，它将自动退订 chatter 话题的消息。
有各种不同的 NodeHandle::subscribe() 函数，可以指定类的成员函数，甚至是 Boost.Function 对象可以调用的任何数据类型。
*/
    ros::spin();
/*ros::spin() 进入自循环，可以尽可能快的调用消息回调函数，会调用主程序中所有回调函数，此处只有chatterCallback()
一旦 ros::ok() 返回 false，ros::spin() 就会立刻跳出自循环。
这有可能是 ros::shutdown() 被调用，或者是用户按下了 Ctrl-C，使得 master 告诉节点要终止运行。
*/
    return 0;
}

总结Subscribe

• 初始化ROS系统
• 订阅话题
• 进入自循环，等待消息的到达，然后调用 chatterCallback() 函数
• 在回调函数中处理消息

三、编译功能包

C++是一种编译语言，在运行之前需要将代码编译成可执行文件，如果使用python等解析语言编写代码，则不需要进行编译（好！之后去学习下）

ROS中的编译器使用的是Cmake，编译规则通过功能包中的CMakeLists.txt文件设置。

再多说一句，上面编写好的publisher节点talker.cpp与subscriber节点listener.cpp放到 ~/<workspace name>/src/<package name>/src/ 中。CmakeLists.txt与package.xml这两个文件在创建功能包的时候，会在 ~/<workspace name>/src/中生成。

接下来修改CmakeLists.txt文件，可以直接添加代码（也可以找到相关被注释的代码，去掉注释并修改）

include_directories(include ${catkin_INCLUDE_DIRS})

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

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

本功能包中用到的四种编译配置项含义：
（1）include_directories

用于设置头文件的相对路径。全局路径默认是功能包所在目录，一般会放在功能包
根目录下的include文件夹中，所以此处需要添加该文件夹。此外，该配置项还包含
ROS catkin编译器默认包含的其他头文件路径${catkin_INCLUDE_DIRS}，比如
ROS默认安装路径、Linux系统路径等。在本例中，所用的是ROS默认安装路径下的头
文件。

（2）add_executable

用于设置需要编译的代码和生成的可执行文件。第一个参数为期望生成的可执行文件
的名称，后边的参数为参与编译的源码文件（cpp），如果需要多个代码文件，则可
在后面依次列出，中间用空格进行分隔。

（3）target_link_libraries

用于设置链接库。很多功能需要使用系统或者第三方的库函数，通过该选项可以配置
执行文件链接的库文件，第一个参数是可执行文件的名称，后面依次列出需要链接的
库。此处编译没有使用其他库，添加默认链接库${catkin_LIBRARIES}即可。

（4）add_dependencies

用于设置依赖。在很多应用中，我们需要定义语言无关的消息类型，消息类型会在编
译过程中产生相应语言的代码，如果编译的可执行文件依赖这些动态生成的代码，则
需要使用add_dependencies添加${PROJECT_NAME}_generate_messages_cpp
配置，即该功能包动态产生的消息代码。该编译规则也可以添加其他需要依赖的功能包。

Cmakelist.txt修改完成后。在工作空间的根路径下开始编译：

$ cd ~/<workspace name>
$ catkin_make

成功编译之后会在~/<workspace name>/devel/lib/<package name>/ 路径下生成两个可执行文件：talker和listener

注意：可能在编译的时候报错

The dependency target "learning_communication_generate_messages_cpp" of target "talker" does not exist.

这是因为Cmake版本不对，CmakeLists.txt文件开头有一句未被注释的代码，内容可能是

cmake_minimum_required(VERSION 3.0.2)

改成

cmake_minimum_required(VERSION 2.8.3)

四、运行

编译完成之后，在运行节点之前，需要在终端中设置环境变量，否则无法找到功能包最终编译生成的可执行文件：

$ cd ~/<workspace name>
$ source ./devel/setup.bash

但如此的话，该环境变量旨在当前终端窗口有效，每次要在新终端窗口运行节点时，都需要重新设置环境变量，所以也可以将环境变量的配置脚本添加到终端的配置文件中：

$ echo "source ~/ros/tr3_6/devel/setup.bash" >> ~/.bashrc
$ source ~/.bashrc

然后开始运行节点

4.1 启动roscore

首先启动roscore，确保ROS Master已经启动：

$ roscore

4.2启动节点

Publisher与Subscriber节点的启动顺序没有要求，最到就是循环等待一会儿。分别新建两个终端窗口，使用rosrun命令启动节点

$ rosrun <package name> talker //<package name>换成你的功能包名字

$ rosrun <package name> listener //<package name>换成你的功能包名字

Publisher发布消息结果如下

Subscriber订阅消息的结果如下：

4.3关闭退出

最后可以 Ctrl + C 退出

参考

ROS总结——ROS消息发布和订阅
ROS 懶人筆記 3 — 以C++撰寫Publisher與Subscriber
《ROS机器人开发实践》