Cpp ROS(一)发布器Publisher和订阅器Subscriber简单例子

ROS发布器Publisher和订阅器Subscriber(roscpp)

#1 环境

Ubuntu 16.04
ros kinetic

#2 概述

本文通过两种方式编译,一种是直接使用CMake,另一种是catkin_make

两种方式任选一种

#3 CMake编译

来源官方文档: 传送门,点我点我

#3.1 开始

1. 创建目录,用于存放Publisher 和 Subscriber代码

mkdir -p pubsub
cd pubsub

2. 编写c++ demo代码

Publisher

vim 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;
}

Subscriber

vim 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;
}

3. 编写CMake代码

vim CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(myproject)

# Build the talker and listener. Each one uses the following ROS packages,
# which we need to find_package() individually:
#   roscpp (the client library)
#   std_msgs (contains the std_msgs/String message type)
find_package(roscpp REQUIRED)
find_package(std_msgs REQUIRED)
# We've found them; now use their variables in the usual way to configure
# the compile and link steps.
# Note: we skip calling link_directories() because ROS packages follow the
# recommended CMake practice of returning absolute paths to libraries
include_directories(${roscpp_INCLUDE_DIRS})
include_directories(${std_msgs_INCLUDE_DIRS})

add_executable(talker talker.cpp)
target_link_libraries(talker ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES})

add_executable(listener listener.cpp)
target_link_libraries(listener ${roscpp_LIBRARIES} ${std_msgs_LIBRARIES})

# (optional) Install the executables.
install(TARGETS talker listener
        DESTINATION bin)

4. 编译

mkdir -p build
cd build
cmake ..
make

5. 运行

// 新开窗口
roscore

./build/talker

./build/listener

通过CMake方式编译,到此完成,生成两个二进制可执行文件

#4 catkin_make编译

#4.1 Publisher

#4.1.1 创建工作区和包

mkdir -p ~/catkin_ws/src # 创建工作区
cd ~/catkin_ws/src # 进入工作区
# catkin_create_pkg package_name(包名称) dependencies（依赖)
catkin_create_pkg my_node_demo roscpp std_msgs #新建一个ROS包, 包名:my_node_demo

创建完成以后,在目录my_node_demo下多出两个文件(package.xml ,CMakeLists.txt)和两个文件夹(src, include)。

其中的package.xml便是之前提到的ROS包配置文件，描述关于包的信息。CMakeLists.txt是用来配置编译过程。

#4.1.2 创建publisher

vim ~/catkin_ws/src/my_node_demo/src/publisher.cpp

#include <ros/ros.h>
#include "std_msgs/String.h"


int main(int argc, char **argv) {
    /*
    void ros::init(
        int &argc,
        char **argv,
        const std::string &name,
        uint32_t options=0
        )
    argc：参数个数，一般由int main(int argc, char ** argv) 提供
    argv：指向字符串数组(即参数文本)的指针，一般由int main(int argc, char ** argv) 提供
    name:节点的名字，必须是基础的名字（基础的含义是不能包括命名空间)
    options:可空,默认options=0
    */
    ros::init(argc, argv, "publisher_demo"); // 初始化节点名

    /*
    NodeHandle类:
        advertise():
            Publisher ros::NodeHandle::advertise(
                const std::string & topic,
                uint32_t queue_size,
                bool latch = false
            )
            ros::NodeHandle handle;
            ros::Publisher pub = handle.advertise<std_msgs::Empty>("my_topic", 1);
        topic: 话题
        queue_size: 队列大小
        latch: 可空,默认为false; 如果为true，当有一个新的订阅者时，会向新的订阅者发送最后一条广播（在其订阅之前的最后一条）
    */
    ros::NodeHandle handle;
    ros::Publisher my_publisher_object = handle.advertise<std_msgs::String>("talker_demo",1);


    std_msgs::String publish_msg; // 创建一个发布器将要使用的消息变量
    /*
     该消息定义在： /opt/ros/kinetic/share/std_msgs
     在ROS中发布的消息都应该提前定义，以便订阅者接收到消息后该如何解读
     Float64消息的定义如下，其中包含一个数据字段data：
     float64 data
     */

    publish_msg.data = "9";

    while (ros::ok()){
//        publish_msg.data = publish_msg.data + 0.001; //每循环一次+0.01
        my_publisher_object.publish(publish_msg); // 发布消息到对应的话题
    }
}

#include <ros/ros.h>表示包含ROS头文件

#include "std_msgs/String.h"表示包含标准消息类型中的String

#4.1.3 修改CMakeLists.txt

vim ~/catkin_ws/src/my_node_demo/CMakeLists.txt

在最后加上下面两行代码:

add_executable(publisher_demo src/publisher.cpp)
target_link_libraries(publisher_demo ${catkin_LIBRARIES})

#4.2 编译运行

1. 编译

cd ~/catkin_ws/
catkin_make

2. 激活环境

source /opt/ros/kinetic/setup.bash
source ~/catkin_ws/devel/setup.bash

3. 启动ROS

roscore

4. 运行publisher

// rosrun 包名 节点名
rosrun my_node_demo publisher_demo 

5. 测试

订阅topic

// rostopic echo topic
rostopic echo talker_demo

#3.1 Subscriber

#3.1.1 创建Subscriber

vim ~/catkin_ws/src/my_node_demo/src/subscriber.cpp

#include "ros/ros.h"
#include "std_msgs/String.h"

// 接收到订阅的消息后，会进入消息回调函数
void chatter_callback(const std_msgs::String::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "listener_demo");

    // 创建节点句柄
    ros::NodeHandle handle;

    // 创建一个Subscriber，订阅名为chatter的topic，注册回调函数chatterCallback
    ros::Subscriber sub = handle.subscribe(
            "talker_demo",
            1000,
            chatter_callback
            );

    // 循环等待回调函数
    ros::spin();

    return 0;
}

#3.1.2 修改CMakeLists.txt

vim ~/catkin_ws/src/my_node_demo/CMakeLists.txt

在最后加上下面两行代码:

add_executable(publisher_demo src/publisher.cpp)
add_executable(subscriber_demo src/subscriber.cpp)
target_link_libraries(publisher_demo ${catkin_LIBRARIES})
target_link_libraries(subscriber_demo ${catkin_LIBRARIES})

#3.1.3 编译运行

1. 编译

cd ~/catkin_ws/
catkin_make

2. 激活环境

新开一个terminal

source /opt/ros/kinetic/setup.bash
source ~/catkin_ws/devel/setup.bash

3. 运行subscriber

// rosrun 包名 节点名
rosrun my_node_demo subscriber_demo