從 ROS1 到 ROS2 無人機編程實戰指南

本書全面介紹了ROS機器人操作系統及其在無人機編程中的應用，
內容涵蓋智能機器人和無人機等從入門到精通所需的技術開發知識要點。
本書從ROS基礎知識入手，由ROS1過渡到ROS2再到兩者的移植與轉換，由淺入深、逐級進階，
以無人機的編程應用為平台，就目前流行的機器人SLAM定位算法、深度學習識別算法、
基於運動控制學的控制算法以及全局加局部的軌跡規划算法等重點和難點，進行了詳細闡述。
全書語言通俗易懂，輔以程序案例及註釋，並通過仿真的形式，讓讀者能夠輕鬆地學習ROS及無人機編程。
本書可供智能機器人及無人機等相關行業技術工作者閱讀參考,
也是ROS愛好者的實戰寶典，還可作為高校相關專業師生的參考書。

第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的bag1 263
5.2.3 ROS2轉ROS1的bag2 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