基於ros2 dashing的建圖導航探索

1、 環境準備

安裝ros2 dashing, 參考連結：

https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Debians/

安裝gazebo

$ sudo apt remove gazebo11 libgazebo11-dev
$ sudo apt install gazebo9 libgazebo9-dev
$ sudo apt install ros-dashing-gazebo-ros-pkgs

安裝cartography:

$ sudo apt install ros-dashing-cartographer
$ sudo apt install ros-dashing-cartographer-ros

安裝navigation：

$ sudo apt install ros-dashing-navigation2
$ sudo apt install ros-dashing-nav2-bringup

安裝vcstool：

$ sudo apt install python3-vcs

安裝turtlebot3包：

使用apt方式：
$ source /opt/ros/dashing/setup.bash
$ sudo apt install ros-dashing-dynamixel-sdk
$ sudo apt install ros-dashing-turtlebot3-msgs
$ sudo apt install ros-dashing-turtlebot3tool

或者使用原始碼方式：

$ sudo apt remove ros-dashing-turtlebot3-msgs
$ sudo apt remove ros-dashing-turtlebot3
$ mkdir -p ~/turtlebot3_ws/src
$ cd ~/turtlebot3_ws/src/
$ git clone -b dashing-devel https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
$ git clone -b dashing-devel https://github.com/ROBOTIS-GIT/turtlebot3.git
$ colcon build --symlink-install
$ source ~/.bashrc

下載gazebo的模型，加速執行gazebo

cd ~/.gazebo/
git clone https://github.com/osrf/gazebo_models models

需要刪除.git目錄，要不執行會出錯

rm -rf models/.git

設定GAZEBO_MODEL_PATH變數, 指定機器人型別為burger

$ echo 'export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:~/tb3_ws/src/turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/models' >> ~/.bashrc  #變更為對應的turtlebot3模型地址
$ echo 'export TURTLEBOT3_MODEL=waffle' >> ~/.bashrc
$ source ~/.bashrc

設定其他環境變數：

$ echo 'source /opt/ros/dashing/setup.bash' >> ~/.bashrc
$ echo 'source ~/turtlebot3_ws/install/setup.bash' >> ~/.bashrc
$ echo 'export ROS_DOMAIN_ID=30 #TURTLEBOT3' >> ~/.bashrc
$ source ~/.bashrc

2、使用cartography建圖相關探索

2.1 建圖流程

開啟模擬世界launch檔案：

$ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

開啟建圖launch檔案：

$ ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True

開啟小車運動控制節點：

$ ros2 run turtlebot3_teleop teleop_keyboard

控制小車移動，直到在rviz裡看到完整的地圖，儲存地圖：

$ ros2 run nav2_map_server map_saver -f ~/map

鐳射資料話題重定向問題：

開啟建圖的launch檔案後，理論上應該會看到小車當前鐳射雷達掃描的狀態，但實際上rviz裡啥也沒有，並且有報錯說frame [map] is not exist.

經過定位問題發現，話題/scan並沒有釋出資料，故而無法生成map, 而後發現該問題是由於模擬器中，雷達資料以/turtlebot3_laserscan/out話題釋出， 需要做個話題remapping,將資料對映到/scan話題中。

解決方法： 參考連結 https://github.com/ros-simulation/gazebo_ros_pkgs/issues/991 中issue所述，問題已經在ros2 e及f版本中解決，在d版本中，開啟檔案：turtlebot3/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf，在160行左右，將檔案改為如下後，在/scan下可正常獲得資料：

<plugin name="turtlebot3_laserscan" filename="libgazebo_ros_ray_sensor.so">
      <ros>
        <!-- <namespace>/tb3</namespace> -->
        <argument>--ros-args</argument>
    <argument>-r</argument>
        <argument>~/out:=scan</argument>
      </ros>
      <output_type>sensor_msgs/LaserScan</output_type>
      <frame_name>base_scan</frame_name>
    </plugin>

2.2 cartography的組態檔

組態檔名稱及路徑：

turtlebot3/turtlebot3/turtlebot3_cartographer/config/turtlebot3_lds_2d.lua

相關設定解讀：

1 map_frame:
用來發布子圖，是poses的父幀，通常是「map」。
和odom最開始的時候是一個原點，但時間累計對產生累積誤差。
2 tracking_frame:
由SLAM演演算法跟蹤的座標系。如果使用IMU，儘管它可能是旋轉的，它也應該在其位置。
一個常見的選擇是「imu_link」。
3 published_frame:
這個frame是用來正在釋出poses的子幀，和map_frame對應。
一般就將其設定為"base_link",這不就是"map->base_link"。
找座標能找到了不！！！
4 odom_frame:
僅在provide_odom_frame為true時使用。
published_frame 和 map_frame之間的框架，用於釋出（非迴圈關閉）local SLAM結果。
預設是「odom」。
5 provide_odom_frame：
如果啟用，則local-slam估計的連續的姿態（不包括迴環）將作為map_frame中 odom_frame釋出。
6 publish_frame_projected_to_2d：
如果啟用，則已釋出的pose將限制為純2D姿勢（無捲動，俯仰或z偏移）。
這可以防止在2D模式中由於"pose extrapolation
step"而發生的"out-of-plane"（如果姿勢應該作為類似「base-footprint」釋出）。
7 use_odommetry:
如果啟用，請在主題「odom」上訂閱nav_msgs/Odometry。
在這種情況下必須提供"Odommetry"，並且資訊將包含在SLAM中。
8 use_nav_sat：
    如果啟用，請在主題「fix」上訂閱sensor_msgs / NavSatFix。 在這種情況下必須提供Navigation data，並且資訊將包含在全域性SLAM中。

