ROS入门之主题和服务、msg和srv（C++）

本文主要是总结一下ROS节点、主题、服务的相关概念、简单的主题发布者和订阅者和简单的服务器端和客户端以及创建srv、msg文件。主要参考了以下文章：

1、Wiki：> http://wiki.ros.org/cn
2、博客：http://blog.csdn.net/mountzf/article/details/52328358

http://blog.csdn.net/liulj95/article/details/47680599

1、概念相关

Nodes:节点,一个节点其实只不过是ROS程序包中的一个可执行文件。ROS节点可以使用ROS客户库与其他节点通信。节点可以发布或接收一个话题（向话题发布消息或者订阅话题以接收消息）。节点也可以提供或使用某种服务。
Messages:消息，消息是一种ROS数据类型，用于订阅或发布到一个话题。
Topics:话题,节点可以发布消息到话题，也可以订阅话题以接收消息。
Servicce:服务，服务是节点之间通讯的另一种方式。服务允许节点发送请求（request） 并获得一个响应（response）。
Msg：描述ROS messages内容的简单text文件，它们用来生成不同语言的messages源码。
Srv：一个srv文件描述一个服务，它由两部分组成：一个请求和一个回应。 msg文件存放在一个软件包的msg目录中，而srv文件则存放在srv目录中。

2、简单的主题发布者和订阅者

2.1 主题发布者节点
在package下创建src/talker.cpp,在cpp文件下输入如下程序：

#include "ros/ros.h"//ros/ros.h是一个实用的头文件，它引用了ROS系统中大部分常用的头文件，使用它会使得编程很简便。  
#include "std_msgs/String.h"//这引用了std_msgs/String 消息, 它存放在std_msgs package里，是由String.msg文件自动生成的头文件。  
#include <sstream>  
/*This tutorial demonstrates simple sending of messages over the ROS system.*/  
int main(int argc, char **argv)  
{  
  /* The ros::init() function needs to see argc and argv so that it can perform any ROS arguments and name remapping that were provided at the command line. For programmatic remappings you can use a different version of init() which takes remappings directly, but for most command-line programs, passing argc and argv is the easiest way to do it.  The third argument to init() is the name of the node.You must call one of the versions of ros::init() before using any other part of the ROS system. */  
  ros::init(argc, argv, "talker");//初始化ROS。它允许ROS通过命令行进行名称重映射——目前，这不是重点。同样，我们也在这里指定我们节点的名称——必须唯一。这里的名称必须是一个base name，不能包含/。  
  /*NodeHandle is the main access point to communications with the ROS system.The first NodeHandle constructed will fully initialize this node, and the last NodeHandle destructed will close down the node.*/  
  ros::NodeHandle n;//为这个进程的节点创建一个句柄。第一个创建的NodeHandle会为节点进行初始化，最后一个销毁的NodeHandle会清理节点使用的所有资源。  
  /* The advertise() function is how you tell ROS that you want to publish on a given topic name. This invokes a call to the ROS master node, which keeps a registry of who is publishing and who is subscribing. After this advertise() call is made, the master node will notify anyone who is trying to subscribe to this topic name, and they will in turn negotiate a peer-to-peer connection with this node.  advertise() returns a Publisher object which allows you to publish messages on that topic through a call to publish().  Once all copies of the returned Publisher object are destroyed, the topic will be automatically unadvertised.The second parameter to advertise() is the size of the message queue used for publishing messages.  If messages are published more quickly than we can send them, the number here specifies how many messages to buffer up before throwing some away.*/  
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);//告诉节点管理器master我们将要在chatter topic上发布一个std_msgs/String的消息。这样master就会告诉所有订阅了chatter topic的节点，将要有数据发布。第二个参数是发布序列的大小。在这样的情况下，如果我们发布的消息太快，缓冲区中的消息在大于1000个的时候就会开始丢弃先前发布的消息。NodeHandle::advertise() 返回一个 ros::Publisher对象,它有两个作用: 1) 它有一个publish()成员函数可以让你在topic上发布消息； 2) 如果消息类型不对,它会拒绝发布。  
  ros::Rate loop_rate(10);//ros::Rate对象可以允许你指定自循环的频率。它会追踪记录自上一次调用Rate::sleep()后时间的流逝，并休眠直到一个频率周期的时间。在这个例子中，我们让它以10hz的频率运行。  
  /*A count of how many messages we have sent. This is used to create a unique string for each message.*/  
  int count = 0;  
  while (ros::ok())  
  {//roscpp会默认安装一个SIGINT句柄，它负责处理Ctrl-C键盘操作——使得ros::ok()返回FALSE。ros::ok()返回false，如果下列条件之一发生：SIGINT接收到(Ctrl-C);被另一同名节点踢出ROS网络;ros::shutdown()被程序的另一部分调用;所有的ros::NodeHandles都已经被销毁.一旦ros::ok()返回false, 所有的ROS调用都会失效。  
    /*This is a message object. You stuff it with data, and then publish it.*/  
    std_msgs::String msg;  
    std::stringstream ss;  
    ss << "hello world " << count;  
    msg.data = ss.str();  
//我们使用一个由msg file文件产生的‘消息自适应’类在ROS网络中广播消息。现在我们使用标准的String消息，它只有一个数据成员"data"。当然你也可以发布更复杂的消息类型。  
    ROS_INFO("%s", msg.data.c_str());//ROS_INFO和类似的函数用来替代printf/cout.   
    /*The publish() function is how you send messages. The parameter 
is the message object. The type of this object must agree with the type 
given as a template parameter to the advertise<>() call, as was done 
in the constructor above.*/  
    chatter_pub.publish(msg);//现在我们已经向所有连接到chatter topic的节点发送了消息。  
    ros::spinOnce();//在这个例子中并不是一定要调用ros::spinOnce()，因为我们不接受回调。然而，如果你想拓展这个程序，却又没有在这调用ros::spinOnce()，你的回调函数就永远也不会被调用。所以，在这里最好还是加上这一语句。  
    loop_rate.sleep();//这条语句是调用ros::Rate对象来休眠一段时间以使得发布频率为10hz。  
    ++count;  
  }  
  return 0;  
}  

