ROS学习2：ROS通信机制

【Autolabor初级教程】ROS机器人入门
机器人操作系统 ROS 快速入门教程

1. 引言

机器人上可能集成各种传感器(雷达、摄像头、GPS等)以及运动控制实现，为了解耦合，在 ROS 中每一个功能点都是一个单独的进程，每一个进程都是独立运行的。更确切的讲，ROS 是进程（也称为Nodes）的分布式框架。因为这些进程甚至还可分布于不同主机，不同主机协同工作，从而分散计算压力
不同的进程是如何通信实现数据交换的？ROS 中的基本通信机制主要有如下三种实现策略:
- 话题通信 (发布订阅模式)
- 服务通信 (请求响应模式)
- 参数服务器 (参数共享模式)

2. 话题通信

话题通信是基于发布订阅模式的：一个节点发布消息，另一个节点订阅该消息，例如：
- 机器人在执行导航功能，使用的传感器是激光雷达，机器人会采集激光雷达感知到的信息并计算，然后生成运动控制信息驱动机器人底盘运动
- 在上述场景中，就不止一次使用到了话题通信
  - 以激光雷达信息的采集处理为例，在 ROS 中有一个节点需要实时发布当前雷达采集到的数据，导航模块中也有节点会订阅并解析雷达数据
  - 再以运动消息的发布为例，导航模块会根据传感器采集的数据时时的计算出运动控制信息并发布给底盘，底盘也可以有一个节点订阅运动信息并最终转换成控制电机的脉冲信号
作用：用于不断更新的、少逻辑处理的数据传输场景

2.1 话题通信理论模型

ROS Master 负责保管 Talker 和 Listener 注册的信息，并匹配话题相同的 Talker 与 Listener，帮助 Talker 与 Listener 建立连接
连接建立后，Talker 可以发布消息，且发布的消息会被 Listener 订阅
整个流程由以下步骤实现

Talker注册
- Talker 启动后，会通过 RPC(Remote Procedure Call，远程过程调用) 在 ROS Master 中注册自身信息，其中包含所发布消息的话题名称。ROS Master 会将节点的注册信息加入到注册表中
Listener注册
- Listener 启动后，也会通过 RPC 在 ROS Master 中注册自身信息，包含需要订阅消息的话题名。ROS Master 会将节点的注册信息加入到注册表中
ROS Master实现信息匹配
- ROS Master 会根据注册表中的信息匹配 Talker 和 Listener，并通过 RPC 向 Listener 发送 Talker 的 RPC 地址信息
Listener向Talker发送请求
- Listener 根据接收到的 RPC 地址，通过 RPC 向 Talker 发送连接请求，传输订阅的话题名称、消息类型以及通信协议(TCP/UDP)
Talker确认请求
- Talker 接收到 Listener 的请求后，也是通过 RPC 向 Listener 确认连接信息，并发送自身的 TCP 地址信息
Listener与Talker建立连接
- Listener 根据步骤 4 返回的消息使用 TCP(Transmission Control Protocol，传输控制协议) 与 Talker 建立网络连接
Talker向Listener发送消息
- 连接建立后，Talker 开始向 Listener 发布消息

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

2.2 话题通信基本操作

需求
- 编写发布订阅实现，要求发布方以10HZ(每秒10次)的频率发布文本消息，订阅方订阅消息并将消息内容打印输出
分析
- 在模型实现中，ROS master 不需要实现，而连接的建立也已经被封装了，需要关注的关键点有三个
  - 发布方
  - 接收方
  - 数据(此处为普通文本)
流程
- 1. 编写发布方实现
- 1. 编写订阅方实现
- 1. 编辑配置文件
- 1. 编译并执行
1. 发布方实现
  首先新建工作空间 demo03_ws ,然后创建功能包 plumbing_pub_sub 并添加依赖

// demo01_pub.cpp
// 1.包含头文件 
#include "ros/ros.h"
#include "std_msgs/String.h" // 普通文本类型的消息
#include <sstream> // 字符串拼接

int main(int argc, char *argv[]) {
    // 设置编码，防止中文乱码
    setlocale(LC_ALL,"");

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

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

    // 5.组织被发布的数据，并编写逻辑发布数据
    // 数据(动态组织)
    std_msgs::String msg;
    int count = 0; //消息计数器

    // 逻辑(发布频率：一秒10次)
    ros::Rate rate(10);

    ros::Duration(3).sleep(); // 延迟第一条数据的发送

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

        // 根据前面制定的发送频率自动休眠 休眠时间 = 1/频率；
        rate.sleep();
        count++; //循环结束前，让 count 自增

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

1. 订阅方实现

// demo02_sub.cpp
// 1.包含头文件 
#include "ros/ros.h"
#include "std_msgs/String.h"

void doMsg(const std_msgs::String::ConstPtr& 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 句柄
    ros::NodeHandle nh;
    // 4.实例化 订阅者 对象
    ros::Subscriber sub = nh.subscribe<std_msgs::String>("fang", 10, doMsg); // doMsg 回调函数
    // 5.处理订阅的消息(回调函数)

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

    return 0;
}

1. 配置 CMakeLists.txt

// 下面这行默认的模板不能删除！
// add_executable(${PROJECT_NAME}_node src/plumbing_pub_sub_node.cpp)
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}
)

