ROS2 & Gazebo
===
###### tags: `Learning Notes`
[TOC]

Gazebo 座標系
---

**<font color="#FF00000">紅色</font>: x軸**
**<font color="#008000">綠色</font>: y軸**
**<font color="#00008B">藍色</font>: z軸**

檔案結構
---

**Workspace**
>**src**
>

#### worlds

<?xml version="1.0" ?>

<sdf version="1.6">
  <world name="default">

    <include>
      <uri>model://sun</uri>
    </include>

    <include>
      <uri>model://ground_plane</uri>
    </include>

    <include>
      <uri>model://first_model</uri>
      <pose>0 0 0 1.5708 0 1.5708</pose>
    </include>

  </world>
</sdf>

在此檔案中可以 include Gazebo 內建的 model，如 sun、ground_plane，並且設定環境變數；另外也可以 inciude 自己編寫的model，如 first_model，檔案的副檔名為 .world。

:::info
如果執行 Gazebo 時，系統說找不到 model，可以將下列的指令輸入至終端機，其中 path-to-model 指的是 models 資料夾的絕對位置。
:::

export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}: path-to-model

>參考: https://blog.csdn.net/wang1997wwe/article/details/106730290

#### models

model.config:

<?xml version="1.0"?>

<model>
  <name>Ummanned Car</name>
  <version>1.0</version>
  <sdf version="1.5">model.sdf</sdf>

  <author>
    <name>Leo</name>
  </author>

  <description>
    Model test
  </description>
</model>

model.sdf:

<?xml version="1.0" ?>
<sdf version="1.6">

  <model name='Ummannd_Car'>
  <static>true</static>

  <link name='car'>

    <collision name='collision'>
      <geometry>
        <mesh>
          <uri>model://first_model/meshes/body.STL</uri>
          <scale>0.001 0.001 0.001</scale>>
        </mesh>
      </geometry>
    </collision>

    <visual name='visual'>
      <geometry>
        <mesh>
          <uri>model://first_model/meshes/body.STL</uri>
          <scale>0.001 0.001 0.001</scale>>
        </mesh>
      </geometry>
    </visual>

    </link>
  </model>
</sdf>

在此資料夾可以存放自己編寫的 model，每個 model 需有一個獨立的資料夾，而每個資料夾都要有上述的兩個檔案，其各自的功能如下:
1. model.config: model 的說明文件。
2. model.sdf: 建立 model 的檔案，其中主要的 tag 說明如下:
>**static: model 是否為靜態。**
>**link: 機器人的單一物件，可以由 joint 將不同的 link 連接。**
>**collision: 負責物理模擬中數學演算的部份。**
>**visual: 機器人物件的外觀，此處可以用 sdf 的格式撰寫，或者也可以使用 .dae 或 .stl。**
>**mesh: 若是有使用 .dae 或是 .stl 檔，須將其檔案的路徑寫在此區塊。**
>==此處要特別注意，因為外觀與物理演算是獨立的，如果兩者對於機器人物件的比例不同，會產生不合理的模擬結果。==

#### meshes
此資料夾須存放在 model 的資料夾中，與 model.config 與 model.sdf 同層，其中存放的內容為 .dae 或是 .stl 檔。

#### CMakeLists

install(
  DIRECTORY
  launch
  models
  worlds
  DESTINATION share/${PROJECT_NAME}/
)

編寫好上述的資料夾與檔案後，將這些指令加入至 CMakeLists 中。

#### launch

import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():

    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
    pkg_car_test_gazebo = get_package_share_directory('car_test')

    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py')
        )
    )

    return LaunchDescription([
        DeclareLaunchArgument(
            'world',
            default_value=[os.path.join(pkg_car_test_gazebo, 'worlds', 'car_empty_world.world'), ''],
            description='SDF world file',
        ),
        gazebo
    ])

在編寫 launch 檔時，須注意副檔名應為 .launch.py，在此範例程式中有幾點需要注意:
1. pkg_gazebo_ros: 此變數是要存取 Gazebo 的 package。
2. pkg_car_test_gazebo: 此變數是存取存放 launch 檔的package。
3. default_value: 此處是需要寫上 worlds 資料夾與要開啟的 .world 的名稱。

#### 運行結果

Gazebo 連結 ROS2
---

在建立好 world 以及 model 的檔案後，就可以將其與 ROS2 進行連接，此處可以參考官方的教學與官方提供的 plugin，使用 plugin 就能讓 gazebo 的 model 建立 ROS2 的 node 與 topic，接下來就能用 ROS2 來控制 gazebo 的 model。

熟悉 plugin 後，可以試著在 model 上加入 sensor，接著使用 sensor 的 plugin 建立 topic，然後在另外編寫一個.cpp檔，讓車子的 model 能接收到 senosr 的 message 自行避開障礙物。

>官方: http://gazebosim.org/tutorials
>plugin 的 github: https://github.com/ros-simulation/gazebo_ros_pkgs/wiki