小结：编写主题发布者节点步骤：
⑴初始化ROS系统
⑵广播消息：在chatter主题上发布std_msgs/String消息到节点管理器，告诉节点管理器master我们将要在chatter topic上发布一个std_msgs/String的消息，这样master就会告诉所有订阅了chatter topic的节点，将要有数据发布。
⑶以指定的频率循环发布消息到chatter主题。

2.2 主题订阅者节点
在package下创建src/listener.cpp,在cpp文件下输入如下程序：

#include "ros/ros.h"  
#include "std_msgs/String.h"  
/*This tutorial demonstrates simple receipt of messages over the ROS system.*/  
void chatterCallback(const std_msgs::String::ConstPtr& msg)  
{  
  ROS_INFO("I heard: [%s]", msg->data.c_str());  
}//这是一个回调函数，当消息到达chatter topic的时候就会被调用。消息是以 boost shared_ptr指针的形式传输，这就意味着你可以存储它而又不需要复制数据  
int main(int argc, char **argv)  
{  
  ros::init(argc, argv, "listener");  
  ros::NodeHandle n;  
  /*The subscribe() call is how you tell ROS that you want to receive messages on a given topic.  This invokes a call to the ROS master node, which keeps a registry of who is publishing and who is subscribing.  Messages are passed to a callback function, here called chatterCallback.  subscribe() returns a Subscriber object that you must hold on to until you want to unsubscribe.  When all copies of the Subscriber object go out of scope, this callback will automatically be unsubscribed from this topic.The second parameter to the subscribe() function is the size of the message queue.  If messages are arriving faster than they are being processed, this is the number of messages that will be buffered up before beginning to throw away the oldest ones.*/  
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);  
//告诉master我们要订阅chatter topic上的消息。当有消息到达topic时，ROS就会调用chatterCallback()函数。第二个参数是队列大小，以防我们处理消息的速度不够快，在缓存了1000个消息后，再有新的消息到来就将开始丢弃先前接收的消息。NodeHandle::subscribe()返回ros::Subscriber对象,你必须让它处于活动状态直到你不再想订阅该消息。当这个对象销毁时，它将自动退订上的消息。有各种不同的NodeHandle::subscribe()函数，允许你指定类的成员函数，甚至是Boost.Function对象可以调用的任何数据类型。  
  /*ros::spin() will enter a loop, pumping callbacks.  With this version, all callbacks will be called from within this thread (the main one).  ros::spin() will exit when Ctrl-C is pressed, or the node is shutdown by the master.*/  
  ros::spin();//ros::spin()进入自循环，可以尽可能快的调用消息回调函数。如果没有消息到达，它不会占用很多CPU，所以不用担心。一旦ros::ok()返回FALSE，ros::spin()就会立刻跳出自循环。这有可能是ros::shutdown()被调用，或者是用户按下了Ctrl-C，使得master告诉节点要shutdown。也有可能是节点被人为的关闭。  
  return 0;  
}  