1. 执行

$ roscore
$ rosrun plumbing_pub_sub demo01_pub
$ rosrun plumbing_pub_sub demo02_sub

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

回调函数：自己写的，但调用时机不受自己控制，而是由外部传入的消息控制调用时机的函数

1. 查看计算图

$ rosrun rqt_graph rqt_graph

使用C++编写的发布方实现可以和使用Python编写的订阅方实现进行节点通信

2.3 话题通信自定义 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 文件
- 在功能包 plumbing_pub_sub 下新建 msg 文件夹，并在该文件夹下新建 Person.msg 文件
```
// Person.msg 内容
string name
uint16 age
float64 height
```

编辑配置文件

// package.xml中添加编译依赖与执行依赖
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

// CMakeLists.txt 编辑 msg 相关配置
// 需要加入 message_generation，必须有 std_msgs
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
)

// 配置 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
)

编译生成可以被 C++ 调用的中间文件
- C++ 需要调用的中间文件(…/工作空间/devel/include/包名/xxx.h)

2.4 话题通信自定义 msg 调用 C++ 实现

vscode 配置：为了方便代码提示以及避免误抛异常，需要先配置 vscode，将前面生成的 head 文件路径配置进 c_cpp_properties.json 的 includepath 属性

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

发布方实现

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

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

    ros::init(argc, argv, "banZhuRen");

    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<plumbing_pub_sub::Person>("liaoTian", 10);

    plumbing_pub_sub::Person person;
    person.name = "zhangsan";
    person.age = 1;
    person.height = 1.70;

    ros::Rate rate(1);

    while(ros::ok()) {
        person.age += 1;
        pub.publish(person);
        ROS_INFO("fabudexiaoxi:%s, %d, %.2f", person.name.c_str(), person.age, person.height);
        
        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}

订阅方实现

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

void doPerson(const plumbing_pub_sub::Person::ConstPtr& person) {
    ROS_INFO("xinxi: %s, %d, %.2f", person->name.c_str(), person->age, person->height);
}

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

    ros::init(argc, argv, "jiaZhang");

    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("liaoTian", 10, doPerson);

    ros::spin();

    return 0;
}

配置 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}
)

2.5 机器人运动控制开源工程

机器人运动控制量
矢量运动速度量纲为 m/s
旋转运动速度量纲为 rad/s，如 3.14 代表 1s 转半圈 180°
速度控制消息包：geometry_msgs/Twist Message
- linear & angular

wpr_simulation 开源工程（提供仿真环境）

$ mkdir -p catkin_ws/src
$ cd catkin_ws
$ catkin_make

$ cd src
$ git clone https://github.com/6-robot/wpr_simulation.git
$ cd wpr_simulation/scripts/
$ ./install_for_melodic.sh // 自动下载和安装编译需要的依赖项

$ cd ~/catkin_ws
$ catkin_make

// 运行 demo 案例
$ source ./devel/setup.bash
$ roslaunch wpr_simulation wpb_simple.launch
$ source ./devel/setup.bash // 重开窗口
$ rosrun wpr_simulation demo_vel_ctrl

复现上述机器人运动控制

实现思路

构建一个新的软件包，包名叫做 vel_pkg

$ catkin_create_pkg vel_pkg roscpp rospy geometry_msgs

在软件包中新建一个节点，节点名叫做 vel_node

// 在 vel_pkg 功能包的 src 下新建 vel_node.cpp 文件

在节点中，向 ROS 大管家 NodeHandle 申请发布话题 /cmd_vel 并拿到发布对象 vel_pub
构建一个 geometry_msgs/Twist 类型的消息包 vel_msg，用来承载要发送的速度值
开启一个 while 循环，不停的使用 vel_pub 对象发送速度消息包 vel msg

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

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

    ros::NodeHandle n;
    ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    geometry_msgs::Twist vel_msg;
    vel_msg.linear.x = 0.1;
    vel_msg.linear.y = 0.0;
    vel_msg.linear.z = 0.0;
    vel_msg.angular.x = 0;
    vel_msg.angular.y = 0;
    vel_msg.angular.z = 0;

    ros::Rate r(30);
    while (ros::ok()) {
        vel_pub.publish(vel_msg);
        r.sleep();
    }
    
    return 0;
}
// 修改 CMakeLists.txt 文件此处略
// 到 catkin_ws 目录下编译

