【ROS】—— ROS通信机制——话题通信（二）

前言

本系列将依托赵虚左老师的ROS课程，写下自己的一些心得与笔记。
课程链接:https://www.bilibili.com/video/BV1Ci4y1L7ZZ
讲义链接:http://www.autolabor.com.cn/book/ROSTutorials/index.html
文章可能存在疏漏的地方，恳请大家指出。

    机器人是一种高度复杂的系统性实现，在机器人上可能集成各种传感器(雷达、摄像头、GPS…)以及运动控制实现，为了解耦合，在ROS中每一个功能点都是一个单独的进程，每一个进程都是独立运行的。更确切的讲，ROS是进程（也称为Nodes）的分布式框架。
    ROS 中的基本通信机制主要有如下三种实现策略:

• 话题通信(发布订阅模式)
• 服务通信(请求响应模式)
• 参数服务器(参数共享模式)

1. 话题通信

    话题通信是ROS中使用频率最高的一种通信模式，话题通信是基于发布订阅模式的，也即:一个节点发布消息，另一个节点订阅该消息.

1.1 话题通讯理论模型

    话题通信实现模型是比较复杂的，该模型如下图所示,该模型中涉及到三个角色:

• ROS Master (管理者)
• Talker (发布者)
• Listener (订阅者)

流程
    master 可以根据话题建立发布者与订阅者之间的连接。

    ROS Master 负责保管 Talker 和 Listener 注册的信息，并匹配话题相同的 Talker 与 Listener，帮助 Talker 与 Listener 建立连接，连接建立后，Talker 可以发布消息，且发布的消息会被 Listener 订阅。
0.Talker 注册
Talker 启动，通过1234端口使用 RPC 向 ROS Master 注册发布者的信息，包含所发布息的话题名； ROS Master 会将节点的注册信息加人注册列表中。

1.Listener 注册
Listener 启动，同样通过 RPC 向 ROS Master 注册订阅者的信息，包含需要订阅的话题名。

2. ROS Master 进行信息匹配
Master 根据 Listener 的订阅信息从注册列表中进行查找，如果没有找到匹配的发布者，则等待发布者的加人；如果找到匹配的发布者信息，则通过 RPC 向 Listener 发送 Talker 的 RPC 地址信息。

3. Listener 发送连接请求
Listener 接收到 Master 发回的 Talker 地址信息，尝试通过 RPC 向 Talker 发送连接请水，传输订阅的话题名、消息类型以及通信协议（ TCP / UDP )。

4.Talker 确认连接请求
Talker 接收到 Listener 发送的连接请求后，继续通过 RPC 向 Listener 确认连接信息，其中包含着自身的地址信息。

5.Listener 尝试与 Talker 建立网络连接
Listener 接收到确认信息后，使用 TCP 栄试与 Talker 建立网络连接。

6.Talker 向 Listener 发布数据
成功建立连接后， Talker 开始向 Listener 发送话题消息数据。

**注意1:**上述实现流程中，前五步使用的 RPC协议，最后两步使用的是 TCP 协议
注意2: Talker 与 Listener 的启动无先后顺序要求
注意3: Talker 与 Listener 都可以有多个
注意4: Talker 与 Listener 连接建立后，不再需要 ROS Master。也即，即便关闭ROS Master，Talker 与 Listern 照常通信。

1.2 话题通信基本操作（C++）

在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:

• 发布方
• 接收方
• 数据(此处为普通文本)

流程:

• 编写发布方实现；
• 编写订阅方实现；
• 编辑配置文件；
• 编译并执行。

1.2.1 简单发布框架的实现

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

/*

    实现流程:
        1.包含头文件 
        ROS中文本类型----> std_msgs/String.h
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 ROS 句柄
        4.实例化 发布者 对象
        5.组织被发布的数据，并编写逻辑发布数据

*/
int main(int argc, char  *argv[])
{
    //初始化 ROS 节点
    ros::init(argc,argv,"publisher");
    //实例化 ROS 句柄
    ros::NodeHandle nh;
    //实例化 发布者 对象
    ros::Publisher pub = nh.advertise<std_msgs::String>("message",20);
    //组织被发布的数据，并编写逻辑发布数据
    //先创建被发布的消息
    std_msgs::String msg;
    //编写循环，循环中发布数据
    while(ros::ok())
    {
        msg.data = "hello";
        pub.publish(msg);

    }
    return 0;

}

