从ROS1到ROS2无人机编程实战指南 马培立 ROS操作系统及无人机编程的实战宝典 智能机器人及无人机等相关行业技术工

1.内容讲解从ROS1过渡到ROS2，再到无人机编程应用，层层递进，从基础到实战
2.理论知识辅以程序案例，读者边学边操作，上手快
3.图文并茂，涵盖面广泛，可满足机器人、无人机研发技术人员各个层次的阅读需要

本书全面介绍了ROS机器人操作系统及其在无人机编程中的应用，内容涵盖智能机器人和无人机等从入门到精通所需的技术开发知识要点。本书从ROS基础知识入手，由ROS1过渡到ROS2再到两者的移植与转换，由浅入深、逐级进阶，以无人机的编程应用为平台，就目前流行的机器人SLAM定位算法、深度学习识别算法、基于运动控制学的控制算法以及全局加局部的轨迹规划算法等重点和难点，进行了详细阐述。全书语言通俗易懂，辅以程序案例及注释，并通过仿真的形式，让读者能够轻松地学习ROS及无人机编程。
本书可供智能机器人及无人机等相关行业技术工作者阅读参考,也是ROS爱好者的实战宝典，还可作为高校相关专业师生的参考书。

马培立，2017年中国机器人技能大赛亚军；2018年国际水下机器人ROV比赛中国区第三名，全球第十名；2018年五一杯全国数学建模联赛一等奖；2019年中国“互联网+”大学生江苏省赛区二等奖；2020年华能海门电厂带压堵漏机器人结题；2021年深圳奥比中光科技有限公司-SLAM算法实习生；2021年追觅科技有限公司-SLAM算法工程师；2021年古月居签约作者；2022年古月居签约作者；2022年CSDN机器人领域创作新星。

第1章 ROS——智能机器人开端 001
1.1 ROS的节点（node） 001
1.1.1 节点 001
1.1.2 节点管理器 001
1.1.3 与节点有关的指令 002
1.2 ROS命令指令与使用 007
1.2.1 与msg相关的命令 007
1.2.2 与topic相关的命令 009
1.2.3 与service相关的命令 015
1.2.4 消息记录与回放命令 017
1.2.5 故障诊断命令 018
1.3 工作空间与功能包的创建 019
1.3.1 工作空间和功能包的组成 020
1.3.2 工作空间的创建 021
1.3.3 编译工作空间 021
1.3.4 设置环境变量 023
1.3.5 检查环境变量 023
1.3.6 功能包的创建 023
1.3.7 package.xml文件内容 025
1.3.8 CMakeLists.txt文件作用 026
1.4 可视化参数指令（Parameter）的使用 029
1.4.1 Parameter Server的使用 029
1.4.2 通过编程实现参数的静态调节 033
1.4.3 实现参数的动态调节 041
1.5 Visual Studio Code环境搭建与美化 045
1.5.1 环境搭建 045
1.5.2 Visual Studio Code 美化 049
1.6 Docker-ROS安装 050
1.6.1 了解Docker 050
1.6.2 Docker的安装 051
1.6.3 在Docker内安装ROS 054
1.6.4 在Docker内安装vncserver 055
1.6.5 测试Docker中ROS及其GUI界面 055
1.7 ROS搭建VSC调试环境 058
1.7.1 安装插件 058
1.7.2 在VScode中配置ROS环境 058
1.7.3 在VScode中debug代码 059

