# Section 2 - 徒手搓一台電動車竟然那麼容易！自動駕駛前奏曲

課前閱讀：[機器人作業系統能幫我寫作業嗎？ROS 2 究竟是何方神聖](https://hackmd.io/@Lani0516/BJptJP06ke)

## 課程講師
| OP & ED | Fusion |Unity | ROS2 Installation | ROS2 Navigation |
|:-------:|:------:|:-----------------:|:---------------:|:------:|
| 劉文恩 | 黃德彥 | 陳俊光 | 覃家璿 | 官子恩 |

## 最低硬體需求
### windows
作業系統：windows 10 64-bit
處理器：Intel core i5-4570
記憶體：16 GB 記憶體
儲存空間：30 GB 可用空間

### macOS
處理器：Apple M1
記憶體：8 GB 記憶體
儲存空間：30 GB 可用空間

### linux
作業系統：任意
硬體要求：同 windows、macOS
>[!Warning]
要對自己系統的 Package Manager、CLI 指令有一定了解
例如：Ubuntu（apt）、CentOS (yum)

## 預先安裝內容（請務必於上課前自行安裝完畢！）
{%preview https://hackmd.io/@RgFC-jP-Qp6J6fJ4BTqdgQ/S1fvJsUa1l %}
{%preview https://hackmd.io/@RgFC-jP-Qp6J6fJ4BTqdgQ/Syk0aUDa1x %}
> Linux 跟 Mac 大同小異，但套件管理工具要使用系統自帶的，不能使用 Homebrew

## Fusion
#### 前情提要
由於在本次工作坊中並不會花太多的時間著墨，因此已經幫大家把模型設立好了，但是如果想要自己嘗試建構模型，可點擊

[fusion360建造urdf車子後匯入unity](https://youtu.be/080UAkOeeUM?si=7oUKPX7KqXpkSWQG)

如果沒有要自行建造， [urdf_model](https://github.com/screamlab/urdf_model) 就是我們提供的模型，請先確認是否已經git clone。

### Find URDF folder
如果是直接使用提供的模型，請打開Powershell後進入`urdf_model`的資料夾。

1. 在`urdf_model`中進入`fourWheelCar_description`資料夾

cd fourWheelLidar_3DModel_urdf/fourWheelCar_description

2. 輸入`ls`檢查是否有這三個檔案在裡面
- `urdf`
- `meshes`
- `launch`


3. 輸入 `pwd` 或 `Get-location` 查詢當前位置，並複製下來。

pwd

### Export URDF
1. 打開新的powershell分頁，進到`fusion_xacro2urdf2unity`資料夾。
2. 將`xacro2urdf.py`複製到剛剛提到的資料夾。這個python腳本是用來將 `xacro` 格式的文件轉換為 `urdf` 格式的文件

python xacro2urdf.py /path/to/your/folder

請將`/path/to/your/folder`的部分改為先前複製的路經。
如果成功的話，應該會跳出success的字樣。

3. 如果想確認的話，在`fourWheelCar_description` 檔案中輸入 `ls` 應該就會看到 `fourWheelCar_to_unity` 資料夾了。

### 自行建構模型(這裡有[示範影片](https://youtu.be/ctNzWya31cA?si=XJce-DJDW1mnEaIw))
>[!Note]
以下部分提供給有安裝fusion且使用自己的模型的人參考，urdf_model內的模型已轉換完畢，若使用此模型可跳過這個部分。

#### 將URDF export複製到fusion上

請到`fusion2urdf folder`的資料夾裡輸入以下指令：
(這個指令讓fusion能夠export Ros2，省下一些步驟)

Windows:

Copy-Item ".\URDF_Exporter\" -Destination "${env:APPDATA}\Autodesk\Autodesk Fusion 360\API\Scripts\" -Recurse

Mac:

cp -r ./URDF_Exporter "$HOME/Library/Application Support/Autodesk/Autodesk Fusion 360/API/Scripts/"

#### 輸出URDF檔
1. 打開fusion自行製作的模型。
在上方的UTILITIES介面找到ADD-INS，點進去後選擇Scripts and Add-Ins

2. 打開Scripts and Add-Ins後，選擇URDF_Exporter_Ros2(這是前面Copt-item得到的)
3. 按下Run之後，應該就會跳出儲存畫面。(建議新增資料夾方便整理)
4. 儲存後如果有跳出success的畫面就代表成功了。

#### Copy address
打開儲存的fusion資料夾，找到`urdf` `meshes` `launch`三個檔案。
>[!tip]
應該會在 xxx_to_description 的資料夾找到

如果是透過檔案總管找到資料夾的話，可以在資料夾上方按右鍵，即可copy address

舉個例子，如果自行製作了 cat_description，在確認有 `urdf` `meshes` `launch`三個檔案後，在上方`cat_description` 按下右鍵即可找到複製路徑的選項。

這樣在xacro轉urdf時會輸入：(只是範例)

python xacro2urdf.py C:\Users\Desktop\fusion\cat_description

一樣也會跳出success的畫面。

## Unity setting
### Unity pkg install
#### how to import a package into Unity:
1. 打開 Unity
2. 點選上方工作欄的 Window → Package Manager
3. 點擊左上方的 `+` 符號
4. 選擇 `Install package from git URL`
5. 輸入 `https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.5.2` 並點擊 `install`

#### Install NuGet
我們需要 NuGet 裡面的 WebSocketSharp，它是連接 ROS 所需的套件
1. 在 [NuGetForUnity 的網頁](https://openupm.com/packages/com.github-glitchenzo.nugetforunity/#close) 點擊 `Manual installation`
2. 將裡面的資料填入 Unity 上方工作欄的 Edit → Project Settings → Package Manager `中的 New Scoped Registry` 裡後，點擊 `Save`
3. 點選 Unity 上方工作欄的 Window → Package Manager
4. 點擊左上方的 `+` 符號
5. 選擇 `Install package from git URL`
6. 輸入 `com.github-glitchenzo.nugetforunity` 並點擊 `install`
> [!Tip]
NuGet 的安裝也可以參考 [安裝教學](https://tedliou.com/unity/nuget/)
7. 下載完成 NuGet 之後，點選上方工作欄的 NuGet → Manage NuGet Packages
8. 搜尋 WebSocketSharp，安裝 WebSocketSharp-netstandard

### Import URDF
1. 在 Project 一欄中打開 urdf 檔案資料夾
2. 把 `xxx_to_unity` 資料夾裡面的 `xxx_description` 資料夾和 `xxx.urdf` 放進 urdf 資料夾
4. 接著(以fourWheelCar為例)，對 xxx(.urdf) 點擊右鍵，選擇 `Import Robot from Selected URDF file`
4. 點擊 Import URDF (如果電腦是 arm64 系統，`Mesh Decomposer` 要選擇 `Unity`)
5. 將 `Urdf Robot (Script)` 和 `Controller (Script)` 關閉
6. 確認每個輪子的 `Articulationbody` 都是開啟的 (如果有輪子的話)，且 `Anchor Rotation` 的數值都相同

## ROS2
### 練習範例
1. 進入 wsl
3. 進入 `pros_app` 資料夾中，輸入指令來創建 `compose_my_bridge_network`

python3 ./control.py -s

並選擇 `rosbridge_server.sh` 對應的數字

3. 改為進入 `pros_car` 資料夾中，執行指令

./car_control.sh

4. 進入 `src` 資料夾中並創建 `myPkg`

cd src
ros2 pkg create myPkg --build-type ament_python --dependencies rclpy

5. 回到 `workspaces` 資料夾，並編譯

colcon build
. ./install/setup.bash

>[!tip]
也可以改為輸入 `r` 來編譯

6. 進入 `src/myPkg/myPkg` 中，並創建 `nodeA` 和 `nodeB` 兩個檔案

cd src/myPkg/mypkg
touch nodeA.py nodeB.py

7. 進入VSCode下載remote Development
8. 於 `nodeA` 中，輸入程式

import rclpy
from rclpy.node import Node

from std_msgs.msg import String


class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        self.publisher_ = self.create_publisher(String, 'HelloTopic', 10)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        msg = String()
        msg.data = 'Hello World: %d' % self.i
        self.publisher_.publish(msg)
        self.i += 1


def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

並於 `nodeB` 中，輸入程式

import rclpy
from rclpy.node import Node

from std_msgs.msg import String


class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__("minimal_subscriber")
        self.subscription = self.create_subscription(
            String, "HelloTopic", self.listener_callback, 10
        )
        self.subscription  # prevent unused variable warning

    def listener_callback(self, msg):
        self.get_logger().info('I heard: "%s"' % msg.data)


def main(args=None):
    rclpy.init(args=args)

    minimal_subscriber = MinimalSubscriber()

    rclpy.spin(minimal_subscriber)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()

9. 在 (`src/myPkg/`)`setup.py` 中，將 `entry_points={}` 改成

entry_points={
    "console_scripts": [
        "publishNode = myPkg.nodeA:main",
        "subscribeNode = myPkg.nodeB:main",
    ],
},

10. 回到 workspaces 資料夾，並編譯

r

11. 開啟兩個 wsl 並分別執行以下兩個指令來啟動 `nodeA` 和 `nodeB`

ros2 run myPkg publishNode

ros2 run myPkg subscribeNode

可以使用第7步的 `Remote Development` 來執行

### Nav

1. 進入 `pros_app` 資料夾(在WSL)，並執行指令

python3 ./control.py -s

然後選擇 ./slam_unity.sh (應該是第2選項)

2. 打開 Unity
3. 於`cd pros_car` 資料夾中，輸入

./car_control.sh

4. 用 `r` 進行編譯

r

5. 執行指令來打開選單

ros2 run pros_car_py robot_control

並選擇 `Control Vehicle` 來控制車輛(按enter即可)

6. 在 Unity 中執行檔案

以下是控制車輛的指令
`w` : Forward

`s` : Backward

`e` : Counterclockwise

`r` : Counterclock

`z`: Stop

`q` : Quit

### Foxglove

1. 打開 foxglove (並且註冊你的帳號)
2. 在 foxglove 中點擊左側工作欄裡面的 `Open connection..`
3. 選擇 `Rosbridge` 後點擊 `Open`

4.在display frame(展示坐標系)項目中選擇map

5.將其中map以及scan選項打開，並將點的大小調至20(推薦)附近
6.回到操作選單並進行操作開車子繞地圖一遍(最快直接繞外圍一圈)，用以掃描地圖

7.當掃描完整個地圖後，在 `pros_app` 選單中選擇 `store_map.sh` 儲存地圖
(使用`store_map.sh` 時，`slam_unity.sh` 必須也在運行中)

儲存完畢後 `store_map.sh` 會顯示 `儲存成功`

### Tips
* Unity中的button可以reset車子的位置

* 若foxglove的地圖掃描出現問題，可以重開slam_unity.sh 輸入d 然後重新選擇slam_unity.sh並按下Unity的button即可

8.接下來，在選單中輸入d，關閉所有運行中的容器，接著開啟localization_unity，載入剛才掃描的map

9.回到unity終點及button，若無法點擊則直接重新運行

10.再來要定localize方位
[影片示範](https://www.youtube.com/watch?v=kxmSMe_qW5o)
影片中藍色箭頭先點擊自身再來點擊前進方向(w方向)

11.打開於左下角publish(發布)並將途中話題改為/goal_pose

12.點擊publish 2D pose(/goal_pose)
將隨之出現的粉色箭頭點基在你想設定的目的地
將左方plan選項開啟，起點至終點的路徑就會被自動規劃好

13.切換控制面板置自動導航面板

選擇manual_auto_nav
點擊任意鍵即可

### Tips
如果導航方向錯誤，至unity fourwheelcar下的lidar_v1_1將rotation改成180度讓方向正確

## UNIX 指令簡介

### 檔案整理

> 如果你下載了一系列資源以後，試圖在 Windows Terminal 的小黑窗裡面找到他們，你可能會用到以下指令

- 顯示所在資料夾

pwd

- 顯示目前目錄內容

ls

- 進入目錄

cd [相對/絕對目錄]

- 建立新目錄

mkdir [欲建立的目錄名稱]

- 刪除空目錄

rmdir [欲刪除的目錄名稱]

- 建立檔案

touch [欲建立的檔案名稱]

- 刪除檔案

rm [欲刪除的檔案名稱]

- 複製檔案/目錄

cp [來源][目標]

- 移動檔案/目錄

mv [來源][目標]

### 至於剩下沒有教的...

你可以:

man [指令名稱]

###### ~~man what can i say~~
來查詢指令說明