ROS学习<四> 发布与订阅，服务与客户（C++）

用C++编写简单的ROS发布与订阅程序

首先进入目录~/catkin_ws/src/beginner_tutorials/src 即工程的源码目录。

创建文件  talker.cpp  为发布话题的程序。

talker.cpp：

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

#include <sstream>

/**
 *Sending message over the ROS system
*/

int main(int argc, char **argv)
{
    /**
     * must call one of version of ros::init()
     */
    ros::init(argc,argv,"talker");
    
    /**
     * using NodeHandle to communicate with the ROS system
     */
    ros::NodeHandle n;
    /*
     * use advertise() to tell ROS the topic name published 
     * invoke a registy(master node) system to tell who is publishing and
     * who is subscribing
     * the master node will create a p2p connect between the topic and the 
     * node who want to subscribe
     */
    ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter",1000);
    ros::Rate loop_rate(10);
    
    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());


	chatter_pub.publish(msg);
	ros::spinOnce();
	loop_rate.sleep();
	++count;
    }
    return 0;
}

对代码进行简要的分析：

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

ros规定必须调用ros::init进行初始化，第三个参数"talker"表示节点的“名字”，可以用于标识这个节点。

对于NodeHandle来说，这是一个对节点进行操作的句柄，在本程序中类似于一个全局指针。

ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter",1000);
ros::Rate loop_rate(10);

我们通过NodeHandle中的advertise方法来获取发布一个话题，话题名为"chatter"，第二个参数1000表示表示发布队列大小，简单的理解就是当发布的数量到达1000之后，才会开始丢弃之前的数据。而对于ros::Rate来说，它将初始化一个特定的频率用于发布话题，这个方法将返回Publisher类型的变量，提供了发布话题的方法。在这里的时间频率时10hz

最后，使用Ctrl+C 将退出ros::ok循环。在这里，在循环中将使用chatter_pub.publish(msg)，发布包含特定消息的话题（此处的消息即helloworld+i）。

listener.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, "listener");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("chatter",1000,chatterCallback);
    ros::spin();
    return 0;

主函数中初始化节点，以listener命名节点，同时通过handle句柄来对刚刚发布的"chtter"话题进行订阅——将体现在回调函数chatterCallback()中。

对于ros::spin()来说，wiki上的解释：它将进入一个循环，一旦消息到达之后，程序将以最快速度进入回调。

我个人的理解：节点之间通过发布和订阅话题进行通信，而通信的实体却是消息。举个例子，你订阅了“XX牌牛奶”（话题），那么这家牛奶公司会每天或者每两天总之以特定的频率来给你送鲜奶（消息），一旦你收到鲜奶，肯定会第一时间签收并喝掉它（时间长了会变质嘛），这也就是进入回调。如果你订阅了一年，那么在这一年的时间内，将会重复进行上述过程，这也就是ros::spin()的作用。那么对于这个示例来说，可以说是消息事件驱动的。

编好了源文件，我们需要修改CMakeLists.txt，主要是添加：

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)

这样，在工程目录~/catkin_ws  使用catkin_make命令生成Makefile文件并编译生成可执行程序。

最后对程序进行测试，在两个终端分别输入如下命令：

rosrun beginner_tutorials talker
rosrun beginner_tutorials listener

用C++编写简单的ROS服务与客户程序

首先进入目录~/catkin_ws/src/beginner_tutorials/src 即工程的源码目录。

创建源文件：add_two_ints_client.cpp  add_two_ints_server.cpp

add_two_ints_server.cpp：

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"

bool add(beginner_tutorials::AddTwoInts::Request &req,
	beginner_tutorials::AddTwoInts::Response &res) 
{
    res.sum = req.a + req.b;
    ROS_INFO("request: x=%ld, y=%ld", (long int)req.a, (long int)req.b);
    ROS_INFO("sending back response: [%ld]", (long int)res.sum);
    return true;
}

int main(int argc, char** argv)
{
    ros::init(argc,argv,"add_two_ints_server");
    ros::NodeHandle n;
    
    ros::ServiceServer service = n.advertiseService("add_two_ints",add);
    ROS_INFO("Ready to add two ints.");
    ros::spin();


    return 0;
}

这里的服务端程序，与上述订阅者业务逻辑类似，这里提供的服务函数（两个整型数据相加）类似于上述回调函数，当客户端发送服务请求（request）的时候将会调用，ros::spin()起到与上述订阅者程序类似的作用。

add_two_ints_client.cpp:

#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
#include <cstdlib>

int main(int argc ,char** argv)
{
    ros::init(argc,argv,"add_two_ints_client");
    if(argc != 3)
    {
	ROS_INFO("usage: add_two_ints_client X Y");
	return 1;
    }
    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");

    beginner_tutorials::AddTwoInts srv;
    srv.request.a = atoll(argv[1]);
    srv.request.b = atoll(argv[2]);
    if(client.call(srv))
    {
	ROS_INFO("Sum: %ld",(long int)srv.response.sum);
    }
    else
    {
	ROS_ERROR("Failed to call service add_two_ints");
	return 1;
    }
    return 0;
}

通过句柄调用serviceClient方法返回ServiceClient类型的变量，从而为这个变量绑定一个服务，ServiceClient提供call方法来向服务端发送请求(request)，在这里用到beginner_tutorials::AddTwoInts 类型是自动生成的，在ROS的概念里面以service class 来命令，在他的定义在~/catkin_ws/src/beginner_tutorials/srv目录下，以'---'分隔符来区别在一个service-client系统中的request成员（本例中分别为request.a  和 request.b）与response（本例中为response.sum）成员。