331 lines
12 KiB
Python
331 lines
12 KiB
Python
"""
|
||
ROS1 + MAVROS：按 ValidatedFlightIntent 顺序执行（Offboard 位姿 / AUTO.LAND / AUTO.RTL）。
|
||
|
||
需在已连接飞控的 MAVROS 环境中运行（与 src/px4_ctrl_offboard_demo.py 相同前提）。
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import math
|
||
from typing import Optional, Tuple
|
||
|
||
import rospy
|
||
from geometry_msgs.msg import PoseStamped
|
||
from mavros_msgs.msg import PositionTarget, State
|
||
from mavros_msgs.srv import CommandBool, SetMode
|
||
|
||
from voice_drone.core.flight_intent import (
|
||
ActionGoto,
|
||
ActionHold,
|
||
ActionHover,
|
||
ActionLand,
|
||
ActionReturnHome,
|
||
ActionTakeoff,
|
||
ActionWait,
|
||
FlightAction,
|
||
ValidatedFlightIntent,
|
||
)
|
||
|
||
|
||
def _yaw_from_quaternion(x: float, y: float, z: float, w: float) -> float:
|
||
siny_cosp = 2.0 * (w * z + x * y)
|
||
cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
|
||
return math.atan2(siny_cosp, cosy_cosp)
|
||
|
||
|
||
class MavrosFlightExecutor:
|
||
"""单次连接 MAVROS；对外提供 execute(intent)。"""
|
||
|
||
def __init__(self) -> None:
|
||
rospy.init_node("flight_intent_mavros_bridge", anonymous=True)
|
||
|
||
self.state = State()
|
||
self.pose = PoseStamped()
|
||
self.has_pose = False
|
||
|
||
rospy.Subscriber("/mavros/state", State, self._state_cb, queue_size=10)
|
||
rospy.Subscriber(
|
||
"/mavros/local_position/pose",
|
||
PoseStamped,
|
||
self._pose_cb,
|
||
queue_size=10,
|
||
)
|
||
self.sp_pub = rospy.Publisher(
|
||
"/mavros/setpoint_raw/local",
|
||
PositionTarget,
|
||
queue_size=20,
|
||
)
|
||
|
||
rospy.wait_for_service("/mavros/cmd/arming", timeout=60.0)
|
||
rospy.wait_for_service("/mavros/set_mode", timeout=60.0)
|
||
self._arming_name = "/mavros/cmd/arming"
|
||
self._set_mode_name = "/mavros/set_mode"
|
||
self.arm_srv = rospy.ServiceProxy(self._arming_name, CommandBool)
|
||
self.mode_srv = rospy.ServiceProxy(self._set_mode_name, SetMode)
|
||
|
||
self.rate = rospy.Rate(20)
|
||
|
||
self.default_takeoff_relative_m = rospy.get_param(
|
||
"~default_takeoff_relative_m",
|
||
0.5,
|
||
)
|
||
self.takeoff_timeout_sec = rospy.get_param("~takeoff_timeout_sec", 15.0)
|
||
self.goto_tol = rospy.get_param("~goto_position_tolerance", 0.15)
|
||
self.goto_timeout_sec = rospy.get_param("~goto_timeout_sec", 60.0)
|
||
self.land_timeout_sec = rospy.get_param("~land_timeout_sec", 45.0)
|
||
self.pre_stream_count = int(rospy.get_param("~offboard_pre_stream_count", 80))
|
||
|
||
def _state_cb(self, msg: State) -> None:
|
||
self.state = msg
|
||
|
||
def _pose_cb(self, msg: PoseStamped) -> None:
|
||
self.pose = msg
|
||
self.has_pose = True
|
||
|
||
def _wait_connected_and_pose(self) -> None:
|
||
rospy.loginfo("等待 FCU 与本地位置 …")
|
||
while not rospy.is_shutdown() and not self.state.connected:
|
||
self.rate.sleep()
|
||
while not rospy.is_shutdown() and not self.has_pose:
|
||
self.rate.sleep()
|
||
|
||
@staticmethod
|
||
def _position_sp(x: float, y: float, z: float) -> PositionTarget:
|
||
sp = PositionTarget()
|
||
sp.header.stamp = rospy.Time.now()
|
||
sp.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
|
||
sp.type_mask = (
|
||
PositionTarget.IGNORE_VX
|
||
| PositionTarget.IGNORE_VY
|
||
| PositionTarget.IGNORE_VZ
|
||
| PositionTarget.IGNORE_AFX
|
||
| PositionTarget.IGNORE_AFY
|
||
| PositionTarget.IGNORE_AFZ
|
||
| PositionTarget.IGNORE_YAW
|
||
| PositionTarget.IGNORE_YAW_RATE
|
||
)
|
||
sp.position.x = x
|
||
sp.position.y = y
|
||
sp.position.z = z
|
||
return sp
|
||
|
||
def _publish_sp(self, sp: PositionTarget) -> None:
|
||
sp.header.stamp = rospy.Time.now()
|
||
self.sp_pub.publish(sp)
|
||
|
||
def _stream_init_setpoint(self, x: float, y: float, z: float) -> None:
|
||
sp = self._position_sp(x, y, z)
|
||
for _ in range(self.pre_stream_count):
|
||
if rospy.is_shutdown():
|
||
return
|
||
self._publish_sp(sp)
|
||
self.rate.sleep()
|
||
|
||
def _refresh_mode_arm_proxies(self) -> None:
|
||
"""MAVROS 重启或链路抖事后，旧 ServiceProxy 可能一直报 unavailable，需重建。"""
|
||
try:
|
||
rospy.wait_for_service(self._set_mode_name, timeout=2.0)
|
||
rospy.wait_for_service(self._arming_name, timeout=2.0)
|
||
except rospy.ROSException:
|
||
return
|
||
self.mode_srv = rospy.ServiceProxy(self._set_mode_name, SetMode)
|
||
self.arm_srv = rospy.ServiceProxy(self._arming_name, CommandBool)
|
||
|
||
def _try_set_mode_arm_offboard(self) -> None:
|
||
try:
|
||
if self.state.mode != "OFFBOARD":
|
||
self.mode_srv(base_mode=0, custom_mode="OFFBOARD")
|
||
if not self.state.armed:
|
||
self.arm_srv(True)
|
||
except (rospy.ServiceException, rospy.ROSException) as exc:
|
||
rospy.logwarn_throttle(2.0, "set_mode/arm: %s", exc)
|
||
|
||
def _current_xyz(self) -> Tuple[float, float, float]:
|
||
p = self.pose.pose.position
|
||
return (p.x, p.y, p.z)
|
||
|
||
def _do_takeoff(self, step: ActionTakeoff) -> None:
|
||
alt = step.args.relative_altitude_m
|
||
dz = float(alt) if alt is not None else float(self.default_takeoff_relative_m)
|
||
self._wait_connected_and_pose()
|
||
x0, y0, z0 = self._current_xyz()
|
||
# 与 px4_ctrl_offboard_demo.py 一致：z_tgt = z0 + dz
|
||
z_tgt = z0 + dz
|
||
rospy.loginfo(
|
||
"takeoff: z0=%.2f Δ=%.2f m -> z_target=%.2f（与 demo 相同约定）",
|
||
z0,
|
||
dz,
|
||
z_tgt,
|
||
)
|
||
|
||
self._stream_init_setpoint(x0, y0, z0)
|
||
t0 = rospy.Time.now()
|
||
deadline = t0 + rospy.Duration(self.takeoff_timeout_sec)
|
||
while not rospy.is_shutdown() and rospy.Time.now() < deadline:
|
||
self._try_set_mode_arm_offboard()
|
||
sp = self._position_sp(x0, y0, z_tgt)
|
||
self._publish_sp(sp)
|
||
z_now = self.pose.pose.position.z
|
||
if (
|
||
abs(z_now - z_tgt) < 0.08
|
||
and self.state.mode == "OFFBOARD"
|
||
and self.state.armed
|
||
):
|
||
rospy.loginfo("takeoff reached")
|
||
break
|
||
self.rate.sleep()
|
||
else:
|
||
rospy.logwarn("takeoff timeout，继续后续步骤")
|
||
|
||
def _do_hold_position(self, _label: str = "hover") -> None:
|
||
x, y, z = self._current_xyz()
|
||
rospy.loginfo("%s: 保持当前点 (%.2f, %.2f, %.2f) NED", _label, x, y, z)
|
||
sp = self._position_sp(x, y, z)
|
||
t_end = rospy.Time.now() + rospy.Duration(1.0)
|
||
while not rospy.is_shutdown() and rospy.Time.now() < t_end:
|
||
self._try_set_mode_arm_offboard()
|
||
self._publish_sp(sp)
|
||
self.rate.sleep()
|
||
|
||
def _do_wait(self, step: ActionWait) -> None:
|
||
sec = float(step.args.seconds)
|
||
rospy.loginfo("wait %.2f s", sec)
|
||
t_end = rospy.Time.now() + rospy.Duration(sec)
|
||
x, y, z = self._current_xyz()
|
||
sp = self._position_sp(x, y, z)
|
||
while not rospy.is_shutdown() and rospy.Time.now() < t_end:
|
||
# Offboard 下建议保持 stream
|
||
if self.state.mode == "OFFBOARD" and self.state.armed:
|
||
self._publish_sp(sp)
|
||
self.rate.sleep()
|
||
|
||
def _ned_delta_from_goto(self, step: ActionGoto) -> Optional[Tuple[float, float, float]]:
|
||
a = step.args
|
||
dx = 0.0 if a.x is None else float(a.x)
|
||
dy = 0.0 if a.y is None else float(a.y)
|
||
dz = 0.0 if a.z is None else float(a.z)
|
||
if a.frame == "local_ned":
|
||
return (dx, dy, dz)
|
||
# body_ned: x前 y右 z下 → 转到 NED 水平增量
|
||
q = self.pose.pose.orientation
|
||
yaw = _yaw_from_quaternion(q.x, q.y, q.z, q.w)
|
||
north = math.cos(yaw) * dx - math.sin(yaw) * dy
|
||
east = math.sin(yaw) * dx + math.cos(yaw) * dy
|
||
return (north, east, dz)
|
||
|
||
def _do_goto(self, step: ActionGoto) -> None:
|
||
delta = self._ned_delta_from_goto(step)
|
||
if delta is None:
|
||
rospy.logwarn("goto: unsupported frame")
|
||
return
|
||
dn, de, dd = delta
|
||
if dn == 0.0 and de == 0.0 and dd == 0.0:
|
||
rospy.loginfo("goto: 零位移，跳过")
|
||
return
|
||
|
||
x0, y0, z0 = self._current_xyz()
|
||
xt, yt, zt = x0 + dn, y0 + de, z0 + dd
|
||
rospy.loginfo(
|
||
"goto: (%.2f,%.2f,%.2f) -> (%.2f,%.2f,%.2f) NED",
|
||
x0,
|
||
y0,
|
||
z0,
|
||
xt,
|
||
yt,
|
||
zt,
|
||
)
|
||
deadline = rospy.Time.now() + rospy.Duration(self.goto_timeout_sec)
|
||
while not rospy.is_shutdown() and rospy.Time.now() < deadline:
|
||
self._try_set_mode_arm_offboard()
|
||
sp = self._position_sp(xt, yt, zt)
|
||
self._publish_sp(sp)
|
||
px, py, pz = self._current_xyz()
|
||
err = math.sqrt(
|
||
(px - xt) ** 2 + (py - yt) ** 2 + (pz - zt) ** 2
|
||
)
|
||
if err < self.goto_tol and self.state.mode == "OFFBOARD":
|
||
rospy.loginfo("goto: reached (err=%.3f)", err)
|
||
break
|
||
self.rate.sleep()
|
||
else:
|
||
rospy.logwarn("goto: timeout")
|
||
|
||
def _do_land(self) -> None:
|
||
rospy.loginfo("land: AUTO.LAND")
|
||
t0 = rospy.Time.now()
|
||
deadline = t0 + rospy.Duration(self.land_timeout_sec)
|
||
fails = 0
|
||
while not rospy.is_shutdown() and rospy.Time.now() < deadline:
|
||
x, y, z = self._current_xyz()
|
||
# OFFBOARD 时若停发 setpoint，PX4 会很快退出该模式，MAVROS 侧 set_mode 可能长期不可用
|
||
if self.state.armed and self.state.mode == "OFFBOARD":
|
||
self._publish_sp(self._position_sp(x, y, z))
|
||
try:
|
||
if self.state.mode != "AUTO.LAND":
|
||
self.mode_srv(base_mode=0, custom_mode="AUTO.LAND")
|
||
except (rospy.ServiceException, rospy.ROSException) as exc:
|
||
fails += 1
|
||
rospy.logwarn_throttle(2.0, "AUTO.LAND: %s", exc)
|
||
if fails == 1 or fails % 15 == 0:
|
||
self._refresh_mode_arm_proxies()
|
||
if not self.state.armed:
|
||
rospy.loginfo("land: disarmed")
|
||
return
|
||
self.rate.sleep()
|
||
rospy.logwarn("land: timeout")
|
||
|
||
def _do_rtl(self) -> None:
|
||
rospy.loginfo("return_home: AUTO.RTL")
|
||
t0 = rospy.Time.now()
|
||
deadline = t0 + rospy.Duration(self.land_timeout_sec * 2)
|
||
fails = 0
|
||
while not rospy.is_shutdown() and rospy.Time.now() < deadline:
|
||
x, y, z = self._current_xyz()
|
||
if self.state.armed and self.state.mode == "OFFBOARD":
|
||
self._publish_sp(self._position_sp(x, y, z))
|
||
try:
|
||
if self.state.mode != "AUTO.RTL":
|
||
self.mode_srv(base_mode=0, custom_mode="AUTO.RTL")
|
||
except (rospy.ServiceException, rospy.ROSException) as exc:
|
||
fails += 1
|
||
rospy.logwarn_throttle(2.0, "RTL: %s", exc)
|
||
if fails == 1 or fails % 15 == 0:
|
||
self._refresh_mode_arm_proxies()
|
||
if not self.state.armed:
|
||
rospy.loginfo("RTL Finished（已 disarm）")
|
||
return
|
||
self.rate.sleep()
|
||
rospy.logwarn("rtl: timeout")
|
||
|
||
def execute(self, intent: ValidatedFlightIntent) -> None:
|
||
rospy.loginfo(
|
||
"执行 flight_intent：steps=%d summary=%s",
|
||
len(intent.actions),
|
||
intent.summary[:80],
|
||
)
|
||
self._wait_connected_and_pose()
|
||
|
||
for i, act in enumerate(intent.actions):
|
||
if rospy.is_shutdown():
|
||
break
|
||
rospy.loginfo("--- step %d/%d: %s", i + 1, len(intent.actions), type(act).__name__)
|
||
self._dispatch(act)
|
||
|
||
rospy.loginfo("flight_intent 序列结束")
|
||
|
||
def _dispatch(self, act: FlightAction) -> None:
|
||
if isinstance(act, ActionTakeoff):
|
||
self._do_takeoff(act)
|
||
elif isinstance(act, (ActionHover, ActionHold)):
|
||
self._do_hold_position("hover" if isinstance(act, ActionHover) else "hold")
|
||
elif isinstance(act, ActionWait):
|
||
self._do_wait(act)
|
||
elif isinstance(act, ActionGoto):
|
||
self._do_goto(act)
|
||
elif isinstance(act, ActionLand):
|
||
self._do_land()
|
||
elif isinstance(act, ActionReturnHome):
|
||
self._do_rtl()
|
||
else:
|
||
rospy.logwarn("未支持的动作: %r", act)
|