从ROS1到ROS2无人机编程实战指南

第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