第2章 ROS编程及插件二次开发 065
2.1 发布者（Publisher)的编程与实现 065
2.1.1 learning_topic功能包的创建 065
2.1.2 ROS中如何实现一个Publisher 066
2.1.3 用C++实现Publisher及代码讲解 066
2.1.4 用Python实现Publisher及代码讲解 069
2.2 订阅者（Subscriber）的编程与实现 071
2.2.1 ROS中如何实现一个Subscriber 072
2.2.2 用C++实现Subscriber及代码讲解 072
2.2.3 用Python实现Subscriber及代码讲解 074
2.3 自定义话题（Topic）实现 076
2.3.1 自定义消息类型的创建 076
2.3.2 编程实现话题（C++） 079
2.3.3 编程实现话题（Python） 082
2.4 客户端（Client）的编程与实现 084
2.4.1 learning_service功能包的创建 085
2.4.2 srv文件的理解 085
2.4.3 ROS中如何实现一个Client 086
2.4.4 用C++实现Client及代码讲解 086
2.4.5 用C++实现Python及代码讲解 088
2.5 服务端（Server）的编程与实现 091
2.5.1 Trigger型文件 091
2.5.2 ROS中如何实现一个Server 092
2.5.3 用C++实现Server及代码讲解 092
2.5.4 用Python实现Server及代码讲解 095
2.6 自定义服务（Service）实现 098
2.6.1 自定义服务类型的创建 098
2.6.2 编程实现服务（C++） 100
2.6.3 编程实现服务（Python） 104
2.7 行为（Action）编程与实现 108
2.7.1 Action的工作机制 108
2.7.2 learning_action功能包的创建 112
2.7.3 编程实现动作（C++） 113
2.7.4 编程实现动作（Python） 121
2.8 多节点启动脚本（launch）文件的编程与实现 123
2.8.1 launch文件 123
2.8.2 launch文件的基本成分 123
2.8.3 launch文件编程 126
2.9 ROS设置plugin插件 128
2.9.1 什么是plugin 128
2.9.2 pluginlib的工作原理 128
2.9.3 实现plugin的步骤 128
2.9.4 plugin的实现 129
2.9.5 在ROS中使用创建的plugin 132
2.10 基于RVIZ的二次开发——plugin 134
2.10.1 plugin的创建 134
2.10.2 补充编译规则 140
2.10.3 实现结果 141
2.11 ROS多消息同步与多消息回调 142
2.11.1 什么是多消息同步与多消息回调 142
2.11.2 实现步骤 142
2.11.3 功能包的创建 143
2.11.4 全局变量形式 ：TimeSynchronizer 143
2.11.5 类成员的形式：message_filters::Synchronizer 144

第3章 ROS可视化功能包与拓展 148
3.1 日志输出工具(rqt_console) 148
3.1.1 rqt_console 148
3.1.2 日志的等级 150
3.1.3 rqt_logger_level 151
3.2 数据绘图工具（rqt_plot） 152
3.3 计算图可视化工具（rqt_graph) 155
3.4 图像渲染工具（rqt_image_view） 157
3.5 PlotJuggler 157
3.5.1 PlotJuggler简介 157
3.5.2 ROS系统中安装PlotJuggler 158
3.5.3 初识PlotJuggler 158
3.6 三维可视化工具（rviz） 162
3.6.1 Displays侧边栏 163
3.6.2 Views侧边栏 164
3.6.3 工具栏 165
3.7 三维物理仿真平台（Gazebo) 165
3.7.1 视图界面 165
3.7.2 模型列表 166
3.7.3 模型属性区 167
3.7.4 上工具栏 167
3.7.5 下工具栏 168
3.8 ROS人机交互软件介绍 168
3.8.1 ROS与QT的交互 169
3.8.2 ROS与Web的交互——rosbridge 170
3.8.3 ROS与Java的交互——rosjava 171
3.9 ROS包选择、过滤与裁剪 172
3.9.1 根据topic过滤 172
3.9.2 根据时间过滤 172
3.9.3 同时过滤topic与时间 173
3.9.4 通过rosbag完成ros包操作 173
3.10 常见GUI快速查询 174
3.10.1 rqt_tf_tree 174
3.10.2 rqt_bag 174
3.10.3 rqt_topic 175
3.10.4 rqt_reconfigure 175
3.10.5 rqt_publisher 176
3.10.6 rqt_top 176
3.10.7 rqt_runtime_monitor 177