运行

$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
$ roslaunch wpr_simulation wpb_simple.launch
$ rosrun vel_pkg vel_node

2.5.1 使用 Rviz 观测传感器数据

Gazebo 是模拟真实机器人发出传感器数据的工具（虚拟的环境），现实中不存在并被真实的实体机器人和真实环境所代替
Rviz 是接收传感器数据并进行显示的工具（真实的环境），只有需要观察某些数据实时变化的时候才用到
```
$ cd ~/catkin_ws/
$ roslaunch wpr_simulation wpb_simple.launch
$ roslaunch wpr_simulation wpb_rviz.launch
```

2.5.2 激光雷达消息包格式

sensor_msgs/LaserScan Message
- 1 / scan_time = LiDAR扫描频率

$ rostopic echo /scan --noarr

上图中超过雷达测距范围的点数值变成INF

2.5.3 获取激光雷达数据的 C++ 节点

获取激光雷达数据的 demo 案例

$ cd ~/catkin_ws/
$ roslaunch wpr_simulation wpb_simple.launch
$ rosrun wpr_simulation demo_lidar_data

复现上述demo案例

构建一个新的软件包，包名叫做 lidar_pkg

$ catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs

在软件包中新建一个节点，节点名叫做 lidar_node

// 在 lidar_pkg 功能包的 src 下新建 lidar_node.cpp 文件

在节点中，向 ROS 大管家 NodeHandle 申请订阅话题 /scan，并设置回调函数为 LidarCallback()
构建回调函数 LidarCallback()，用来接收和处理雷达数据
调用 ROS_INFO() 显示雷达检测到的前方障碍物距离

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>

void LidarCallback(const sensor_msgs::LaserScan msg) {
    float fMidDist = msg.ranges[180]; // 见下图，机器人正前方起始点
    ROS_INFO("qianfangceju ranges[180] = %f m", fMidDist);
}

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

    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);

    ros::spin();

    return 0;
}

2.5.4 激光雷达避障 C++ 节点

实现步骤
- 让大管家 NodeHandle 发布速度控制话题 /cmd_vel
- 构建速度控制消息包 vel_cmd
- 根据激光雷达的测距数值，实时调整机器人运动速度，从而避开障碍物

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>

ros::Publisher vel_pub; // 全局变量，保证回调函数也能使用
int nCount = 0;

void LidarCallback(const sensor_msgs::LaserScan msg) {
    float fMidDist = msg.ranges[180];
    ROS_INFO("qianfangceju ranges[180] = %f m", fMidDist);

    if(nCount > 0) {
        nCount--;
        return;
    }

    geometry_msgs::Twist vel_cmd; // 构建速度控制消息包 vel_cmd
    if(fMidDist < 1.5) {
        vel_cmd.angular.z = 0.3;
        nCount = 40; // 确保机器人旋转的角度足够避开障碍物
    } else {
        vel_cmd.linear.x = 0.1;
    }
    vel_pub.publish(vel_cmd);
}

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

    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10); // 发布速度控制话题 /cmd_vel

    ros::spin();

    return 0;
}

2.5.5 IMU 消息包

sensor_msgs/Imu Message

2.5.6 获取 IMU 数据的 C++ 节点

ROS 官方 IMU 话题种类标准

实现步骤

构建一个新的软件包叫做 imu_pkg
在软件包中新建一个节点叫做 imu_node
在节点中向 ROS 大管家 NodeHandle 申请订阅话题 /imu/data，并设置回调函数为 IMUCallback()
构建回调函数 IMUCallback()，用来接收和处理 IMU 数据
使用 TF 工具将四元数转换成欧拉角
调用 ROS_INFO() 显示转换后的欧拉角数值

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>

void IMUCallback(sensor_msgs::Imu msg) {
    // 先对四元数 orientation 的协方差矩阵第一个数值进行判断，如果小于0
    // 说明四元数的值不存在，具体查看sensor_msgs/Imu Message说明
    if(msg.orientation_covariance[0] < 0) {
        return;
    }
    // 使用 tf 工具将四元数转换成 tf 四元数对象
    tf::Quaternion quaternion ( 
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );
    double roll, pitch, yaw; // 用来装载转换后的欧拉角结果
    // 先将 quaternion 转换成一个 tf 的 3×3 矩阵对象，然后调用 getRPY 转换成欧拉角
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    // 将弧度值转换为角度值
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;

    ROS_INFO("roll = %.0f pitch = %.0f yaw = %.0f", roll, pitch, yaw);
}

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

    ros::NodeHandle n;
    ros::Subscriber imu_sub = n.subscribe("/imu/data", 10, IMUCallback);

    ros::spin();

    return 0;
}
// 修改 CMakeLists.txt 文件此处略
// 到 catkin_ws 目录下编译

