编写简单的发布者和订阅者（C++）

  27 #include "ros/ros.h"
  28 #include "std_msgs/String.h"
  29 
  30 #include <sstream>
  31 
  32 /**
  33  * This tutorial demonstrates simple 
sending of messages over the ROS system.
  34  */
  35 int main(int argc, char **argv)
  36 {
  37   /**
  38    * The ros::init() function needs
 to see argc and argv so that it can perform
  39    * any ROS arguments and name 
remapping that were provided at the command line.
  40    * For programmatic remappings 
you can use a different version of init() which takes
  41    * remappings directly, but for
 most command-line programs, passing argc and argv is
  42    * the easiest way to do it.  
The third argument to init() is the name of the node.
  43    *
  44    * You must call one of the 
versions of ros::init() before using any other
  45    * part of the ROS system.
  46    */
  47   ros::init(argc, argv, "talker");
  48 
  49   /**
  50    * NodeHandle is the main access point 
to communications with the ROS system.
  51    * The first NodeHandle constructed will 
fully initialize this node, and the last
  52    * NodeHandle destructed will close down 
the node.
  53    */
  54   ros::NodeHandle n;
  55 
  56   /**
  57    * The advertise() function 
is how you tell ROS that you want to
  58    * publish on a given topic 
name. This invokes a call to the ROS
  59    * master node, which keeps 
a registry of who is publishing and who
  60    * is subscribing. After this
 advertise() call is made, the master
  61    * node will notify anyone who
 is trying to subscribe to this topic name,
  62    * and they will in turn negotiate
 a peer-to-peer connection with this
  63    * node.  advertise() returns a
 Publisher object which allows you to
  64    * publish messages on that topic
 through a call to publish().  Once
  65    * all copies of the returned
 Publisher object are destroyed, the topic
  66    * will be automatically 
unadvertised.
  67    *
  68    * The second parameter to 
advertise() is the size of the message queue
  69    * used for publishing messages.  
If messages are published more quickly
  70    * than we can send them, the number 
here specifies how many messages to
  71    * buffer up before throwing some 
away.
  72    */
  73   ros::
Publisher chatter_pub = n.advertise<std_msgs
::String>("chatter", 1000);
  74 
  75   ros::Rate loop_rate(10);
  76 
  77   /**
  78    * A count of how many messages we
 have sent. This is used to create
  79    * a unique string for each message.
  80    */
  81   int count = 0;
  82   while (ros::ok())
  83   {
  84     /**
  85      * This is a message object. You
 stuff it with data, and then publish it.
  86      */
  87     std_msgs::String msg;
  88 
  89     std::stringstream ss;
  90     ss << "hello world " << count;
  91     msg.data = ss.str();
  92 
  93     ROS_INFO("%s", msg.data.c_str());
  94 
  95     /**
  96      * The publish() function is how 
you send messages. The parameter
  97      * is the message object. The type 
of this object must agree with the type
  98      * given as a template parameter to
 the advertise<>() call, as was done
  99      * in the constructor above.
 100      */
 101     chatter_pub.publish(msg);
 102 
 103     ros::spinOnce();
 104 
 105     loop_rate.sleep();
 106     ++count;
 107   }
 108 
 109 
 110   return 0;
 111 }

1.2 解释

现在，让我们把代码分解。

  27 #include "ros/ros.h"
  28

ros/ros.h是一个很便利的include，它包括了使用ROS系统中最常见的公共部分所需的全部头文件。

  28 #include "std_msgs/String.h"
  29

它引用了位于std_msgs包里的std_msgs/String消息。这是从std_msgs包里的String.msg文件中自动生成的头文件。有关消息定义的更多信息，请参见msg页面。

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

初始化ROS。这使得ROS可以通过命令行进行名称重映射——不过目前不重要。这也是我们给节点指定名称的地方。节点名在运行的系统中必须是唯一的。注意：名称必须是基本名称，例如不能包含任何斜杠/。

  54   ros::NodeHandle n;

为这个进程的节点创建句柄。创建的第一个NodeHandle实际上将执行节点的初始化，而最后一个被销毁的NodeHandle将清除节点所使用的任何资源。

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

告诉主节点我们将要在chatter话题上发布一个类型为std_msgs/String的消息。这会让主节点告诉任何正在监听chatter的节点，我们将在这一话题上发布数据。第二个参数是发布队列的大小。在本例中，如果我们发布得太快，它将最多缓存1000条消息，不然就会丢弃旧消息。

NodeHandle::advertise()返回一个ros::Publisher对象，它有2个目的：其一，它包含一个publish()方法，可以将消息发布到创建它的话题上；其二，当超出范围时，它将自动取消这一宣告操作。

  75   ros::Rate loop_rate(10);

