无人机按点飞行脚本

08 Apr 2026 — 5 min read

#!/usr/bin/env python
# 指定使用系统中的 python 解释器来运行该脚本（Linux 下 ROS 必须）

import rospy
# ROS 的 Python 客户端库，用于节点、话题、服务等操作

import mavros
# MAVROS 的 Python 接口库，用于和飞控（PX4/ArduPilot）通信

from geometry_msgs.msg import PoseStamped
# 引入带时间戳的位姿消息，用于发布位置控制指令

from mavros_msgs.msg import State
# 引入飞控状态消息（连接状态、模式、解锁状态等）

from mavros_msgs.srv import CommandBool, SetMode
# 引入解锁服务（CommandBool）和模式切换服务（SetMode）

from geometry_msgs.msg import Point
# 引入三维点类型（这里其实没用到，可以删）

# ---------------- 全局变量 ----------------

current_state = State()
# 保存当前飞控状态的全局变量

# ---------------- 回调函数 ----------------

def state_callback(state):
global current_state
# 声明使用全局变量 current_state

current_state = state
# 将订阅到的飞控状态保存到全局变量中

# ---------------- MAVROS 命名空间 ----------------

mavros.set_namespace()
# 设置 MAVROS 命名空间（一般为 /mavros）

des_pos = PoseStamped()
# 期望位置（目标点），用于 offboard 位置控制

# ---------------- 订阅 & 发布 ----------------

state_sub = rospy.Subscriber(
'mavros/state',
State,
callback=state_callback
)
# 订阅飞控状态话题 /mavros/state

position_pub = rospy.Publisher(
'mavros/setpoint_position/local',
PoseStamped,
queue_size=10
)
# 发布本地坐标系下的位置期望，用于 OFFBOARD 模式

# ---------------- 服务客户端 ----------------

arming_drone = rospy.ServiceProxy(
'/mavros/cmd/arming',
CommandBool
)
# 解锁/上锁飞控的服务客户端

setting_mode = rospy.ServiceProxy(
'mavros/set_mode',
SetMode
)
# 设置飞控模式（如 OFFBOARD）的服务客户端

# ---------------- 主控制函数 ----------------

def offb_control_sample():
print("Init Node: Offboard_node")
# 打印提示信息

rospy.init_node('Offboard_node', anonymous=True)
# 初始化 ROS 节点，节点名为 Offboard_node

prev_state = current_state
# 保存之前的状态（这里实际上没用到）

rate = rospy.Rate(20.0)
# 设置循环频率为 20Hz（OFFBOARD 至少 2Hz，推荐 >10Hz）

# -------- 初始目标点 --------
des_pos.pose.position.x = 0
des_pos.pose.position.y = 0
des_pos.pose.position.z = 1
# 设置初始目标位置：起飞到高度 1 米

# -------- 等待飞控连接 --------
while(not rospy.is_shutdown() and not current_state.connected):
print("Waiting for FCU connection")
# 如果还未连接飞控，持续等待

rate.sleep()
# 按 20Hz 睡眠，避免死循环

print("FCU connected")
# 成功连接飞控

# -------- OFFBOARD 前必须先发送一段 setpoint --------
for i in range(60):
# 发送 60 次位置指令（约 3 秒）

if(rospy.is_shutdown()):
break
# 如果 ROS 被关闭，跳出循环

position_pub.publish(des_pos)
# 发布目标位置

rate.sleep()

print("Switching to offboard Mode")
# 提示：开始切换 OFFBOARD 模式

setting_mode(base_mode=0, custom_mode="OFFBOARD")
# 请求切换飞控模式为 OFFBOARD

last_request = rospy.get_rostime()
# 记录当前时间

while((rospy.get_rostime() - last_request) < rospy.Duration(2.0)):
# 持续 2 秒发布 setpoint，防止 OFFBOARD 失效

position_pub.publish(des_pos)
rate.sleep()

print("Arming the drone on offboard Mode")
# 提示：在 OFFBOARD 模式下解锁电机

if current_state.mode == "OFFBOARD":
# 确认当前模式是 OFFBOARD

arming_drone(True)
# 解锁飞控（电机上电）

# -------- 起飞并悬停 --------
print("Take off to Point 1")