2.5.7 IMU 航向锁定的 C++ 节点

实现步骤

让 ROS 大管家 NodeHandle 发布速度控制话题 /cmd_vel
设定一个目标朝向角，当姿态信息中的朝向角和目标朝向角不一致时，控制机器人转向目标朝向角

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"
#include "geometry_msgs/Twist.h"

// 速度消息发布对象（全局变量）
ros::Publisher vel_pub;

// IMU 回调函数
void IMUCallback(const sensor_msgs::Imu msg) {
    // 检测消息包中四元数数据是否存在
    if(msg.orientation_covariance[0] < 0)
        return;
    // 四元数转成欧拉角
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );
    double roll, pitch, yaw;
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    // 弧度换算成角度
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;
    ROS_INFO("滚转 = %.0f 俯仰 = %.0f 朝向 = %.0f", roll, pitch, yaw);
    // 速度消息包
    geometry_msgs::Twist vel_cmd;
    // 目标朝向角
    double target_yaw = 90;
    // 计算速度
    double diff_angle = target_yaw - yaw;
    vel_cmd.angular.z = diff_angle * 0.01;
    vel_cmd.linear.x = 0.1;
    vel_pub.publish(vel_cmd);
}

int main(int argc, char **argv) {
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "imu_node"); 

    ros::NodeHandle n;
    // 订阅 IMU 的数据话题
    ros::Subscriber sub = n.subscribe("imu/data", 100, IMUCallback);
    // 发布速度控制话题
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    
    ros::spin();
    return 0;
}

2.5.8 ROS 标准消息包 std_msgs

ROS 消息包分类
标准消息包 std_msgs 分类

2.5.9 ROS 几何包 geometry_msgs 和传感器包 sensor_msgs

几何消息包 geometry_msgs 分类

其中一些消息包带了 stamped 关键词，这些消息包都是多了一个 Header，也就是多了时间和坐标系ID，将空间量和时间量进行了绑定，多用于一些预测和滤波算法中
传感器消息包 sensor_msgs 分类

2.5.10 ROS 中的栅格地图格式

导航所使用的地图数据，就是 ROS 导航软件包里的 map_server 节点在话题 /map 中发布的消息数据，消息类型是 nav_msgs 消息包的 OccupancyGrid（占据栅格）地图，就是一种正方形小格子组成的地图，每个格子里填入一个数值，表示障碍物占据情况
nav_msgs 消息包的 OccupancyGrid 占据栅格地图形象解释
- 在地面上划分出一个个大小一样的正方形栅格，然后在栅格中填充不同的颜色表示占据情况，没有障碍物的栅格为白色，被占据的栅格为黑色（除去中间的墙面，剩下的黑白方格便是栅格地图）
- 栅格尺寸大小可变，越小则黑色的区域越精细，也就越接近障碍物的轮廓，但同时地图的数据量就越大，处理的时候计算量就越大，所以一般会给栅格设置一个适当的尺寸
- 栅格尺寸：指的是栅格的单边长度，体现了地图的精细程度，常被用来表示栅格地图的分辨率，ROS 中栅格地图的默认分辨率为 0.05m，也就是每个栅格的边长为5cm
- 数字表示栅格
  - 将栅格一行一行拼接起来变成一个数组，有了这个数组，再加上栅格的行、列数等信息，就能通过具体的数值将这个栅格地图描述清楚了
ROS 官网 nav_msgs/OccupancyGrid Message
nav_msgs/MapMetaData

2.5.11 C++ 节点发布地图

实现步骤

构建一个软件包 map_pkg，依赖项里加上 nav_msgs

$ cd ~/catkin_ws/src
$ catkin_create_pkg map_pkg roscpp rospy nav_msgs
$ cd ..
$ code .

在 map_pkg 里创建一个节点 map_pub_node

// 在 map_pkg 功能包的 src 下新建 map_pub_node.cpp 文件

