ROS：通信机制

一、话题通信简介

话题在ROS中使用最为频繁，其通信模型也较为复杂。在ROS中有两个节点：一个是发布者Talker，另一个是订阅着Listener。两个节点分别发布、订阅同一个话题，启动顺序没有强制要求，此处假设Talker首先启动，可分为如下七步分析建立通信的详细过程。

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确认连接信息，其中包含自身的TCP地址信息。
5、Listener尝试与Talker建立网络连接
Listener接收到确认信息后，使用TCP尝试与Talker建立网络连接。
6、Talker向Listener发布数据
成功建立连接后，Talker开始向Listener发送话题消息数据。
从上面的分析中可以发现，前五个步骤使用的通信协议都是RPC，最后发布数据的过程才使用到TCP。ROS Master在节点建立连接的过程中起到了重要作用，但是并不参与节点之间最终的数据传输。

二、话题通信实操（C++）

2.1分析

在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:
发布方
接收方
数据(此处为普通文本)

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

2.2发布方代码

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

/*
发布方实现：
1.包含头文件
ROS中文本类型--->td_msgs/String.h
2.初始化ros节点
3.创建节点句柄
4.创建发布者对象
5.编写发布逻辑并发布数据
*/



int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化ros节点
    ros::init(argc,argv,"erGouZi");
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建发布者对象
    ros::Publisher pub = nh.advertise<std_msgs::String>("fang",10);
    //5.编写发布逻辑并发布数据
    //要求以10HZ的频率发布数据，并且文本后添加编号
    //先创建被发布的消息
    std_msgs::String msg;
    //发布频率
    ros::Rate rate(10);
    //设置编号
    int count =0;
    //编写循环，循环中发布数据
    while (ros::ok())
    {
        count++;
        //msg.data = "hello";
        //实现字符串拼接数字
        std::stringstream ss;
        ss << "hello ---> " <<count;
        msg.data = ss.str();
        pub.publish(msg);

        //添加日志
        ROS_INFO("发布的数据是：%s",ss.str().c_str());

        rate.sleep();根据前面制定的发送贫频率自动休眠 休眠时间 = 1/频率；

        ros::spinOnce();//官方建议添加回调函数
    }
    
    return 0;
}

2.3订阅方代码

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


/*
订阅方实现：
1.包含头文件
ROS中文本类型--->td_msgs/String.h
2.初始化ros节点
3.创建节点句柄
4.创建订阅者对象
5.处理订阅数据
6.sain()函数
*/

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

}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化ros节点
    ros::init(argc,argv,"cuihua");
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe("fang",10,doMsg);
    //5.处理订阅数据

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




    return 0;
}

2.4配置CMakeLists.txt

add_executable(demo01_pub src/demo01_pub.cpp)
add_executable(demo02_sub src/demo02_sub.cpp)

target_link_libraries(demo01_pub
  ${catkin_LIBRARIES}
)
target_link_libraries(demo02_sub
  ${catkin_LIBRARIES}
)

2.5执行

1.启动 roscore;

2.启动发布节点;

3.启动订阅节点。

2.6注意

补充0:

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

补充1:

ros/ros.h No such file or directory …

检查 CMakeList.txt find_package 出现重复,删除内容少的即可

参考资料:https://answers.ros.org/question/237494/fatal-error-rosrosh-no-such-file-or-directory/

补充2:

find_package 不添加一些包，也可以运行啊， ros.wiki 答案如下

You may notice that sometimes your project builds fine even if you did not call find_package with all dependencies. This is because catkin combines all your projects into one, so if an earlier project calls find_package, yours is configured with the same values. But forgetting the call means your project can easily break when built in isolation.

补充3:

订阅时，第一条数据丢失

原因: 发送第一条数据时， publisher 还未在 roscore 注册完毕

解决: 注册后，加入休眠 ros::Duration(3.0).sleep(); 延迟第一条数据的发送

三、话题通信实操（python）

3.1分析

分析:

在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:
发布方
接收方
数据(此处为普通文本)

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

3.2发布方代码

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

