ROS三种通信方式之话题通信

一、话题通信的理论模型

话题通信是ROS中最常见通信方式之一，话题通信的实现是比较复杂的，但是话题通信的实现已经被ROS封装好了，我们只需要调用话题通信的接口就能实现轻松话题通信。

在话题通信中有三个角色，分别是Master，Listener，Talker。其实我们简单的翻译一下就是一个主人，一个说话的，一个听话的，master对于经常看日漫的同学应该不陌生。我们可以称Master为管理者，Talker为发布者，Listener为订阅者，这就是发布订阅模型。发布者可以有很多，订阅者也可以有很多，其实就是说话的可以有好几个，听别人说话的也可以有好几个，但是老师只能有一个，说话的可以发布一个话题，然后开始说，听话的可以向管理者说我想听那个话题，管理者告诉听话的到哪儿去听，也就是订阅某个话题，Listener就可以听到Talker发布的信息了。

他的理论模型图如下图所示，我们就不详细展开了。

二、C++编写一个发布者Talker和一个订阅者Listener

这里我们需要新建一个工程目录，因为我们是第一次创建工程。

# 回到用户目录
cd ~
# 我们的工作空间被命名为catkin_ws_cpp
mkdir -p catkin_ws_cpp/src
# 进入工作空间
cd catkin_ws_cpp
# 编译
catkin_make

我们需要创建一个功能包，并为功能包添加依赖，添加依赖的意思是这个功能包的运行是需要其他的程序包的支持的。

cd src
# 这里注意Talker_Listener_test是自己定义的包名
catkin_create_pkg Talker_Listener_test roscpp rospy std_msgs

这里简单介绍一下三个目录

/src 这个目录是存放源代码的
/devel 这个目录是存放编译的中间文件的
/build 这个目录是存放编译后的可执行文件的

2.1、实现Talker

我们在上一步中已经实现了创建一个工作空间，和这个工作空间下的一个功能包，这个功能包的名字叫做 Talker_Listener_test，这个功能包下这么几个文件

├── CMakeLists.txt
├── include
│   └── Talker_Listener_test
├── package.xml
└── src

3 directories, 2 files

我们很显然的知道，代码要放在 /src 下，所以

cd src
touch Test_Talker.cpp
# 这里不会vim的同学可以替换为gedit
vim Test_Talker.cpp

我们加入下面的代码

#include "ros/ros.h"
#include "std_msgs/String.h" //普通文本类型的消息

int main(int argc, char  *argv[])
{
    //设置编码,不设置编码无法正常显示中文
    setlocale(LC_ALL,"");

    // 初始化 ROS 节点
    // 参数1和参数2 后期为节点传值会使用
    // 参数3 是节点名称，是一个标识符，需要保证运行后，在 ROS 网络拓扑中唯一
    ros::init(argc,argv,"Test_Talker");
    // 实例化 ROS 句柄，这个类封装了一些基础的操作
    ros::NodeHandle nh;

    // 实例化 发布者 对象
    // 泛型: 发布的消息类型,这里是标准的数据类型
    // 参数1: 要发布到的话题
    // 参数2: 队列中最大保存的消息数，超出此阀值时，先进的先销毁(时间早的先销毁)
    ros::Publisher pub = nh.advertise<std_msgs::String>("chatter",10);

    // 组织被发布的数据，并编写逻辑发布数据
    // 数据(动态组织)
    std_msgs::String msg;
    // 写入数据
    msg.data = "Hello 你好";

    //逻辑(一秒10次)
    ros::Rate r(10);

    //节点不死
    while (ros::ok())
    {
        //发布消息
        pub.publish(msg);
 
 	    //加入调试，打印发送的消息,这里需要将C++的字符串转换为C的字符串，因为日志输出信息是C的格式
        ROS_INFO("发送的消息:%s",msg.data.c_str());

        //根据前面制定的发送贫频率自动休眠 休眠时间 = 1/频率；
        r.sleep();
 
        // 暂无应用
        ros::spinOnce();
    }
    return 0;
}

C++我们是需要编译运行的，所以我们需要修改Cmake文件CMakeLists.txt。加入下面两行代码

# 添加需要编译的源文件，并给他起一个别名，这里的别名一般我们要和cpp源文件的名字保持一致
add_executable(Test_Talker src/Test_Talker.cpp)
# 添加源文件需要链接的库，这里我们要写入我们cpp源文件当时编译时用的别名
target_link_libraries(Test_Talker
  ${catkin_LIBRARIES}
)

运行该节点，打开shell

# 到项目根目录
cd catkin_ws_cpp
# 编译
catkin_make
# 启动master
roscore
# 刷新脚本文件
source ./devel/setup.zsh
# 运行节点
# rosrun [包名] [节点名]
rosrun Talker_Listener_test Test_Talker

此时可以看到输出的日志信息，即表明发布者节点开始发布信息了。

2.2、实现订阅者Listener

首先创建Listener.cpp文件

cd src
touch Test_Listener.cpp
# 这里不会vim的同学可以替换为gedit
vim Test_Listener.cpp

