#!/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")
last_request = rospy.get_rostime()
while((rospy.get_rostime() - last_request) < rospy.Duration(8.0)):
持续 8 秒向飞控发送起飞目标点
position_pub.publish(des_pos)
rate.sleep()
-------- 飞往第二个点 --------
print("Fly to Point 2")
des_pos.pose.position.x = 0
des_pos.pose.position.y = 0.5
des_pos.pose.position.z = 1
修改目标点坐标
last_request = rospy.get_rostime()
while((rospy.get_rostime() - last_request) < rospy.Duration(2.0)):
position_pub.publish(des_pos)
rate.sleep()
-------- 飞往第三个点 --------
print("Fly to Point 3")
des_pos.pose.position.x = 0.5
des_pos.pose.position.y = 0.5
des_pos.pose.position.z = 1
last_request = rospy.get_rostime()
while((rospy.get_rostime() - last_request) < rospy.Duration(2.0)):
position_pub.publish(des_pos)
rate.sleep()
-------- 飞往第四个点 --------
print("Fly to Point 4")
des_pos.pose.position.x = 0.5
des_pos.pose.position.y = 0
des_pos.pose.position.z = 1
last_request = rospy.get_rostime()
while((rospy.get_rostime() - last_request) < rospy.Duration(2.0)):
position_pub.publish(des_pos)
rate.sleep()
-------- 返回起点 --------
print("Fly back to Point 1")
des_pos.pose.position.x = 0
des_pos.pose.position.y = 0
des_pos.pose.position.z = 1
last_request = rospy.get_rostime()
while((rospy.get_rostime() - last_request) < rospy.Duration(3.0)):
position_pub.publish(des_pos)
rate.sleep()
-------- 任务结束 --------
print("Vehicle Landing")
注意：这里只是打印，没有真正发送降落指令
print("Mission End")
---------------- 程序入口 ----------------
if name == 'main':
try:
offb_control_sample()
调用主控制函数
except rospy.ROSInterruptException:
pass
捕获 ROS 中断异常，防止程序崩溃
🔍 关键理解点（帮你快速吃透）
-
OFFBOARD 必须持续发布 setpoint（否则会自动退出）
-
解锁前必须已经是 OFFBOARD
-
PoseStamped使用的是 本地坐标系（ENU） -
这个程序 没有真正降落，只是结束了任务
发 setpoint（>2Hz）
↓
切 OFFBOARD
↓
确认 mode == OFFBOARD
↓
解锁（Arming）