9 use_landmarks：
如果啟用，請在主題「Landmarks」上訂閱cartographer_ros_msgs / LandmarkList。
在這種情況下必須提供Landmarks，並且資訊將包含在SLAM中。
10 num_laser_scans：
訂閱的鐳射掃描主題數量。
在一個鐳射掃描器的「掃描」主題上訂閱sensor_msgs /LaserScan
或在多個鐳射掃描器上訂閱主題「scan_1」，「scan_2」等。
11 num_subdivisions_per_laser_scan：
將每個接收到的（multi-echo）鐳射掃描分割成的點雲數。
細分掃描可以在掃描器移動時取消掃描獲取的掃描。
有一個相應的軌跡構建器選項可將細分掃描累積到"用於scan_matching的點雲"中。
12 num_multi_echo_laser_scans：
訂閱的multi-echo鐳射掃描主題的數量。
在一個鐳射掃描器的「echoes」主題上訂閱sensor_msgs / MultiEchoLaserScan，
或者為多個鐳射掃描器訂閱主題「echoes_1」，「echoes_2」等。
時間間隔設定引數：
13 lookup_transform_timeout_sec：
使用tf2查詢變換的超時時間
14 submap_publish_period_sec：
釋出子圖的時間間隔，單位是秒
15 pose_publish_period_sec：
釋出pose的時間間隔，比如：5e-3頻率是200Hz
16 trajectory_publish_period_sec：
以秒為單位釋出軌跡標記的間隔，例如， 30e-3持續30毫秒。

其中，不使用imu資料參與建圖，可將use_imu_data設為false：

TRAJECTORY_BUILDER_2D.use_imu_data = false

其中，不使用odom資料參與建圖，可將use_odometry設為false,並將published_frame從odom修改為base_link或base_footprint

  use_odometry = false
  published_frame = "base_link"  #否則tf轉換會出現問題，導致無法建圖，參考： https://answers.ros.org/question/311263/odometry-with-cartographer/

3. navigation導航相關探索

3.1 導航流程

參考連結可正常執行：

https://github.com/mlherd/navigation2/tree/new-tutorials/doc/tb3_tutorial

啟動gazebo:

ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

啟動navigation:

ros2 launcturtlebot3_navigation2 navigation2.launch.py use_sim_time:=True  map:='/opt/ros/dashing/share/nav2_bringup/launch/turtlebot3_world.yaml' # map需改為對應的檔案及路徑

啟動rviz：

 ros2 run rviz2 rviz2 -d /opt/ros/dashing/share/nav2_bringup/launch/nav2_default_view.rviz

在rviz點選左下角startup啟動按鈕：

在rviz點選啟動設定初始位姿按鈕2D Pose Estimate，並設定初始位姿：

在rviz點選啟動設定初始位姿按鈕Navigation2 Goal，設定目標點位姿：

此時可見生成了一條運動軌跡，並在gazebo裡能看到小車按照軌跡在運動：

遇到的問題1：

剛開始執行的時候參考了連結： https://emanual.robotis.com/docs/en/platform/turtlebot3/slam_simulation/， 以及連結： https://www.ncnynl.com/archives/202008/3852.html.

執行ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True map:=$HOME_PATH/map.yaml失敗，發現navigation2.launch.py中所需要的bringup.launch.py在dashing 版本的nav2_bringup中不存在，在foxy版本中才有，故而嘗試將navigation2.launch.py中的launch檔案更新為nav2_bringup_launch.py

IncludeLaunchDescription(
	    PythonLaunchDescriptionSource([nav2_launch_file_dir, '/nav2_bringup_launch.py']),  #需要修改此處
	    launch_arguments={
	        'map': map_dir,
	        'use_sim_time': use_sim_time,
	        'params': param_dir}.items(),
	),

遇到的問題2：
執行後發現rviz裡不能發現map, 也沒有相關地圖顯示，檢索後發現到了以下issue: https://github.com/ros-planning/navigation2/issues/1042， 指出需要在rviz裡點選一些startup按鈕，並需要指定初始位姿，才可以開始導航。

參考連結：
https://blog.csdn.net/SimileciWH/article/details/84861718
https://www.cnblogs.com/jiangxinyu1/p/12458699.html