第4章 ROS2——智能机器人新起点 178
4.1 ROS2的新特性 178
4.1.1 ROS1与ROS2程序书写的不同 178
4.1.2 ROS1与ROS2通信机制的不同 179
4.1.3 ROS1与ROS2功能包、工作空间、环境的不同 180
4.2 ROS2之DDS 180
4.2.1 什么是DDS 181
4.2.2 DDS多机通信 181
4.2.3 中间件RMW 182
4.2.4 DDS调优 183
4.3 Docker—ROS2安装 184
4.3.1 安装 184
4.3.2 安装测试 185
4.3.3 编译并运行示例程序 186
4.3.4 ROS2 docker 安装 187
4.4 ROS2搭建VSC调试环境 191
4.4.1 编译设置 191
4.4.2 Debug设置 192
4.4.3 开启Debug 194
4.5 ROS2工作空间介绍 195
4.5.1 工作空间组成 195
4.5.2 创建一个简单的功能包 196
4.5.3 编译功能包 197
4.6 ROS2的POP和OOP 198
4.6.1 POP和OOP是什么 198
4.6.2 POP与OOP对比 199
4.6.3 小结 199
4.7 发布者（Publisher）的编程与实现 200
4.7.1 ROS2发布者功能确定 200
4.7.2 编写代码（C++实现） 201
4.7.3 编写代码（Python实现） 203
4.7.4 编译代码 204
4.7.5 运行代码 204
4.8 订阅者（Subscriber）的编程与实现 205
4.8.1 ROS2订阅者功能确定 205
4.8.2 编写代码（C++实现） 205
4.8.3 编写代码（Python实现） 207
4.8.4 编译代码 208
4.8.5 运行代码 208
4.9 客户（Client）的编程与实现 209
4.9.1 ROS2服务的简单调用 209
4.9.2 ROS2客户功能确定 210
4.9.3 编写代码（C++实现） 210
4.9.4 编写代码（Python实现） 212
4.9.5 运行代码 213
4.10 服务（Service）的编程与实现 214
4.10.1 ROS2服务任务确定 214
4.10.2 编写代码（C++实现） 214
4.10.3 编写代码（Python实现） 216
4.10.4 运行代码 217
4.11 自定义msg以及srv 218
4.11.1 自定义msg以及srv的意义 218
4.11.2 创建自己的msg、srv文件 218
4.11.3 在其他功能包里引用 219
4.12 ROS2参数（Parameter） 220
4.12.1 参数是什么 220
4.12.2 任务确定 220
4.12.3 程序编写（C++） 220
4.12.4 程序编写（Python） 221
4.12.5 编译并运行代码 222
4.13 ROS2如何一键启动多个脚本 223
4.13.1 ROS2的launch系统 223
4.13.2 在自己的功能包中添加launch文件（C++） 225
4.13.3 在自己的功能包中添加launch文件（Python） 226
4.13.4 编译及运行 227
4.14 Action（server & client）的编程与实现 227
4.14.1 任务确定 228
4.14.2 根据任务创建对应的Action 228
4.14.3 程序编写(C++) 229
4.14.4 程序编写(Python) 233
4.14.5 程序执行 235
4.15 ROS2子节点以及多线程 236
4.15.1 ROS1—Node 和 Nodelets 236
4.15.2 ROS2—统一API 237
4.15.3 component初体验 237
4.15.4 自定义component 239
4.15.5 ROS2中的多线程—callbackgroup 241
4.15.6 多线程的大致流程 242
4.15.7 自定义多线程程序 243
4.16 ROS2中常用命令行工具 243
4.16.1 功能包 243
4.16.2 节点 244
4.16.3 ROS2话题 245
4.16.4 参数（param）命令 247
4.16.5 action命令 248
4.16.6 interface工具 248
4.16.7 doctor工具 250
4.16.8 ROS2可视化GUI与仿真工具 251

第5章 从ROS1移植到ROS2 255
5.1 ROS1移植到ROS2常见的问题 255
5.1.1 CMakeList编写 255
5.1.2 launch文件 256
5.1.3 parameter 257
5.1.4 代码移植部分 258
5.2 ROS1与ROS2包的互相转换及使用 261
5.2.1 使用ROS2录制小海龟包 261
5.2.2 ROS2转ROS1的bag包1 263
5.2.3 ROS2转ROS1的bag包2 264
5.2.4 ROS1转ROS2的bag包 264
5.2.5 自定义类型msg的bag包转换 264