写入程序

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


//6.设置循环调用回调函数
void doMsg(const std_msgs::String::ConstPtr& msg_p){
    ROS_INFO("我听见:%s",msg_p->data.c_str());
    // ROS_INFO("我听见:%s",(*msg_p).data.c_str());
}

int main(int argc, char *argv[]) {

    setlocale(LC_ALL,"");
    //2.初始化 ROS 节点:命名(唯一)
    ros::init(argc,argv,"Test_listener");
    //3.实例化 ROS 句柄
    ros::NodeHandle nh;

    //4.实例化 订阅者 对象
    ros::Subscriber sub = nh.subscribe<std_msgs::String>("chatter",10,doMsg);
    //5.处理订阅的消息(回调函数)

    ros::spin();//循环读取接收的数据，并调用回调函数处理

    return 0;
}

修改Cmake文件

add_executable(Test_Listener src/Test_Listener.cpp)
add_executable(Test_Talker src/Test_Talker.cpp)

target_link_libraries(Test_Talker
  ${catkin_LIBRARIES}
)

target_link_libraries(Test_Listener
  ${catkin_LIBRARIES}
)

编译并运行节点

# 到项目根目录
cd catkin_ws_cpp
# 编译
catkin_make
# 刷新脚本文件
source ./devel/setup.zsh
# 启动master
roscore
# 运行节点
# rosrun [包名] [节点名]
rosrun Talker_Listener_test Test_Talker
rosrun Talker_Listener_test Test_Listener

三、使用相关命令rostopic&&rosmsg

rostopic bw     显示主题使用的带宽
rostopic delay  显示带有 header 的主题延迟
rostopic echo   打印消息到屏幕
rostopic find   根据类型查找主题
rostopic hz     显示主题的发布频率
rostopic info   显示主题相关信息
rostopic list   显示所有活动状态下的主题
rostopic pub    将数据发布到主题
rostopic type   打印主题类型

rosmsg show    显示消息描述
rosmsg info    显示消息信息
rosmsg list    列出所有消息
rosmsg md5    显示 md5 加密后的消息
rosmsg package    显示某个功能包下的所有消息
rosmsg packages    列出包含消息的功能包

3.1、rostopic list

控制台将打印当前运行状态下的主题名称

3.2、rostopic pub

可以直接调用命令向订阅者发布消息

rostopic pub /主题名称 消息类型 消息内容
rostopic pub /chatter std_msgs gagaxixi

3.3、rostopic echo

获取指定话题当前发布的消息

3.4、rosmsg list

会列出当前 ROS 中的所有 msg

3.5、rosmsg show

列出某个包下所有的msg

3.6、rosmsg info

作用和show一样

四、自定义话题通信

msgs只是简单的文本文件，每行具有字段类型和字段名称，可以使用的字段类型有：

• int8, int16, int32, int64 (或者无符号类型: uint*)
• float32, float64
• string
• time, duration
• other msg files
• variable-length array[] and fixed-length array[C]

ROS中还有一种特殊类型：Header，标头包含时间戳和ROS中常用的坐标帧信息。会经常看到msg文件的第一行具有Header标头。

4.1、定义msg文件

功能包下新建 msg 目录，添加文件 Person.msg

string name
uint16 age
float64 height

4.2、编辑配置文件

package.xml中添加编译依赖与执行依赖

  <build_depend>message_generationbuild_depend>
  <exec_depend>message_runtimeexec_depend>
  

CMakeLists.txt编辑 msg 相关配置

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)
# 需要加入 message_generation,必须有 std_msgs

## 配置 msg 源文件
add_message_files(
  FILES
  Person.msg
)

# 生成消息时依赖于 std_msgs
generate_messages(
  DEPENDENCIES
  std_msgs
)

#执行时依赖
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES demo02_talker_listener
  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

编辑完这些配置文件我们可以编译一下，我们发现在devel/include/Talker_Listener_test目录下生成了一个cpp的头文件Persion.h。这是一个编译的中间文件，在实际的调用中都是调用中间文件，我们自定义的消息类型每个发出的消息包或者接受的消息包都是c++的一个个对象。

4.3、C++实现发布者

这里有一个小技巧，我们需要包含上面我们定义的cpp头文件，但是在vscode中没有配置相关路径的话，会出现头文件标红错误，虽然不影响编译，但是看着很难受，所以可以修改一下。将前面生成的 head 文件路径配置进 c_cpp_properties.json 的 includepath属性:

{
    "configurations": [
        {
            "browse": {
                "databaseFilename": "",
                "limitSymbolsToIncludedHeaders": true
            },
            "includePath": [
                "/opt/ros/noetic/include/**",
                "/usr/include/**",
                "/xxx/yyy工作空间/devel/include/**" //配置 head 文件的路径 
            ],
            "name": "ROS",
            "intelliSenseMode": "gcc-x64",
            "compilerPath": "/usr/bin/gcc",
            "cStandard": "c11",
            "cppStandard": "c++17"
        }
    ],
    "version": 4
}

