95 lines
3.2 KiB
Python
95 lines
3.2 KiB
Python
#!/usr/bin/env python3
|
||
"""
|
||
ROS1 伴飞桥节点：订阅 JSON（std_msgs/String），校验 flight_intent v1 后调用 MAVROS 执行。
|
||
|
||
话题（默认）：
|
||
全局 /input（std_msgs/String），与 rostopic pub、语音端 ROCKET_FLIGHT_BRIDGE_TOPIC 一致。
|
||
可通过私有参数 ~input_topic 覆盖（须带前导 / 才是全局名）。
|
||
|
||
示例：
|
||
rostopic pub -1 /input std_msgs/String \\
|
||
'{data: "{\"is_flight_intent\":true,\"version\":1,\"actions\":[{\"type\":\"land\",\"args\":{}}],\"summary\":\"降\"}"}'
|
||
|
||
前提是：已 roslaunch mavros px4.launch …，且 /mavros/state connected。
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import json
|
||
import threading
|
||
|
||
import rospy
|
||
from std_msgs.msg import String
|
||
|
||
from voice_drone.core.flight_intent import parse_flight_intent_dict
|
||
from voice_drone.flight_bridge.ros1_mavros_executor import MavrosFlightExecutor
|
||
|
||
|
||
def _coerce_flight_intent_dict(raw: dict) -> dict:
|
||
"""允许仅传 {actions, summary?}，补全顶层字段。"""
|
||
if raw.get("is_flight_intent") is True and raw.get("version") == 1:
|
||
return raw
|
||
actions = raw.get("actions")
|
||
if isinstance(actions, list) and actions:
|
||
summary = str(raw.get("summary") or "bridge").strip() or "bridge"
|
||
return {
|
||
"is_flight_intent": True,
|
||
"version": 1,
|
||
"actions": actions,
|
||
"summary": summary,
|
||
}
|
||
raise ValueError("JSON 须为完整 flight_intent 或含 actions 数组")
|
||
|
||
|
||
class FlightIntentBridgeNode:
|
||
def __init__(self) -> None:
|
||
self._exec = MavrosFlightExecutor()
|
||
self._busy = threading.Lock()
|
||
# 默认用绝对名 /input：若用相对名 "input"，anonymous 节点下会变成 /flight_intent_mavros_bridge_*/input，与 rostopic pub /input 不一致。
|
||
topic = rospy.get_param("~input_topic", "/input")
|
||
self._sub = rospy.Subscriber(
|
||
topic,
|
||
String,
|
||
self._on_input,
|
||
queue_size=1,
|
||
)
|
||
rospy.loginfo("flight_intent_bridge 就绪：订阅 %s", topic)
|
||
|
||
def _on_input(self, msg: String) -> None:
|
||
data = (msg.data or "").strip()
|
||
if not data:
|
||
return
|
||
if not self._busy.acquire(blocking=False):
|
||
rospy.logwarn("上一段 flight_intent 仍在执行，忽略本条")
|
||
return
|
||
|
||
def _run() -> None:
|
||
try:
|
||
try:
|
||
raw = json.loads(data)
|
||
except json.JSONDecodeError as e:
|
||
rospy.logerr("JSON 解析失败: %s", e)
|
||
return
|
||
if not isinstance(raw, dict):
|
||
rospy.logerr("顶层须为 JSON object")
|
||
return
|
||
raw = _coerce_flight_intent_dict(raw)
|
||
parsed, errors = parse_flight_intent_dict(raw)
|
||
if errors or parsed is None:
|
||
rospy.logerr("flight_intent 校验失败: %s", errors)
|
||
return
|
||
self._exec.execute(parsed)
|
||
finally:
|
||
self._busy.release()
|
||
|
||
threading.Thread(target=_run, daemon=True, name="flight-intent-exec").start()
|
||
|
||
|
||
def main() -> None:
|
||
FlightIntentBridgeNode()
|
||
rospy.spin()
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|