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 
    

     出现发布信息:

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

     

     运行listener节点:

    rosrun beginner_tutorials listener 
    

    出现接收界面:
    ROS学习笔记10-写一个简单的订阅者和发布者(C++版本)

     

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

上一篇:mysqli连接的PHP函数给出错误


下一篇:Kafka Internals: Consumers