在节点中发布话题 /map，消息类型为 nav_msgs::OccupancyGrid
构建一个 nav_msgs::OccupancyGrid 地图消息包，并对其进行赋值
将地图消息包发送到话题 /map
编译并运行节点

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "map_pub_node");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 10);

    ros::Rate r(1);

    while(ros::ok()) {
        nav_msgs::OccupancyGrid msg;

        msg.header.frame_id = "map";
        msg.header.stamp = ros::Time::now();

        msg.info.origin.position.x = 0;
        msg.info.origin.position.y = 0;
        msg.info.resolution = 1.0;
        msg.info.width = 4;
        msg.info.height = 2;

        msg.data.resize(4 * 2);
        msg.data[0] = 100;
        msg.data[1] = 100;
        msg.data[2] = 0;
        msg.data[3] = -1;

        pub.publish(msg);
        r.sleep();
    }
 
    return 0;
}
// 修改 CMakeLists.txt 文件此处略
// 到 catkin_ws 目录下编译

启动 Rviz，订阅话题 /map，显示地图

$ roscore
$ rosrun map_pkg map_pub_node
$ rviz

3. 服务通信

服务通信是基于请求响应模式的，是一种应答机制。也即：一个节点 A 向另一个节点 B 发送请求，B 接收处理请求并产生响应结果返回给 A。比如如下场景
- 机器人巡逻过程中，控制系统分析传感器数据发现可疑物体或人，此时需要拍摄照片并留存，这个过程中就使用到了服务通信
  - 一个节点需要向相机节点发送拍照请求，相机节点处理请求，并返回拍照结果
概念：以请求响应的方式实现不同节点之间数据交互的通信模式
作用：用于偶然的、对实时性有要求、有一定逻辑处理需求的数据传输场景

3.1 服务通信理论模型

ROS Master 负责保管 Server 和 Client 注册的信息，并匹配话题相同的 Server 与 Client ，帮助 Server 与 Client 建立连接
连接建立后，Client 发送请求信息，Server 返回响应信息
整个流程由以下步骤实现
0.Server注册
- Server 启动后，会通过RPC在 ROS Master 中注册自身信息，其中包含提供的服务的名称。ROS Master 会将节点的注册信息加入到注册表中
1.Client注册
- Client 启动后，也会通过RPC在 ROS Master 中注册自身信息，包含需要请求的服务的名称。ROS Master 会将节点的注册信息加入到注册表中
2.ROS Master实现信息匹配
- ROS Master 会根据注册表中的信息匹配Server和 Client，并通过 RPC 向 Client 发送 Server 的 TCP 地址信息
3.Client发送请求
- Client 根据步骤2 响应的信息，使用 TCP 与 Server 建立网络连接，并发送请求数据
4.Server发送响应
- Server 接收、解析请求的数据，并产生响应结果返回给 Client

客户端请求被处理时，需要保证服务器已经启动
服务端和客户端都可以存在多个

3.2 服务通信自定义srv

需求
- 服务通信中，客户端提交两个整数至服务端，服务端求和并响应结果到客户端，请创建服务器与客户端通信的数据载体

流程

srv 文件内的可用数据类型与 msg 文件一致，且定义 srv 实现流程与自定义 msg 实现流程类似

按照固定格式创建 srv 文件
- 首先在工作空间 demo03_ws 中创建功能包 plumbing_server_client 并添加依赖
- 在功能包下新建 srv 目录，并添加 AddInts.srv 文件
```
// AddInts.srv 文件，在 .srv 文件中请求和响应使用---分割
# 客户端请求时发送的两个数字
int32 num1
int32 num2
---
# 服务器响应发送的数据
int32 sum
```

编辑配置文件

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

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

CMakeLists.txt 编辑 srv 相关配置

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

add_service_files(
  FILES
  AddInts.srv
)

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
        )

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

3.3 服务通信自定义 srv 调用 C++ 实现

vscode配置（同2.4节）：需要像之前自定义 msg 实现一样配置 c_cpp_properies.json 文件，如果以前已经配置且没有变更工作空间，可以忽略，如果需要配置，配置方式与之前相同

服务端实现

// demo01_server.cpp
#include <ros/ros.h>
#include <plumbing_server_client/AddInts.h>

// bool 返回值由于标志是否处理成功
bool doNums(plumbing_server_client::AddInts::Request &request,
            plumbing_server_client::AddInts::Response &response) {
    int num1 = request.num1;
    int num2 = request.num2;

    ROS_INFO("服务器接收到的请求数据为:num1 = %d, num2 = %d",num1, num2);

    // 逻辑处理
    if(num1 < 0 || num2 < 0) {
        ROS_ERROR("提交的数据异常:数据不可以为负数");
        return false;
    }

    // 如果没有异常，那么相加并将结果赋值给 response
    response.sum = num1 + num2;            
    
    return true;
}

int main(int argc, char *argv[]) {
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "heiShui");
    ros::NodeHandle nh;
    ros::ServiceServer server = nh.advertiseService("addInts", doNums);

    ROS_INFO("服务已经启动....");
    ros::spin(); // 由于请求有多个，需要调用 ros::spin();

    return 0;
}