第6章 无人机相机定位 268
6.1 定位算法概述 268
6.1.1 主流定位算法 268
6.1.2 室内定位算法——RFID定位 268
6.1.3 室内定位算法——WIFI定位 269
6.1.4 室内定位算法——UWB定位 269
6.1.5 室外定位算法——GPS/RTK基站定位 270
6.1.6 通用定位算法——激光定位 270
6.1.7 通用定位算法——视觉定位 272
6.1.8 定位算法精度以及规模化难易程度比较 273
6.2 VINS的集大成者——VINS FUSION 274
6.2.1 VSLAM是什么 274
6.2.2 视觉SLAM技术发展 274
6.2.3 VINS-FUSION安装 279
6.3 从单目VIO初始化开始 280
6.3.1 整体架构 280
6.3.2 前端程序的入口 282
6.3.3 特征点跟踪 284
6.3.4 IMU预积分 290
6.3.5 中值滤波 293
6.4 边缘化与优化 298
6.4.1 关键帧检测 298
6.4.2 标定外参坐标系转化 298
6.4.3 摄像头+IMU初始化 300
6.4.4 BA优化-IMU 303
6.4.5 BA优化-图像 306
6.4.6 基于舒尔补的边缘化 309
6.4.7 后操作 313
6.5 最后的工作——回环检测 314
6.5.1 回环检测-入口函数 314
6.5.2 回环检测-关键帧获取 316
6.5.3 后端优化-图优化 317
6.5.4 全局融合 319
6.5.5 小结 321

第7章 无人机二维激光雷达定位 322
7.1 Cartographer 322
7.1.1 Cartographer与Cartographer_ros 322
7.1.2 2D SLAM发展 322
7.1.3 Cartographer安装 324
7.2 cartographer_ros数据传入 326
7.2.1 cartographer_ros目录结构 326
7.2.2 cartographer_ros 327
7.2.3 cartographer_node 328
7.2.4 lua文件详解 330
7.2.5 Cartographer构造函数消息处理 332
7.2.6 轨迹跟踪和传感器数据获取 334
7.3 前后端链接的桥梁 335
7.3.1 地图构建的桥梁—可视化 335
7.3.2 地图构建的桥梁—添加轨迹 335
7.3.3 地图构建的桥梁—其他函数 337
7.3.4 传感器构建的桥梁—雷达数据 337
7.3.5 传感器构建的桥梁—其他函数 339
7.4 地图构建器 340
7.4.1 Cartographer中的地图参数获取 340
7.4.2 地图接口实现 342
7.4.3 map_builder其他函数 345
7.4.4 链接前端与后端的桥梁 345
7.4.5 添加传感器后端优化接口 347
7.5 Local SLAM-子图的匹配 349
7.5.1 Local SLAM的开端 349
7.5.2 子图的维护 351
7.5.3 占用栅格地图 353
7.5.4 查找表与占用栅格更新 355
7.5.5 核心函数—AddRangeData 358
7.5.6 实时相关性分析的扫描匹配器 363
7.5.7 Ceres扫描匹配 364
7.6 Global SLAM全局地图的匹配 365
7.6.1 Global SLAM的开端 365
7.6.2 位姿图创建与更新 367
7.6.3 线程池管理下的后端优化 372
7.6.4 约束构建器 375
7.6.5 分支定界闭环检测 378
7.6.6 后端优化 383
7.6.7 小结 389

第8章 无人机三维激光雷达定位 390
8.1 LOAM工业化落地-SC-LeGO-LOAM 390
8.1.1 激光SLAM与视觉SLAM优劣对比 390
8.1.2 3D SLAM发展 390
8.1.3 SC-LeGO-LOAM安装 393
8.2 点云数据输入与地面点分割 394
8.2.1 为什么选择SC-LeGO-LOAM 394
8.2.2 launch 文件 394
8.2.3 点云输入预处理以及地面点分割、点云分割 395
8.3 激光特征提取与关联 402
8.3.1 入口函数 402
8.3.2 特征提取—畸变去除 404
8.3.3 特征提取—计算平滑 406
8.3.4 特征提取—去除不可靠点 407
8.3.5 特征提取—角点提取 408
8.3.6 数据关联—更新初始化位姿 410
8.3.7 数据关联—更新变换矩阵 410
8.3.8 数据关联—线面特征提取 411
8.3.9 数据关联—迭代优化 414
8.3.10 数据关联—更新累计变化矩阵 416
8.4 回环检测—ScanContext 417
8.4.1 回环检测与坐标转换 417
8.4.2 点云预处理 418
8.4.3 帧与地图的优化 419
8.4.4 关键帧以及ScanContext提取 420
8.4.5 大回环与优化 422
8.4.6 融合里程计 425
8.4.7 小结 427