ros::Rate对象能让你指定循环的频率。它会记录从上次调用Rate::sleep()到现在已经有多长时间，并休眠正确的时间。在本例中，我们告诉它希望以10Hz运行。

  81   int count = 0;
  82   while (ros::ok())
  83   {

默认情况下，roscpp将安装一个SIGINT处理程序，它能够处理Ctrl+C操作，让ros::ok()返回false。

ros::ok()在以下情况会返回false：

收到SIGINT信号（Ctrl+C）

被另一个同名的节点踢出了网络

ros::shutdown()被程序的另一部分调用

所有的ros::NodeHandles都已被销毁

一旦ros::ok()返回false, 所有的ROS调用都会失败。

  87     std_msgs::String msg;
  88 
  89     std::stringstream ss;
  90     ss << "hello world " << count;
  91     msg.data = ss.str();

的类在ROS上广播消息，该类通常由msg文件生成。更复杂的数据类型也可以，不过我们现在将使用标准的String消息，它有一个成员：data。

 101     chatter_pub.publish(msg);

现在，我们实际上把这个信息广播给了任何已连接的节点。

  93     ROS_INFO("%s", msg.data.c_str());

ROS_INFO和它的朋友们可用来取代printf/cout。参见rosconsole文档获取更多信息。

 103     ros::spinOnce();

此处调用ros::spinOnce()对于这个简单程序来说没啥必要，因为我们没有接收任何回调。然而，如果要在这个程序中添加订阅，但此处没有ros::spinOnce()的话，回调函数将永远不会被调用。所以还是加上吧。

 105     loop_rate.sleep();

现在我们使用ros::Rate在剩下的时间内睡眠，以让我们达到10Hz的发布速率。

对上边的内容进行一下总结：

初始化ROS系统

向主节点宣告我们将要在chatter话题上发布std_msgs/String类型的消息

以每秒10次的速率向chatter循环发布消息

接下来我们要编写一个节点来接收消息。

2 编写订阅者节点

2.1 代码

在src目录下创建talker.cpp文件并粘贴以下内容进去：

https://raw.github.com/ros/ros_tutorials
/kinetic-devel/roscpp_tutorials/listener/listener.cpp

  28 #include "ros/ros.h"
  29 #include "std_msgs/String.h"
  30 
  31 /**
  32  * This tutorial demonstrates 
simple receipt of messages over the ROS system.
  33  */
  34 void 
chatterCallback(const std_msgs::String:
:ConstPtr& msg)
  35 {
  36   ROS_INFO("I heard: [%s]", msg->data.c_str());
  37 }
  38 
  39 int main(int argc, char **argv)
  40 {
  41   /**
  42    * The ros::init() function needs to 
see argc and argv so that it can perform
  43    * any ROS arguments and name remapping
 that were provided at the command line.
  44    * For programmatic remappings you can 
use a different version of init() which takes
  45    * remappings directly, but for most 
command-line programs, passing argc and argv is
  46    * the easiest way to do it.  The 
third argument to init() is the name of the node.
  47    *
  48    * You must call one of the versions 
of ros::init() before using any other
  49    * part of the ROS system.
  50    */
  51   ros::init(argc, argv, "listener");
  52 
  53   /**
  54    * NodeHandle is the main access point
 to communications with the ROS system.
  55    * The first NodeHandle constructed will 
fully initialize this node, and the last
  56    * NodeHandle destructed will close down the
 node.
  57    */
  58   ros::NodeHandle n;
  59 
  60   /**
  61    * The subscribe() call is how you 
tell ROS that you want to receive messages
  62    * on a given topic.  This invokes a 
call to the ROS
  63    * master node, which keeps a registry 
of who is publishing and who
  64    * is subscribing.  Messages are passed 
to a callback function, here
  65    * called chatterCallback.  subscribe() 
returns a Subscriber object that you
  66    * must hold on to until you want to
 unsubscribe.  When all copies of the Subscriber
  67    * object go out of scope, this callback 
will automatically be unsubscribed from
  68    * this topic.
  69    *
  70    * The second parameter to the subscribe() 
function is the size of the message
  71    * queue.  If messages are arriving faster 
than they are being processed, this
  72    * is the number of messages that
 will be buffered up before beginning to throw
  73    * away the oldest ones.
  74    */
  75   ros::Subscriber sub = n.subscribe("
chatter", 1000, chatterCallback);
  76 
  77   /**
  78    * ros::spin() will enter a loop, pumping 
callbacks.  With this version, all
  79    * callbacks will be called from within
 this thread (the main one).  ros::spin()
  80    * will exit when Ctrl-C is pressed, or 
the node is shutdown by the master.
  81    */
  82   ros::spin();
  83 
  84   return 0;
  85 }

2.2 解释

下面我们将逐条解释代码，之前说过的代码就不再赘述了。

  34 void chatterCallback(const 
std_msgs::String::ConstPtr& msg)
  35 {
  36   ROS_INFO("I heard: [%s]
", msg->data.c_str());
  37 }

这是一个回调函数，当有新消息到达chatter话题时它就会被调用。该消息是用boost shared_ptr智能指针传递的，这意味着你可以根据需要存储它，即不用担心它在下面被删除，又不必复制底层（underlying）数据。

  75   ros::Subscriber sub = n.
subscribe("chatter", 1000, chatterCallback);

通过主节点订阅chatter话题。每当有新消息到达时，ROS将调用chatterCallback()函数。第二个参数是队列大小，以防我们处理消息的速度不够快。在本例中，如果队列达到1000条，再有新消息到达时旧消息会被丢弃。

NodeHandle::subscribe()返回一个ros::Subscriber对象，你必须保持它，除非想取消订阅。当Subscriber对象被析构，它将自动从chatter话题取消订阅。

还有另一些版本的NodeHandle::subscribe()函数，可以让你指定为类的成员函数，甚至是可以被Boost.Function对象调用的任何函数。请参见roscpp概览。

  82   ros::spin();

ros::spin()启动了一个自循环，它会尽可能快地调用消息回调函数。不过不要担心，如果没有什么事情，它就不会占用太多CPU。另外，一旦ros::ok()返回false，ros::spin()就会退出，这意味着ros::shutdown()被调用了，主节点让我们关闭（或是因为按下Ctrl+C，它被手动调用）。

还有其他的方法可以进行回调，但是我们在这里不考虑这些。可以自己研究下roscpp_tutorials包中的一些示例程序，或看看roscpp概览。

同样地，我们来总结一下：

初始化ROS系统

订阅chatter话题

开始spin自循环，等待消息的到达

当消息到达后，调用chatterCallback()函数

3 构建节点

你在上一篇教程中使用了catkin_create_pkg，它创建了一个和一个package.xml和CMakeLists.txt文件。

生成的CMakeLists.txt看起来应该是这样的（修改自创建ROS消息和服务教程，删除了未使用的注释和示例）：

https://raw.github.com/ros/catkin_tutorials/master/create
_package_modified/catkin_ws/src/beginner
_tutorials/CMakeLists.txt

   1 cmake_minimum_required(VERSION 2.8.3)
   2 project(beginner_tutorials)
   3 
   4 ## Find catkin and any catkin packages
   5 find_package(catkin REQUIRED 
COMPONENTS roscpp rospy std_msgs genmsg)
   6 
   7 ## Declare ROS messages and services
   8 add_message_files(DIRECTORY msg FILES Num.msg)
   9 add_service_files(DIRECTORY srv
 FILES AddTwoInts.srv)
  10 
  11 ## Generate added messages and services
  12 generate_messages(DEPENDENCIES std_msgs)
  13 
  14 ## Declare a catkin package
  15 catkin_package()

不要担心修改注释(#)示例，只需将这几行添加到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)

最终的CMakeLists.txt长这样：

https://raw.github.com/ros/catkin_tutorials/master/create
_package_pubsub/catkin_ws/src/beginner_
tutorials/CMakeLists.txt

   1 cmake_minimum_required(VERSION 2.8.3)
   2 project(beginner_tutorials)
   3 
   4 ## Find catkin and any catkin packages
   5 find_package(catkin 
REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)
   6 
   7 ## Declare ROS messages and services
   8 add_message_files(FILES Num.msg)
   9 add_service_files(FILES AddTwoInts.srv)
  10 
  11 ## Generate added messages and services
  12 generate_messages(DEPENDENCIES std_msgs)
  13 
  14 ## Declare a catkin package
  15 catkin_package()
  16 
  17 ## Build talker and listener
  18 include_directories
(include ${catkin_INCLUDE_DIRS})
  19 
  20 add_executable(talker src/talker.cpp)
  21 target_link_libraries
(talker ${catkin_LIBRARIES})
  22 add_dependencies(talker
 beginner_tutorials_generate_messages_cpp)
  23 
  24 add_executable(listener src/listener.cpp)
  25 target_link_libraries
(listener ${catkin_LIBRARIES})
  26 add_dependencies(listener beginner_
tutorials_generate_messages_cpp)

这将创建两个可执行文件talker和listener，默认情况下，它们将被放到软件包目录下的devel空间中，即~/catkin_ws/devel/lib/<package name>。

请注意，您必须为可执行目标添加依赖项到消息生成目标：

add_dependencies(talker 
beginner_tutorials_generate_messages_cpp)

这确保了在使用此包之前生成了该包的消息头。如果使用来自你catkin工作空间中的其他包中的消息，则还需要将依赖项添加到各自的生成目标中，因为catkin将所有项目并行构建。在ROS Groovy及更新版本中，还可以使用以下变量来依赖所有必需的目标：

target_link_libraries(talker ${catkin_LIBRARIES})

你可以直接调用可执行文件，也可以使用rosrun来调用它们。它们没有被放在<prefix>/bin中，因为在将软件包安装到系统时会污染PATH环境变量。但如果你希望在安装时将可执行文件放在PATH中，可以配置安装目标，参见CMakeLists.txt。

现在可以运行catkin_make：

# 在你的catkin工作空间下
$ cd ~/catkin_ws
$ catkin_make

注意：如果你又添加了新的包，可能需要通过指定--force-cmake参数来让catkin强制生成。参见使用工作空间。

现在你已经编写了一个简单的发布者和订阅者，让我们来测试发布者和订阅者。

370 次浏览	2次

< 首页

< 下一页