/*
发布方实现：
1.包含头文件
ROS中文本类型--->td_msgs/String.h
2.初始化ros节点
3.创建节点句柄
4.创建发布者对象
5.编写发布逻辑并发布数据
*/



int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    //2.初始化ros节点
    ros::init(argc,argv,"erGouZi");
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建发布者对象
    ros::Publisher pub = nh.advertise<std_msgs::String>("fang",10);
    //5.编写发布逻辑并发布数据
    //要求以10HZ的频率发布数据，并且文本后添加编号
    //先创建被发布的消息
    std_msgs::String msg;
    //发布频率
    ros::Rate rate(10);
    //设置编号
    int count =0;
    //编写循环，循环中发布数据
    while (ros::ok())
    {
        count++;
        //msg.data = "hello";
        //实现字符串拼接数字
        std::stringstream ss;
        ss << "hello ---> " <<count;
        msg.data = ss.str();
        pub.publish(msg);

        //添加日志
        ROS_INFO("发布的数据是：%s",ss.str().c_str());

        rate.sleep();//根据前面制定的发送贫频率自动休眠 休眠时间 = 1/频率；

        ros::spinOnce();//官方建议添加回调函数
    }
    
    return 0;
}

3.3订阅方代码

#! /usr/bin/env python

import rospy
from std_msgs.msg import String #发布消息的类型

"""
使用py 实现消息订阅
1.导包
2.初始化ROS节点
3.创建订阅者对象
4.回调函数处理数据
spin

"""
def doMsg(msg):
    rospy.loginfo("我订阅的数据：%s",msg.data)


if __name__ == "__main__":
    # 2.初始化ROS节点
    rospy.init_node("huahua")#传入节点名称
    # 3.创建订阅者对象
    sub = rospy.Subscriber("che",String,doMsg,queue_size=10)
    # 4.回调函数处理数据
    #5.spin
    rospy.spin()

3.4添加可执行权限

终端下进入 scripts 执行:chmod +x *.py

3.5配置 CMakeLists.txt

catkin_install_python(PROGRAMS
  scripts/demo01_pub_p.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
catkin_install_python(PROGRAMS
  scripts/demo02_sub_p.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

3.6执行

1.启动 roscore;

2.启动发布节点;

3.启动订阅节点。

注：可以使用 rqt_graph 查看节点关系。

四、话题通信自定义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标头。

4.1自定义msg实现

4.1.1需求

创建自定义消息，该消息包含人的信息:姓名、身高、年龄等。

4.1.2流程

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

4.1.3定义msg文件

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

string name
uint16 age
float64 height

4.1.4编辑配置文件

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

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>
  <!-- 
  exce_depend 以前对应的是 run_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
)

4.1.5编译

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

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

4.2将自定义msg信息发布和订阅（C++）

4.2.1需求

编写发布订阅实现，要求发布方以10HZ(每秒10次)的频率发布自定义消息，订阅方订阅自定义消息并将消息内容打印输出。

4.2.2流程

分析:

在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:
发布方
接收方
数据(此处为自定义消息)

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

4.2.3vscode 配置

为了方便代码提示以及避免误抛异常，需要先配置 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
}

4.2.4发布方

#include "ros/ros.h"
#include "plumbing_pub_sub/Person.h"

/*
发布方：发布人的消息
1.包含头文件；
2.初始化ROS节点；
3.创建节点句柄
4.创建发布者对象
5.编写发布逻辑，发布数据

*/

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ROS_INFO("这是消息的发布方");
// 2.初始化ROS节点；
    ros::init(argc,argv,"banZhuRrn");
// 3.创建节点句柄
    ros::NodeHandle nh;
// 4.创建发布者对象
    ros::Publisher pub = nh.advertise<plumbing_pub_sub::Person>("LiaoTian",10);
// 5.编写发布逻辑，发布数据
//5.1创建发布数据
    plumbing_pub_sub::Person person;
    person.name = "张三";
    person.age = 0;
    person.height = 1.73;

//5.2发布频率
    ros::Rate rate(1);

//5.3循环发布数据
    while(ros::ok())
    {
        person.age +=1;
        //核心：发布数据
        pub.publish(person);
        ROS_INFO("发布的消息：%s,%d,%.2f",person.name.c_str(),person.age,person.height);
        //休眠
        rate.sleep();
        //调用回头函数
        ros::spinOnce();
    
    }

    return 0;
}

