# 無人機模擬環境與 ORB-SLAM3、YOLO 偵測技術文件 & 實體飛行測試文件
P.S. 程式與工研院筆電版本相同

## 目錄
- [1. Docker 使用](#1-Docker)
- [2. 模擬無人機專案建置](#2-模擬無人機專案建置)
- [3. ORB-SLAM3 指令與流程](#3-ORB-SLAM3指令與流程)
- [4. YOLO 指令與流程](#4-YOLO指令與流程)
- [5. EVO軌跡評估工具](#5-EVO軌跡評估工具)
- [6. ROS 常用除錯工具與技巧](#6-ROS常用除錯工具與技巧)
- [7. 實體飛行無人機參數設定](#7-實體無人機參數設定)

---
## 1. Docker
Docker 是一種開源軟體平台，其核心理念是「Build and Ship any Application Anywhere」，旨在讓開發者能在一個輕量、可攜的虛擬化環境中，輕鬆地開發、部署及管理任何應用程式。對於像無人機這樣涉及複雜軟體（如 ROS、SLAM 演算法、AI 模型）的專案，Docker 提供了標準化且可重現的解決方案。

### 本專案的雙容器架構

為了實現模組化，本專案將採用**雙容器**的架構。不同的核心功能將被拆分到兩個獨立的容器中，它們之間透過 ROS (Robot Operating System) 的網路通訊機制（Topics, Services）進行溝通。
p.s. 請使用ubuntu 2204 以下的版本
#### Container 1: `rtabmap_drone_example` (模擬與導航核心)

- **功能與內容**:
- **模擬環境**: 內建 **Gazebo** 模擬器，提供一個虛擬世界讓無人機飛行與互動。
- **飛行控制**: 包含 **PX4-SITL** (Software in the Loop)，模擬無人機的飛行控制器。
- **通訊橋梁**: 透過 **MAVROS** 將 ROS 指令轉換為 PX4 能理解的 MAVLink 訊息。
- **基礎導航**: 整合 **RTAB-Map** 作為視覺 SLAM 引擎，並搭配 **`move_base`** 導航堆疊，實現基本的自主路徑規劃與避障。
#### Container 2: `ORB-SLAM3 & YOLO detection` (進階感知與定位)

- **功能與內容**:
- **高精度 SLAM**: 搭載 **ORB-SLAM3** 函式庫。ORB-SLAM3 是一個功能強大的視覺慣性 SLAM 系統，能提供比 RTAB-Map 更高精度的定位與建圖能力。將其獨立打包成容器是常見作法，因為其依賴複雜且版本穩定，便於重複使用。
- **物件偵測**: 整合 **YOLO** 模型。此模型會接收來自模擬環境的影像串流，進行即時物件偵測，並將偵測結果（如物件類別、深度）透過 ROS Topic 發佈出去。

### 啟動前的準備 (X11 Forwarding)
為了讓容器內的圖形化介面應用（如 Gazebo, RViz）能顯示在你的主機螢幕上，需要在本機先執行以下指令授權 X server 連線：

xhost +local:root
XAUTH=/tmp/.docker.xauth

## 可以試試看
touch $XAUTH
xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - 

### Docker 啟動指令 (以 NVIDIA GPU 版本為例)

#ubuntu 建議22.04以下 不然rviz需要使用到的OpenGL會有衝突 
docker run -it \
    --name="<給容器取個名字>" \
    --privileged \
    --net=host \
    --gpus=all \
    --env="DISPLAY=$DISPLAY" \
    --env="QT_X11_NO_MITSHM=1" \
    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
    --env="XAUTHORITY=$XAUTH" \
    --volume="$XAUTH:$XAUTH" \
    --volume="<你的本機路徑>:<容器內對應路徑>" \
    <你的映像檔名稱> \
    /bin/bash                      (進入bash)

## 2. 模擬無人機專案建置

### 專案概述
本專案 `rtabmap_drone_example` 提供一個完整的範例，展示如何使用 `move_base`、`mavros/px4` 以及 `RTAB-Map` 視覺 SLAM 技術，在模擬環境中實現無人機的自主導航。專案的 GitHub 儲存庫與詳細安裝教學請參考：[rtabmap_drone_example](https://github.com/matlabbe/rtabmap_drone_example)。

<figure style="text-align: center;">
<img src="https://hackmd.io/_uploads/rJCEAwVNge.png" alt="模擬環境展示圖">
<figcaption style="font-style: italic;">圖1：rtabmap_drone_example 專案於 Gazebo 中的視覺化介面</figcaption>
</figure>

### 核心組件與執行流程
要完整運行此模擬專案，需在**四個獨立的終端機**中依序啟動以下核心節點：

1. **啟動 Gazebo 模擬環境**

    roslaunch rtabmap_drone_example gazebo.launch

> **作用**：此指令會啟動 Gazebo 模擬器，載入預設的世界地圖，並在其中生成一台 IRIS 四旋翼無人機模型。此模型已搭載模擬的攝影機與 IMU 感測器。

2. **啟動 RTAB-Map SLAM**

    roslaunch rtabmap_drone_example slam.launch

> **作用**：啟動 `rtabmap` 節點，進入 SLAM 模式。它會訂閱來自 Gazebo 無人機的影像與 IMU 數據，即時進行同步定位與建圖 (Simultaneous Localization and Mapping)，並發布地圖 (`/map`)、無人機位置 (`tf`) 與里程計 (`/rtabmap/odom`) 等資訊。

3. **啟動 RVIZ 視覺化介面**

    roslaunch rtabmap_drone_example rviz.launch

> **作用**：啟動 RVIZ，並載入預設的設定檔。在此介面中，你可以看到 RTAB-Map 所建立的 2D/3D 地圖、無人機的即時位置，並能透過 **"2D Nav Goal"** 工具下達自主導航的目標點。

4. **啟動飛行控制節點**

    # 執行 C++ 版本
    rosrun rtabmap_drone_example offboard
    
    # 改寫的 Python 版本
    rosrun rtabmap_drone_example offboard_python.py

> **作用**：它負責透過 MAVROS 與 PX4 飛行控制器溝通，將無人機切換至 `OFFBOARD` 模式，並執行解鎖與自動起飛。原專案提供的是 C++ 版本，但我們也可以使用 Python 版本進行替換。

<details>
<summary>可以用python改寫起飛程式碼</summary>

#!/usr/bin/env python3
# coding: utf-8
import rospy
import tf2_ros
import tf2_geometry_msgs
from geometry_msgs.msg import PoseStamped, Twist
from mavros_msgs.msg import State, PositionTarget
from mavros_msgs.srv import CommandBool, CommandBoolRequest, CommandLong, CommandLongRequest, SetMode, SetModeRequest
import math

# 定義不同控制模式的 type_mask（MAVROS 規範）
VELOCITY2D_CONTROL = 0b011111000011    # 僅控制 X, Y 速度
VELOCITY_CONTROL = 0b011111000111      # 控制 X, Y, Z 速度
POSITION_CONTROL = 0b101111111000      # 控制位置

velocity_mask = VELOCITY2D_CONTROL     # 預設為 2D 速度控制

def state_cb(msg):
    # 訂閱 mavros/state，取得無人機目前狀態（連線、解鎖、模式等）
    global current_state
    current_state = msg

def twist_cb(msg):
    # 訂閱 /cmd_vel，接收速度指令，切換為速度控制模式
    global current_goal, lastTwistReceived, velocity_mask
    if current_goal.type_mask == POSITION_CONTROL:
        rospy.loginfo("Switch to velocity control")
        current_goal.coordinate_frame = PositionTarget.FRAME_BODY_NED
        current_goal.type_mask = velocity_mask
    # 設定速度指令
    current_goal.velocity.x = msg.linear.x
    current_goal.velocity.y = msg.linear.y
    current_goal.velocity.z = 0 if velocity_mask == VELOCITY2D_CONTROL else msg.linear.z
    current_goal.position.z = 1.5
    current_goal.yaw_rate = msg.angular.z
    lastTwistReceived = rospy.Time.now()

def get_yaw_from_quaternion(q):
    # 從四元數計算出 yaw 角度
    siny_cosp = 2 * (q.w * q.z + q.x * q.y)
    cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
    return math.atan2(siny_cosp, cosy_cosp)

# === 新增：目標點指令相關 ===
target_received = False     # 是否收到目標點指令
target_pose = None          # 目標點 Pose

def my_target_cb(msg):
    # 訂閱 /my_target_point，收到目標點指令時觸發
    global target_received, target_pose
    target_pose = msg
    target_received = True
    rospy.loginfo("收到新的目標點指令: x=%.2f, y=%.2f, z=%.2f" % (msg.pose.position.x, msg.pose.position.y, msg.pose.position.z))

if __name__ == "__main__":
    # 初始化 ROS 節點
    rospy.init_node("offboard_node")

    # 初始化控制目標與狀態
    current_goal = PositionTarget()
    current_state = State()
    lastTwistReceived = rospy.Time.now()
    current_pose = PoseStamped()

    # 訂閱無人機狀態
    state_sub = rospy.Subscriber("mavros/state", State, state_cb)
    # 訂閱速度指令
    twist_sub = rospy.Subscriber("/cmd_vel", Twist, twist_cb)
    # 訂閱目標點指令
    my_target_sub = rospy.Subscriber("/my_target_point", PoseStamped, my_target_cb)

    # 建立控制指令與定位資訊的 publisher
    local_pos_pub = rospy.Publisher("mavros/setpoint_raw/local", PositionTarget, queue_size=10)    # 發布位置/速度目標給 MAVROS
    vision_pos_pub = rospy.Publisher("mavros/vision_pose/pose", PoseStamped, queue_size=10)        # 發布視覺定位資訊給 MAVROS

    # 等待 MAVROS 服務（arming, command, set_mode）可用
    rospy.wait_for_service("/mavros/cmd/arming")
    arming_client = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)
    rospy.wait_for_service("/mavros/cmd/command")
    command_client = rospy.ServiceProxy("/mavros/cmd/command", CommandLong)
    rospy.wait_for_service("/mavros/set_mode")
    set_mode_client = rospy.ServiceProxy("/mavros/set_mode", SetMode)

    # 初始化 tf2 buffer 與 listener，用於查詢 /map → /base_link 的座標轉換
    tf_buffer = tf2_ros.Buffer()
    tf_listener = tf2_ros.TransformListener(tf_buffer)

    rate = rospy.Rate(50)

    # 等待與飛控連線
    while not rospy.is_shutdown() and not current_state.connected:
        rospy.loginfo_throttle(2, "wait for FCU connection...")
        rate.sleep()

    rospy.loginfo("Setting offboard mode... (5 seconds)")

    found_tf = False
    # 查詢一次 /map → /base_link 的 tf，取得初始位置
    for _ in range(50):
        try:
            trans = tf_buffer.lookup_transform("map", "base_link", rospy.Time(0), rospy.Duration(5))
            found_tf = True
            break
        except (tf2_ros.LookupException, tf2_ros.ExtrapolationException):
            rate.sleep()

    if not found_tf:
        rospy.logerr("Cannot get current position between /map and /base_link")
        exit(1)

    # 設定起飛初始點（座標、姿態）
    current_goal.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
    current_goal.type_mask = POSITION_CONTROL
    current_goal.position.x = trans.transform.translation.x
    current_goal.position.y = trans.transform.translation.y
    current_goal.position.z = 1.5    # 預設起飛高度(有改過)
    q = trans.transform.rotation        
    current_goal.yaw = get_yaw_from_quaternion(q)
    current_goal.velocity.x = 0
    current_goal.velocity.y = 0
    current_goal.velocity.z = 0
    current_goal.yaw_rate = 0
    current_goal.acceleration_or_force.x = 0
    current_goal.acceleration_or_force.y = 0
    current_goal.acceleration_or_force.z = 0

    rospy.loginfo("Initial position=(%.2f,%.2f,%.2f) yaw=%.2f",
                  current_goal.position.x, current_goal.position.y, trans.transform.translation.z, current_goal.yaw)

    # 起飛前先連續發送一段時間的 setpoint，讓 PX4 進入 offboard 模式
    for _ in range(100):
        current_goal.header.stamp = rospy.Time.now()
        local_pos_pub.publish(current_goal)
        rate.sleep()

    # 準備服務請求物件
    offb_set_mode = SetModeRequest()
    offb_set_mode.custom_mode = "OFFBOARD"
    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True
    disarm_cmd = CommandLongRequest()
    disarm_cmd.broadcast = False
    disarm_cmd.command = 400

    last_request = rospy.Time.now()
    lastTwistReceived = rospy.Time.now()
    current_pose = PoseStamped()
    current_pose.header.frame_id = "map"

    # 進入主迴圈
    while not rospy.is_shutdown():
        try:
            # 持續查詢 /map → /base_link 的 tf，取得無人機目前位置
            trans = tf_buffer.lookup_transform("map", "base_link", rospy.Time(0), rospy.Duration(0.05))
            current_pose.pose.position.x = trans.transform.translation.x
            current_pose.pose.position.y = trans.transform.translation.y
            current_pose.pose.position.z = trans.transform.translation.z
            current_pose.pose.orientation = trans.transform.rotation
        except (tf2_ros.LookupException, tf2_ros.ExtrapolationException):
            pass

        # 若尚未進入 OFFBOARD 模式，且間隔超過 5 秒則切換模式
        if current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_request > rospy.Duration(5.0)):
            if set_mode_client.call(offb_set_mode).mode_sent:
                rospy.loginfo("Offboard enabled")
                rospy.loginfo("Vehicle arming... (5 seconds)")
                last_request = rospy.Time.now()

        # 若尚未解鎖，且間隔超過 5 秒則解鎖
        elif (not current_state.armed and
              not (current_goal.velocity.z < -0.4 and current_goal.yaw_rate < -0.4) and
              (rospy.Time.now() - last_request > rospy.Duration(5.0))):
            if arming_client.call(arm_cmd).success:
                rospy.loginfo("Vehicle armed")
                rospy.loginfo("Take off at 1.5 meter... to position=(%.2f,%.2f,%.2f) yaw=%.2f",
                              current_goal.position.x, current_goal.position.y, current_goal.position.z, current_goal.yaw)
                last_request = rospy.Time.now()

        # 若速度與 yaw_rate 為負值（如搖桿下拉），則執行解鎖
        elif (current_goal.velocity.z < -0.4 and current_goal.yaw_rate < -0.4 and
              (rospy.Time.now() - last_request > rospy.Duration(5.0))):
            if command_client.call(disarm_cmd).success:
                rospy.loginfo("Vehicle disarmed")
                rospy.signal_shutdown("Disarmed by stick command")
            else:
                rospy.loginfo("Disarming failed! Still in flight?")
                last_request = rospy.Time.now()

        # 若超過 1 秒未收到速度指令，則切回位置控制模式
        if (current_goal.header.stamp.to_sec() - lastTwistReceived.to_sec() > 1 and
            current_goal.type_mask != POSITION_CONTROL):
            current_goal.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
            current_goal.type_mask = POSITION_CONTROL
            current_goal.position.x = current_pose.pose.position.x
            current_goal.position.y = current_pose.pose.position.y
            current_goal.position.z = 1.5
            q = current_pose.pose.orientation
            current_goal.yaw = get_yaw_from_quaternion(q)
            rospy.loginfo("Switch to position control (x=%.2f, y=%.2f, z=%.2f, yaw=%.2f)",
                          current_goal.position.x, current_goal.position.y, current_goal.position.z, current_goal.yaw)

        # === 新增：收到目標點指令才會飛行 ===
        if target_received and target_pose is not None:
            # 目標點模式：切換為位置控制，並設定目標點
            current_goal.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
            current_goal.type_mask = POSITION_CONTROL
            current_goal.position.x = target_pose.pose.position.x
            current_goal.position.y = target_pose.pose.position.y
            current_goal.position.z = target_pose.pose.position.z
            q = target_pose.pose.orientation
            current_goal.yaw = get_yaw_from_quaternion(q)
            rospy.loginfo("執行目標點飛行: x=%.2f, y=%.2f, z=%.2f, yaw=%.2f",
                          current_goal.position.x, current_goal.position.y, current_goal.position.z, current_goal.yaw)
            target_received = False

        # 發布 setpoint 與 vision pose，供 MAVROS 及 PX4 使用
        current_goal.header.stamp = rospy.Time.now()
        local_pos_pub.publish(current_goal)
        current_pose.header.stamp = current_goal.header.stamp
        vision_pos_pub.publish(current_pose)
        rate.sleep()

</details>

## 3. ORB-SLAM3指令與流程
### ORB-SLAM3
ORB-SLAM3 是目前最先進的開源視覺 SLAM 方案之一，其最大的優勢在於支援多種感測器組合（單目、雙目、RGB-D，以及各自的慣性融合版本）並具備高精度的定位與地圖重建能力。它基於特徵點法，透過稀疏地圖進行定位，並整合了完整的重定位、迴圈閉環與地圖合併功能。
專案的 GitHub 儲存庫與詳細安裝教學請參考：[ORB-SLAM3](https://github.com/UZ-SLAMLab/ORB_SLAM3)
雖然 ORB-SLAM3 官方提供了 ROS 範例，但我們選用了一個由社群開發、更專注於 ROS 整合的版本：[orb_slam3_ros](https://github.com/thien94/orb_slam3_ros)

### **1. 啟動 ORB-SLAM3 節點**
使用 `roslaunch` 指令啟動對應的 SLAM 模式。

#舉例
roslaunch orb_slam3_ros_wrapper t265.launch

### **2. 播放 Rosbag 資料集**

#舉例
rosbag play --pause MVS/Dataset/indoor_flying1_data.bag \
/davis/left/image_raw:=/camera/left/image_raw \
/davis/right/image_raw:=/camera/right/image_raw

**指令參數詳解：**
-   `--pause`: **(關鍵參數)** 播放開始時立即暫停。
-   `MVS/Dataset/... .bag`: 指定要播放的 `.bag` 檔案路徑。
-   `/davis/left/image_raw:=/camera/left/image_raw`: 
    這是 **Topic 重對映 (Remapping)**，也可以直接寫在launch檔裡。

ORB-SLAM3 畫面如下，會顯示bag檔影像的畫面，以及演算法的定位軌跡。
<figure style="text-align: center;">
<img src="https://hackmd.io/_uploads/Bkcf2mSElx.png" alt="ORB-SLAM3 Rviz 畫面">
<figcaption style="font-style: italic;">圖2：ORB-SLAM3 移動軌跡</figcaption>
</figure>

使用dronetest launch檔可以接取模擬無人機環境中的影像，指令如下：

＃同時運行rtabmap_drone_example
roslaunch orb_slam3_ros_wrapper dronetest.launch

<figure style="text-align: center;">
<img src="https://hackmd.io/_uploads/Hkw5TQSNgx.png" alt="ORB-SLAM3 Rviz 畫面">
<figcaption style="font-style: italic;">圖3：模擬環境中無人機飛行軌跡</figcaption>
</figure>

## 4. YOLO指令與流程
**同步訂閱數據**：節點會同時訂閱來自模擬器的兩個關鍵 Topic：
- `/camera/color/image_raw`: 標準的彩色影像，用於 YOLO 進行物件辨識。
- `/camera/depth/image_raw`: **深度影像**。這是一張灰階圖，但每個像素點的值直接對應著該點在真實世界中的距離（單位：公尺）。

### **1. 安裝 Ultralytics YOLO**
透過 `pip` 即可一鍵安裝所有必要的套件。

pip install ultralytics

### **2. 啟動YOLO節點**

roslaunch orb_slam3_ros_wrapper yolo_detect.launch

運行後，會接收模擬環境中無人機的拍攝到的影像，做物件偵測以及顯示該物體與無人機的距離（深度）
<figure style="text-align: center;">
<img src="https://hackmd.io/_uploads/H1Ici4BEel.png" alt="YOLO偵測結果">
<figcaption style="font-style: italic;">圖4：YOLO偵測結果</figcaption>
</figure>

## 5. EVO軌跡評估工具
在完成 SLAM 任務後，我們需要一個客觀、量化的方法來評估演算法的表現。我們使用 EVO (Evaluation of Odometry and SLAM)評估工具。可參考[這篇](https://blog.csdn.net/qq_28087491/article/details/129043366)
> 評估指標：APE vs. RPE
>
> **絕對軌跡誤差 (Absolute Pose Error - APE)**
> **評估目的**：衡量軌跡的**全域一致性 (Global Consistency)**。它直接比較估計軌跡與真實軌跡（Ground Truth）在對齊後的位置差異，最終給出一個整體的誤差值（如 RMSE - 均方根誤差）。APE 值越小，代表整個軌跡的形狀和位置越接近真實情況。
>
> **相對位姿誤差 (Relative Pose Error - RPE)**
> **評估目的**：衡量系統的**局部精度或漂移 (Drift)**。它關注的是軌跡在短時間內的變化量，非常適合用來評估系統的漂移累積速度。例如，它可以告訴我們無人機每飛行 1 公尺，會產生多少公分的平移或角度誤差。

### evo_traj：軌跡視覺化與初步檢查

evo_traj tum \
Examples/ROS/ORB_SLAM3/FrameTrajectory_TUM_Format.txt \
--ref MVS_inflying1_gt_od.tum \
-as -p --plot_mode=xyz

#參數說明
tum: 指定輸入檔案的格式為 TUM。
FrameTrajectory_TUM_Format.txt: SLAM 系統輸出的估計軌跡。
--ref MVS_inflying1_gt_od.tum: 指定真實軌跡 (Ground Truth)。
-as: 表示在繪圖前，先對兩條軌跡進行對齊與尺度校正 (Sim(3) alignment)。
-p 或 --plot: 指示 EVO 產生一個繪圖視窗。
--plot_mode=xyz: 指定繪製一個 3D 軌跡圖。

<figure style="text-align: center;">
<img src="https://hackmd.io/_uploads/S1Aa8OBNxx.png" alt="seq6_trajetories">
<figcaption style="font-style: italic;">圖5：軌跡視覺化比較圖</figcaption>
</figure>

### evo_ape：計算絕對軌跡誤差

evo_ape tum \
MVS_inflying1_gt_od.tum \
Examples/ROS/ORB_SLAM3/FrameTrajectory_TUM_Format.txt \
--align --plot

### evo_rpe：計算相對位姿誤差

evo_rpe tum \
MVS_inflying1_gt_od.tum \
Examples/ROS/ORB_SLAM3/FrameTrajectory_TUM_Format_second.txt \
--pose_relation angle_deg --delta 1 --delta_unit m --plot

## 6. ROS常用除錯工具與技巧
### rostopic：檢查Topic狀況
`rostopic` 是一個多功能的指令列工具集，用於深入探查單一 Topic 的詳細資訊。
**常用指令**：
1. **`rostopic list`**
- **用途**：列出當前所有正在被發布的 Topic。
2. **`rostopic info <topic_name>`**
- **用途**：顯示特定 Topic 的詳細資訊，包括它的訊息類型、發布者和訂閱者。
3. **`rostopic echo <topic_name>`**
- **用途**：**(最重要的除錯工具)** 直接在終端機中即時印出某個 Topic 正在傳輸的原始數據內容。
### rqt_image_view：視覺化影像數據
**用途**：對於影像類的 Topic，`rostopic echo` 會輸出一大堆無意義的數字。`rqt_image_view` 則提供了一個圖形化介面，可以直接將影像 Topic 的內容以圖片形式顯示出來。

#啟動
rqt_image_view

> 啟動後，在左上角的下拉式選單中，選擇您想查看的影像 Topic 即可。

## 7. 實體無人機參數設定及啟動流程
這裡彙整了從模擬環境轉向實體無人機時，所需的參數設定與軟體啟動流程。

### QGroundControl
<figure style="text-align: center;">
<img src="https://hackmd.io/_uploads/Hy1wnA0kbl.png
" alt="QGC">
<figcaption style="font-style: italic;">圖6：QGroundControl 軟體介面圖</figcaption>
</figure>

開源的地面控制站 (GCS) 軟體，主要使用到的功能如下:
- 參數配置：這是 QGC 最核心的功能之一。您可以存取飛控（如 PX4）中的所有參數。
- 硬體校準：包括校準指南針（羅盤）、加速度計、陀螺儀、遙控器 (RC) 和電變 (ESC) 等。
- 飛行模式：設定遙控器上的開關對應到哪些飛行模式（例如：Manual、Position、Offboard）。

### 7.1 PX4 飛控參數設定 (QGroundControl)

這一步是讓飛控「知道」它需要聽取外部視覺定位數據（如 ORB-SLAM3），而不是依賴 GPS。所有這些參數都可以在 QGroundControl (QGC) 中進行設定。

> 進入參數設定： 在 QGC 中，點選上方工具列的**「參數」**（Parameters）選項。

* **`EKF2_HGT_MODE`**
* **用途**：設定高度估計的來源。
* **設定**：在搜尋框中輸入 `EKF2_HGT_MODE`。將這個參數的值從 `GPS` 改為 **`Vision`** 。這會告訴飛控的高度估計應該來自外部視覺系統。(如果要室內飛行的話，要轉成依靠視覺定位)

* **`EKF2_EV_DELAY`**
* **用途**：設定視覺數據從感應器傳輸到飛控的延遲時間。
* **設定**：搜尋 `EKF2_EV_DELAY`。精確的數值非常關鍵。可以先從一個保守的值開始，例如 **100 毫秒 (100ms)**，然後根據實際情況進行微調。如果飛行時出現抖動或不穩定，這個值可能需要調整。(這是室內飛行才會用到的)

* **`MPC_XY_VEL_MAX`**
* **用途**：最大水平速度限制。這是飛控允許的最高水平速度（單位 m/s）。
* **設定**：在位置模式 (Position Mode) 或 Offboard 模式下，您透過搖桿或程式碼（例如 MAVROS 的 `setpoint_velocity`）發出的指令速度，最終會被限制在這個值以下。(慢速飛行比較能防止暴衝)

* **`MPC_LAND_SPEED`**
* **用途**：修改自動降落 (Auto Land) 時的下降速度。
* **設定**：可透過 `rosservice` 動態調整。例如，設定為 **0.3 m/s**
(QGC只能調到0.6m/s 可以用指令調)

        # 修改 Auto Land 降落速度
        rosservice call /mavros/param/set "{param_id: 'MPC_LAND_SPEED', value: {integer: 0, real: 0.3}}"
        
        # 查看目前參數
        rosservice call /mavros/param/get "{param_id: 'MPC_LAND_SPEED'}"

<figure style="text-align: center;">
<img src="https://hackmd.io/_uploads/HJD5h0R1Zx.png
" alt="QGC">
<figcaption style="font-style: italic;">圖7：Parameters 介面圖</figcaption>
</figure>

### 7.2 實體飛行啟動流程 (ORB-SLAM3 + Mavros)

這是在實體無人機上運行視覺定位的啟動順序。(可以啟動但尚未實機飛行)

1. **啟動 MavROS (無人機)**
* **用途**：建立機載電腦 (G700) 與 PX4 飛控之間的 MAVLink 通訊。
* **指令** (範例)：

        # 假設飛控連接到 /dev/ttyS2，波特率為 57600
        roslaunch mavros px4.launch fcu_url:=/dev/ttyS2:57600

2. **開啟相機 SDK**
* **用途**：啟動 RealSense 相機驅動，發布影像 Topic。
* **指令** (範例：D415)：

        # 啟動紅外線雙目
        roslaunch realsense2_camera rs_camera.launch \
                infra_width:=1280 infra_height:=720 infra_fps:=30 \
                enable_infra1:=true enable_infra2:=true

3. **開啟 ORB-SLAM3**
* **用途**：運行 SLAM 演算法，訂閱相機 Topic，並發布估計的攝影機姿態。
* **指令** (範例)：

        # 啟動適用於 RealSense 的 SLAM 節點
        roslaunch orb_slam3_ros realsense_d415.launch

4. **開啟 POSE 資訊轉換 (SLAM -> Mavros)**
* **用途**：啟動橋接腳本（見 7.3 節），將 ORB-SLAM3 輸出的姿態 Topic (如 `/orb_slam3_stereo/camera_pose`) 轉換並發布到 MAVROS 監聽的 Topic (如 `/mavros/vision_pose/pose`)。
* **指令** (範例)：

        # 假設你的橋接腳本名稱為 orb_mavros.py
        rosrun realsense2_camera orb_mavros.py

5. **驗證資料流**
* **用途**：確認 MAVROS 確實有接收到來自橋接腳本的視覺定位數據。
* **指令**：

        rostopic echo mavros/vision_pose/pose

* **檢查**：如果終端機持續輸出 `PoseStamped` 訊息，表示整個視覺定位管線已成功運作。

### 7.3 範例：SLAM 定位至 MavLink 橋接腳本

這個 Python 腳本（節點）的任務是訂閱 ORB-SLAM3 的姿態輸出，並將其轉發給 MAVROS。

#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import PoseStamped
import time

class SlamToMavrosBridge:
    def __init__(self):
        """
        初始化 SLAM 到 Mavros 的橋接節點。
        """
        rospy.init_node('slam_to_mavros_bridge', anonymous=True)
        
        # 建立一個發布者，將 PoseStamped 訊息發布給 Mavros
        # Mavros 會監聽 /mavros/vision_pose/pose 這個 Topic 來獲取外部定位資料
        self.vision_pose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=10)
        
        # 建立一個訂閱者，訂閱 ORB-SLAM3 發布的定位資訊
        # 根據你的 ORB-SLAM3 設置，這個 Topic 名稱可能需要調整
        rospy.Subscriber('/orb_slam3_stereo/camera_pose', PoseStamped, self.vision_pose_callback)
        
        # 等待 Mavros 服務啟動
        rospy.loginfo("Waiting for Mavros to be ready...")
        time.sleep(5) # 暫停 5 秒，確保 Mavros 已啟動

        rospy.loginfo("Bridge node started. Publishing vision poses to Mavros.")

    def vision_pose_callback(self, data):
        """
        將 ORB-SLAM3 的 PoseStamped 訊息直接轉發給 Mavros。
        此處假設 ORB-SLAM3 的輸出座標系與 Mavros 的期望座標系（例如 ENU）一致。
        """
        # 創建一個新的 PoseStamped 訊息，以確保時間戳是最新的
        px4_pose = PoseStamped()
        px4_pose.header = data.header
        px4_pose.header.stamp = rospy.Time.now()

        # 直接複製位置和姿態
        px4_pose.pose.position = data.pose.position
        px4_pose.pose.orientation = data.pose.orientation

        # 發布訊息
        self.vision_pose_pub.publish(px4_pose)

if __name__ == '__main__':
    try:
        # 創建並運行橋接器
        bridge = SlamToMavrosBridge()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass