（3）写简单发布节点和订阅节点

ROS与C++入门教程-写简单发布节点和订阅节点

说明：

• 介绍用C++编写简单发布节点和订阅节点

准备

• 第三节已经建立的基本的工作空间catkin_ws和教程包beginner_tutorials

编写发布节点

• 进入教程目录,并新建源目录和发布节点talker.cpp（代码看参考github），实现发布hello world

$ roscd beginner_tutorials$ mkdir src$ touch src/talker.cpp$ vim src/talker.cpp

• 内容如下：

#include "ros/ros.h"#include "std_msgs/String.h"#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");  /**   * 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 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);  ros::Rate loop_rate(10);  /**   * 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())  {    /**     * 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());    /**     * 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();    loop_rate.sleep();    ++count;  }  return 0;}

代码分析：

• 代码：

#include "ros/ros.h"

• 解释：

  • 导入ROS系统包含核心公共头文件

• 代码：

#include "std_msgs/String.h"

• 解释：

  • 导入std_msgs/String消息头文件，这个是由std_msgs包的string.msg文件自动生成。
  • std_msgs这是标准的消息包，可参考std_msgs ，更多的标准消息定义，可参考msg

• 代码：

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

• 解释：

  • 初始化ROS，指定节点名称为“talker”，节点名称要保持唯一性。名称定义参考

• 代码：

ros::NodeHandle n;

• 解释：

  • 实例化节点

• 代码：

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

• 解释：

  • 发布一个消息类型为std_msgs/String，命名为chatter的话题。
  • 定义消息队列大小为1000，即超过1000条消息之后，旧的消息就会丢弃。

• 代码：

ros::Rate loop_rate(10);

• 解释：

  • 指定发布消息的频率，这里指10Hz，也即每秒10次
  • 通过 Rate::sleep()来处理睡眠的时间来控制对应的发布频率。

• 代码：

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

• 解释：

  • 默认roscpp会植入一个SIGINT处理机制，当按下Ctrl-C，就会让ros::ok()返回false,那循环就会结束。
  • ros::ok() 返回false的几种情况：
    • SIGINT收到(Ctrl-C)信号
    • 另一个同名节点启动，会先中止之前的同名节点
    • ros::shutdown()被调用
    • 所有的ros::NodeHandles被销毁

• 代码：

std_msgs::String msg;std::stringstream ss;ss << "hello world " << count;msg.data = ss.str();

• 解释：

  • 实例化消息msg, 定义字符串流“hello world”并赋值给ss, 最后转成为字符串赋值给msg.data

• 代码：

chatter_pub.publish(msg);

• 解释：

  • 实际发布的函数

• 代码：

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

• 解释：

  • 输出调试信息，更多调试相关参考Verbosity Levels, rosconsole

• 代码：

ros::spinOnce();

• 解释：
  • 不是必需的，但是保持增加这个调用，是好习惯。
  • 如果程序里也有订阅话题，就必需，否则回调函数不能起作用。

• 代码：

loop_rate.sleep();

• 解释：
  • 根据之前ros::Rate loop_rate(10)的定义来控制发布话题的频率。定义10即为每秒10次

编写订阅节点

• 进入beginner_tutorials,新建src/listener.cpp文件，源码参考

cd beginner_tutorialstouch src/listener.cppvim src/listener.cpp

• 添加代码如下：

#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());}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);  /**   * 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();  return 0;}

代码分析：

• 代码：

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

• 解释：
  • 定义回调函数chatterCallback，当收到chatter话题的消息就会调用这个函数。
  • 消息通过boost shared_ptr（共享指针）来传递。
  • 但收到消息，通过ROS_INFO函数显示到终端

• 代码：

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

• 解释：

  • 定义订阅节点，名称为chatter，指定回调函数chatterCallback
  • 但收到消息，则调用函数chatterCallback来处理。
  • 参数1000，定义队列大小，如果处理不够快，超过1000，则丢弃旧的消息。

• 代码：

ros::spin();

• 解释：
  • 调用此函数才真正开始进入循环处理，直到 ros::ok()返回false才停止。

构建节点

• 在之前使用catkin_create_pkg创建包beginner_tutorials，会得到 package.xml 和 CMakeLists.txt两个文件
• 增加如下代码到CMakeLists.txt：

include_directories(include ${catkin_INCLUDE_DIRS})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)

• 完整代码类似：

cmake_minimum_required(VERSION 2.8.3)project(beginner_tutorials)## Find catkin and any catkin packagesfind_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)## Declare ROS messages and servicesadd_message_files(FILES Num.msg)add_service_files(FILES AddTwoInts.srv)## Generate added messages and servicesgenerate_messages(DEPENDENCIES std_msgs)## Declare a catkin packagecatkin_package()## Build talker and listenerinclude_directories(include ${catkin_INCLUDE_DIRS})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)

• 创建两个执行文件， talker and listener，默认在

~/catkin_ws/devel/lib/<package name>

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

add_dependencies(talker beginner_tutorials_generate_messages_cpp)

• 这确保此包的消息头在使用前生成。如果你使用的信息在你的catkin工作空间的其他包里面，你需要添加依赖各自生成目标，因为catkin会平行构建的所有项目。

target_link_libraries(talker ${catkin_LIBRARIES})

• 进入工作空间编译

$ cd ~/catkin_ws$ catkin_make  

运行节点

• 新开终端执行：

$ roscore

• 新开终端执行talker：

source ~/catkin_ws/devel/setup.bashrosrun beginner_tutorials talker

• 新开终端执行listener：

source ~/catkin_ws/devel/setup.bashrosrun beginner_tutorials listener

• talker终端效果：

[ INFO] I published [Hello there! This is message [0]][ INFO] I published [Hello there! This is message [1]][ INFO] I published [Hello there! This is message [2]]

• listener终端效果：

[ INFO] Received [Hello there! This is message [20]][ INFO] Received [Hello there! This is message [21]][ INFO] Received [Hello there! This is message [22]]