---
tags: 交接, VSCLab, ROS, Navigation
---
ROS Navigation Stack 研究
==
<!-- Style -->
<style>
.ph_center {
display : block;
margin-left : auto ;
margin-right: auto;
}
</style>
<br>
---
## :warning: 疑慮
+ 模擬環境會提供，但還不知道實際運行在 youBot 上的時候 ```/odom``` 怎麼來。查了一下發現 [youBot_wrapper](http://wiki.ros.org/youbot_common#Published_Topics) 會發布，但因為是剛出廠時官方給的 package(?)，所以不確定能不能用
- 基於事前建好的圖跑的導航，如果途中遇到新增的障礙物或是會移動的障礙物又要怎麼辦？
+ 實際執行起來也可以體會，但 SLAM 標榜的不是「同時建立地圖與定位」嗎？
- 目前都是用 ROS 預設的方法：gmapping + amcl + move_base，來解決的，但也要考慮之後自定義要怎麼做
- 建圖還有 hector 和 cartographer，cartographer 的強健性比 gmapping 來得更好，而且實作上似乎沒有比較麻煩，之後要來試試
- 定位上比較少看到特別介紹其他方法的，如果表現不是很差，參數的影響大概也不大，但也是可以看看
- 導航的部份可以改的比較多，除了 planner 演算法之外，對於各類區域的定義也可以下手
- 如果要導入 RL 的概念，整體架構可能會改變很多就是了。可能是完全取代部份步驟，或是插入途中
---
<br>
# Overview
**STEP 1 | 建立地圖**
+ 利用 gmapping、cartographer 等演算法建立、並儲存地圖資訊
+ 透過 ROS ```map_server``` 讀取並發布以供後續使用
**STEP 2 | 機器人定位**
+ 目前看到的資料基本上都是用 ```amcl```
+ 此階段的檔案會在下一階段繼續沿用
**STEP 3 | 同時定位與導航**
+ 繼承上一階段的結果，同時執行定位、路徑規劃與導航，在 ROS 中主要由 ```move_base``` 執行
+ 在路徑規劃（global / local）上可以以不同演算法替換，而 ROS 與 turtlebot3 的預設範例都是 DWA 演算法
**要點注意**
1. 每個步驟都需要建立 ```/tf```，而且雖然現在都用別人建好的模型所以沒遇到，但很多資料都指出，```/tf``` 常常是出錯的關鍵
2. 都是要用 launch 檔串起來的
# Navigation Stack 架構
這裡以 [ROS Wiki 的說明](http://wiki.ros.org/navigation/Tutorials/RobotSetu)為主軸來探討，主要可分為以下幾個部分：
+ [Robot Configuration](#01--Robot-Configuration)
+ [Costmap Configuration](#02--Costmap-Configuration)
+ [Base Local Planner Configuration](#03--Base-Local-Planner-Configuration)
## 01 | Robot Configuration
用來定義一些與機器人結構有關的部分，ROS 的範例中包含以下幾項：
+ 感測器：基本上就是 LaserScan 或 PointCloud
+ 機器人與各外加部件之間的 tf 架構
+ base 的里程計定義
想要順利執行 ROS Navigation 功能，這些都是缺一不可的，但事實上，在找到的範例程式中很少有真的像 [ROS 官方範例](http://wiki.ros.org/navigation/Tutorials/RobotSetup#Creating_a_Robot_Configuration_Launch_File)，特別包成單一個 launch 檔來處理的。
## 02 | Costmap Configuration
Costmap 將整個移動型機器人的工作空間轉換為離散方格圖（可以用 cell、pixel、observation 等等的方式稱呼），並考量到機器人本身形狀、移動方式與障礙物狀態等各種元素，進而評估各方格的分數，作為路徑規劃的參考。
Costmap 分為全域（gloabal）與局部（local）兩種，前著綜觀完整的工作環境，用於長時間、長路徑的規劃，主要目標是要抵達終點；後者則著眼於機器人周遭一定範圍的區域，主要用意在於當下的避障。
在 ROS Navigation Stack 中將這部分的參數定義分成三種：全域（global）、局部（local）和共通（common），這裡用來研究的範例檔案如下：
+ [ROS 官方範例](http://wiki.ros.org/navigation/Tutorials/RobotSetup#Costmap_Configuration_.28local_costmap.29_.26_.28global_costmap.29)
- costmap_common_params.yaml
- local_costmap_params.yaml
- global_costmap_params.yaml
+ turtlebot3_navigation
+ [costmap_common_params_burger.yaml](https://github.com/ROBOTIS-GIT/turtlebot3/blob/master/turtlebot3_navigation/param/costmap_common_params_burger.yaml)
+ [global_costmap_params.yaml](https://github.com/ROBOTIS-GIT/turtlebot3/blob/master/turtlebot3_navigation/param/global_costmap_params.yaml)
+ [local_costmap_params.yaml](https://github.com/ROBOTIS-GIT/turtlebot3/blob/master/turtlebot3_navigation/param/local_costmap_params.yaml)
接下來則就這些範例中的參數進行介紹，而在開始之前，有一些定義得先知道：
+ [官方的 costmap_2d 文件](http://wiki.ros.org/costmap_2d)
+ [CSDN | ROS導航包中的 costmap 的一些參數理解](https://blog.csdn.net/jinking01/article/details/104193666)
+ [Robotics Knowledgebase | Cost Maps in ROS](https://roboticsknowledgebase.com/wiki/state-estimation/ros-cost-maps/)
### 02-1 | Common Configuration
global 和 local 的 costmap 共通的參數，完整詳細的項目和定義可以在 [costmap_2d 官方文件](http://wiki.ros.org/costmap_2d#Component_API)中的「8.2 Layer Specifications」去查詢。
```
obstacle_range: 2.5
raytrace_range: 3.0
```
+ 這兩個參數主要與 occupied-space 和 free-space 的偵測距離有關
+ 第一行表示，如果目標 observation source 被設為「marking」，且距離 base（更準確的說是 sensor frame）小於 2.5 公尺，則該 observation 將被判定為「lethal obstacle」
+ 第二行則是在說，如果目標 observation source 被認定將要「clearing」，且距離 base 小於 3.0 公尺，則該 observation 與 sensor_frame 之間所有 cell 都將被判定為「free」
```
footprint: [[x0, y0], [x1, y1], ... [xn, yn]]
```
+ 以機器人中心為 (0, 0) 而得到的機體各端點座標，必須為一閉集合，此例表示一圓形機器人，而 ```[[0.1, 0.1], [0.1, -0.1], [-0.1, -0.1], [-0.1, 0.1]]``` 則表示一台邊長為 0.2 公尺的正方形機器人
```
cost_scaling_factor: 3.0
```
+ 用來計算 inflation layer cost 遞減狀況的常數
```
map_type: costmpa
```
+ "costmap" 或 "voxel"
```
observation_sources: laser_scan_sensor point_cloud_sensor
laser_scan_sensor: {sensor_frame: frame_name, data_type: LaserScan, topic: topic_name, marking: true, clearing: true}
point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}
```
+ ```observation_sources``` 列出一連串以空白鍵分開的感測器名稱，可以自訂，提供用於 costmap 的資訊
+ 而接下來的幾行則是實際去定義這些感測器：
+ frame_name：該感測器所被賦予的 cordinate frame 名稱
+ data_type：LaserScan 或 PointCloud
+ topic_name：所發布的目標 topic
+ marking 和 clearing：表示該感測器被允許執行的功能
### 02-2 | Global Configuration (global_costmap)
專屬於 global costmap 的參數定義，這裡列出官方範例：
```
global_costmap:
global_frame: /map
robot_base_frame: base_link
update_frequency: 5.0
static_map: true
```
+ 一樣在這裡列出一些參數說明：
```
transform_tolerance: 0.5
```
+ 定義 transform(tf) 可容忍的延遲秒數，是用來應對短時間 ```tf``` 資訊丟失的保護機制
+ 若 ```global_frame``` 和 ```robot_base_frame``` （皆定義在 global costmap 參數中）之間的 tf 轉換延遲超過 ```transform_tolerance``` 所定義時長，則 navigation stack 將中止該機器人的行為
<br>
```
static_map: true
```
+ 表示 costmap 的初始化是基於 ```/map_server``` 提供的資訊來定義的
### 02-3 | Local Configuration (local_costmap)
專屬於 local costmap 的參數定義
```
local_costmap:
global_frame: odom
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 2.0
static_map: false
rolling_window: true
width: 6.0
height: 6.0
resolution: 0.05
```
+ 部份參數定義：
```
publish_frequency: 2.0
```
+ 發布影像資訊的頻率，單位為 Hz
<br>
```
rolling_window: true
```
+ costmap 會持續移動以保持機器人於地圖中心
<br>
```
resolution: 0.05
```
+ 地圖解析度，建議與 static map 相同，但若有需要也可以設為不同值
## 03 | Base Local Planner Configuration
:::info
在 Groovy 之後 ROS 又提供了 ```dwa_local_planner``` 套件，以讓這部份的架構更加模組化
:::
在規劃路徑時，planner 會根據地圖資訊產生一張 grid map，依演算法特性，可能會有不同的 local 範圍，而其中每一格都會有根據 value function 所算出來的 cost 值（應該就是像 A* 或 DWA 那樣的鋪法）。而控制器再根據這些資訊決定要給 base 的速度命令（dx, dy, dtheta）
ROS 官方以 DWA 演算法 (Dynamic Window Approach) 為例，流程簡述如下：
<br>
<img src=https://i.imgur.com/NUWsVA0.png class=ph_center width=70%>
<br><br>
1. 以離散的方式對機器人的控制空間取樣（dx, dy, dtheta）
2. 對每個取樣速度，執行未來一小段時間的預測模擬，並評估其中可行軌跡的優劣
3. 取最佳結果並執行對應速度命令
**03-1 | Map Grid**
+ 範圍與 local costmp 相當，用於評算軌跡分數
+ 採用曼哈頓距離計算，也意味著所採路徑必須依照棋盤格的方式走，所以才會有研究引用其他可以走在棋盤格中的演算法，以縮短路徑長
+ 由於 global goal 在大多數的時候都坐落於 local window 之外，因此會以一個大致定位出來的 local goal 來替代。這表示 window 範圍內的 path 起始點會延伸一段連續點至範圍外，而這延伸的範圍則由 ```move_base``` 決定
此外還是有一些參數的定義或是函式的範例，如果要代換演算法的話可能會派上用場，但這裡就先不贅述，詳情可以繼續到 [base_local_planner 的官方說明](http://wiki.ros.org/base_local_planner)去看看：
+ [Turtlebot3 | dwa_local_planner_params_burger.yaml](https://github.com/ROBOTIS-GIT/turtlebot3/blob/master/turtlebot3_navigation/param/dwa_local_planner_params_burger.yaml)
+ [ROS Wiki | Base Local Planner Configuration](http://wiki.ros.org/navigation/Tutorials/RobotSetup#Base_Local_Planner_Configuration)
---
**曼哈頓距離 (Manhattan distance or Manhattan length)**
:::spoiler
<br>
又稱為計程車幾何 (Taxicab geometry)，正式意義為「L1-距離」或「城市區塊距離」，也就是在歐幾里得空間的固定直角坐標系上兩點所形成的線段對軸產生的投影的距離總和。
\begin{equation}
d(x,y) = \left| x_1-x_2 \right| + \left| y_1-y_2 \right|
\end{equation}
如下圖，綠線表示其歐幾里德距離約為 8.48（= 6 $\times$ √2），而紅、藍、黃線則都有一樣的曼哈頓距離（= 12）
<img src=https://i.imgur.com/o2ASMfh.png class=ph_center width=40%>
> 參考資料：[維基百科](https://zh.wikipedia.org/zh-tw/%E6%9B%BC%E5%93%88%E9%A0%93%E8%B7%9D%E9%9B%A2)
:::
---
### 04 | move_base configuration
這部份在 ROS Navigation 文件和 lzrobot 中沒有特別提到，但在 turtlebot3 裡是有特別針對這部份撰寫的 yaml 檔的：
```yaml
shutdown_costmaps: false
controller_frequency: 10.0
planner_patience: 5.0
controller_patience: 15.0
conservative_reset_dist: 3.0
planner_frequency: 5.0
oscillation_timeout: 10.0
oscillation_distance: 0.2
```
還沒用到所以先略過，同樣地，在[官方文件](http://wiki.ros.org/move_base)裡可以找到這些參數的定義。
### 05 | 完成 Navigation Stack
就跟前面說過的一樣，用 launch 檔將各環節串連起來，官方範例可以看[這裡](http://wiki.ros.org/navigation/Tutorials/RobotSetup#Creating_a_Launch_File_for_the_Navigation_Stack)，而這裡則以 turtlebot 程式作為主要研究對象：
+ 定義變數 / 參數
```xml
<launch>
<!-- Arguments -->
<arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
<arg name="map_file" default="$(find turtlebot3_navigation)/maps/map.yaml"/>
<arg name="open_rviz" default="true"/>
<arg name="move_forward_only" default="false"/>
```
+ 開啟 turtlebot3 架構等相關資訊
```xml
<!-- Turtlebot3 -->
<include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
<arg name="model" value="$(arg model)" />
</include>
```
+ 開啟 map_server
```xml
<!-- Map server -->
<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>
```
+ 開啟 amcl
```xml
<!-- AMCL -->
<include file="$(find turtlebot3_navigation)/launch/amcl.launch"/>
```
> amcl.launch
> ::: spoiler
> ```xml
> <launch>
> <!-- Arguments -->
> <arg name="scan_topic" default="scan"/>
> <arg name="initial_pose_x" default="0.0"/>
> <arg name="initial_pose_y" default="0.0"/>
> <arg name="initial_pose_a" default="0.0"/>
>
> <!-- AMCL -->
> <node pkg="amcl" type="amcl" name="amcl">
>
> <param name="min_particles" value="500"/>
> <param name="max_particles" value="3000"/>
> <param name="kld_err" value="0.02"/>
> <param name="update_min_d" value="0.20"/>
> <param name="update_min_a" value="0.20"/>
> <param name="resample_interval" value="1"/>
> <param name="transform_tolerance" value="0.5"/>
> <param name="recovery_alpha_slow" value="0.00"/>
> <param name="recovery_alpha_fast" value="0.00"/>
> <param name="initial_pose_x" value="$(arg initial_pose_x)"/>
> <param name="initial_pose_y" value="$(arg initial_pose_y)"/>
> <param name="initial_pose_a" value="$(arg initial_pose_a)"/>
> <param name="gui_publish_rate" value="50.0"/>
>
> <remap from="scan" to="$(arg scan_topic)"/>
> <param name="laser_max_range" value="3.5"/>
> <param name="laser_max_beams" value="180"/>
> <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_likelihood_max_dist" value="2.0"/>
> <param name="laser_model_type" value="likelihood_field"/>
>
> <param name="odom_model_type" value="diff"/>
> <param name="odom_alpha1" value="0.1"/>
> <param name="odom_alpha2" value="0.1"/>
> <param name="odom_alpha3" value="0.1"/>
> <param name="odom_alpha4" value="0.1"/>
> <param name="odom_frame_id" value="odom"/>
> <param name="base_frame_id" value="base_footprint"/>
>
> </node>
> </launch>
> ```
> :::
+ 開啟 move_base
```xml
<!-- move_base -->
<include file="$(find turtlebot3_navigation)/launch/move_base.launch">
<arg name="model" value="$(arg model)" />
<arg name="move_forward_only" value="$(arg move_forward_only)"/>
</include>
```
> move_base.launch
> ::: spoiler
> ```xml
> <launch>
> <!-- Arguments -->
> <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
> <arg name="cmd_vel_topic" default="/cmd_vel" />
> <arg name="odom_topic" default="odom" />
> <arg name="move_forward_only" default="false"/>
>
> <!-- move_base -->
> <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
> <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
> <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
> <rosparam file="$(find turtlebot3_navigation)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
> <rosparam file="$(find turtlebot3_navigation)/param/local_costmap_params.yaml" command="load" />
> <rosparam file="$(find turtlebot3_navigation)/param/global_costmap_params.yaml" command="load" />
> <rosparam file="$(find turtlebot3_navigation)/param/move_base_params.yaml" command="load" />
> <rosparam file="$(find turtlebot3_navigation)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
> <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
> <remap from="odom" to="$(arg odom_topic)"/>
> <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
> </node>
> </launch>
> ```
> :::
+ 開啟 rviz
```xml
<!-- rviz -->
<group if="$(arg open_rviz)">
<node pkg="rviz" type="rviz" name="rviz" required="true"
args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
</group>
</launch>
```
---
**\<param> vs. \<rosparam>**
:::spoiler
<br>
在這裡主要分為：
+ 讀取 yaml 檔時使用 ```<rosparam file="... .yaml" command="load"/>```
+ 其餘參數定義使用 ```<param name="..." value="..." />```
而不考慮範例，根據官方說明（[param](http://www.ros.org/wiki/roslaunch/XML/param) / [rosparam](https://wiki.ros.org/roslaunch/XML/rosparam)）以及[這篇 ROS Answers](https://answers.ros.org/question/37916/when-to-use-param-and-rosparam-on-launch-file/)，可以得到兩者特點分別如下：
+ **\<param>** 大多用來讀取「單一個」參數值，可以讀取的格式主要有：str、int、double 、bool 和 txt 檔
+ **\<rosparam>** 大多用來讀取「一整個群組的」參數值，通常會透過載入一個「yaml」檔案的方式來執行，格式規定如[此處](https://wiki.ros.org/rosparam#YAML_Format)所示
:::
---
## 實作
+ 用了 ROS navigation 的 base_local_planner 參數，雖然可以一邊定位一邊導航，但果不其然成功率並不好，很容易卡死在障礙物旁
- 實行步驟如下：
1. 開啟 gazebo 模型
2. 執行 ros1_navigation.launch
3. 利用 rviz 的「2D Nav Goal」發布 ```/goal```
4. 隨時透過 ```rosrun teleop_twist_keyboard teleop_twist_keyboard.py``` 微調機器人位置，以脫離卡死障礙物的困境
+ 開啟以下 rviz 顯示物件，就可以呈現如右下的圖片
* Map「Add > by Topic > /map」：先前建好的完整全域地圖
* PoseWithCovariance「Add > by Topic > /amcl_pose」：
顯示 amcl 演算法得到的機器人所在位置與朝向
* Pose「Add > by Topic > /goal」：顯示給定目標位置與朝向
* Path「Add > by Topic > global planner/...」：顯示全域規劃完整路徑
* Path「Add > by Topic > local planner/...」：顯示居部規劃短期路徑
* Map「Add > by Topic > global costmap」：顯示 global costmap
* Map「Add > by Topic > local costmap」：顯示 local costmap
---
**Reference**
+ [ROS Answers | obstacle_range & raytrace_range - precise explanation?](https://answers.ros.org/question/72265/obstacle_range-raytrace_range-precise-explanation/)
+ [Navigation 2 文件 | Costmap 2D](https://navigation.ros.org/configuration/packages/configuring-costmaps.html#costmap2d-ros-parameters)
+ [ROS | costmap_2d 官方文件](http://wiki.ros.org/costmap_2d#Component_API)
+ [ROS Wiki | costmap_2d/flat](http://wiki.ros.org/costmap_2d/flat)
+ [costmap_2d C++ API](https://docs.ros.org/en/api/costmap_2d/html/classcostmap__2d_1_1Costmap2D.html)
+ [ROS | base_local_planner 官方文件](http://wiki.ros.org/base_local_planner)
+ [ROS | dwa_local_planner 官方文件](http://wiki.ros.org/dwa_local_planner)
---
# Costmap Layer 定義
## 01 | Inflation Layer
**Inflation layer** 定義了自 occupied cell 向外遞減的 cost 值，其中又細分為五種類別，由大到小排序如下：
1. **Lethal cost:** 此 cell 為障礙物本身，若機器人的中心所在 cell 的 cost 值等於此值，則表示發生碰撞。通常定義為 254
2. **Inscribed cost:** 若機器人距離障礙物小於所定義的 inscribed radius，亦即機器人中心 cell 所在位置的 cost 值在該值以上，則表示發生碰撞。通常定義為 253
3. **Possibly circumscribed cost:** 若機器人中心 cell 所在位置的 cost 值大於或等於此值，則根據機器人方向決定是否發生碰撞。可以用來定義非障礙物，但不希望機器人進入的區域。與前兩項相比，並非必要的定義，定義上也更為彈性
4. **Freespace cost:** 什麼都沒有，可以任意前進，通常定義為 0
5. **Unknown cost:** 無法或還沒有得到任何資訊區域
**參數**
+ ```inflation_radius``` (double, default: 0.55)
- ```cost_scaling_factor``` (double, default: 10.0)
用於計算 inflation 區間 cost 值的 scaling factor 值，計算式如下：
```
exp(-1.0 x cost_scaling_factor x (distance_from_obstacle - inscribed_radius))
x (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)
```
其中 ```costmap_2d::INSCRIBED_INFLATED_OBSTACLE ``` = 254。由於 ```cost_scaling_factor``` 前乘上 -1，因此 inflation cost 值會隨著距離增加而遞減
---
**Reference**
+ [ROS Wiki | costmap_2d/Inflation Layer](http://wiki.ros.org/costmap_2d/hydro/inflation)
+ [Learn LaTeX in 30 minutes](https://www.overleaf.com/learn/latex/Learn_LaTeX_in_30_minutes#Abstracts)
---
+ 與行人有關：http://wiki.ros.org/social_navigation_layers
> https://github.com/r06921017/lzrobot/tree/master/launch
Sign in with Wallet
Connect another wallet