PX4 官方指南的 ROS/MAVROS Offboard Control 模式

這是基於 PX4 官方指南的 ROS/MAVROS Offboard Control 模式 範例教學，目標是讓模擬的四旋翼（例如 Iris 模型）自動起飛到指定高度（通常是 2 公尺）。

這個範例使用 Python 編寫，是最常用且易於理解的 Offboard 控制基礎。

---

🚀 MAVROS Offboard Control 模式範例教學 (Python)

Offboard 模式允許您從外部電腦（透過 MAVROS）發送高層級的目標設定點（例如位置、速度或姿態），從而完全控制飛行器的導航。

步驟一：建立 ROS 工作空間與套件

如果您尚未建立 ROS 工作空間 (catkin_ws)，請先建立並編譯。如果已經有了，則直接在其中建立新的功能套件。

Bash

# 進入 catkin 工作空間的 src 目錄
roscd # 進入 catkin_ws/devel
cd ..
cd src

# 建立一個名為 offboard_py 的新套件，依賴 rospy
catkin_create_pkg offboard_py rospy geometry_msgs mavros_msgs std_msgs

# 返回工作空間根目錄並編譯
cd .. 
catkin build 

# 載入新的環境變數 (重要)
source devel/setup.bash

# 進入新套件目錄
roscd offboard_py

步驟二：編寫 Python 控制腳本

在 offboard_py 套件目錄下，建立一個 scripts 資料夾，並創建您的控制腳本。

Bash

# 創建 scripts 資料夾並進入
mkdir scripts
cd scripts

# 創建 Python 腳本並賦予執行權限
touch offb_node.py
chmod +x offb_node.py

接下來，用您慣用的編輯器（如 nano 或 vim）打開 offb_node.py，並貼入以下程式碼（此為官方範例的簡化版本，用於起飛到 2 公尺）：

Python

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest

current_state = State()

def state_cb(msg):
    """回調函數：儲存飛控的當前狀態 (連線, 加解鎖, 模式)"""
    global current_state
    current_state = msg

if __name__ == "__main__":
    rospy.init_node("offb_node_py")
    
    # 訂閱飛控狀態
    state_sub = rospy.Subscriber("mavros/state", State, callback = state_cb)
    
    # 發布本地位置設定點
    local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
    
    # 服務客戶端：用於加解鎖 (Arming) 和模式切換 (SetMode)
    arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
    set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)
    
    # **Offboard 命令的發布頻率必須 > 2Hz**，否則飛控會退出 Offboard 模式
    rate = rospy.Rate(20.0)

    # 1. 等待飛控連線
    rospy.loginfo("Waiting for FCU connection...")
    while(not rospy.is_shutdown() and not current_state.connected):
        rate.sleep()

    # 2. 定義目標位置：(0, 0, 2) - 在本地 NED 座標系下，2 米高度
    pose = PoseStamped()
    pose.pose.position.x = 0
    pose.pose.position.y = 0
    pose.pose.position.z = 2

    # 3. 確保飛控在進入 Offboard 模式前，至少已經接收到 100 個設定點
    rospy.loginfo("Sending initial setpoints...")
    for i in range(100):
        if(rospy.is_shutdown()):
            break
        local_pos_pub.publish(pose)
        rate.sleep()

    # 4. 準備 Offboard 模式和 Arming 命令
    offb_set_mode = SetModeRequest()
    offb_set_mode.custom_mode = 'OFFBOARD'

    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True

    last_request = rospy.get_time()

    # 5. 進入主控制循環
    rospy.loginfo("Starting Offboard Control Loop...")
    while(not rospy.is_shutdown()):
        
        # 發送設定點
        local_pos_pub.publish(pose)

        # 檢查當前狀態，並嘗試切換模式和加解鎖
        if(current_state.mode != "OFFBOARD" and (rospy.get_time() - last_request > 5.0)):
            # 嘗試切換到 OFFBOARD 模式
            if(set_mode_client.call(offb_set_mode).mode_sent == True):
                rospy.loginfo("Offboard enabled")
            last_request = rospy.get_time()
        
        elif(not current_state.armed and (rospy.get_time() - last_request > 5.0)):
            # 嘗試加解鎖 (Arm)
            if(arming_client.call(arm_cmd).success == True):
                rospy.loginfo("Vehicle armed")
            last_request = rospy.get_time()

        rate.sleep()

步驟三：啟動模擬與控制

您需要開啟 兩個 終端機視窗來運行模擬和 ROS 控制節點。

終端機 1：啟動 PX4 SITL 和 Gazebo

進入您的 PX4-Autopilot 原始碼目錄，運行 SITL 模擬。

Bash

cd ~/PX4-Autopilot
# 啟動 Iris 模型和 Gazebo
make px4_sitl gazebo_iris

（如果您使用的是 Gazebo Classic，請使用 make px4_sitl gazebo-classic_iris）

成功啟動後，您會看到 Gazebo 窗口和一個 PX4 Shell 提示符。

終端機 2：運行 ROS 控制節點

開啟第二個終端機，載入 ROS 工作空間環境，並運行您剛編寫的 Python 腳本。

Bash

# 進入您的 ROS 工作空間
cd ~/catkin_ws
source devel/setup.bash

# 運行 offboard 控制節點
rosrun offboard_py offb_node.py

預期結果

1. 終端機 2 會開始打印日誌，先是等待飛控連線，然後發送初始化設定點。

2. 幾秒後，終端機 2 會打印 Offboard enabled 和 Vehicle armed。

3. 在 Gazebo 窗口中，Iris 四旋翼模型將會自動起飛，並穩定懸停在 2 公尺 的高度。

---

警告： Offboard 控制模式是危險的。在真實飛行器上操作時，請務必設置實體開關，以便在緊急情況下迅速切換回手動控制。