ROS学习笔记10-写一个简单的订阅者和发布者（C++版本）

本文档来源于：http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

1. 写发布者节点
   如前所述，节点是连接到ROS网络的一个可执行程序，在该例中，写一个节点名为Talker，该节点对外不断发布消息。

   先转到包路径：

   roscd begginner_tutorials

   先创建一个src目录用于存放源代码：

   mkdir -p src
   

   然后在其中创建一个talker.cpp源文件，并将如下内容粘贴其中（代码解读见其中的中文注释）。

   /*
    * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
    *
    * Redistribution and use in source and binary forms, with or without
    * modification, are permitted provided that the following conditions are met:
    *   * Redistributions of source code must retain the above copyright notice,
    *     this list of conditions and the following disclaimer.
    *   * Redistributions in binary form must reproduce the above copyright
    *     notice, this list of conditions and the following disclaimer in the
    *     documentation and/or other materials provided with the distribution.
    *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
    *     contributors may be used to endorse or promote products derived from
    *     this software without specific prior written permission.
    *
    * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
    * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
    * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
    * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
    * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
    * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
    * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
    * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
    * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
    * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    * POSSIBILITY OF SUCH DAMAGE.
    */
   // %Tag(FULLTEXT)%
   // %Tag(ROS_HEADER)%
   //Comment by spy: ros/ros.h为ros系统基本功能所需的头文件
   #include "ros/ros.h"
   // %EndTag(ROS_HEADER)%
   // %Tag(MSG_HEADER)%
   //Comment by spy: std_msgs/String.h为std_msgs包中的一个消息头文件，由String.msg文件生成
   #include "std_msgs/String.h"
   // %EndTag(MSG_HEADER)%
   
   #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.
      */
   // %Tag(INIT)%
   // 初始化ROS，这允许我们通过命令行进行重命名，也在该出指定我们的节点名，该命名需要在ROS系统下面唯一的（不能重名）
   // 该命名不能含有斜杠（/）。
     ros::init(argc, argv, "talker");
   // %EndTag(INIT)%
   
     /**
      * 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.
      */
   // %Tag(NODEHANDLE)%
   // 创建一个节点句柄，该句柄作为节点进程的句柄，第一次创建时实际上初始化节点，最后一个析构时会释放资源。
     ros::NodeHandle n;
   // %EndTag(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.
      */
   // %Tag(PUBLISHER)%
   // 该句告诉master主控节点，我们将在chatter主题中发布std_msgs的String消息，在我们发布消息时，
   // 主控节点将会告知所有订阅该主题的节点，消息队列大小为1000，即在队列里有消息超过1000个之后，才会丢弃以前老的消息
     ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
   // %EndTag(PUBLISHER)%
   
   // %Tag(LOOP_RATE)%
   // 指定运行频率为10hz，在调用Rate::sleep()之前都会运行。
     ros::Rate loop_rate(10);
   // %EndTag(LOOP_RATE)%
   
     /**
      * A count of how many messages we have sent. This is used to create
      * a unique string for each message.
      */
   // %Tag(ROS_OK)%
     int count = 0;
   // 在ros::ok返回true的时候，持续运行，返回false的时候，中断，在以下情况下返回false：
   // 1.收到中断信号，SIGINT，键盘输入Ctrl+C会触发中断信号。
   // 2.被同名节点从网络上踢出。
   // 3.程序的其他部分调用了ros::shutdown()。
   // 4.所有的ros::NodeHandles被销毁。
     while (ros::ok())
     {
   // %EndTag(ROS_OK)%
       /**
        * This is a message object. You stuff it with data, and then publish it.
        */
   // %Tag(FILL_MESSAGE)%
   // 使用标准消息填充一个字符串。
       std_msgs::String msg;
   
       std::stringstream ss;
       ss << "hello world " << count;
       msg.data = ss.str();
   // %EndTag(FILL_MESSAGE)%
   
   // %Tag(ROSCONSOLE)%
   // ROS下的输出语句，代替std::cout标准输出。见ros信息级别小节。
       ROS_INFO("%s", msg.data.c_str());
   // %EndTag(ROSCONSOLE)%
   
       /**
        * 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.
        */
   // %Tag(PUBLISH)%
   // 发布消息
       chatter_pub.publish(msg);
   // %EndTag(PUBLISH)%
   // 当添加一个订阅时，ros::spinOnce()会保证，你可以触发回调函数(callback)，如果没有该语句，则不会触发。
   // %Tag(SPINONCE)%
       ros::spinOnce();
   // %EndTag(SPINONCE)%
   
   // %Tag(RATE_SLEEP)%
   // 休眠以确保10hz运行。
       loop_rate.sleep();
   // %EndTag(RATE_SLEEP)%
       ++count;
     }
   
     return 0;
   }
   // %EndTag(FULLTEXT)%
   

   该文件也可以在github如下路径中找到：https://raw.github.com/ros/ros_tutorials/kinetic-devel/roscpp_tutorials/talker/talker.cpp大致步骤如下：

   1. 　初始化ros系统：

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

   2. 创建句柄和发布者：

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

   3. 指定频率循环发送。