客服端实现

// demo02_client.cpp
#include <ros/ros.h>
#include <plumbing_server_client/AddInts.h>

int main(int argc, char *argv[]) {
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "heiShui");
    ros::NodeHandle nh;
    // 创建一个客户端对象
    ros::ServiceClient client = nh.serviceClient<plumbing_server_client::AddInts>("addInts");
    
    // 提交请求并处理响应
    plumbing_server_client::AddInts ai;
        // 组织请求
    ai.request.num1 = 100;
    ai.request.num2 = 200;
    
    // 如果客户端先启动则挂起（而不是报错），等待服务器启动后再正常请求
    // 这是一个阻塞式函数，只有服务启动成功后才会继续执行
    ros::service::waitForService("addInts");
        // 处理响应
    bool flag = client.call(ai);
    if(flag) {
        ROS_INFO("请求正常处理,响应结果 = %d", ai.response.sum);
    } else {
        ROS_ERROR("请求处理失败....");
        return 1;
    }

    return 0;
}

配置 CMakeLists.txt

add_executable(demo01_server src/demo01_server.cpp)
add_executable(demo02_client src/demo02_client.cpp)


add_dependencies(demo01_server ${PROJECT_NAME}_gencpp)
add_dependencies(demo02_client ${PROJECT_NAME}_gencpp)


target_link_libraries(demo01_server
  ${catkin_LIBRARIES}
)
target_link_libraries(demo02_client
  ${catkin_LIBRARIES}
)

执行
- 需要先启动服务端：rosrun plumbing_server_client demo01_server.cpp
- 再调用客户端：rosrun plumbing_server_client demo02_client.cpp

4. 参数服务器

参数服务器在 ROS 中主要用于实现不同节点之间的数据共享。参数服务器相当于是独立于所有节点的一个公共容器，可以将数据存储在该容器中，被不同的节点调用，当然不同的节点也可以往其中存储数据
关于参数服务器的典型应用场景如下
- 导航实现时，会进行路径规划，比如: 全局路径规划，设计一个从出发点到目标点的大致路径。本地路径规划，会根据当前路况生成实时的行进路径
上述场景中，全局路径规划和本地路径规划时，就会使用到参数服务器
- 路径规划时，需要参考小车的尺寸，我们可以将这些尺寸信息存储到参数服务器，全局路径规划节点与本地路径规划节点都可以从参数服务器中调用这些参数

概念：以共享的方式实现不同节点之间数据交互的通信模式
作用：存储一些多节点共享的数据，类似于全局变量
案例：实现参数增、删、改、查操作

4.1 参数服务器理论模型

ROS Master 作为一个公共容器保存参数，Talker 可以向容器中设置参数，Listener 可以获取参数
整个流程由以下步骤实现
1. Talker 设置参数
- Talker 通过 RPC 向参数服务器发送参数(包括参数名与参数值)，ROS Master 将参数保存到参数列表中
1. Listener 获取参数
- Listener 通过 RPC 向参数服务器发送参数查找请求，请求中包含要查找的参数名
1. ROS Master 向 Listener 发送参数值
- ROS Master 根据步骤2请求提供的参数名查找参数值，并将查询结果通过 RPC 发送给 Listener

参数可使用数据类型：32-bit integers、booleans、strings、doubles、iso8601 dates、lists、base64-encoded binary data、字典
注意：参数服务器不是为高性能而设计的，因此最好用于存储静态的非二进制的简单数据

4.2 参数操作

首先在工作空间 demo03_ws 中创建功能包 plumbing_param_server 并添加依赖
参数服务器新增(修改)参数

#include <ros/ros.h>

int main(int argc, char *argv[]) {
    ros::init(argc, argv, "set_param_c");
    ros::NodeHandle nh;
    
    // 参数新增
    // 方案1：ros::NodeHandle::setParam("键",值)
    nh.setParam("type", "xiaoHuang");
    nh.setParam("radius", 0.15);

    // 方案2：ros::param::setParam("键",值)
    ros::param::set("type_param", "xiaoBai");
    ros::param::set("radius_param", 0.15);

    // 参数修改（覆盖上面的值）
    nh.setParam("radius", 0.2);
    ros::param::set("radius_param", 0.25);

    return 0;
}

参数服务器获取参数

