從ROS1到ROS2無人機編程實戰(zhàn)指南

第1章　ROS——智能機器人開端 001
1.1　ROS的節(jié)點（node） 001
1.1.1　節(jié)點 001
1.1.2　節(jié)點管理器 001
1.1.3　與節(jié)點有關的指令 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　工作空間與功能包的創(chuàng)建 019
1.3.1　 工作空間和功能包的組成 020
1.3.2　 工作空間的創(chuàng)建 021
1.3.3　 編譯工作空間 021
1.3.4　 設置環(huán)境變量 023
1.3.5　 檢查環(huán)境變量 023
1.3.6　 功能包的創(chuàng)建 023
1.3.7　 package.xml文件內(nèi)容 025
1.3.8　CMakeLists.txt文件作用 026
1.4　可視化參數(shù)指令（Parameter）的使用 029
1.4.1　Parameter Server的使用 029
1.4.2　通過編程實現(xiàn)參數(shù)的靜態(tài)調(diào)節(jié) 033
1.4.3　實現(xiàn)參數(shù)的動態(tài)調(diào)節(jié) 041
1.5　 Visual Studio Code環(huán)境搭建與美化 045
1.5.1　 環(huán)境搭建 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內(nèi)安裝ROS 054
1.6.4　在Docker內(nèi)安裝vncserver 055
1.6.5　測試Docker中ROS及其GUI界面 055
1.7　 ROS搭建VSC調(diào)試環(huán)境 058
1.7.1　 安裝插件 058
1.7.2　 在VScode中配置ROS環(huán)境 058
1.7.3　 在VScode中debug代碼 059
第2章　ROS編程及插件二次開發(fā) 065
2.1　 發(fā)布者（Publisher)的編程與實現(xiàn) 065
2.1.1　learning_topic功能包的創(chuàng)建 065
2.1.2　ROS中如何實現(xiàn)一個Publisher 066
2.1.3　用C 實現(xiàn)Publisher及代碼講解 066
2.1.4　用Python實現(xiàn)Publisher及代碼講解 069
2.2　訂閱者（Subscriber）的編程與實現(xiàn) 071
2.2.1　ROS中如何實現(xiàn)一個Subscriber 072
2.2.2　用C 實現(xiàn)Subscriber及代碼講解 072
2.2.3　用Python實現(xiàn)Subscriber及代碼講解 074
2.3　自定義話題（Topic）實現(xiàn) 076
2.3.1　自定義消息類型的創(chuàng)建 076
2.3.2　編程實現(xiàn)話題（C ） 079
2.3.3　編程實現(xiàn)話題（Python） 082
2.4　客戶端（Client）的編程與實現(xiàn) 084
2.4.1　learning_service功能包的創(chuàng)建 085
2.4.2　srv文件的理解 085
2.4.3　ROS中如何實現(xiàn)一個Client 086
2.4.4　用C 實現(xiàn)Client及代碼講解 086
2.4.5　用C 實現(xiàn)Python及代碼講解 088
2.5　服務端（Server）的編程與實現(xiàn) 091
2.5.1　Trigger型文件 091
2.5.2　ROS中如何實現(xiàn)一個Server 092
2.5.3　用C 實現(xiàn)Server及代碼講解 092
2.5.4　用Python實現(xiàn)Server及代碼講解 095
2.6　自定義服務（Service）實現(xiàn) 098
2.6.1　自定義服務類型的創(chuàng)建 098
2.6.2　編程實現(xiàn)服務（C ） 100
2.6.3　編程實現(xiàn)服務（Python） 104
2.7　行為（Action）編程與實現(xiàn) 108
2.7.1　Action的工作機制 108
2.7.2　learning_action功能包的創(chuàng)建 112
2.7.3　編程實現(xiàn)動作（C ） 113
2.7.4　編程實現(xiàn)動作（Python） 121
2.8　多節(jié)點啟動腳本（launch）文件的編程與實現(xiàn) 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　實現(xiàn)plugin的步驟 128
2.9.4　plugin的實現(xiàn) 129
2.9.5　在ROS中使用創(chuàng)建的plugin 132
2.10　基于RVIZ的二次開發(fā)——plugin 134
2.10.1　plugin的創(chuàng)建 134
2.10.2　補充編譯規(guī)則 140
2.10.3　實現(xiàn)結(jié)果 141
2.11　ROS多消息同步與多消息回調(diào) 142
2.11.1　什么是多消息同步與多消息回調(diào) 142
2.11.2　實現(xiàn)步驟 142
2.11.3　功能包的創(chuàng)建 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　數(shù)據(jù)繪圖工具（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系統(tǒng)中安裝PlotJuggler 158
3.5.3　初識PlotJuggler 158
3.6　三維可視化工具（rviz） 162
3.6.1　Displays側(cè)邊欄 163
3.6.2　Views側(cè)邊欄 164
3.6.3　工具欄 165
3.7　三維物理仿真平臺（Gazebo) 165
3.7.1　視圖界面 165
3.7.2　模型列表 166
3.7.3　模型屬性區(qū) 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　根據(jù)topic過濾 172
3.9.2　根據(jù)時間過濾 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功能包、工作空間、環(huán)境的不同 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調(diào)優(yōu) 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調(diào)試環(huán)境 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　創(chuàng)建一個簡單的功能包 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　小結(jié) 199
4.7　發(fā)布者（Publisher）的編程與實現(xiàn) 200
4.7.1　ROS2發(fā)布者功能確定 200
4.7.2　編寫代碼（C 實現(xiàn)） 201
4.7.3　編寫代碼（Python實現(xiàn)） 203
4.7.4　編譯代碼 204
4.7.5　運行代碼 204
4.8　訂閱者（Subscriber）的編程與實現(xiàn) 205
4.8.1　ROS2訂閱者功能確定 205
4.8.2　編寫代碼（C 實現(xiàn)） 205
4.8.3　編寫代碼（Python實現(xiàn)） 207
4.8.4　編譯代碼 208
4.8.5　運行代碼 208
4.9　客戶（Client）的編程與實現(xiàn) 209
4.9.1　ROS2服務的簡單調(diào)用 209
4.9.2　ROS2客戶功能確定 210
4.9.3　編寫代碼（C 實現(xiàn)） 210
4.9.4　編寫代碼（Python實現(xiàn)） 212
4.9.5　運行代碼 213
4.10　服務（Service）的編程與實現(xiàn) 214
4.10.1　ROS2服務任務確定 214
4.10.2　編寫代碼（C 實現(xiàn)） 214
4.10.3　編寫代碼（Python實現(xiàn)） 216
4.10.4　運行代碼 217
4.11　自定義msg以及srv 218
4.11.1　自定義msg以及srv的意義 218
4.11.2　創(chuàng)建自己的msg、srv文件 218
4.11.3　在其他功能包里引用 219
4.12　ROS2參數(shù)（Parameter） 220
4.12.1　參數(shù)是什么 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系統(tǒng) 223
4.13.2　在自己的功能包中添加launch文件（C ） 225
4.13.3　在自己的功能包中添加launch文件（Python） 226
4.13.4　編譯及運行 227
4.14　Action（server & client）的編程與實現(xiàn) 227
4.14.1　任務確定 228
4.14.2　根據(jù)任務創(chuàng)建對應的Action 228
4.14.3　程序編寫(C ) 229
4.14.4　程序編寫(Python) 233
4.14.5　程序執(zhí)行 235
4.15　ROS2子節(jié)點以及多線程 236
4.15.1　ROS1—Node 和 Nodelets 236
4.15.2　ROS2—統(tǒng)一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　節(jié)點 244
4.16.3　ROS2話題 245
4.16.4　參數(shù)（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包的互相轉(zhuǎn)換及使用 261
5.2.1　使用ROS2錄制小海龜包 261
5.2.2　ROS2轉(zhuǎn)ROS1的bag包1 263
5.2.3　ROS2轉(zhuǎn)ROS1的bag包2 264
5.2.4　ROS1轉(zhuǎn)ROS2的bag包 264
5.2.5　自定義類型msg的bag包轉(zhuǎn)換 264
第6章　無人機相機定位 268
6.1　定位算法概述 268
6.1.1　主流定位算法 268
6.1.2　室內(nèi)定位算法——RFID定位 268
6.1.3　室內(nèi)定位算法——WIFI定位 269
6.1.4　室內(nèi)定位算法——UWB定位 269
6.1.5　室外定位算法——GPS/RTK基站定位 270
6.1.6　通用定位算法——激光定位 270
6.1.7　通用定位算法——視覺定位 272
6.1.8　定位算法精度以及規(guī)模化難易程度比較 273
6.2　VINS的集大成者——VINS FUSION 274
6.2.1　VSLAM是什么 274
6.2.2　視覺SLAM技術發(fā)展 274
6.2.3　VINS-FUSION安裝 279
6.3　從單目VIO初始化開始 280
6.3.1　整體架構(gòu) 280
6.3.2　前端程序的入口 282
6.3.3　特征點跟蹤 284
6.3.4　IMU預積分 290
6.3.5　中值濾波 293
6.4　邊緣化與優(yōu)化 298
6.4.1　關鍵幀檢測 298
6.4.2　標定外參坐標系轉(zhuǎn)化 298
6.4.3　攝像頭 IMU初始化 300
6.4.4　BA優(yōu)化-IMU 303
6.4.5　BA優(yōu)化-圖像 306
6.4.6　基于舒爾補的邊緣化 309
6.4.7　后操作 313
6.5　最后的工作——回環(huán)檢測 314
6.5.1　回環(huán)檢測-入口函數(shù) 314
6.5.2　回環(huán)檢測-關鍵幀獲取 316
6.5.3　后端優(yōu)化-圖優(yōu)化 317
6.5.4　全局融合 319
6.5.5　小結(jié) 321
第7章　無人機二維激光雷達定位 322
7.1　Cartographer 322
7.1.1　Cartographer與Cartographer_ros 322
7.1.2　2D SLAM發(fā)展 322
7.1.3　Cartographer安裝 324
7.2　cartographer_ros數(shù)據(jù)傳入 326
7.2.1　cartographer_ros目錄結(jié)構(gòu) 326
7.2.2　cartographer_ros 327
7.2.3　cartographer_node 328
7.2.4　lua文件詳解 330
7.2.5　Cartographer構(gòu)造函數(shù)消息處理 332
7.2.6　軌跡跟蹤和傳感器數(shù)據(jù)獲取 334
7.3　前后端鏈接的橋梁 335
7.3.1　地圖構(gòu)建的橋梁—可視化 335
7.3.2　地圖構(gòu)建的橋梁—添加軌跡 335
7.3.3　地圖構(gòu)建的橋梁—其他函數(shù) 337
7.3.4　傳感器構(gòu)建的橋梁—雷達數(shù)據(jù) 337
7.3.5　傳感器構(gòu)建的橋梁—其他函數(shù) 339
7.4　地圖構(gòu)建器 340
7.4.1　Cartographer中的地圖參數(shù)獲取 340
7.4.2　地圖接口實現(xiàn) 342
7.4.3　map_builder其他函數(shù) 345
7.4.4　鏈接前端與后端的橋梁 345
7.4.5　添加傳感器后端優(yōu)化接口 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　核心函數(shù)—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　位姿圖創(chuàng)建與更新 367
7.6.3　線程池管理下的后端優(yōu)化 372
7.6.4　約束構(gòu)建器 375
7.6.5　分支定界閉環(huán)檢測 378
7.6.6　后端優(yōu)化 383
7.6.7　小結(jié) 389
第8章　無人機三維激光雷達定位 390
8.1　LOAM工業(yè)化落地-SC-LeGO-LOAM 390
8.1.1　激光SLAM與視覺SLAM優(yōu)劣對比 390
8.1.2　3D SLAM發(fā)展 390
8.1.3　SC-LeGO-LOAM安裝 393
8.2　點云數(shù)據(jù)輸入與地面點分割 394
8.2.1　為什么選擇SC-LeGO-LOAM 394
8.2.2　launch 文件 394
8.2.3　點云輸入預處理以及地面點分割、點云分割 395
8.3　激光特征提取與關聯(lián) 402
8.3.1　入口函數(shù) 402
8.3.2　特征提取—畸變?nèi)コ?404
8.3.3　特征提取—計算平滑 406
8.3.4　特征提取—去除不可靠點 407
8.3.5　特征提取—角點提取 408
8.3.6　數(shù)據(jù)關聯(lián)—更新初始化位姿 410
8.3.7　數(shù)據(jù)關聯(lián)—更新變換矩陣 410
8.3.8　數(shù)據(jù)關聯(lián)—線面特征提取 411
8.3.9　數(shù)據(jù)關聯(lián)—迭代優(yōu)化 414
8.3.10　數(shù)據(jù)關聯(lián)—更新累計變化矩陣 416
8.4　回環(huán)檢測—ScanContext 417
8.4.1　回環(huán)檢測與坐標轉(zhuǎn)換 417
8.4.2　點云預處理 418
8.4.3　幀與地圖的優(yōu)化 419
8.4.4　關鍵幀以及ScanContext提取 420
8.4.5　大回環(huán)與優(yōu)化 422
8.4.6　融合里程計 425
8.4.7　小結(jié) 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代碼結(jié)構(gòu) 443
9.2.6　Apriltag_ros環(huán)境搭建 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　深度學習環(huán)境搭建 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　線性時不變系統(tǒng) 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章　無人機軌跡規(guī)劃 507
11.1　Dijkstra算法 507
11.1.1　規(guī)劃方案 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算法的出現(xiàn) 516
11.3.2　RRT流程說明 516
11.4　RRT*算法 520
11.4.1　RRT*算法的出現(xiàn) 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　電子調(diào)速器（ESC） 532
12.3.2　電機 533
12.4　無人機硬件—通信 533
12.4.1　無線數(shù)傳 533
12.4.2　FrSky數(shù)傳 534
12.5　仿真通信 534
12.6　Prometheus仿真環(huán)境搭建 535
12.6.1　prometheus_px4配置 535
12.6.2　Prometheus配置 536
12.6.3　測試Prometheus 538
12.7　通過mavros實現(xiàn)對期望動作的發(fā)布 539
12.7.1　從終端控制飛機探討mavros用法 539
12.7.2　對期望動作的發(fā)送 540
12.8　通過mavros實現(xiàn)對當前位置發(fā)送 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節(jié)點介紹 550
12.9.6　tutorial_demo模塊 551
12.9.7　起飛降落 551
12.10　YOLO在普羅米修斯中的使用 552
12.10.1　概述 552
12.10.2　環(huán)境配置與安裝 553
12.10.3　程序核心邏輯 554
12.10.4　無人機控制 555
12.11　A*在普羅米修斯中的使用 556
12.11.1　A*在普羅米修斯中的場景 556
12.11.2　A*在普羅米修斯中的代碼解析 558
參考文獻 569