22.1 Offboard 模式控制
PX4 进入 Offboard 模式前必须满足：至少以 2 Hz（推荐 20Hz+）持续发送设定点，切换前至少发送 2 秒，中断超过 0.5 秒将自动退出。
22.1.1 位置控制
return d < threshold
def arm_and_offboard(self):
for _ in range(40):
rclpy.spin_once(self, timeout_sec=0.05)
req = SetMode.Request(); req.custom_mode = 'OFFBOARD'
f = self.mode_client.call_async(req)
rclpy.spin_until_future_complete(self, f)
req = CommandBool.Request(); req.value = True
f = self.arm_client.call_async(req)
rclpy.spin_until_future_complete(self, f)
return f.result() and f.result().success
def main(args=None):
rclpy.init(args=args)
ctrl = OffboardPositionControl()
while not ctrl.current_state.connected:
rclpy.spin_once(ctrl, timeout_sec=0.5)
ctrl.arm_client.wait_for_service()
ctrl.mode_client.wait_for_service()
if not ctrl.arm_and_offboard():
return
waypoints = (0,0,5,0), (5,0,5,0), (5,5,5,90), (0,5,5,180), (0,0,5,270)
try:
for i, (x,y,z,yaw) in enumerate(waypoints):
ctrl.get_logger().info(f'航点 {i+1}: ({x},{y},{z}) yaw={yaw}')
ctrl.set_target(x, y, z, yaw)
for _ in range(300):
rclpy.spin_once(ctrl, timeout_sec=0.1)
if ctrl.is_at_target():
break
except KeyboardInterrupt:
pass
finally:
ctrl.destroy_node()
rclpy.shutdown()
if name == 'main':
main()
22.1.2 速度控制
#!/usr/bin/env python3
"""offboard_velocity.py - Offboard 速度控制"""
import rclpy, time
from rclpy.node import Node
from geometry_msgs.msg import TwistStamped
from mavros_msgs.srv import CommandBool, SetMode
from mavros_msgs.msg import State
class OffboardVelocityControl(Node):
def init(self):
super().init('offboard_velocity_control')
self.state = State()
self.vx = self.vy = self.vz = self.vyaw = 0.0
self.create_subscription(State, '/mavros/state',
lambda m: setattr(self, 'state', m), 10)
self.vel_pub = self.create_publisher(
TwistStamped, '/mavros/setpoint_velocity/cmd_vel', 10)
self.arm_client = self.create_client(CommandBool, '/mavros/cmd/arming')
self.mode_client = self.create_client(SetMode, '/mavros/set_mode')
self.create_timer(0.05, self.publish_vel)
def publish_vel(self):
msg = TwistStamped()
msg.header.stamp = self.get_clock().now().to_msg()
msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z = self.vx, self.vy, self.vz
msg.twist.angular.z = self.vyaw
self.vel_pub.publish(msg)
def set_velocity(self, vx, vy, vz, vyaw=0.0):
self.vx, self.vy, self.vz, self.vyaw = vx, vy, vz, vyaw
def main(args=None):
rclpy.init(args=args)
ctrl = OffboardVelocityControl()
while not ctrl.state.connected:
rclpy.spin_once(ctrl, timeout_sec=0.5)
ctrl.set_velocity(0, 0, 0)
for _ in range(40): rclpy.spin_once(ctrl, timeout_sec=0.05)
req = SetMode.Request(); req.custom_mode = 'OFFBOARD'
f = ctrl.mode_client.call_async(req)
rclpy.spin_until_future_complete(ctrl, f)
req = CommandBool.Request(); req.value = True
f = ctrl.arm_client.call_async(req)
rclpy.spin_until_future_complete(ctrl, f)
try:
ctrl.set_velocity(0, 0, 1.0); time.sleep(5) # 上升
ctrl.set_velocity(1.0, 0, 0); time.sleep(5) # 前进
ctrl.set_velocity(0, 0, 0); time.sleep(3) # 悬停
ctrl.set_velocity(0, 0, -0.5); time.sleep(10) # 下降
except KeyboardInterrupt:
pass
finally:
ctrl.destroy_node(); rclpy.shutdown()
if name == 'main':
main()
22.2 自主起飞-巡航-降落完整流程
self.state = State()
self.pose = PoseStamped()
self.target = 0.0, 0.0, 0.0, 0.0
self.create_subscription(State, '/mavros/state',
lambda m: setattr(self, 'state', m), 10)
self.create_subscription(PoseStamped, '/mavros/local_position/pose',
lambda m: setattr(self, 'pose', m), 10)
self.pub = self.create_publisher(PoseStamped, '/mavros/setpoint_position/local', 10)
self.arm_client = self.create_client(CommandBool, '/mavros/cmd/arming')
self.mode_client = self.create_client(SetMode, '/mavros/set_mode')
self.land_client = self.create_client(CommandTOL, '/mavros/cmd/land')
self.create_timer(0.05, self._send_sp)
def _send_sp(self):
msg = PoseStamped()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = 'map'
msg.pose.position.x, msg.pose.position.y, msg.pose.position.z = \
self.target0, self.target1, self.target2
msg.pose.orientation.z = math.sin(self.target3 / 2.0)
msg.pose.orientation.w = math.cos(self.target3 / 2.0)
self.pub.publish(msg)
def dist(self, x, y, z):
p = self.pose.pose.position
return math.sqrt((p.x-x)**2 + (p.y-y)**2 + (p.z-z)**2)
def goto(self, x, y, z, yaw_deg=0, timeout=30):
self.target = x, y, z, math.radians(yaw_deg)
start = time.time()
while rclpy.ok():
if self.dist(x, y, z) < 0.5:
return True
if time.time() - start > timeout:
return False
rclpy.spin_once(self, timeout_sec=0.1)
def run(self, alt, waypoints):
while not self.state.connected: rclpy.spin_once(self, timeout_sec=0.5)
self.arm_client.wait_for_service()
self.mode_client.wait_for_service()
预发送设定点 3 秒
self.target = 0, 0, alt, 0
for _ in range(60): rclpy.spin_once(self, timeout_sec=0.05)
Offboard + 解锁
req = SetMode.Request(); req.custom_mode = 'OFFBOARD'
rclpy.spin_until_future_complete(self, self.mode_client.call_async(req))
req = CommandBool.Request(); req.value = True
rclpy.spin_until_future_complete(self, self.arm_client.call_async(req))
起飞
self.get_logger().info(f'起飞到 {alt}m...')
self.goto(0, 0, alt, timeout=20)
time.sleep(2)
巡航
for i, (x,y,z,yaw) in enumerate(waypoints):
self.get_logger().info(f'航点 {i+1}/{len(waypoints)}')
self.goto(x, y, z, yaw, timeout=60)
time.sleep(1)
返回 + 降落
self.goto(0, 0, alt, timeout=60)
req = CommandTOL.Request()
rclpy.spin_until_future_complete(self, self.land_client.call_async(req))
self.get_logger().info('任务完成！')
def main(args=None):
rclpy.init(args=args)
m = AutoFlightMission()
try:
m.run(alt=5.0, waypoints=[
(10,0,8,0), (10,10,8,90), (0,10,10,180), (0,0,8,270)])
except KeyboardInterrupt:
pass
finally:
m.destroy_node(); rclpy.shutdown()
if name == 'main':
main()
22.3 航点任务编程（Mission Protocol）
#!/usr/bin/env python3
"""mission_planner.py - 航点任务规划与上传"""
import rclpy, time
from rclpy.node import Node
from mavros_msgs.srv import WaypointPush, WaypointClear, SetMode
from mavros_msgs.msg import Waypoint, State
class MissionPlanner(Node):
def init(self):
super().init('mission_planner')
self.state = State()
self.create_subscription(State, '/mavros/state',
lambda m: setattr(self, 'state', m), 10)
self.push_client = self.create_client(WaypointPush, '/mavros/mission/push')
self.clear_client = self.create_client(WaypointClear, '/mavros/mission/clear')
self.mode_client = self.create_client(SetMode, '/mavros/set_mode')
@staticmethod
def make_wp(lat, lon, alt, cmd=16, frame=3, current=False):
w = Waypoint()
w.frame = frame; w.command = cmd; w.is_current = current
w.autocontinue = True; w.x_lat = lat; w.y_long = lon; w.z_alt = alt
return w
def upload(self, waypoints):
req = WaypointPush.Request(); req.waypoints = waypoints
f = self.push_client.call_async(req)
rclpy.spin_until_future_complete(self, f)
r = f.result()
ok = r and r.success
self.get_logger().info(f'上传 {"成功 "+str(r.wp_transferred)+" 航点" if ok else "失败"}')
return ok
def start(self):
req = SetMode.Request(); req.custom_mode = 'AUTO.MISSION'
f = self.mode_client.call_async(req)
rclpy.spin_until_future_complete(self, f)
return f.result() and f.result().mode_sent
def main(args=None):
rclpy.init(args=args)
p = MissionPlanner()
while not p.state.connected: rclpy.spin_once(p, timeout_sec=0.5)
p.push_client.wait_for_service()
矩形航线（北京测试场坐标示例）
lat, lon, alt = 39.9042, 116.4074, 30.0
wps = [
p.make_wp(lat, lon, alt, cmd=22, current=True), # 起飞
p.make_wp(lat, lon+0.0009, alt), # 东 100m
p.make_wp(lat+0.0009, lon+0.0009, alt), # 东北
p.make_wp(lat+0.0009, lon, alt), # 北
p.make_wp(lat, lon, alt), # 返回
p.make_wp(lat, lon, 0, cmd=21), # 降落
]
rclpy.spin_until_future_complete(p, p.clear_client.call_async(
import('mavros_msgs.srv', fromlist='WaypointClear').WaypointClear.Request()))
time.sleep(1)
if p.upload(wps):
time.sleep(1)
p.start()
p.destroy_node(); rclpy.shutdown()
if name == 'main':
main()
22.4 视觉辅助功能
AprilTag 精准降落
#!/usr/bin/env python3
"""apriltag_landing.py - AprilTag 精准降落"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import TwistStamped
from cv_bridge import CvBridge
import numpy as np
class AprilTagLanding(Node):
def init(self):
super().init('apriltag_landing')
self.bridge = CvBridge()
self.tag_detected = False
self.tag_info = None
self.tag_size = 0.15 # 米
self.kp_xy = 0.5
self.land_speed = 0.3
self.create_subscription(Image, '/camera/image_raw', self.image_cb, 10)
self.cmd_pub = self.create_publisher(
TwistStamped, '/mavros/setpoint_velocity/cmd_vel', 10)
self.create_timer(0.05, self.control_cb)
def image_cb(self, msg):
try:
import apriltag, cv2
img = self.bridge.imgmsg_to_cv2(msg, 'mono8')
detector = apriltag.Detector(apriltag.DetectorOptions(families='tag36h11'))
results = detector.detect(img)
if results:
tag = max(results, key=lambda t: t.decision_margin)
self.tag_detected = True
h, w = img.shape:2
cx, cy = tag.center
px_size = max(np.linalg.norm(tag.corners0-tag.corners1),
np.linalg.norm(tag.corners1-tag.corners2))
dist = (self.tag_size * 600.0) / px_size # 近似距离
self.tag_info = {
'dx': (cx - w/2) / (w/2) * dist * 0.5,
'dy': (cy - h/2) / (h/2) * dist * 0.5,
'dist': dist
}
else:
self.tag_detected = False
self.tag_info = None
except ImportError:
pass
def control_cb(self):
cmd = TwistStamped()
cmd.header.stamp = self.get_clock().now().to_msg()
if self.tag_detected and self.tag_info:
cmd.twist.linear.x = -self.kp_xy * self.tag_info'dx'
cmd.twist.linear.y = -self.kp_xy * self.tag_info'dy'
cmd.twist.linear.z = -self.land_speed * (0.3 if self.tag_info'dist' < 0.1 else 0.5)
self.cmd_pub.publish(cmd)
def main(args=None):
rclpy.init(args=args)
rclpy.spin(AprilTagLanding())
rclpy.shutdown()
if name == 'main':
main()
22.5 避障与路径规划集成
#!/usr/bin/env python3
"""obstacle_avoidance.py - 基于点云的 3D 避障"""
import rclpy, struct
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
from geometry_msgs.msg import TwistStamped
import numpy as np
class ObstacleAvoidance(Node):
def init(self):
super().init('obstacle_avoidance')
self.min_dist = 2.0
self.slow_dist = 5.0
self.max_speed = 2.0
self.create_subscription(PointCloud2, '/camera/depth/points', self.cloud_cb, 10)
self.cmd_pub = self.create_publisher(TwistStamped, '/avoidance/cmd_vel', 10)
def cloud_cb(self, msg):
pts = self._parse(msg)
if pts is None or len(pts) == 0: return
fwd = pts(pts\[:,0>0) & (pts:,0<self.slow_dist) &
(np.abs(pts:,1)<2.0) & (np.abs(pts:,2)<1.5)]
cmd = TwistStamped()
cmd.header.stamp = self.get_clock().now().to_msg()
if len(fwd) > 0:
md = np.min(fwd:,0)
if md < self.min_dist:
left = np.sum(fwd:,1>0); right = np.sum(fwd:,1<0)
cmd.twist.linear.y = 0.5 if left < right else -0.5
cmd.twist.linear.z = 0.15
self.get_logger().warn(f'障碍物 {md:.1f}m, 避障')
else:
cmd.twist.linear.x = self.max_speed * (md-self.min_dist)/(self.slow_dist-self.min_dist)
else:
cmd.twist.linear.x = self.max_speed
self.cmd_pub.publish(cmd)
def _parse(self, msg):
try:
off = {f.name: f.offset for f in msg.fields if f.name in 'xyz'}
if len(off) < 3: return None
n = msg.width * msg.height
pts = np.zeros((n, 3))
for i in range(n):
s = i * msg.point_step
for j, k in enumerate('xyz'):
ptsi,j = struct.unpack_from('f', msg.data, s+offk)0
return pts
except: return None
def main(args=None):
rclpy.init(args=args); rclpy.spin(ObstacleAvoidance()); rclpy.shutdown()
if name == 'main':
main()
Nav2 适配配置 nav2_drone_params.yaml：
controller_server:
ros__parameters:
controller_frequency: 20.0
FollowPath:
plugin: "dwb_core::DWBLocalPlanner"
max_vel_x: 3.0 # 最大前进速度 m/s
max_vel_y: 2.0 # 最大横向速度
acc_lim_x: 1.5
acc_lim_y: 1.0
planner_server:
ros__parameters:
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 1.0
22.6 多机协同编队通信架构
#!/usr/bin/env python3
"""formation_commander.py - 多机编队指挥节点"""
import rclpy, math, json, time
from rclpy.node import Node
from std_msgs.msg import String, Float64MultiArray
from geometry_msgs.msg import PoseStamped
class FormationCommander(Node):
def init(self, num_drones=3):
super().init('formation_commander')
self.num = num_drones
self.states = {}
self.cmd_pub = self.create_publisher(String, '/formation/command', 10)
self.tgt_pub = self.create_publisher(Float64MultiArray, '/formation/targets', 10)
for i in range(1, num_drones+1):
self.create_subscription(PoseStamped, f'/drone{i}/mavros/local_position/pose',
self._mk_cb(i), 10)
def _mk_cb(self, did):
return lambda m: self.states.update({did: (m.pose.position.x, m.pose.position.y, m.pose.position.z)})
def _line(self, c, sp=3.0):
return (c\[0+(i-self.num/2)*sp, c1, c2) for i in range(self.num)]
def _triangle(self, c, sp=3.0):
pts = (c\[0, c1+sp, c2), (c0-sp, c1-sp, c2), (c0+sp, c1-sp, c2)]
return pts:self.num
def _circle(self, c, r=5.0):
return (c\[0+r*math.cos(2*math.pi*i/self.num),
c1+r*math.sin(2*math.pi*i/self.num), c2) for i in range(self.num)]
def set_formation(self, name, center, **kw):
fn = {'line': self._line, 'triangle': self._triangle, 'circle': self._circle}.get(name)
if not fn: return
targets = fn(center, **kw)
self.cmd_pub.publish(String(data=json.dumps({'formation': name, 'targets': targets})))
msg = Float64MultiArray(); msg.data.extend(t) for t in targets
self.tgt_pub.publish(msg)
self.get_logger().info(f'编队: {name} -> {targets}')
def main(args=None):
rclpy.init(args=args)
cmd = FormationCommander(3)
try:
time.sleep(2)
cmd.set_formation('triangle', (10,0,5))
time.sleep(10)
cmd.set_formation('line', (20,0,5))
time.sleep(10)
cmd.set_formation('circle', (30,0,8), r=4.0)
rclpy.spin(cmd)
except KeyboardInterrupt:
pass
finally:
cmd.destroy_node(); rclpy.shutdown()
if name == 'main':
main()
22.7 飞行数据记录与 ROS2 Bag 回放分析
import numpy as np, math, csv
class FlightAnalyzer:
def init(self, bag_path):
self.bag_path = bag_path
self.ts, self.px, self.py, self.pz = \[\], \[\], \[\], \[\]
self.vx, self.vy, self.vz = \[\], \[\], \[\]
self.roll, self.pitch, self.yaw = \[\], \[\], \[\]
self.volt, self.lat, self.lon, self.alt = \[\], \[\], \[\], \[\]
def read(self):
reader = SequentialReader()
reader.open(StorageOptions(uri=self.bag_path, storage_id='sqlite3'),
ConverterOptions('cdr', 'cdr'))
while reader.has_next():
topic, data, t = reader.read_next()
ts = t / 1e9
if topic == '/mavros/local_position/pose':
m = deserialize_message(data, PoseStamped)
self.ts.append(ts)
self.px.append(m.pose.position.x)
self.py.append(m.pose.position.y)
self.pz.append(m.pose.position.z)
q = m.pose.orientation
self.roll.append(math.degrees(math.atan2(2*(q.w*q.x+q.y*q.z), 1-2*(q.x**2+q.y**2))))
self.pitch.append(math.degrees(math.asin(max(-1,min(1,2*(q.w*q.y-q.z*q.x))))))
self.yaw.append(math.degrees(math.atan2(2*(q.w*q.z+q.x*q.y), 1-2*(q.y**2+q.z**2))))
elif topic == '/mavros/battery':
m = deserialize_message(data, BatteryState)
self.volt.append(m.voltage)
def stats(self):
print("\n=== 飞行统计 ===")
if self.pz: print(f" 最大高度: {max(self.pz):.2f} m")
if self.vx:
spd = np.sqrt(np.array(self.vx)**2+np.array(self.vy)**2+np.array(self.vz)**2)
print(f" 最大速度: {np.max(spd):.2f} m/s, 平均: {np.mean(spd):.2f} m/s")
if self.ts: print(f" 飞行时长: {self.ts-1-self.ts0:.1f} s")
if self.volt: print(f" 电压范围: {min(self.volt):.1f}V ~ {max(self.volt):.1f}V")
def export_csv(self, path='report.csv'):
with open(path, 'w', newline='') as f:
w = csv.writer(f)
w.writerow('ts','x','y','z','roll','pitch','yaw')
for i in range(len(self.ts)):
w.writerow(self.ts\[i,self.pxi,self.pyi,self.pzi,
self.rolli,self.pitchi,self.yawi])
print(f"CSV: {path}")
if name == 'main':
import sys
a = FlightAnalyzer(sys.argv1 if len(sys.argv)>1 else 'flight_data')
a.read(); a.stats(); a.export_csv()