ROS话题发布和订阅节点的C++&Python实现

本文将分别使用C++和Python来实现话题发布者和订阅者，首先创建一个功能包，命名为topic_pub_sub，添加roscpp，rospy等依赖项。

C++实现

• 创建话题发布者

在该功能包的src目录下创建publisher.cpp文件，添加如下程序：

#include <iostream>
#include <sstream>
#include <ros/ros.h> // 该文件包含了ROS系统中大部分常用的头文件
#include <std_msgs/String.h> // 消息类型的头文件，由.msg文件自动生成

int main(int argc, char** argv){
    // 初始化ROS
    ros::init(argc, argv, "publisher");
    // 为该节点创建一个句柄，创建后将会初始化这个节点
    ros::NodeHandle nh;
    // 创建一个类型为std_msgs::String的话题发布器，话题名为"msg"，最后一个参数表示话题序列的大小
    ros::Publisher pub = nh.advertise<std_msgs::String>("msg", 1000);
    // 设置循环频率，单位: Hz
    ros::Rate loop_rate(10)
    int cnt = 0;
    while(ros::ok()){
        // 准备消息内容
        std_msgs::String msg;
        std::stringstream ss;
        ss << "hello world! " << cnt;
        msg.data = ss.str();
        
        // 打印发布的消息
        ROS_INFO("%s", msg.data.c_str());
        // 发布消息
        pub.publish(msg);
        // 消息回调处理，本程序不必调用该函数
        ros::spinOnce();
        // 休眠
        loop_rate.sleep();        
    }
}

• 创建话题订阅者

在src目录下创建subscriber.cpp文件，添加如下程序：

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

/**
 * @brief 回调函数
 * @param  msg              接收到的消息
 */
void callback(std_msgs::String::ConstPtr msg){
    ROS_INFO("%s", msg->data.c_str());
}

int main(int argc, char** argv){
    ros::init(argc, argv, "subscriber");
    ros::NodeHandle nh;

    // 订阅名为"msg"的话题，最后一个参数为回调函数，当接收到新消息时会调用这个函数
    ros::Subscriber sub = nh.subscribe("msg", 10, callback);
    // 回调处理函数
    ros::spin();
}

• 修改CMakeList.txt

编辑该功能包的CMakeLists.txt文件，如下：

cmake_minimum_required(VERSION 3.0.2)
project(topic_pub_sub)

add_compile_options(-std=c++11)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

# 生成话题发布节点
add_executable(publisher src/publisher.cpp)
target_link_libraries(publisher
  ${catkin_LIBRARIES}
)
# 生成话题订阅节点
add_executable(subscriber src/subscriber.cpp)
target_link_libraries(subscriber
  ${catkin_LIBRARIES}
)

接下来使用catkin_make进行编译即可

• 测试

需要运行三个终端，分别运行roscore，发布节点，订阅节点

1. 运行roscore，在第一个终端输入：

roscore

2. 运行话题发布者，在第二个终端中输入：

source ./devel/setup.bash
rosrun topic_pub_sub publisher

3. 运行话题订阅者，在第三个终端中输入：

source ./devel/setup.bash
rosrun topic_pub_sub subscriber

结果如下：

• 使用ROS自带工具查看结果

使用rqt_graph命令可以查看节点间的关系，如图

使用rostopic可以查看当前的话题状态、内容等数据，用法如下：

Python实现

相较于C++，Python实现会简单很多，也不需要编译，在一些简单的、对性能要求不高的场景，可以使用Python快速完成程序原型的设计

在开始前，要先在topic_pub_sub功能包目录下新建一个scripts文件夹

• 创建话题发布者

在scripts目录下创建publisher.py文件，内容如下：

#!/usr/bin/env python3
# #-*- coding: UTF-8 -*- 

import rospy
from std_msgs.msg import String

# 初始化ROS节点
rospy.init_node("publisher")
# 创建话题发布器，话题名称为"msg"，类型为String
pub = rospy.Publisher("msg", String, queue_size=1000)
# 设置循环频率，单位: Hz
loop_rate = rospy.Rate(10)
cnt = 0
while not rospy.is_shutdown():
    msg = String()
    cnt += 1
    msg.data = "hello python! " + str(cnt)
    # 打印发布的消息
    rospy.loginfo(msg.data)
    # 发布消息
    pub.publish(msg)
    # 休眠
    loop_rate.sleep()

编写完成后，需要给这个文件执行权限，使用如下命令

sudo chmod 777 publisher.py

• 创建话题订阅者

在scripts目录下创建subscriber.py文件，内容如下：

#!/usr/bin/env python3
# #-*- coding: UTF-8 -*- 

import rospy
from std_msgs.msg import String

# 消息回调函数
def callback(msg):
    rospy.loginfo(msg.data)

# 初始化ROS节点
rospy.init_node("subscriber")
# 订阅名为"msg"的话题，最后一个参数为回调函数，当接收到新消息时会调用这个函数
sub = rospy.Subscriber("msg", String, callback)
# 回调处理
rospy.spin()

同样需要提高它的执行权限：

sudo chmod 777 subscriber.py

• 测试

操作流程基本和C++实现一节基本一致，运行节点时可以直接使用rosrun topic_pub_sub xxxx.py来运行