第9章 无人机识别避障 428
9.1 识别算法综述 428
9.1.1 深度学习分类 428
9.1.2 深度学习步骤 429
9.1.3 图像分类 430
9.1.4 目标识别—两阶段 433
9.1.5 目标识别—一阶段 437
9.2 无人机AprilTag识别 439
9.2.1 AprilTag基本原理 439
9.2.2 AprilTag如何生成 440
9.2.3 AprilTag识别步骤 440
9.2.4 AprilTag编码解码 442
9.2.5 AprilTag代码结构 443
9.2.6 Apriltag_ros环境搭建 443
9.2.7 Apriltag_ros定位实例 445
9.3 无人机行人识别 446
9.3.1 HOG算子 446
9.3.2 SVM算法 449
9.3.3 基于OpenCV行人识别流程 451
9.3.4 OpenCV识别代码实例 452
9.3.5 深度学习环境搭建 454
9.3.6 YOLOv3测试 456
9.3.7 YOLOv3 ros代码解析 458
9.4 无人机行人骨骼点识别 460
9.4.1 骨骼点介绍 460
9.4.2 Kinect关键点检测 461
9.4.3 关键点检测算法 463
9.4.4 OpenPose 原理介绍 465
9.4.5 Openpose_ros测试 467
9.4.6 代码注释 470

第10章 无人机运动控制 473
10.1 滤波算法 473
10.1.1 滑动均值滤波法 473
10.1.2 限幅滤波法 474
10.1.3 中位值滤波法 475
10.1.4 中位值平均滤波法 476
10.1.5 一阶滞后滤波法 477
10.2 卡尔曼滤波（KF） 478
10.2.1 场景举例 478
10.2.2 线性时不变系统 479
10.2.3 高斯分布 479
10.2.4 卡尔曼滤波 480
10.2.5 卡尔曼滤波的封装 481
10.2.6 卡尔曼滤波的实际应用 484
10.3 拓展卡尔曼滤波（EKF） 486
10.3.1 场景举例 486
10.3.2 EKF拓展卡尔曼滤波 486
10.3.3 拓展卡尔曼滤波实例 487
10.4 无迹卡尔曼滤波（UKF） 491
10.4.1 引入 491
10.4.2 UKF之Sigma点 491
10.4.3 UKF 无迹卡尔曼滤波 494
10.4.4 无迹卡尔曼滤波实例 494
10.5 粒子滤波（PF） 497
10.5.1 设计粒子滤波的动机 497
10.5.2 贝叶斯滤波 498
10.5.3 蒙特卡洛采样 499
10.5.4 粒子滤波 499
10.5.5 粒子滤波示例 501

第11章 无人机轨迹规划 507
11.1 Dijkstra算法 507
11.1.1 规划方案 507
11.1.2 Dijkstra流程介绍 507
11.1.3 Dijkstra示例代码 508
11.2 A*算法 512
11.2.1 A*与Dijkstra算法 512
11.2.2 距离计算方式 512
11.2.3 A*流程说明 513
11.2.4 A*算法示例代码 515
11.3 RRT算法 516
11.3.1 RRT算法的出现 516
11.3.2 RRT流程说明 516
11.4 RRT*算法 520
11.4.1 RRT*算法的出现 520
11.4.2 RRT*算法的流程说明 520
11.5 DWA算法 523
11.5.1 DWA 523
11.5.2 DWA流程说明 524