发布成功后，可以使用rostopic echo 【话题名称】查看消息的发布

rostopic echo 【话题名称】

1.2.2 发布逻辑的实现

上述程序只是简单的框架，不能满足一些需求，如发布频率、字符串拼接、日志等。

/*
    需求: 实现基本的话题通信，一方发布数据，一方接收数据，
         实现的关键点:
         1.发送方
         2.接收方
         3.数据(此处为普通文本)

         PS: 二者需要设置相同的话题


    消息发布方:
        循环发布信息:HelloWorld 后缀数字编号

    实现流程:
        1.包含头文件 
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 ROS 句柄
        4.实例化 发布者 对象
        5.组织被发布的数据，并编写逻辑发布数据

*/
// 1.包含头文件 
#include "ros/ros.h"
#include "std_msgs/String.h" //普通文本类型的消息
#include 

int main(int argc, char  *argv[])
{   
    //设置编码
    setlocale(LC_ALL,"");

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

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

    //5.组织被发布的数据，并编写逻辑发布数据
    //数据(动态组织)
    std_msgs::String msg;
    // msg.data = "你好啊！！！";
    std::string msg_front = "Hello 你好！"; //消息前缀
    int count = 0; //消息计数器

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

    //节点不死
    while (ros::ok())
    {
        //使用 stringstream 拼接字符串与编号
        std::stringstream ss;
        ss << msg_front << count;
        msg.data = ss.str();
        //发布消息
        pub.publish(msg);
        //加入调试，打印发送的消息
        ROS_INFO("发送的消息:%s",msg.data.c_str());

        //根据前面制定的发送贫频率自动休眠 休眠时间 = 1/频率；
        r.sleep();
        count++;//循环结束前，让 count 自增
        //暂无应用
        ros::spinOnce();
    }


    return 0;
}

1.2.3 订阅方的实现

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

    实现流程:
        1.包含头文件 
        ROS中文本类型----> std_msgs/String.h
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 ROS 句柄
        4.实例化 订阅对象
        5.处理订阅到的数据

*/

void doMSG(const std_msgs::String::ConstPtr &msg)
{   //通过msg获取并操作订阅到的数据
    ROS_INFO("receive_messages:%s",msg->data.c_str());

}

int main(int argc, char  *argv[])
{
        //设置编码
    setlocale(LC_ALL,"");
    
       //2.初始化 ROS 节点:命名(唯一)
       ros::init(argc,argv,"listener");
        //3.实例化 ROS 句柄
        ros::NodeHandle nh;
        //4.实例化 订阅对象
        ros::Subscriber sub = nh.subscribe("chatter",10,doMSG);
       // 5.处理订阅到的数据

        ros::spin();
    return 0;
}

• ros::Subscriber sub = nh.subscribe("chatter",10,doMsg);中的不一定要添加，可以通过回调函数进行自动推到。话题"chatter"、队列长度10需要同发布者一致。
• void doMSG(const std_msgs::String::ConstPtr &msg)
  回调函数传入的是订阅的消息的常量指针的引用。
• ros::spin();回去处理回调函数

补充1:

vscode 中的 main 函数 声明 int main(int argc, char const *argv[]){}，默认生成 argv 被 const 修饰，需要去除该修饰符

补充2:

ros/ros.h No such file or directory …
检查 CMakeList.txt find_package 出现重复,删除内容少的即可

补充3:

订阅时，第一条数据丢失
原因: 发送第一条数据时， publisher 还未在 roscore 注册完毕
解决: 注册后，加入休眠 ros::Duration(3.0).sleep(); 延迟第一条数据的发送

1.3 话题通信基本操作（python）

流程:

1. 编写发布方实现；
2. 编写订阅方实现；
3. 为python文件添加可执行权限；
4. 编辑配置文件；
5. 编译并执行。

1.3.1 发布的实现

