ROS学习之八：编写、测试发布者和订阅者

编写发布者（Publisher）

节点是连接到ROS网络的可执行文件，编写发布者节点（talker节点），首先需要进入软件包的src目录下：

roscd package_1
cd src

并在其中创建如下的源代码文件：

#include "ros/ros.h" // 包括了使用ROS系统中最常见的公共部分所需的全部头文件
#include "std_msgs/String.h" // 引用了位于std_msgs包里的std_msgs/String消息

#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可以通过命令行进行名称重映射）

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

  ros::Rate loop_rate(10); // 指定循环频率，单位Hz（录从上次调用Rate::sleep()到现在已经有多长时间，并休眠正确的时间）

  /**
   * 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()) // 当收到Ctrl+C信号、被另一个同名节点踢出网络、ros::shutdown()被程序的另一部分调用或所有的ros::NodeHandles都已被销毁时，ros::ok()返回false
  {
    /**
     * 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();

    ROS_INFO("%s", msg.data.c_str()); // 类似于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); // 将信息广播给任何已连接的节点

    ros::spinOnce(); // 如果在程序中添加订阅，但此处没有ros::spinOnce()的话，回调函数将永远不会被调用

    loop_rate.sleep(); // 使用ros::Rate在剩余的时间内睡眠，使发布速率为10Hz
    ++count;
  }


  return 0;
}

总之，在发布者的源代码文件中需要：

初始化ROS系统
通知主节点将要在chatter话题上发布std_msgs/String类型的消息
以每秒10次的速率向chatter循环发布消息

编写订阅者（Subscriber）

同样，在软件包的src目录下编写订阅者节点（listener节点）：

#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) // 回调函数，当有新消息到达chatter话题时被调用
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

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.
   */
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback); // 通过主节点订阅chatter话题，每当有新消息到达时，ROS将调用chatterCallback()函数，第二个参数是队列大小，如果队列达到1000条，再有新消息到达时旧消息会被丢弃

  /**
   * 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::ok()返回false，ros::spin()就会退出

  return 0;
}

总之，在订阅者的源代码文件中需要：

初始化ROS系统
订阅chatter话题
开始spin自循环，等待消息的到达
当消息到达后，调用chatterCallback()函数

构建节点

将如下内容添加在软件包中CMakeLists.txt文件的末尾：

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker package_1_generate_messages_cpp)

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener package_1_generate_messages_cpp)

add_executable将在工作空间的/devel/lib目录下创建两个可执行文件
add_dependencies为可执行目标添加依赖项到消息生成目标，这确保了在使用此包之前生成了该包的消息头。如果使用了工作空间内其他包中的消息，则还需要将依赖项添加到各自的生成目标中，因为所有项目被并行构建
target_link_libraries用来依赖所有必须的目标

完成上述步骤后，需要在工作空间路径下执行catkin_make命令