小结：编写主题订阅者节点步骤：
⑴初始化ROS系统
⑵从chatter主题订阅消息
⑶spin，然后等待消息到达
⑷当消息到达时，chattercallbck（）函数被调用

3、简单的服务器和客户端

3.1服务器端
在package下创建src/ add_two_inits_server.cpp并输入如下程序：

#include "ros/ros.h"  
#include"beginner_tutorials/AddTwoInts.h"//beginner_tutorials/AddTwoInts.h是由编译系统自动根据我们先前创建的srv文件生成的对应该srv文件的头文件。  
//这个add函数提供两个int值求和的服务，int值从请求request里面获取，而返回数据装入响应response内，这些数据类型都定义在srv文件内部，函数返回一个boolean值。  
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值已经相加，并存入了response。然后一些关于request和response的信息被记录下来。最后，service完成计算后返回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);  
//这里，service已经建立起来，并在ROS内发布出来。  
  ROS_INFO("Ready to add two ints.");  
  ros::spin();  

  return 0;  
}  

3.2客户端

#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");  
//这段代码为add_two_ints service创建一个client。ros::ServiceClient 对象待会用来调用service。  
  beginner_tutorials::AddTwoInts srv;  
  srv.request.a = atoll(argv[1]);  
  srv.request.b = atoll(argv[2]);  
//这里，我们实例化一个由ROS编译系统自动生成的service类，并给其request成员赋值。一个service类包含两个成员request和response。同时也包括两个类定义Request和Response。  
  if (client.call(srv))//这段代码是在调用service。由于service的调用是模态过程（调用的时候占用进程阻止其他代码的执行），一旦调用完成，将返回调用结果。如果service调用成功，call()函数将返回true，srv.response里面的值将是合法的值。如果调用失败，call()函数将返回false，srv.response里面的值将是非法的。  
  {  
    ROS_INFO("Sum: %ld", (long int)srv.response.sum);  
  }  
  else  
  {  
    ROS_ERROR("Failed to call service add_two_ints");  
    return 1;  
  }  

  return 0;  
}  

4、创建和使用Ros srv和msg文件

具体的创建srv和msg文件的方法请参照博文,在此不再赘述，

http://blog.csdn.net/mountzf/article/details/52328358

这里主要是分析srv和msg文件的使用
4.1 Srv文件的使用
如上3.1服务器端和3.2客户端所述，

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

这里的beginner_tutorials/AddTwoInts.h是由编译系统自动根据我们先前创建的srv文件生成的对应该srv文件的头文件。

  beginner_tutorials::AddTwoInts srv;  
  srv.request.a = atoll(argv[1]);  
  srv.request.b = atoll(argv[2]); 

这里，实例化一个由ROS编译系统自动生成的service类，并给其request成员赋值，即将执行编译后的可执行文件时输入的1、2两个参数均赋值给srv文件中定义的request变量。一个service类包含两个成员request和response。同时也包括两个类定义Request和Response。

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; 

这里，这个add函数提供两个int值求和的服务，int值从请求request里面获取，而返回数据装入响应response内，这些数据类型都定义在srv文件内部。

4.2 Msg文件的使用
同样的，如上2.1主题发布和2.2主题订阅所述，

#include "std_msgs/String.h"

这引用了std_msgs/String 消息, 它存放在std_msgs package里，是由String.msg文件自动生成的头文件。

    std_msgs::String msg;  
    std::stringstream ss;  
    ss << "hello world " << count;  
    msg.data = ss.str();  

这里，使用一个由msg file文件产生的‘消息自适应’类在ROS网络中广播消息。把“hello world”赋给msg.data作为发布的消息。