#! /usr/bin/env python
#-- coding:UTF-8 --
"""
    需求: 实现基本的话题通信，一方发布数据，一方接收数据，
         实现的关键点:
         1.发送方
         2.接收方
         3.数据(此处为普通文本)

         PS: 二者需要设置相同的话题


    消息发布方:
        循环发布信息:HelloWorld 后缀数字编号

    实现流程:
        1.导包 
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 发布者 对象
        4.组织被发布的数据，并编写逻辑发布数据


"""
#1.导包 
import rospy
from std_msgs.msg import String

if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("talker_p")
    #3.实例化 发布者 对象
    pub = rospy.Publisher("chatter",String,queue_size=10)
    #4.组织被发布的数据，并编写逻辑发布数据
    msg = String()  #创建 msg 对象
    msg_front = "hello 你好"
    count = 0  #计数器 
    # 设置循环频率
    rate = rospy.Rate(1)
    # rospy.sleep(3)
    while not rospy.is_shutdown():

        #拼接字符串
        msg.data = msg_front + str(count)
        #发布数据
        pub.publish(msg)
        rate.sleep()
        rospy.loginfo("写出的数据:%s",msg.data)
        count += 1

1.3.2 订阅的实现

#! /usr/bin/env python
#-- coding:UTF-8 --
"""
    需求: 实现基本的话题通信，一方发布数据，一方接收数据，
         实现的关键点:
         1.发送方
         2.接收方
         3.数据(此处为普通文本)


    消息订阅方:
        订阅话题并打印接收到的消息

    实现流程:
        1.导包 
        2.初始化 ROS 节点:命名(唯一)
        3.实例化 订阅者 对象
        4.处理订阅的消息(回调函数)
        5.设置循环调用回调函数



"""
#1.导包 
import rospy
from std_msgs.msg import String

def doMsg(msg):
    rospy.loginfo("I heard:%s",msg.data)

if __name__ == "__main__":
    #2.初始化 ROS 节点:命名(唯一)
    rospy.init_node("listener_p")
    #3.实例化 订阅者 对象
    sub = rospy.Subscriber("chatter",String,doMsg,queue_size=10)
    #4.处理订阅的消息(回调函数)
    #5.设置循环调用回调函数
    rospy.spin()

• 同样的rospy.sleep(3)延迟第一条数据的发送
• 注意官方中的

  17     try:
  18         talker()
  19     except rospy.ROSInterruptException:
  20         pass

   1 #!/usr/bin/env python
   2 # license removed for brevity
   3 import rospy
   4 from std_msgs.msg import String
   5 
   6 def talker():
   7     pub = rospy.Publisher('chatter', String, queue_size=10)
   8     rospy.init_node('talker', anonymous=True)
   9     rate = rospy.Rate(10) # 10hz
  10     while not rospy.is_shutdown():
  11         hello_str = "hello world %s" % rospy.get_time()
  12         rospy.loginfo(hello_str)
  13         pub.publish(hello_str)
  14         rate.sleep()
  15 
  16 if __name__ == '__main__':
  17     try:
  18         talker()
  19     except rospy.ROSInterruptException:
  20         pass

• ROS 的解耦合特性——发布与订阅可以由不同的编程语言实现

1.4 话题通信自定义msg

在 ROS 通信协议中，数据载体是一个较为重要组成部分，ROS 中通过 std_msgs
封装了一些原生的数据类型,比如:String、Int32、Int64、Char、Bool、Empty… 但是，这些数据一般只包含一个
data 字段，结构的单一意味着功能上的局限性，当传输一些复杂的数据，比如: 激光雷达的信息… std_msgs
由于描述性较差而显得力不从心，这种场景下可以使用自定义的消息类型。

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标头。

流程:

• 按照固定格式创建 msg 文件
• 编辑配置文件
• 编译生成可以被 Python 或 C++ 调用的中间文件

1.4.1 定义msg文件

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

string name
uint16 age
float64 height

1.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
)

简单理解 find_package为编译时的依赖，catkin_package为执行时的依赖。前者可能会出现编译不通过的情况，后者可能会出现编译通过，但程序运行失败的情况。

1.4.3 编译

编译后的中间文件查看:
C++ 需要调用的中间文件(…/工作空间/devel/include/包名/xxx.h)