第12章 无人机终体验 528
12.1 飞控介绍 528
12.1.1 什么是飞控 528
12.1.2 飞控能做什么 528
12.2 无人机硬件—感知 529
12.2.1 气压计 529
12.2.2 光流 529
12.2.3 磁罗盘与GPS 529
12.2.4 距离传感器 530
12.2.5 双目摄像头（以t265为例） 530
12.2.6 深度相机（以D435i为例） 531
12.2.7 IMU（Inertial Measurement Unit） 531
12.2.8 MoCap（Motion Capture） 532
12.2.9 UWB（Ultra Wide Band Positioning） 532
12.3 无人机硬件—控制 532
12.3.1 电子调速器（ESC） 532
12.3.2 电机 533
12.4 无人机硬件—通信 533
12.4.1 无线数传 533
12.4.2 FrSky数传 534
12.5 仿真通信 534
12.6 Prometheus仿真环境搭建 535
12.6.1 prometheus_px4配置 535
12.6.2 Prometheus配置 536
12.6.3 测试Prometheus 538
12.7 通过mavros实现对期望动作的发布 539
12.7.1 从终端控制飞机探讨mavros用法 539
12.7.2 对期望动作的发送 540
12.8 通过mavros实现对当前位置发送 543
12.9 零门槛的普罗米修斯遥控仿真 545
12.9.1 PX4-Gazebo仿真原理 545
12.9.2 Prometheus代码框架 547
12.9.3 仿真中的遥控器使用说明 548
12.9.4 无人机各种情况下的操作说明 549
12.9.5 uav_control节点介绍 550
12.9.6 tutorial_demo模块 551
12.9.7 起飞降落 551
12.10 YOLO在普罗米修斯中的使用 552
12.10.1 概述 552
12.10.2 环境配置与安装 553
12.10.3 程序核心逻辑 554
12.10.4 无人机控制 555
12.11 A*在普罗米修斯中的使用 556
12.11.1 A*在普罗米修斯中的场景 556
12.11.2 A*在普罗米修斯中的代码解析 558

参考文献 569

机器人的发展横跨七八十年，经历了三个重要时期：
●电气时代——2000年之前
主要应用于工业生产，称为工业机器人，由示教器操控，帮助工厂释放劳动力，此时的机器人并没有太多智能，完全按照人类的命令执行动作，更加关注电气层面的驱动器、伺服电机、减速机、控制器等设备，这是机器人的电气时代。
●数字时代——2000～2015年
计算机和视觉技术逐渐应用，机器人的类型不断丰富，出现了AGV、视觉检测等应用，此时的机器人传感器更加丰富，但是依然缺少自主思考的过程，智能化有限，只能感知局部环境，这是机器人的数字时代，也是机器人大时代的前夜。
●智能时代——2015年之后
随着人工智能技术的快速发展，机器人成了AI技术的最佳载体，家庭服务机器人、送餐机器人、四足仿生机器狗、自动驾驶汽车等应用呈井喷式爆发，智能机器人时代正式拉开序幕。
智能机器人的快速发展，必将对机器人开发提出更高的要求，软件层面最为热点的技术之一就是机器人操作系统。
时光回到2007年美国斯坦福大学的机器人实验室，他们正在讨论如何实现一款机器人家庭助手。这个实验室聚集了全球精英，但是他们依然无法从零制造一个能够走进千家万户的机器人，于是这群人诞生了一个想法：为什么不把更多人的智慧集合到一起呢？
于是他们脑洞大开，做了一套针对机器人的软件框架，把开源社区里很多优秀的软件集成其中，这样就可以在上边快速开发做饭、做家务等应用了。所谓“软件框架”，就像房子的骨架一样，再高的楼房都是先把“框架”搭起来，然后在里边装修每个房间。所以有了这样一个机器人的软件框架，做机器人就更加简单了。他们为这个软件框架起了一个名字——Robot Operating System，机器人操作系统，简称ROS。
在十几年的时间里，ROS得到了广泛应用，从ROS1进化到ROS2，大大提高了机器人的开发与部署效率，在分工协作、软件维护和系统扩展中具有重要意义。时至今日，ROS已成为机器人领域的普遍标准，正在变革无人驾驶汽车、云脑机器人、新一代工业机器人等诸多领域，结合人工智能技术引领机器人新一轮的技术浪潮。
2011年底，我创建了古月居（www.guyuehome.com），主要分享个人在ROS学习之路上的一些总结。伴随ROS的发展，古月居也在不断成长，如今已经成为华语地区知名的ROS机器人社区，致力于为机器人学习者提供优质的交流学习平台。
在ROS机器人开发的道路上，本书的作者们也在一路同行。本书覆盖ROS1及ROS2核心知识点，以无人机为开发平台，介绍了具体的开发原理和实践方法，带领读者进行由浅入深、再到实践的系统化学习，适合ROS无人机方向各阶段开发者使用，希望大家可以从中受益，起航机器人开发。
ROS机器人开发涉及的知识点众多，在不断探索ROS应用的道路上，很多开发者犹豫过、彷徨过，我也曾是其中的一员，分享胡适先生的一句名言，愿你我共勉：怕什么真理无穷，进一寸有一寸的欢喜。

古月
ROS机器人社区“古月居”创始人