4.2.5订阅方

#include "ros/ros.h"
#include "plumbing_pub_sub/Person.h"


/*
订阅方实现：
1.包含头文件
2.初始化ros节点
3.创建节点句柄
4.创建订阅者对象
5.处理订阅数据
6.spin()函数
*/

void doPerson(const plumbing_pub_sub::Person::ConstPtr & person)
{
    //通过msg获取并操作订阅到的数据
    ROS_INFO("订阅的的数据：%s,%d,%.2f",person->name.c_str(),person->age,person->height);

}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ROS_INFO("这是消息的订阅方");
    //2.初始化ros节点
    ros::init(argc,argv,"jiaZhang");
    //3.创建节点句柄
    ros::NodeHandle nh;
    //4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe("LiaoTian",10,doPerson);
    //5.处理订阅数据

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




    return 0;
}

4.2.6配置 CMakeLists.txt

需要添加 add_dependencies 用以设置所依赖的消息相关的中间文件。

add_executable(demo03_pub_person src/demo03_pub_person.cpp)
add_executable(demo04_sub_person src/demo04_sub_person.cpp)

add_dependencies(demo03_pub_person ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(demo04_sub_person ${PROJECT_NAME}_generate_messages_cpp)

target_link_libraries(demo03_pub_person
  ${catkin_LIBRARIES}
)
target_link_libraries(demo04_sub_person
  ${catkin_LIBRARIES}
)

4.2.7执行

1.启动 roscore;

2.启动发布节点;

3.启动订阅节点。

4.3将自定义msg信息发布和订阅（python）

4.3.1需求

编写发布订阅实现，要求发布方以1HZ(每秒1次)的频率发布自定义消息，订阅方订阅自定义消息并将消息内容打印输出。

4.3.2流程

分析:
在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个:
发布方
接收方
数据(此处为自定义消息)

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

4.3.3vscode配置

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

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

4.3.4发布方

#! /usr/bin/env python

import rospy
from plumbing_pub_sub.msg import Person #发布消息的类型

"""
使用py 实现人的消息发布
1.导包
2.初始化ROS节点
3.创建发布者对象
4.编写发布逻辑并发布数据

"""

if __name__ == "__main__":

#2.初始化ROS节点
    rospy.init_node("dama")#传入节点名称
# 3.创建发布者对象
    pub = rospy.Publisher("jiaoshetou",Person,queue_size=10)
# 4.编写发布逻辑并发布数据
    #创建数据
    p =Person()
    p.name = "奥特曼"
    p.age = 8
    p.height = 1.85
    #指定发布频率
    rate = rospy.Rate(1)
    #设置计数器
    count = 0
    #使用循环发布数据
    while not rospy.is_shutdown():
    #发布数据
        pub.publish(p)
        rospy.loginfo("发布的数据是：%s,%d,%.2f",p.name,p.age,p.height)
        rate.sleep()

4.3.5订阅方

#! /usr/bin/env python

import rospy
from plumbing_pub_sub.msg import Person #发布消息的类型

"""
使用py 实现消息订阅
1.导包
2.初始化ROS节点
3.创建订阅者对象
4.回调函数处理数据
spin

"""
def doPerson(p):
    rospy.loginfo("订阅的数据是：%s,%d,%.2f",p.name,p.age,p.height)


if __name__ == "__main__":
    # 2.初始化ROS节点
    rospy.init_node("daye")#传入节点名称
    # 3.创建订阅者对象
    sub = rospy.Subscriber("jiaoshetou",Person,doPerson,queue_size=10)
    # 4.回调函数处理数据
    #5.spin
    rospy.spin()

4.3.6权限设置

终端下进入 scripts 执行:chmod +x *.py

4.3.7配置 CMakeLists.txt

catkin_install_python(PROGRAMS
  scripts/demo03_pub_person_p.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
catkin_install_python(PROGRAMS
  scripts/demo04_sub_person_p.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

4.3.8执行

启动 roscore;

启动发布节点;

启动订阅节点。