# Autonomous Navigation ใน Gazebo
Autonomous Navigation หรือการนำทางอัตโนมัติ เป็นหัวใจสำคัญของหุ่นยนต์เคลื่อนที่อัตโนมัติ (Autonomous Mobile Robot: AMR) ซึ่งต้องสามารถรับรู้สภาพแวดล้อม, วางแผนเส้นทาง และเคลื่อนที่ไปยังจุดหมายโดยอัตโนมัติ โดยอาศัยเซ็นเซอร์ เช่น LiDAR, กล้อง 3D, และ IMU ประกอบกับอัลกอริทึมการนำทาง เช่น SLAM (Simultaneous Localization and Mapping) และ Path Planning \
ในหัวข้อที่ 11 เราได้เรียนรู้เกี่ยวกับการติดตั้งแพ็คเกจที่จำเป็น และวิธีใช้งาน Gazebo เพื่อจำลองสภาพแวดล้อมสำหรับหุ่นยนต์ ในบทนี้ เราจะศึกษาเกี่ยวกับการใช้งาน Gazebo ซึ่งเป็นตัวจำลองสภาพแวดล้อมเสมือน (Simulation) ที่สามารถใช้ทดสอบระบบนำทางของหุ่นยนต์โดยไม่ต้องใช้งานฮาร์ดแวร์จริงโดยใช้แพ็กเกจ TurtleBot3 Simulation ในการสาธิต
(https://emanual.robotis.com/docs/en/platform/turtlebot3/simulation/)

## สร้างแผนที่ SLAM (Simultaneous Localization and Mapping)
การสร้างแผนที่และวางแผนเส้นทาง โดยใช้ SLAM (Simultaneous Localization and Mapping) เพื่อให้หุ่นยนต์สามารถสร้างแผนที่ของพื้นที่และกำหนดตำแหน่งของตนเองภายในแผนที่
### เปิดโลกจำลองใน Gazebo
เปิด Terminator 3 หน้าต่าง (ในกรณีที่มีหน้าต่างอยู่ก่อนหน้าปิดแล้วเปิดใหม่):
```{image} images/c16/16.1.1.1.png
:width: 80%
:align: center

```
 Terminator หน้าต่างที่ 1 รันคำสั่งดังนี้ : 
```{code-block} bash
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

```
เพื่อทำการเปิด Gazebo จำลองในระบบ Simulator จะปรากฏดังภาพ
```{image} images/c16/16.1.1.2.png
:width: 80%
:align: center

```
### รัน SLAM Node
 Terminator หน้าต่างที่ 2 รันคำสั่งดังนี้: 
```{code-block} bash
ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True

```
คำสั่งนี้เป็นการ รันระบบ SLAM (Simultaneous Localization and Mapping) โดยใช้แพ็กเกจ turtlebot3_cartographer ใน ROS 2 ที่ชื่อว่า cartographer.launch.py และกำหนดให้ใช้เวลาในโหมดจำลอง (use_sim_time:=True).

จะปรากฏดังภาพ
```{image} images/c16/16.1.2.1.png
:width: 80%
:align: center

```

### ควบคุมหุ่นยนต์ด้วย Teleoperation
เพื่อให้หุ่นยนต์สามารถเคลื่อนที่ไปรอบๆ และสร้างแผนที่ ต้องใช้ Teleoperation Node เพื่อควบคุมหุ่นยนต์

Terminator ที่ 3 รันคำสั่งดังนี้:
```{code-block} bash
ros2 run turtlebot3_teleop teleop_keyboard

```
```{image} images/c16/16.1.3.1.png
:width: 80%
:align: center

```

เมื่อรันคำสั่งนี้ จะสามารถใช้ แป้นพิมพ์ เพื่อควบคุมการเคลื่อนที่ของหุ่นยนต์\
w : เดินหน้า\
x : ถอยหลัง \
a : เลี้ยวซ้าย\
d : เลี้ยวขวา\
space, s : หยุด\
(ถ้ากด w, x ซ้ำๆ จะเป็นการเดินหน้าพร้อมกับเร่งสปีดในแนวเชิงเส้น)\
(ถ้ากด a, d ซ้ำๆ จะเป็นการเดินหน้าพร้อมกับเร่งสปีดในแนวเชิงมุม)

ภารกิจ : เคลื่อนที่ไปรอบๆ เพื่อให้ LiDAR เก็บข้อมูลและสร้างแผนที่ได้รอบห้องแบบรูปด้านล่าง
```{image} images/c16/16.1.3.2.png
:width: 80%
:align: center

```

### บันทึกแผนที่
เมื่อสามารถเก็บแผนที่ได้ดังรูปด้านบน ให้เปิด Terminator หน้าต่างที่ 4 แล้วรันคำสั่งดังนี้: 
Terminator ที่ 3 รันคำสั่งดังนี้:
```{code-block} bash
ros2 run nav2_map_server map_saver_cli -f ~/map

```
เมื่อรันคำสั่งเซฟแผนที่ จะมี ไฟล์ .pgm และ .yaml ปรากฏขึ้นมาใน Home โฟลเดอร์
```{image} images/c16/16.1.4.1.png
:width: 80%
:align: center

```

## Navigation2 (Nav2)
เป็นการนำทางอัตโนมัติหลังจากที่สร้างแผนที่แล้ว  (ปิดหน้าต่าง Terminator เดิมให้หมดจากหัวข้อ 12.1 สร้างแผนที่ SLAM และเปิด Terminator ขึ้นมาใหม่ 2หน้าต่าง)
### เปิดโลกจำลองใน Gazebo Terminal หน้าต่างที่ 1 ให้รันคำสั่งดังนี้ :
```{code-block} bash
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

```
คำสั่งนี้เป็นการ เปิดโลกจำลอง (Simulation Environment) ของ TurtleBot3 โดยใช้แพ็กเกจ turtlebot3_gazebo ใน ROS 2 และรันไฟล์ Launch ชื่อ turtlebot3_world.launch.py

```{image} images/c16/16.2.1.png
:width: 80%
:align: center

```
### รัน Navigation2 Node Terminal หน้าต่างที่ 2 ให้รันคำสั่งดังนี้ :
```{code-block} bash
ros2 launch turtlebot3_navigation2 navigation2.launch.py use_sim_time:=True 

```
คำสั่งนี้ใช้สำหรับ เปิดระบบ Navigation ของ TurtleBot3 โดยใช้แพ็กเกจ turtlebot3_navigation2 ใน ROS 2 และรันไฟล์ Launch ที่ชื่อว่า navigation2.launch.py พร้อมกำหนดให้ใช้เวลาจำลอง (use_sim_time:=True).\
หลังจากรันคำสั่งนี้ ระบบ Navigation2 จะเริ่มทำงาน
```{image} images/c16/16.2.2.png
:width: 80%
:align: center

```
### กำหนดตำแหน่งเริ่มต้นของหุ่นยนต์ (Estimate Initial Pose)
ก่อนเริ่มนำทาง ต้องตั้งค่าตำแหน่งเริ่มต้นของหุ่นยนต์ใน RViz2 เพื่อให้ตรงกับตำแหน่งจริงของมันในแผนที่

**วิธีตั้งค่า Initial Pose**
1. กดปุ่ม "2D Pose Estimate" ในเมนูของ RViz2
2. คลิกบนแผนที่ตำแหน่งที่หุ่นยนต์อยู่จริงใน Gazebo และลาก ลูกศรสีเขียว ไปในทิศทางที่หุ่นยนต์กำลังหันหน้า
3. ทำซ้ำจนกว่า Laser จะซ้อนทับกับแผนเหมาะสม
```{image} images/c16/16.2.3.png
:width: 80%
:align: center

```

### ตั้งค่าจุดหมายปลายทาง (Set Navigation Goal)
เมื่อกำหนดตำแหน่งเริ่มต้นเรียบร้อยแล้ว ให้ตั้งค่าจุดหมายปลายทางให้หุ่นยนต์นำทางไปยังตำแหน่งที่ต้องการ

**วิธีตั้งค่า Navigation Goal**
1. กดปุ่ม "Nav to Goal" ใน RViz2
2. คลิกบนแผนที่ในตำแหน่งที่ต้องการให้หุ่นยนต์เคลื่อนที่ไป (คลิกบนพื้นที่ว่าง ที่ไม่ใช่นอกแผนที่ หรือ พื้นที่มีเสา)
3. ลาก ลูกศรสีเขียว ไปในทิศทางที่หุ่นยนต์ควรหันหน้า
4. หุ่นยนต์จะเริ่มเคลื่อนที่ไปยังจุดหมายปลายทางโดยอัตโนมัติ

```{image} images/c16/16.2.4.png
:width: 80%
:align: center

```