ROS学习笔记四：用C++编写ROS发布与订阅

一、创建并编译功能包

1.1 创建功能包

在工作空间的 src 目录下创建功能包：

$ cd ~/dev/catkin_ws/src
$ catkin_create_pkg chapter2_tutorials std_msgs roscpp

1.2 编译功能包

进入工作目录下编译全部功能包：

$ cd ~/dev/catkin_ws/
$ catkin_make

如果到达100%，表示一切顺利，否则会报错停止编译。

二、创建并编译节点

2.1 创建节点

进入功能包的 src 目录下，在这个文件夹中，创建两个新文件：example1_a.cpp 和 example1_b.cpp。

example1_a.cpp（发布节点）：

#include "ros/ros.h"                     //"ros/ros.h"包含ROS节点所有节点的必要文件
#include "std_msgs/String.h"             //"std_msgs/String.h"包含消息类型
#include <sstream>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "example1_a"); //启动该节点并设置其名称（example1_a），该名称是唯一的
    ros::NodeHandle n;                   //设置节点进程的句柄
    ros::Publisher chatter_pub = n.advertise<std_msgs::String>("message", 1000); //把这个节点设置成发布者，并把发布主题的类型告诉节点管理器。第一个参数是消息名称“message”，第二个参数将缓冲区设置为1000个消息
    ros::Rate loop_rate(10);             //设置频率10Hz

    while (ros::ok())                    //一直运行，直到CTRL+C停止运行
    {
        std_msgs::String msg;            //创建消息变量，变量类型必须符合发送的要求
        std::stringstream ss;
        ss << " I am the example1_a node "; //要发布的消息内容
        msg.data = ss.str();
        chatter_pub.publish(msg);        //发布消息
        ros::spinOnce();                 //如果出现订阅者，ROS会更新和读取所有主题
        loop_rate.sleep();               //按频率挂起
    }

    return 0;
}

example1_b.cpp（订阅节点）：

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

/*接收消息然后发布*/
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, "example1_b");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("message", 1000, chatterCallback); //创建一个订阅者，从主题获取以“message”为名称的消息，缓冲区为1000，处理消息句柄的回调函数chatterCallback
    ros::spin();  //ros::spin()库是响应循环，消息到达时调用函数chatterCallback，CTRL+C结束循环
    return 0;
}

2.2 编译节点

将下列几行代码添加到CMakeLists.txt文件中include_directories(include ${catkin_INCLUDE_DIRS})之后：

add_executable(chap2_example1_a src/example1_a.cpp)
add_executable(chap2_example1_b src/example1_b.cpp)
add_dependencies(chap2_example1_a chapter2_tutorials_generate_messages_cpp)
add_dependencies(chap2_example1_b chapter2_tutorials_generate_messages_cpp)
target_link_libraries(chap2_example1_a ${catkin_LIBRARIES})
target_link_libraries(chap2_example1_b ${catkin_LIBRARIES})

回到工作目录下，编译功能包：

$ cd ~/dev/catkin_ws/
$ catkin_make chapter2_tutorials

如果顺利编译的话，将会生成两个可执行程序 example1_a 和 example1_b 文件，默认生成可执行文件的目录是catkin工作空间的devel目录下的lib目录。

3 检验编写的节点

首先要确认在carkin_make之后加载了工作空间的环境变量，然后分别在三个新的终端运行如下命令：

$ roscore
$ rosrun chapter2_tutorials chap2_example1_a
$ rosrun chapter2_tutorials chap2_example1_b

使用rqt_graph命令能够创建一个显示当前系统运行情况的动态图形，如下图所示。example1_a 节点发布 /message 主题，同时 example1_b 节点订阅了这个主题。