#include "ros/ros.h"
/*
ros::NodeHandle
        1. param(键,默认值) 
            存在，返回对应结果，否则返回默认值
        2. getParam(键,存储结果的变量)
            存在,返回 true,且将值赋值给参数2
            若果键不存在，那么返回值为 false，且不为参数2赋值
        3. getParamCached键,存储结果的变量)--提高变量获取效率
            存在,返回 true,且将值赋值给参数2
            若果键不存在，那么返回值为 false，且不为参数2赋值
        4. getParamNames(std::vector<std::string>)
            获取所有的键,并存储在参数 vector 中 
        5. hasParam(键)
            是否包含某个键，存在返回 true，否则返回 false
        6. searchParam(参数1，参数2)
            搜索键，参数1是被搜索的键，参数2存储搜索结果的变量
*/
int main(int argc, char *argv[]) {
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "get_param_c");
    ros::NodeHandle nh;

    // 1. param
    double radius = nh.param("radius", 0.5);
    ROS_INFO("radius = %.2f", radius);

    // 2. getParam
    double radius2 = 0.0;
    // bool result = nh.getParam("radius", radius2);
    // 3. getParamCached
    bool result = nh.getParamCached("radius", radius2);
    if (result) {
        ROS_INFO("get radius: %.2f", radius2);
    } else {
        ROS_INFO("bucunzai");
    }

    // 4. getParamNames
    std::vector<std::string> names;
    nh.getParamNames(names);
    for (auto &&name : names) {
        ROS_INFO("bianlideyuansu: %s", name.c_str());
    }

    // 5. hasParam
    bool flag1 = nh.hasParam("radius");
    bool flag2 = nh.hasParam("radiusxxx");
    ROS_INFO("radius cunzaima? %d", flag1);
    ROS_INFO("radiusxxx cunzaima? %d", flag2);

    // 6. searchParam
    std::string key;
    nh.searchParam("radius", key);
    ROS_INFO("sousuojieguo: %s", key.c_str());

    // ros::param -----------------------
    double radius_param = ros::param::param("radius", 100.5);
    ROS_INFO("radius_param = %.2f", radius_param);

    std::vector<std::string> names_param;
    ros::param::getParamNames(names_param);
    for (auto &&name : names_param) {
        ROS_INFO("key: %s", name.c_str());
    }

    return 0;
}

参数服务器删除参数

#include "ros/ros.h"
/* 
    参数服务器操作之删除_C++实现:
    1. ros::NodeHandle
        deleteParam("键")
        根据键删除参数，删除成功，返回 true，否则(参数不存在)，返回 false
    2. ros::param
        del("键")
        根据键删除参数，删除成功，返回 true，否则(参数不存在)，返回 false
*/
int main(int argc, char *argv[]) {
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"param_del_c");
    ros::NodeHandle nh;

    // 1. ros::NodeHandle
    bool flag1 = nh.deleteParam("radius");
    if (flag1) {
        ROS_INFO("delete!");
    } else {
        ROS_INFO("No delete!");
    }

    // 2. ros::param
    bool flag2 = ros::param::del("radius_param");
    if (flag2) {
        ROS_INFO("delete!");
    } else {
        ROS_INFO("No delete!");
    }

    return 0;
}

5. 通信机制常用命令

机器人系统中启动的节点少则几个，多则十几个、几十个，不同的节点名称各异，通信时使用话题、服务、消息、参数等等都各不相同，一个显而易见的问题是: 当需要自定义节点和其他某个已经存在的节点通信时，如何获取对方的话题、以及消息载体的格式呢？
在 ROS 同提供了一些实用的命令行工具，可以用于获取不同节点的各类信息，常用的命令如下
- rosnode : 操作节点
- rostopic : 操作话题
- rosservice : 操作服务
- rosmsg : 操作msg消息
- rossrv : 操作srv消息
- rosparam : 操作参数
作用：和之前介绍的文件系统操作命令比较，文件操作命令是静态的，操作的是磁盘上的文件，而上述命令是动态的，在ROS程序启动后，可以动态的获取运行中的节点或参数的相关信息；此外，这些命令还可在测试的时候使用

5.1 rosnode

rosnode 是用于获取节点信息的命令

rosnode ping    测试到节点的连接状态
rosnode list    列出活动节点
rosnode info    打印节点信息
rosnode machine    列出指定设备上节点
rosnode kill    杀死某个节点
rosnode cleanup    清除不可连接的节点
// 清除无用节点，启动乌龟节点，然后 ctrl + c 关闭，该节点并没被彻底清除，可以使用 cleanup 清除节点

5.2 rostopic

rostopic 包含 rostopic 命令行工具，用于显示有关 ROS 主题的调试信息，包括发布者、订阅者、发布频率和 ROS 消息。它还包含一个实验性 Python 库，用于动态获取有关主题的信息并与之交互

