文章目錄
- 前言
- 一、導航概述
- 1.1、導航子產品
- 1.2、導航之坐标系
- 二、導航實作
- 準備工作（安裝導航包和建立工程包）
- 2.1、SLAM建圖
- 2.1.1、認識gmapping
- 2.1.2、實操
- 2.2、地圖服務（map_server）
- 2.2.1、認識map_server
- 2.2.2、實操—儲存地圖
- 2.2.3、實操—讀取地圖
- 2.3、定位（amcl）
- 2.3.1、認識amcl
- 2.3.2、坐标變換介紹
- 2.3.3、實操
- 2.4、路徑規劃（move_base）
- 2.4.1、認識move_base
- 2.4.2、move_base與代價地圖
- 2.4.3、實操—目的地導航
- 2.4.4、rviz訂閱全局地圖、本地地圖
- 2.5、實踐—自主實作建圖（slam+move_base）
- 三、導航消息
- 3.1、導航—地圖
- 3.2、導航—裡程計
- 3.3、導航—坐标變換
- 3.4、導航—目标點與路徑規劃
- 3.5、導航—雷射雷達
- 3.6、導航—相機
- 3.7、深度圖像轉雷射資料
- 3.7.1、認識depthimage_to_laserscan
- 3.7.2、實操
前言
馬上開學，目前學校很多實驗室都是人工智能這塊，大部分都是和機器人相關，然後軟體這塊就是和cv、ros相關，就打算開始學習一下。
本章節是虛拟機安裝Ubuntu18.04以及安裝ROS的環境。
學習教程：【Autolabor初級教程】ROS機器人入門，部落格中一些知識點是來源于趙老師的筆記線上筆記，本部落客要是做歸納總結，如有侵權請聯系删除。
視訊中的案例都基本敲了遍，這裡給出我自己的源代碼檔案：
連結：https://pan.baidu.com/s/13CAzXk0vAWuBsc4oABC-_g
提取碼：0hws 所有部落格檔案目錄索引：部落格目錄索引(持續更新)
一、導航概述
1.1、導航子產品
關鍵技術有如下五點:
- 全局地圖：實作導航，需要參考一張全局性質的地圖，這就涉及到地圖模組化技術（SLAM ）。
- 自身定位：确定自身的位置，ros中提供了定位功能包（amcl）。
- 路徑規劃：從A->B點需要計算全局路線，且在運動過程中需要根據實時路況進行調整。（move_base，包含全局路徑和本地實時路徑）
- 運動控制：需要借助話題"cmd_vel"釋出
geometry_msgs/Twist
- 環境感覺：感覺周圍環境，環境感覺也是一重要子產品實作，它為其他子產品提供了支援。其他子產品諸如: SLAM、amcl、move_base 都需要依賴于環境感覺。
slam介紹
SLAM問題可以描述為: 機器人在未知環境中從一個未知位置開始移動,在移動過程中根據位置估計和地圖進行自身定位，同時在自身定位的基礎上建造增量式地圖，以繪制出外部環境的完全地圖。
SLAM 實作的技術如：gmapping、hector_slam、cartographer、rgbdslam、ORB_SLAM …
- 對于地圖的生成也需要進行儲存：在ROS中儲存地圖的功能包是map_server。
完成SLAM依賴的實體傳感器（需要擷取周圍環境深度資訊的能力）：雷射雷達、攝像頭、RGB-D攝像頭…
總結：SLAM 雖然是機器人導航的重要技術之一，但是 二者并不等價，确切的講，SLAM 隻是實作地圖建構和即時定位。
amcl介紹
amcl(adaptiveMonteCarloLocalization)：自适應的蒙特卡洛定位,是用于2D移動機器人的機率定位系統。它實作了自适應（或KLD采樣）蒙特卡洛定位方法，該方法使用粒子過濾器根據已知地圖跟蹤機器人的姿态。
move_base
該功能包由兩大規劃器組成：
1、全局路徑規劃(gloable_planner)：根據給定的目标點和全局地圖實作總體的路徑規劃，使用 Dijkstra 或 A* 算法進行全局路徑規劃，計算最優路線，作為全局路線、
2、本地時時規劃(local_planner)：在實際導航過程中，機器人可能無法按照給定的全局最優路線運作，比如:機器人在運作中，可能會随時出現一定的障礙物… 本地規劃的作用就是使用一定算法(Dynamic Window Approaches) 來實作障礙物的規避，并選取目前最優路徑以盡量符合全局最優路徑。
全局路徑規劃與本地路徑規劃是相對的，全局路徑規劃側重于全局、宏觀實作，而本地路徑規劃側重與目前、微觀實作。
運動控制
導航功能包集假定它可以通過話題"cmd_vel"釋出
geometry_msgs/Twist
類型的消息，這個消息基于機器人的基座坐标系，它傳遞的是運動指令。這意味着必須有一個節點訂閱"cmd_vel"話題， 将該話題上的速度指令轉換為電機指令并發送。
環境感覺
感覺周圍環境資訊，比如: 攝像頭、雷射雷達、編碼器…，攝像頭。
例如：雷射雷達可以用于感覺外界環境的深度資訊，編碼器可以感覺電機的轉速資訊，進而可以擷取速度資訊并生成裡程計資訊。
1.2、導航之坐标系
介紹
定位：就是參考某個坐标系(比如:以機器人的出發點為原點建立坐标系)在該坐标系中标注機器人。
定位實作需要依賴于機器人自身，機器人需要逆向推導參考系原點并計算坐标系相對關系，該過程實作常用方式有兩種：
- 通過裡程計定位：時時收集機器人的速度資訊計算并釋出機器人坐标系與父級參考系的相對關系。
- 通過傳感器定位：通過傳感器收集外界環境資訊通過比對計算并釋出機器人坐标系與父級參考系的相對關系。
兩者特點
兩者來進行配合最佳，前者根據行駛的距離來進行定位，後者則是依據不同的位置來進行定位：
坐标系變換
機器人坐标系一般使用機器人模型中的根坐标系(base_link 或 base_footprint)
- 裡程計定位：父級坐标系一般稱之為 odom。
- 傳感器定位：父級參考系一般稱之為 map。
當二者結合使用時，map 和 odom 都是機器人模型根坐标系的父級，這是不符合坐标變換中"單繼承"的原則的，是以，一般會将轉換關系設定為: map -> doom -> base_link 或 base_footprint。
二、導航實作
準備工作（安裝導航包和建立工程包）
安裝ROS下面章節各個導航包：
# 安裝 gmapping 包(用于建構地圖)
sudo apt install ros-melodic-gmapping
# 安裝地圖服務包(用于儲存與讀取地圖)
sudo apt install ros-melodic-map-server
# 安裝 navigation 包(用于定位以及路徑規劃)
sudo apt install 建立新的工程包：
# 進入到工程下的src目錄
cd /home/workspace/roslearn/src
# 建立名為08nav_demo的包 2.1、SLAM建圖
2.1.1、認識gmapping
官網：gmapping
介紹：gmapping 是ROS開源社群中較為常用且比較成熟的SLAM算法之一，gmapping可以根據移動機器人裡程計資料和雷射雷達資料來繪制二維的栅格地圖。
gmapping
包對于硬體的要求：
- 該移動機器人可以釋出裡程計消息。
- 機器人需要釋出雷達消息(該消息可以通過水準固定安裝的雷達釋出，或者也可以将深度相機消息轉換成雷達消息)
gmapping
功能包的核心節點：
slam_gmapping
，需要額外關注的是該節點訂閱的話題、釋出的話題、服務以及相關參數。
訂閱topic
tf (tf/tfMessage)：用于雷達、底盤與裡程計之間的坐标變換消息。
scan(sensor_msgs/LaserScan)：SLAM所需的雷達資訊。
釋出topic
map_metadata(nav_msgs/MapMetaData)：地圖中繼資料，包括地圖的寬度、高度、分辨率等，該消息會固定更新。
map(nav_msgs/OccupancyGrid)：地圖栅格資料，一般會在rviz中以圖形化的方式顯示。
~entropy(std_msgs/Float64)：機器人姿态分布熵估計(值越大，不确定性越大)。
服務service
dynamic_map(nav_msgs/GetMap)：用于擷取地圖資料。
參數（常見參數）
~base_frame(string, default:“base_link”)：機器人基坐标系。
~map_frame(string, default:“map”)：地圖坐标系。
~odom_frame(string, default:“odom”)：裡程計坐标系。
~map_update_interval(float, default: 5.0)：地圖更新頻率，根據指定的值設計更新間隔。
~maxUrange(float, default: 80.0)：雷射探測的最大可用範圍(超出此門檻值，被截斷)。
~maxRange(float)：雷射探測的最大範圍。
所需的坐标變換
雷達坐标系→基坐标系：一般由 robot_state_publisher 或 static_transform_publisher 釋出。
基坐标系→裡程計坐标系：一般由裡程計節點釋出。
釋出的坐标變換
地圖坐标系→裡程計坐标系：地圖到裡程計坐标系之間的變換。
2.1.2、實操
步驟一：編寫slam的launch檔案
nav01_slam.launch
：其中包含了gmapping的一個包對于slam的具體實作以及rviz的配置
<launch>
<param name="use_sim_time" value="true"/>
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<remap from="scan" to="scan"/><!-- 雷達話題 -->
<param name="base_frame" value="base_footprint"/><!--底盤坐标系-->
<param name="odom_frame" value="odom"/> <!--裡程計坐标系-->
<param name="map_update_interval" value="5.0"/>
<param name="maxUrange" value="16.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="srr" value="0.1"/>
<param name="srt" value="0.2"/>
<param name="str" value="0.1"/>
<param name="stt" value="0.2"/>
<param name="linearUpdate" value="1.0"/>
<param name="angularUpdate" value="0.5"/>
<param name="temporalUpdate" value="3.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="30"/>
<param name="xmin" value="-50.0"/>
<param name="ymin" value="-50.0"/>
<param name="xmax" value="50.0"/>
<param name="ymax" value="50.0"/>
<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
</node>
<node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
<node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
<!-- <node pkg="rviz" type="rviz" name="rviz" /> -->
<!-- 可以儲存 rviz 配置并後期直接使用-->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find 08nav_demo)/conf/gmapping.rviz"/>
</launch> - 其中remap表示重命名以前的話題scan為scan。
- 另外兩個就是對應的坐标系名稱。
步驟二：依次啟動Gazebo、slam的launch檔案以及鍵盤控制
# 啟動gazebo(第7章節部分的，複用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch
# 啟動slam及rviz
roslaunch 08nav_demo nav01_slam.launch
# 啟動鍵盤控制 rviz打開之後，來添加一個map元件：指定訂閱主題為
/map
令小車進行自轉來完成建圖工作：
# 向/cmd_vel主題去釋出消息
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.3" 2.2、地圖服務（map_server）
2.2.1、認識map_server
通過使用gmapping的建構地圖并在rviz中顯示了地圖，此時地圖資料還是儲存在記憶體中，此時就有個問題對于建構地圖之後若是服務程式結束，那麼下次就需要重建立立，如何解決呢？此時就涉及到了地圖的持久化存儲。
本質：會将地圖資料進行序列化持久存儲到磁盤中，後期會進行反序列話讀取地圖資料。
此時我們就可以使用ros中的map_server包來進行地圖的持久化存儲！
介紹map_server：map_server功能包中提供了兩個節點: map_saver 和 map_server，前者用于将栅格地圖儲存到磁盤，後者讀取磁盤的栅格地圖并以服務的方式提供出去。
訂閱的topic
map(nav_msgs/OccupancyGrid)
：訂閱此話題用于生成地圖檔案。
2.2.2、實操—儲存地圖
前提說明：需要在SLAM建圖完成之後，我們才可進行一個地圖儲存服務（啟動2.1中的案例後再去執行該地圖儲存服務）。
nav02_map_save.launch
：
<launch>
<arg name="filename" value="$(find 08nav_demo)/map/nav" />
<node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch> 接着我們可以去用鍵盤控制小車移動，等待地圖繪制完成後，我們就可以去執行地圖儲存服務了！
# 運作地圖儲存服務 - nav.pgm：儲存的地圖圖檔。
- nav.yaml：儲存的地圖配置。
效果如下：
2.2.3、實操—讀取地圖
nav02_map_read.launch
·：
<launch>
<!-- 設定地圖的配置檔案 -->
<arg name="map" default="nav.yaml" />
<!-- 運作地圖伺服器，并且加載設定的地圖-->
<node name="map_server" pkg="map_server" type="map_server" args="$(find 08nav_demo)/map/$(arg map)"/>
</launch> 我們可以在首先先啟動地圖服務讀取節點，這樣我們再去運作rviz後，選擇map元件即可看到原先繪制的map地圖了：
# 運作地圖讀取服務 2.3、定位（amcl）
2.3.1、認識amcl
amcl已經被內建到了navigation包。
認識：``AMCL`(adaptive Monte Carlo Localization) 是用于2D移動機器人的機率定位系統，它實作了自适應（或KLD采樣）蒙特卡洛定位方法，可以根據已有地圖使用粒子濾波器推算機器人位置。
訂閱topic
scan(sensor_msgs/LaserScan)：雷射雷達資料。
tf(tf/tfMessage)：坐标變換消息。
initialpose(geometry_msgs/PoseWithCovarianceStamped)：用來初始化粒子濾波器的均值和協方差。
map(nav_msgs/OccupancyGrid)：擷取地圖資料。
釋出的Topic
amcl_pose(geometry_msgs/PoseWithCovarianceStamped)：機器人在地圖中的位姿估計。
particlecloud(geometry_msgs/PoseArray)：位姿估計集合，rviz中可以被 PoseArray 訂閱然後圖形化顯示機器人的位姿估計集合。
tf(tf/tfMessage)：釋出從 odom 到 map 的轉換。
提供的服務service
global_localization(std_srvs/Empty)：初始化全局定位的服務。
request_nomotion_update(std_srvs/Empty)：手動執行更新和釋出更新的粒子的服務。
set_map(nav_msgs/SetMap)：手動設定新地圖和姿态的服務。
調用的服務
static_map(nav_msgs/GetMap)：調用此服務擷取地圖資料。
常見參數
~odom_model_type(string, default:“diff”)：裡程計模型選擇: “diff”,“omni”,“diff-corrected”,“omni-corrected” (diff 差速、omni 全向輪)
~odom_frame_id(string, default:“odom”)：裡程計坐标系。
~base_frame_id(string, default:“base_link”)：機器人極坐标系。
~global_frame_id(string, default:“map”)：地圖坐标系。
2.3.2、坐标變換介紹
裡程計本身也是可以協助機器人定位的，不過裡程計存在累計誤差且一些特殊情況時(車輪打滑)會出現定位錯誤的情況，amcl 則可以通過估算機器人在地圖坐标系下的姿态，再結合裡程計提高定位準确度。
- 裡程計定位:隻是通過裡程計資料實作 /odom_frame 與 /base_frame 之間的坐标變換。
- amcl定位: 可以提供 /map_frame 、/odom_frame 與 /base_frame 之間的坐标變換。
2.3.3、實操
前提說明：需要預先準備好全局地圖配置資訊。
步驟一：編寫amcl的配置檔案
nav03_amcl.launch
：
<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/><!-- 裡程計模式為差分 -->
<param name="odom_alpha5" value="0.1"/>
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/><!-- 裡程計坐标系 -->
<param name="base_frame_id" value="base_footprint"/><!-- 添加機器人基坐标系 -->
<param name="global_frame_id" value="map"/><!-- 添加地圖坐标系 -->
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch> - 重點關注四個有中文注釋的内容需要配置清楚。
步驟二：配置測試launch檔案（內建地圖讀取服務、rviz以及amcl定位）
nav03_amcl_test.launch
·：
<!-- 測試檔案 -->
<launch>
<!-- 加載地圖服務 -->
<include file="$(find 08nav_demo)/launch/nav02_map_read.launch" />
<!-- 啟動rviz -->
<node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
<node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find 08nav_demo)/conf/gmapping.rviz"/>
<!-- amcl檔案 -->
<include file="$(find 08nav_demo)/launch/nav03_amcl.launch" />
</launch> 接着來運作各個服務節點：
# 啟動gazebo(第7章節部分的，複用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch
# 啟動amcl測試檔案（內建map讀取、rviz、amcl）
roslaunch 08nav_demo nav03_amcl_test.launch
# 啟動鍵盤控制 打開rviz後需要添加PoseArray元件，并且訂閱/particlecloud主題：
接着我們控制鍵盤來讓小車進行移動，接着我們來檢視效果：
2.4、路徑規劃（move_base）
2.4.1、認識move_base
move_base已經被內建到了navigation包。
介紹：move_base 功能包提供了基于動作(action)的路徑規劃實作，move_base 可以根據給定的目标點，控制機器人底盤運動至目标位置，并且在運動過程中會連續回報機器人自身的姿态與目标點的狀态資訊。
組成：move_base主要由全局路徑規劃與本地路徑規劃組成。
動作訂閱
move_base/goal(move_base_msgs/MoveBaseActionGoal)：move_base 的運動規劃目标。
move_base/cancel(actionlib_msgs/GoalID)：取消目标。
動作釋出
move_base/feedback(move_base_msgs/MoveBaseActionFeedback)：連續回報的資訊，包含機器人底盤坐标。
move_base/status(actionlib_msgs/GoalStatusArray)：發送到move_base的目标狀态資訊。
move_base/result(move_base_msgs/MoveBaseActionResult)：操作結果(此處為空)。
訂閱topic
move_base_simple/goal(geometry_msgs/PoseStamped)：運動規劃目标(與action相比，沒有連續回報，無法追蹤機器人執行狀态)。
釋出topic
cmd_vel(geometry_msgs/Twist)：輸出到機器人底盤的運動控制消息。
服務
~make_plan(nav_msgs/GetPlan)：請求該服務，可以擷取給定目标的規劃路徑，但是并不執行該路徑規劃。
~clear_unknown_space(std_srvs/Empty)：允許使用者直接清除機器人周圍的未知空間。
~clear_costmaps(std_srvs/Empty)：允許清除代價地圖中的障礙物，可能會導緻機器人與障礙物碰撞，請慎用。
2.4.2、move_base與代價地圖
ROS中的地圖其實就是一張圖檔，這張圖檔有寬度、高度、分辨率等中繼資料，在圖檔中使用灰階值來表示障礙物存在的機率。
不過SLAM建構的地圖在導航中是不可以直接使用的，主要原因如下：
- SLAM建構的地圖是靜态地圖，而導航過程中，障礙物資訊是可變的，可能障礙物被移走了，也可能添加了新的障礙物，導航中需要時時的擷取障礙物資訊。
- 在靠近障礙物邊緣時，雖然此處是空閑區域，但是機器人在進入該區域後可能由于其他一些因素，比如：慣性、或者不規則形體的機器人轉彎時可能會與障礙物産生碰撞，安全起見，最好在地圖的障礙物邊緣設定警戒區，盡量禁止機器人進入…
靜态地圖無法直接應用于導航，其基礎之上需要添加一些輔助資訊的地圖，比如實時擷取的障礙物資料，基于靜态地圖添加的膨脹區等資料。
組成：global_costmap(全局代價地圖) 和 local_costmap(本地代價地圖)
- 前者用于全局路徑規劃，後者用于本地路徑規劃。
兩張代價地圖都可以多層疊加,一般有以下層級:
- Static Map Layer：靜态地圖層，SLAM建構的靜态地圖。
- Obstacle Map Layer：障礙地圖層，傳感器感覺的障礙物資訊。
- Inflation Layer：膨脹層，在以上兩層地圖上進行膨脹（向外擴張），以避免機器人的外殼會撞上障礙物。
- Other Layers：自定義costmap。
碰撞算法：
上圖中，橫軸是距離機器人中心的距離，縱軸是代價地圖中栅格的灰階值。
- 緻命障礙:栅格值為254，此時障礙物與機器人中心重疊，必然發生碰撞；
- 内切障礙:栅格值為253，此時障礙物處于機器人的内切圓内，必然發生碰撞；
- 外切障礙:栅格值為[128,252]，此時障礙物處于其機器人的外切圓内，處于碰撞臨界，不一定發生碰撞；
- 非自由空間:栅格值為(0,127]，此時機器人處于障礙物附近，屬于危險警戒區，進入此區域，将來可能會發生碰撞；
- 自由區域:栅格值為0，此處機器人可以自由通過；
- 未知區域:栅格值為255，還沒探明是否有障礙物。
膨脹空間的設定可以參考非自由空間。
2.4.3、實操—目的地導航
步驟一：編寫move_base配置
nav04_path.launch
：在該launch檔案中包含了多個配置項
<launch>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
<rosparam file="$(find 08nav_demo)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find 08nav_demo)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find 08nav_demo)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find 08nav_demo)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find 08nav_demo)/param/base_local_planner_params.yaml" command="load" />
</node>
</launch> 步驟二：編寫各類配置檔案
base_local_planner_params.yaml
：
TrajectoryPlannerROS:
# Robot Configuration Parameters
max_vel_x: 0.5 # X 方向最大速度
min_vel_x: 0.1 # X 方向最小速速
max_vel_theta: 1.0 #
min_vel_theta: -1.0
min_in_place_vel_theta: 1.0
acc_lim_x: 1.0 # X 加速限制
acc_lim_y: 0.0 # Y 加速限制
acc_lim_theta: 0.6 # 角速度加速限制
# Goal Tolerance Parameters，目标公差
xy_goal_tolerance: 0.10
yaw_goal_tolerance: 0.05
# Differential-drive robot configuration
# 是否是全向移動機器人
holonomic_robot: false
# Forward Simulation Parameters，前進模拟參數
sim_time: 0.8
vx_samples: 18
vtheta_samples: 20
sim_granularity: 0.05
costmap_common_params.yaml
：
#機器人幾何參，如果機器人是圓形，設定 robot_radius,如果是其他形狀設定 footprint
robot_radius: 0.12 #圓形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形狀
obstacle_range: 3.0 # 用于障礙物探測，比如: 值為 3.0，意味着檢測到距離小于 3 米的障礙物時，就會引入代價地圖
raytrace_range: 3.5 # 用于清除障礙物，比如：值為 3.5，意味着清除代價地圖中 3.5 米以外的障礙物
#膨脹半徑，擴充在碰撞區域以外的代價區域，使得機器人規劃路徑避開障礙物
inflation_radius: 0.2
#代價比例系數，越大則代價值越小
cost_scaling_factor: 3.0
#地圖類型
map_type: costmap
#導航包所需要的傳感器
observation_sources: scan
#對傳感器的坐标系和資料進行配置。這個也會用于代價地圖添加和清除障礙物。例如，你可以用雷射雷達傳感器用于在代價地圖添加障礙物，再添加kinect用于導航和清除障礙物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}
global_costmap_params.yaml
：
global_costmap:
global_frame: map #地圖坐标系
robot_base_frame: base_footprint #機器人坐标系
# 以此實作坐标變換
update_frequency: 1.0 #代價地圖更新頻率
publish_frequency: 1.0 #代價地圖的釋出頻率
transform_tolerance: 0.5 #等待坐标變換釋出資訊的逾時時間
static_map: true # 是否使用一個地圖或者地圖伺服器來初始化全局代價地圖，如果不使用靜态地圖，這個參數為false.
local_costmap_params.yaml
：
local_costmap:
global_frame: odom #裡程計坐标系
robot_base_frame: base_footprint #機器人坐标系
update_frequency: 10.0 #代價地圖更新頻率
publish_frequency: 10.0 #代價地圖的釋出頻率
transform_tolerance: 0.5 #等待坐标變換釋出資訊的逾時時間
static_map: false #不需要靜态地圖，可以提升導航效果
rolling_window: true #是否使用動态視窗，預設為false，在靜态的全局地圖中，地圖不會變化
width: 3 # 局部地圖寬度 機關是 m
height: 3 # 局部地圖高度 機關是 m
resolution: 0.05 # 局部地圖分辨率 機關是 m，一般與靜态地圖分辨率保持一緻 步驟三：編寫路徑規劃測試檔案
nav05_path_test.launch
：
<launch>
<!-- 設定地圖的配置檔案 -->
<arg name="map" default="nav.yaml" />
<!-- 運作地圖伺服器，并且加載設定的地圖-->
<node name="map_server" pkg="map_server" type="map_server" args="$(find 08nav_demo)/map/$(arg map)"/>
<!-- slam（重新繪制地圖使用） -->
<!-- <include file="$(find 08nav_demo)/launch/nav01_slam.launch" /> -->
<!-- 啟動AMCL節點 -->
<include file="$(find 08nav_demo)/launch/nav03_amcl.launch" />
<!-- 運作move_base節點 -->
<include file="$(find 08nav_demo)/launch/nav04_path.launch" />
<!-- 運作rviz -->
<node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
<node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find 08nav_demo)/conf/gmapping.rviz"/>
</launch> 此時我們就可以來運作節點了：
# 啟動gazebo(第7章節部分的，複用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch
# 運作路徑規劃測試launch（地圖加載、amcl、movebase、rviz） 接着我們使用2D Nav Goal來指定目标位置來進行移動：
若是我們在行進的過程中放置障礙物，也能夠實作自主避障，到達目的地：
2.4.4、rviz訂閱全局地圖、本地地圖
訂閱全局地圖：我們可以重新在rviz中添加一個map，重命名為map_global，訂閱主題為
/move_base/global_costmap/costmap
訂閱本地map地圖：訂閱主題為
/move_base/global_costmap/costmap
2.5、實踐—自主實作建圖（slam+move_base）
nav06_auto_slam.launch
：
<launch>
<!-- 啟動SLAM節點 -->
<include file="$(find 08nav_demo)/launch/nav01_slam.launch" />
<!-- 運作move_base節點 -->
<include file="$(find 08nav_demo)/launch/nav04_path.launch" />
<!-- 運作rviz（在slim節點中已經有了） -->
<!-- <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
<node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find 08nav_demo)/conf/gmapping.rviz"/> -->
</launch> 接着運作launch檔案來啟動服務：
# 啟動gazebo(第7章節部分的，複用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch
# 僅僅隻包含slam、move_base、rviz節點 在rviz中我們打開了全局地圖、本地地圖，初始還沒有完整建圖的效果如下：
以往我們需要使用的之前的鍵盤來操控小車移動，這裡的話我們就可以直接使用2D Naw Goal來進行手動定位目的地來完成建圖！
最終的效果如下：
等待建圖完成之後，我們就可以去儲存全局地圖了：
# 同樣也是執行之前的全局地圖指令儲存節點 三、導航消息
上面的各個導航服務內建都使用到了消息元件，對應元件就需要使用到相關消息的釋出、訂閱。
3.1、導航—地圖
地圖相關的消息主要有兩個:
1、
nav_msgs/MapMetaData
：地圖中繼資料，包括地圖的寬度、高度、分辨率等。
# 檢視消息體指令
rosmsg info nav_msgs/MapMetaData
# 消息結構如下：
time map_load_time
float32 resolution #地圖分辨率
uint32 width #地圖寬度
uint32 height #地圖高度
geometry_msgs/Pose origin #地圖位姿資料 2、
nav_msgs/OccupancyGrid
：地圖栅格資料，一般會在rviz中以圖形化的方式顯示。
# 檢視消息體指令
rosmsg info nav_msgs/OccupancyGrid
# 消息結構如下：
std_msgs/Header header
uint32 seq
time stamp
string frame_id
#--- 地圖中繼資料
nav_msgs/MapMetaData info
time map_load_time
float32 resolution
uint32 width
uint32 height
geometry_msgs/Pose origin
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
#--- 地圖内容資料，數組長度 = width * height
int8[] 3.2、導航—裡程計
裡程計相關消息是：
nav_msgs/Odometry
# 檢視消息體指令
rosmsg info nav_msgs/Odometry
# 消息結構如下：
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/Pose pose #裡程計位姿
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[36] covariance
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist #速度
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
# 協方差矩陣
float64[36] 3.3、導航—坐标變換
坐标變換相關消息是：
tf/tfMessage
# 檢視消息體指令
rosmsg info tf/tfMessage
# 消息結構如下：
geometry_msgs/TransformStamped[] transforms #包含了多個坐标系相對關系資料的數組
std_msgs/Header header
uint32 seq
time 3.4、導航—定位
定位相關消息是：
geometry_msgs/PoseArray
# 檢視消息體指令
rosmsg info geometry_msgs/PoseArray
# 消息結構如下：
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose[] poses #預估的點位姿組成的數組 3.4、導航—目标點與路徑規劃
目标點相關消息是：
move_base_msgs/MoveBaseActionGoal
# 檢視消息體指令
rosmsg info move_base_msgs/MoveBaseActionGoal
# 消息結構如下：
std_msgs/Header header
uint32 seq
time stamp
string frame_id
actionlib_msgs/GoalID goal_id
time stamp
string id
move_base_msgs/MoveBaseGoal goal
geometry_msgs/PoseStamped target_pose
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose #目标點位姿 路徑規劃相關消息是：nav_msgs/Path
# 檢視消息體指令
rosmsg info nav_msgs/Path
# 消息結構如下：
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/PoseStamped[] poses #由一系列點組成的數組
std_msgs/Header header
uint32 seq
time 3.5、導航—雷射雷達
雷射雷達相關消息是：
sensor_msgs/LaserScan
# 檢視消息體指令
rosmsg info sensor_msgs/LaserScan
# 消息結構如下：
std_msgs/Header header
uint32 seq
time stamp
string frame_id
float32 angle_min #起始掃描角度(rad)
float32 angle_max #終止掃描角度(rad)
float32 angle_increment #測量值之間的角距離(rad)
float32 time_increment #測量間隔時間(s)
float32 scan_time #掃描間隔時間(s)
float32 range_min #最小有效距離值(m)
float32 range_max #最大有效距離值(m)
float32[] ranges #一個周期的掃描資料
float32[] intensities #掃描強度資料，如果裝置不支援強度資料，該數組為空 3.6、導航—相機
對應的一般的圖像資料：
sensor_msgs/Image
# 檢視消息體指令
rosmsg info sensor_msgs/Image
# 消息結構如下：
std_msgs/Header header
uint32 seq
time stamp
string frame_id
uint32 height #高度
uint32 width #寬度
string encoding #編碼格式:RGB、YUV等
uint8 is_bigendian #圖像大小端存儲模式
uint32 step #一行圖像資料的位元組數，作為步進參數
uint8[] data #圖像資料，長度等于 step * height 對應壓縮後的圖像資料：
sensor_msgs/CompressedImage
# 檢視消息體指令
rosmsg info sensor_msgs/CompressedImage
# 消息結構如下：
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string format #壓縮編碼格式(jpeg、png、bmp)
uint8[] data #壓縮後的資料 點雲資料(帶有深度資訊的圖像資料)：
sensor_msgs/PointCloud2
# 檢視消息體指令
rosmsg info sensor_msgs/PointCloud2
# 消息結構如下：
std_msgs/Header header
uint32 seq
time stamp
string frame_id
uint32 height #高度
uint32 width #寬度
sensor_msgs/PointField[] fields #每個點的資料類型
uint8 INT8=1
uint8 UINT8=2
uint8 INT16=3
uint8 UINT16=4
uint8 INT32=5
uint8 UINT32=6
uint8 FLOAT32=7
uint8 FLOAT64=8
string name
uint32 offset
uint8 datatype
uint32 count
bool is_bigendian #圖像大小端存儲模式
uint32 point_step #單點的資料位元組步長
uint32 row_step #一行資料的位元組步長
uint8[] data #存儲點雲的數組，總長度為 row_step * height
bool is_dense #是否有無效點 3.7、深度圖像轉雷射資料
3.7.1、認識depthimage_to_laserscan
ros功能包：depthimage_to_laserscan，該功能包可以将深度圖像資訊轉換成雷射雷達資訊。
應用場景：在諸多SLAM算法中，一般都需要訂閱雷射雷達資料用于建構地圖，因為雷射雷達可以感覺周圍環境的深度資訊，而深度相機也具備感覺深度資訊的功能，且最初雷射雷達價格比價比較昂貴，實際上傳感器選型可以使用深度相機來去替代雷射雷達。
原理：depthimage_to_laserscan将實作深度圖像與雷達資料轉換的原理比較簡單，雷達資料是二維的、平面的，深度圖像是三維的，是若幹二維(水準)資料的縱向疊加，如果将三維的資料轉換成二維資料，隻需要取深度圖的某一層即可，為了方面了解。
優缺點：
- 優點：深度相機的成本一般低于雷射雷達，可以降低硬體成本；
- 缺點：深度相機較之于雷射雷達無論是檢測範圍還是精度都有不小的差距，SLAM效果可能不如雷射雷達理想。
訂閱topic
image(sensor_msgs/Image)：輸入圖像資訊。
camera_info(sensor_msgs/CameraInfo)：關聯圖像的相機資訊。通常不需要重新映射，因為camera_info将從與image相同的命名空間中進行訂閱。
釋出topic
scan(sensor_msgs/LaserScan)：釋出轉換成的雷射雷達類型資料。
參數
該節點參數較少，隻有如下幾個，一般需要設定的是: output_frame_id。
~scan_height(int, default: 1 pixel)：設定用于生成雷射雷達資訊的象素行數。
~scan_time(double, default: 1/30.0Hz (0.033s))：兩次掃描的時間間隔。
~range_min(double, default: 0.45m)：傳回的最小範圍。結合range_max使用，隻會擷取 range_min 與 range_max 之間的資料。
~range_max(double, default: 10.0m)：傳回的最大範圍。結合range_min使用，隻會擷取 range_min 與 range_max 之間的資料。
~output_frame_id(str, default: camera_depth_frame)：雷射資訊的ID。
3.7.2、實操
# 安裝depthimage-to-laserscan
sudo apt-get install
nav07_depthimage_to_laserscan.launch
：編寫launch檔案執行，将深度資訊轉換成雷達資訊
<launch>
<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
<remap from="image" to="/camera/depth/image_raw" />
<param name="output_frame_id" value="camera" />
</node>
</launch> 接着我們來啟動各個launch檔案：
# 啟動gazebo(第7章節部分的，複用之前的)
roslaunch 07urdf_gazebo demo03_evn.launch
# 深度圖像轉雷射資料
roslaunch 08nav_demo nav07_depthimage_to_laserscan.launch
# 運作路徑規劃測試launch（地圖加載、amcl、movebase、rviz） 在rviz中添加一個雷達掃描：