#### Model 使用 plugin 連結 ROS2

  <plugin name="wheel_drive_controller" filename="libgazebo_ros_diff_drive.so">

    <!-- ROS topic -->
    <ros>

      <namespace>/demo_car</namespace>
      <remapping>cmd_vel:=cmd_vel</remapping>
      <remapping>odom:=odom_demo</remapping>

    </ros>

    <!-- Wheel pair of car -->
    <num_wheel_pairs>1</num_wheel_pairs>
    <left_joint>rotate_left_wheel_joint</left_joint>
    <right_joint>rotate_right_wheel_joint</right_joint>

    <!-- Wheel diameter & separation -->
    <wheel_separation>0.3</wheel_separation>
    <wheel_diameter>0.08</wheel_diameter>

    <!-- Dynamics -->
    <max_wheel_torque>20.0</max_wheel_torque>
    <max_wheel_acceleration>10.0</max_wheel_acceleration>

    <publish_odom>true</publish_odom>
    <publish_odom_tf>true</publish_odom_tf>
    <publish_wheel_tf>true</publish_wheel_tf>

    <update_rate>500</update_rate>

    <!-- Frame -->
    <odometry_frame>odom_demo</odometry_frame>
    <robot_base_frame>car</robot_base_frame>

  </plugin>

這個 plugin 使官方提供的，其主要的功能是控制車子輪子的線性速度與角速度。使用的方式為在 model.sdf 中添加 plugin 的 tag，其中 tag 的說明如下:
1. plugin: 要使用 plugin 就需要添加此 tag，其中的 name 可以自行命名，而 filename 則需要與 plugin 資料夾中有提供的一致，並且要加上 .so 作為副檔名。
2. ros: ros tag 可以建立 topic，每個 plugin 能建立的 topic 都有所不同。
3. publish_odom: 此 tag 可以 publish odometry 的資訊。
4. robot_base_frame: 此處應填入 base link，對我們的 model 而言，就是要填上 car。

>編寫好的 model : https://github.com/Leo125Jan/Ummannd_Car-Project.git

#### 運行結果
{%youtube S5d3QBMoAiQ %}

環境建置與執行
---
1. Edit -> Building Eidtor
可以建立房間格局 ex. wall, door, windows
完成可以save as model使用
2. Edit -> Model Editor
（之後可以將房間格局設定為一個model在複製上較為方便）
3. 有了 models 可以直接拉入 world 中使用，儲存檔案為要為.world檔


於其他電腦使用模擬自訂的gazebo環境
---
1. 建立新的workspace和src檔
2. 進入src檔案中抓取github的檔案
3. build package
4. 啟動新建的workspace
5. ros2 launch gazebo模擬

$ mkdir -p /[name_ws]/src  #[ws-name]: 自訂的workspace名字
$ cd /[name_ws]/src
$ git clone https://github.com/Leo125Jan/Ummannd_Car-Project
$ colcon build --symlink-install --packages-select [package name]
$ source install/setup.bash
$ ros2 launch [package name] [world.launch.py]

常用指令
---

#### 只編譯一個package

colcon build --symlink-install --packages-select [package name]

#### 關閉 Gazebo

killall gzserver

常見問題
---
#### Gazebo閃退問題
檢查：
1. 若Gazebo不為最新版本(Gazebo11)需要更新
2. 是否為虛擬器，虛擬器需要將3D圖形加速關閉：
https://blog.csdn.net/wangguchao/article/details/88777162
3. 若雙系統則輸入下面程式碼即可：

sudo apt-get update

https://blog.csdn.net/weixin_39967405/article/details/111550932

Reference
---

#### Gazebo tutorials:
[Gazebo in 5 mins](https://www.youtube.com/watch?v=qi2A32WgRqI&list=PLK0b4e05LnzbHiGDGTgE_FIWpOCvndtYx)
[Gazebo Simulator & GUI explanation](https://www.youtube.com/watch?v=O0729K-7VEY&list=PLNw2RD-1J5YYvFGiMafRD_axHrBUGvuIg)
[ROS2 Gazebo](https://www.youtube.com/watch?v=T4iRJqESQAk&list=PLK0b4e05LnzYAfZ_GCZw5S-tpQSynP-fC&index=4)
[ROS2 Gazebo page](https://www.theconstructsim.com/exploring-ros2-with-wheeled-robot-1-launch-ros2-simulation/)
[Gazabo official](http://gazebosim.org/tutorials?tut=ros2_installing&cat=connect_ros)

#### 參考用的 model
[Model](https://github.com/chapulina/dolly)

#### ROS2 connect Gazebo
[Getting Ready for ROS Part 8: Simulating with Gazebo](https://articulatedrobotics.xyz/ready-for-ros-8-gazebo/)
[Robot Operating System: Controlling a Robot with the ROS Gazebo Plugins](https://admantium.com/blog/ros09_controlling_a_robot_in_gazebo/)

Q&A
---

[SDF vs URDF what should one use](https://answers.gazebosim.org//question/62/sdf-vs-urdf-what-should-one-use/)
[為什麼選擇SDF](https://zhuanlan.zhihu.com/p/129660153)
[Gazebo中sdf文件和urdf和xacro文件的區別](https://blog.csdn.net/lxlong89940101/article/details/93486875?spm=1001.2101.3001.6650.1&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1.pc_relevant_default&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1.pc_relevant_default&utm_relevant_index=2)