ython 需要调用的中间文件(…/工作空间/devel/lib/python3/dist-packages/包名/msg)

后续调用相关 msg 时，是从这些中间文件调用的

1.5 话题通信自定义msg调用(C++)

流程:

1. 编写发布方实现；
2. 编写订阅方实现；
3. 编辑配置文件；
4. 编译并执行。

1.5.1 vscode 配置

为了方便代码提示以及避免误抛异常，需要先配置 vscode，将前面生成的 head 文件路径配置进 c_cpp_properties.json 的 includepath属性:
ps: 可以直接把 c_cpp_properties.json文件删除，再重新启动vscode，自动添加相关路径。文件路径/home/yuan/catkin_ws/.vscode/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
}

1.5.2 发布方的实现

#include "ros/ros.h"
#include "publisher/person.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<publisher::person>("chatter_person",10);

    //4.组织被发布的消息，编写发布逻辑并发布消息
    //创建被发布的数据
    publisher::person p;
    p.name = "zhangsan";
    p.age = 20;
    p.height = 1.83;
    int year =2022;
    //发布频率
    ros::Rate r(1);
    while (ros::ok())
    {
        pub.publish(p);
        p.age += 1;
        
        ROS_INFO("我叫:%s,%d年%d岁,高%.2f米", p.name.c_str(), year,p.age, p.height);
        //休眠
        year++;
        r.sleep();
        //建议
        ros::spinOnce();
    }

    return 0;
}

rostopic echo chatter_person查看消息的发布

1.5.3 订阅方的实现

#include "ros/ros.h"
#include "publisher/person.h"//配置的头文件

void doPerson(const publisher::person::ConstPtr& p){
    ROS_INFO("订阅的人信息:%s, %d, %.2f", p->name.c_str(), p->age, 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("chatter_person",10,doPerson);

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

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

1.5.4 配置 CMakeLists.txt

add_dependencies(person_listener ${PROJECT_NAME}_generate_messages_cpp)

add_executable(person_talker src/person_talker.cpp)
add_executable(person_listener src/person_listener.cpp)

add_dependencies(person_talker ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(person_listener ${PROJECT_NAME}_generate_messages_cpp)

target_link_libraries(person_talker
  ${catkin_LIBRARIES}
)
target_link_libraries(person_listener
  ${catkin_LIBRARIES}
)

1.6 话题通信自定义msg调用(Python)

流程:

• 编写发布方实现；
• 编写订阅方实现；
• 为python文件添加可执行权限；
• 编辑配置文件；
• 编译并执行。

1.6.1 vscode配置

为了方便代码提示以及误抛异常，需要先配置 vscode，将前面生成的 python 文件路径配置进 settings.json

{
    "python.autoComplete.extraPaths": [
        "/opt/ros/noetic/lib/python3/dist-packages",
        "/xxx/yyy工作空间/devel/lib/python3/dist-packages"
    ]
}

1.6.2 发布方实现

#! /usr/bin/env python
#-- coding:UTF-8 --

import rospy
from publisher.msg import person

if __name__ == "__main__":
    #1.初始化 ROS 节点
    rospy.init_node("talker_person_p")
    #2.创建发布者对象
    pub = rospy.Publisher("chatter_person",person,queue_size=10)
    #3.组织消息
    p = person()
    p.name = "葫芦瓦"
    p.age = 18
    p.height = 0.75

    #4.编写消息发布逻辑
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        pub.publish(p)  #发布消息
        rate.sleep()  #休眠
        rospy.loginfo("姓名:%s, 年龄:%d, 身高:%.2f",p.name, p.age, p.height)

1.6.3 订阅方实现

#! /usr/bin/env python
#-- coding:UTF-8 --

import rospy
from publisher.msg import person

def doPerson(p):
    rospy.loginfo("接收到的人的信息:%s, %d, %.2f",p.name, p.age, p.height)


if __name__ == "__main__":
    #1.初始化节点
    rospy.init_node("listener_person_p")
    #2.创建订阅者对象
    sub = rospy.Subscriber("chatter_person",person,doPerson,queue_size=10)
    rospy.spin() #4.循环

python文件的权限设置以及配置与之前一致。