2. 编写一个订阅者
   同样，在包中创建一个listener.cpp，并粘贴以下代码（代码解释见代码中的中文注释）：

   /*
    * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
    *
    * Redistribution and use in source and binary forms, with or without
    * modification, are permitted provided that the following conditions are met:
    *   * Redistributions of source code must retain the above copyright notice,
    *     this list of conditions and the following disclaimer.
    *   * Redistributions in binary form must reproduce the above copyright
    *     notice, this list of conditions and the following disclaimer in the
    *     documentation and/or other materials provided with the distribution.
    *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
    *     contributors may be used to endorse or promote products derived from
    *     this software without specific prior written permission.
    *
    * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
    * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
    * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
    * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
    * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
    * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
    * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
    * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
    * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
    * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    * POSSIBILITY OF SUCH DAMAGE.
    */
   
   // %Tag(FULLTEXT)%
   #include "ros/ros.h"
   #include "std_msgs/String.h"
   
   /**
    * This tutorial demonstrates simple receipt of messages over the ROS system.
    */
   // %Tag(CALLBACK)%
   // 接收到主题消息时的回调函数
   void chatterCallback(const std_msgs::String::ConstPtr& msg)
   {
     ROS_INFO("I heard: [%s]", msg->data.c_str());
   }
   // %EndTag(CALLBACK)%
   
   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, "listener");
   
     /**
      * 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;
   
     /**
      * 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.
      */
   // %Tag(SUBSCRIBER)%
   // 订阅指定主题，并指定回调函数，1000为队列大小，当我们来不及处理消息时，会存储在该队列中，若队列元素大于1000，则会抛弃老的消息
     ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
   // %EndTag(SUBSCRIBER)%
   
     /**
      * 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.
      */
   // %Tag(SPIN)%
   // spin会进入循环，并在有消息到达时处理消息，Ctrl+C会结束该循环，或者主控节点关闭该节点时也会结束循环。
     ros::spin();
   // %EndTag(SPIN)%
   
     return 0;
   }
   // %EndTag(FULLTEXT)%
   

3. 编译节点
   在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_make，代码如下：

   # In your catkin workspace
   $ cd ~/catkin_ws
   $ catkin_make
   

   编译成功显示如下：

   Base path: /home/shao/catkin_ws
   Source space: /home/shao/catkin_ws/src
   Build space: /home/shao/catkin_ws/build
   Devel space: /home/shao/catkin_ws/devel
   Install space: /home/shao/catkin_ws/install
   ####
   #### Running command: "make cmake_check_build_system" in "/home/shao/catkin_ws/build"
   ####
   ####
   #### Running command: "make -j8 -l8" in "/home/shao/catkin_ws/build"
   ####
   [  0%] Built target std_msgs_generate_messages_eus
   [  0%] Built target std_msgs_generate_messages_lisp
   [  0%] Built target std_msgs_generate_messages_nodejs
   [  0%] Built target std_msgs_generate_messages_py
   [  0%] Built target std_msgs_generate_messages_cpp
   [  0%] Built target _begginner_tutorials_generate_messages_check_deps_AddTwoInts
   [  0%] Built target _begginner_tutorials_generate_messages_check_deps_Num
   [  5%] Generating EusLisp code from begginner_tutorials/Num.msg
   [ 11%] Generating EusLisp manifest code for begginner_tutorials
   [ 17%] Generating EusLisp code from begginner_tutorials/AddTwoInts.srv
   [ 23%] Generating Javascript code from begginner_tutorials/Num.msg
   [ 29%] Generating C++ code from begginner_tutorials/AddTwoInts.srv
   [ 35%] Generating C++ code from begginner_tutorials/Num.msg
   [ 41%] Generating Python from MSG begginner_tutorials/Num
   [ 47%] Generating Lisp code from begginner_tutorials/Num.msg
   [ 52%] Generating Javascript code from begginner_tutorials/AddTwoInts.srv
   [ 58%] Generating Lisp code from begginner_tutorials/AddTwoInts.srv
   [ 64%] Generating Python code from SRV begginner_tutorials/AddTwoInts
   [ 64%] Built target begginner_tutorials_generate_messages_nodejs
   [ 64%] Built target begginner_tutorials_generate_messages_lisp
   [ 70%] Generating Python msg __init__.py for begginner_tutorials
   [ 76%] Generating Python srv __init__.py for begginner_tutorials
   [ 76%] Built target begginner_tutorials_generate_messages_cpp
   [ 88%] Building CXX object begginner_tutorials/CMakeFiles/talker.dir/src/talker.cpp.o
   [ 88%] Building CXX object begginner_tutorials/CMakeFiles/listener.dir/src/listener.cpp.o
   [ 88%] Built target begginner_tutorials_generate_messages_py
   [ 88%] Built target begginner_tutorials_generate_messages_eus
   [ 88%] Built target begginner_tutorials_generate_messages
   [ 94%] Linking CXX executable /home/shao/catkin_ws/devel/lib/begginner_tutorials/talker
   [ 94%] Built target talker
   [100%] Linking CXX executable /home/shao/catkin_ws/devel/lib/begginner_tutorials/listener
   [100%] Built target listener
    

4. 运行节点
   打开一个新终端：

   roscore
   

   然后运行talker节点：

   rosrun beginner_tutorials talker
   

   出现发布信息：

   

   运行listener节点：

   rosrun beginner_tutorials listener
   

   出现接收界面：
   

   运行成功。好久不见，hello world。