rostopic bw     列出消息发布带宽
rostopic delay  显示带有 header 的主题延迟
rostopic echo   获取指定话题当前发布的消息，并打印消息到屏幕
rostopic find   根据消息类型查找话题
rostopic hz     列出消息发布频率
rostopic info   显示主题相关信息（消息类型、发布者信息和订阅者信息）
rostopic list   显示所有活动状态下的主题，控制台将打印当前运行状态下的主题名称
rostopic type   列出话题的消息类型
rostopic pub    将数据发布到主题
/* rostopic pub /主题名称 消息类型 消息内容
rostopic pub /turtle1/cmd_vel geometry_msgs/Twist
 "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 2.0"
// 只发布一次运动信息

rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist
 "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 2.0"
// 以 10HZ 的频率循环发送运动信息
*/

5.3 rosmsg

rosmsg 是用于显示有关 ROS 消息类型的信息的命令行工具

rosmsg show    显示消息描述
/*
//rosmsg show 消息名称
rosmsg show turtlesim/Pose

结果:
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity
*/
rosmsg info    显示消息信息 // 作用与 rosmsg show 一样
rosmsg list    列出所有消息
rosmsg md5    显示 md5 加密后的消息
rosmsg package    显示某个功能包下的所有消息
rosmsg packages    列出包含消息的功能包

5.4 rosservice

rosservice 包含用于列出和查询 ROS Services 的 rosservice 命令行工具

调用部分服务时，如果对相关工作空间没有配置 path，需要进入工作空间调用 source ./devel/setup.bash

rosservice args    打印服务参数
rosservice call    使用提供的参数调用服务
rosservice find    按照服务类型查找服务
rosservice info    打印有关服务的信息
rosservice list    列出所有活动的服务
rosservice type    打印服务类型
rosservice uri     打印服务的 ROSRPC uri

5.5 rossrv

rossrv 是用于显示有关 ROS 服务类型的信息的命令行工具，与 rosmsg 使用语法高度雷同

rossrv show    显示服务消息详情
rossrv info    显示服务消息相关信息
rossrv list    列出所有服务信息
rossrv md5    显示 md5 加密后的服务消息
rossrv package    显示某个包下所有服务消息
rossrv packages    显示包含服务消息的所有包

5.6 rosparam

rosparam 包含 rosparam 命令行工具，用于使用 YAML 编码文件在参数服务器上获取和设置 ROS 参数

rosparam set    设置参数
rosparam get    获取参数
rosparam load    从外部文件加载参数(先准备 yaml 文件)
rosparam dump    将参数写出到外部文件
rosparam delete    删除参数
rosparam list    列出所有参数

6. 通信机制比较

三种通信机制中，参数服务器是一种数据共享机制，可以在不同的节点之间共享数据，话题通信与服务通信是在不同的节点之间传递数据的，三者是ROS中最基础也是应用最为广泛的通信机制，这其中，话题通信和服务通信有一定的相似性也有本质上的差异，在此将二者做一下简单比较
二者的实现流程是比较相似的，都是涉及到四个要素
- 要素1: 消息的发布方/客户端 (Publisher/Client)
- 要素2: 消息的订阅方/服务端 (Subscriber/Server)
- 要素3: 话题名称 (Topic/Service)
- 要素4: 数据载体 (msg/srv)
可以概括为: 两个节点通过话题关联到一起，并使用某种类型的数据载体实现数据传输

文章出处登录后可见！

已经登录？立即刷新

ROS学习2：ROS通信机制

1. 引言

2. 话题通信

2.1 话题通信理论模型

2.2 话题通信基本操作

2.3 话题通信自定义 msg

2.4 话题通信自定义 msg 调用 C++ 实现

2.5 机器人运动控制开源工程

2.5.1 使用 Rviz 观测传感器数据

2.5.2 激光雷达消息包格式

2.5.3 获取激光雷达数据的 C++ 节点

2.5.4 激光雷达避障 C++ 节点

2.5.5 IMU 消息包

2.5.6 获取 IMU 数据的 C++ 节点

2.5.7 IMU 航向锁定的 C++ 节点

2.5.8 ROS 标准消息包 std_msgs

2.5.9 ROS 几何包 geometry_msgs 和 传感器包 sensor_msgs

2.5.10 ROS 中的栅格地图格式

2.5.11 C++ 节点发布地图

3. 服务通信

3.1 服务通信理论模型

3.2 服务通信自定义srv

3.3 服务通信自定义 srv 调用 C++ 实现

4. 参数服务器

4.1 参数服务器理论模型

4.2 参数操作

5. 通信机制常用命令

5.1 rosnode

5.2 rostopic

5.3 rosmsg

5.4 rosservice

5.5 rossrv

5.6 rosparam

6. 通信机制比较

相关推荐

2.5.9 ROS 几何包 geometry_msgs 和传感器包 sensor_msgs