""" 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)