（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_tutorials
touch src/listener.cpp
vim 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 packages
find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)

## Declare ROS messages and services
add_message_files(FILES Num.msg)
add_service_files(FILES AddTwoInts.srv)

## Generate added messages and services
generate_messages(DEPENDENCIES std_msgs)

## Declare a catkin package
catkin_package()

## Build talker and listener
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)

创建两个执行文件， 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.bash
rosrun beginner_tutorials talker

新开终端执行listener：

source ~/catkin_ws/devel/setup.bash
rosrun 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]]

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

猜你喜欢