我们创建一个cpp文件 Test_Talker_Msg.cpp，并写入下面的代码

#include "ros/ros.h"
#include "Talker_Listener_test/Persion.h"

int main(int argc, char *argv[]){
    setlocale(LC_ALL,"");

    //1.初始化 ROS 节点
    ros::init(argc,argv,"talker_person");

    //2.创建 ROS 句柄
    ros::NodeHandle nh;

    //3.创建发布者对象
    ros::Publisher pub = nh.advertise<Talker_Listener_test::Persion>("chatter_person",100);

    //4.组织被发布的消息，编写发布逻辑并发布消息
    Talker_Listener_test::Persion p;
    p.name = "sunwukong";
    p.age = 2000;
    p.height = 1.45;

    ros::Rate r(1);
    while (ros::ok()) {
        pub.publish(p);
        p.age += 1;
        r.sleep();
        ros::spinOnce();
    }

    return 0;
}

4.3、C++实现订阅者

我们创建一个cpp文件 Test_Listener_Msg.cpp，并写入下面的代码

#include "ros/ros.h"
#include "Talker_Listener_test/Persion.h"

void doPerson(const Talker_Listener_test::Persion::ConstPtr& persion_p){
    ROS_INFO("订阅的人信息:%s, %d, %.2f", persion_p->name.c_str(), persion_p->age, persion_p->height);
}

int main(int argc, char *argv[]) {   
    setlocale(LC_ALL,"");

    //1.初始化 ROS 节点
    ros::init(argc,argv,"listener_person");
    //2.创建 ROS 句柄
    ros::NodeHandle nh;
    //3.创建订阅对象
    ros::Subscriber sub = nh.subscribe<Talker_Listener_test::Persion>("chatter_person",10,doPerson);

    //4.回调函数中处理 person

    //5.ros::spin();
    ros::spin();    
    return 0;
}

配置Cmake文件

add_executable(Test_Talker_Msg src/Test_Talker_Msg.cpp)
add_executable(Test_Listener_Msg src/Test_Listener_Msg.cpp)

target_link_libraries(Test_Talker_Msg
  ${catkin_LIBRARIES}
)

target_link_libraries(Test_Listener_Msg
  ${catkin_LIBRARIES}
)

4.5、运行

source ./devel/setup.sh
# 启动发布者节点
rosrun Talker_Listener_test Test_Talker_Msg
# 启动订阅者节点
rosrun Talker_Listener_test Test_Listener_Msg

五、控制小乌龟实例

5.1、发布消息

我们知道小乌龟案例中，有两个节点，一个是显示节点，另一个是键盘控制节点，我们先启动这两个节点。

rosrun turtlesim turtlesim_node
rosrun turtlesim turtle_teleop_key

可以通过计算图查看话题

rqt_graph

通过rostopic列出话题

rostopic list

通过rostopic获取消息类型

rostopic type /turtle1/cmd_vel

通过rosmsg获取消息格式

rosmsg info geometry_msgs/Twist

我们可以看到我们获取到的消息格式是下面的样子

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

linear(线速度) 下的xyz分别对应在x、y和z方向上的速度(单位是 m/s)；

angular(角速度)下的xyz分别对应x轴上的翻滚、y轴上俯仰和z轴上偏航的速度(单位是rad/s)。

所以我们实现一个发布节点是很简单的。

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[]) {
    ros::init(argc,argv,"Test_turtle_Talker");

    ros::NodeHandle nh;

    ros::Publisher p = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);

    geometry_msgs::Twist t;

    t.angular.x = 0;
    t.angular.y = 0;
    t.angular.z = 1.0;

    t.linear.x = 1.0;
    t.linear.y = 0;
    t.linear.z = 0;

    ros::Rate r(10);

    while (ros::ok()){

        p.publish(t);

        r.sleep();

        ros::spinOnce();
    }
    return 0;
}

这里我们就不在解释怎么配置配置文件了，我们直接运行两个节点，我们发现小乌龟可以做圆周运动了

# 运行两个节点
rosrun Talker_Listener_test Test_turtle_Talker
rosrun turtlesim turtlesim_node  

5.2、订阅消息

已知turtlesim中的乌龟显示节点，会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向)，要求控制乌龟运动，并时时打印当前乌龟的位姿。

获取话题:/turtle1/pose

rostopic list

获取消息类型:turtlesim/Pose

rostopic type  /turtle1/pose

获取消息格式:

rosmsg info turtlesim/Pose

响应结果:

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

所以我们订阅消息的节点也很好编写

#include "ros/ros.h"
#include "turtlesim/Pose.h"

void doPose(const turtlesim::Pose::ConstPtr& p){
    ROS_INFO("乌龟位姿信息:x=%.2f,y=%.2f,theta=%.2f,lv=%.2f,av=%.2f",
        p->x,p->y,p->theta,p->linear_velocity,p->angular_velocity
    );
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"Test_turtle_Listener");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    // 5.回调函数处理订阅的数据
    // 6.spin
    ros::spin();
    return 0;
}