我的ROS入门（五）：总算搞通ROS的服务节点订阅发布消息话题了

总算搞通ROS的服务节点订阅发布消息主题了。可以实现那几个东西。记录一下吧。

首先要一个工作空间。

在当前系统用户的home目录下的.bashrc文件中添加source /opt/ros/jade/setup.bash，才能运行ros相关的命令。

开始创建一个catkin工作空间

$ mkdir -p ~/catkin_lljws/src
$ cd ~/catkin_lljws/src

p参数可以创建多级父级目录，这里同时创建了catkin_ws文件夹和src文件夹。里面什么都没有，但是也可以build它

$ cd ~/catkin_ws/
$ catkin_make

这个时候记得要source一下devel中的setup.bash，否则不会将当前工作空间设置在ROS工作环境的最顶层。

$ source devel/setup.bash

或者直接将这个放到.bashrc文件中去。

下一步创建一个软件包

首先切换到catkin工作空间中的src目录下：

$ cd ~/catkin_lljws/src

使用catkin_create_pkg命令，这个程序包依赖于std_msgs、roscpp和rospy：

$ catkin_create_pkg llj_package std_msgs rospy roscpp

这些依赖包随后保存在 package.xml文件中。自定义 package.xml：

描述标签<description>，维护者标签maintainer，许可标签license，依赖项标签depend，在本例中，因为在编译和运行时我们需要用到所有指定的依赖包，因此还需要将每一个依赖包分别添加到run_depend标签中。

编译软件包，直接和之前一样build即可。

node节点，一个节点即为一个可执行文件，它可以通过ROS与其它节点进行通信。

message消息，消息是一种ROS数据类型，用于订阅或发布到一个话题。

topic话题，节点可以发布消息到话题，也可以订阅话题以接收消息。

master节点管理器，ROS名称服务 (比如帮助节点找到彼此)。

rosout节点用于收集和记录节点调试输出信息，所以它总是在运行的。

节点与节点之间通过话题相互通信。

话题之间的通信是通过在节点之间发送ROS消息实现的。

发布器和订阅器之间必须发送和接收相同类型的消息。

话题的类型是由发布在它上面的消息类型决定的。

服务（services）是节点之间通讯的另一种方式。服务允许节点发送请求（request） 并获得一个响应（response）

rosparam使得我们能够存储并操作ROS参数服务器搞（Parameter Server ）上的数据。参数服务器能够存储整型、浮点、布尔、字符串、字典和列表等数据类型。

rqt_console属于ROS日志框架(logging framework)的一部分，用来显示节点的输出信息。

roslaunch可以用来启动定义在launch文件中的多个节点。

消息和服务

msg文件就是一个描述ROS中所使用消息类型的简单文本。它们会被用来生成不同语言的源代码。

注意，在构建的时候，我们只需要"message_generation"。然而，在运行的时候，我们只需要"message_runtime"。

查看package.xml, 确保它包含以下两条语句: <build_depend>message_generation</build_depend> <run_depend>message_runtime</run_depend>

在  CMakeLists.txt文件中，利用find_packag函数，增加对 message_generation的依赖，这样就可以生成消息了。

同样，你需要确保你设置了运行依赖：catkin_package( ... CATKIN_DEPENDS message_runtime ... ...)

add_message_files( FILES Num.msg)

手动添加.msg文件后，我们要确保CMake知道在什么时候重新配置我们的project。 确保添加了如下代码:generate_messages()

以上就是创建消息的所有步骤。

一个srv文件描述一项服务。它包含两个部分：请求和响应。

编写简单的消息发布器和订阅器 (C++)

在package里创建src/talker.cpp文件，创建 src/listener.cpp文件，修改 CMakeLists.txt文件，build即可。

#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;
}

#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;
}

编写简单的Service和Client (C++)

#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;
}

#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;
}