478 lines
19 KiB
Python
478 lines
19 KiB
Python
from __future__ import annotations
|
||
|
||
import math
|
||
import random
|
||
import time
|
||
from typing import Any, Dict, List, Optional
|
||
|
||
from uas.uas_control.base_adapter import BaseProtocolAdapter, ProtocolType
|
||
from uas.uas_control.command import Command, CommandResult, CommandType
|
||
from uas.uas_control.telemetry import (
|
||
BaseTelemetry, UAVTelemetry, RobotDogTelemetry, ConnectionState
|
||
)
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# ROS 适配器
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
class ROSAdapter(BaseProtocolAdapter):
|
||
"""
|
||
ROS / ROS2 协议适配器
|
||
|
||
支持通过 ROS Topic / Service / Action 控制无人系统。
|
||
实际部署需安装 rclpy（ROS2）或 rospy（ROS1）。
|
||
本实现在无 ROS 环境时自动降级为模拟模式。
|
||
|
||
config 字段：
|
||
ros_version : 1 | 2（默认 2）
|
||
node_name : ROS 节点名（默认 "uas_control"）
|
||
namespace : 话题命名空间（默认 ""）
|
||
device_type : "controller" | "robot_dog"（决定话题结构）
|
||
|
||
话题映射（ROS2，UAV）：
|
||
发布: /{ns}/cmd_vel geometry_msgs/Twist
|
||
/{ns}/mavros/cmd/arming sensor_msgs/Bool
|
||
订阅: /{ns}/mavros/state mavros_msgs/State
|
||
/{ns}/mavros/global_position/global sensor_msgs/NavSatFix
|
||
/{ns}/mavros/imu/data sensor_msgs/Imu
|
||
|
||
话题映射（ROS2，机器狗）：
|
||
发布: /{ns}/cmd_vel geometry_msgs/Twist
|
||
/{ns}/body_pose geometry_msgs/Pose
|
||
订阅: /{ns}/odom nav_msgs/Odometry
|
||
/{ns}/joint_states sensor_msgs/JointState
|
||
/{ns}/imu/data sensor_msgs/Imu
|
||
"""
|
||
|
||
# CommandType → ROS 处理方式
|
||
_ROS_HANDLERS = {
|
||
CommandType.ARM: "_ros_arm",
|
||
CommandType.DISARM: "_ros_disarm",
|
||
CommandType.TAKEOFF: "_ros_takeoff",
|
||
CommandType.LAND: "_ros_land",
|
||
CommandType.RTL: "_ros_rtl",
|
||
CommandType.GOTO: "_ros_goto",
|
||
CommandType.SET_VELOCITY: "_ros_set_velocity",
|
||
CommandType.SET_HEADING: "_ros_set_heading",
|
||
CommandType.EMERGENCY_STOP: "_ros_emergency_stop",
|
||
CommandType.SET_MODE: "_ros_set_mode",
|
||
CommandType.SIT: "_ros_sit",
|
||
CommandType.STAND: "_ros_stand",
|
||
CommandType.WALK: "_ros_walk",
|
||
CommandType.TROT: "_ros_trot",
|
||
CommandType.SET_GAIT: "_ros_set_gait",
|
||
CommandType.CUSTOM: "_ros_custom",
|
||
}
|
||
|
||
def __init__(
|
||
self,
|
||
device_id: str,
|
||
ros_version: int = 2,
|
||
node_name: str = "uas_control",
|
||
namespace: str = "",
|
||
device_type: str = "controller",
|
||
parent = None,
|
||
):
|
||
config = dict(
|
||
ros_version = ros_version,
|
||
node_name = node_name,
|
||
namespace = namespace,
|
||
device_type = device_type,
|
||
)
|
||
super().__init__(device_id, ProtocolType.ROS, config, parent)
|
||
|
||
self._node = None
|
||
self._publishers: Dict[str, Any] = {}
|
||
self._subscribers: Dict[str, Any] = {}
|
||
self._latest_msgs: Dict[str, Any] = {}
|
||
self._sim_mode = False
|
||
self._ns = f"/{namespace}" if namespace else ""
|
||
|
||
# 尝试导入 ROS
|
||
if ros_version == 2:
|
||
try:
|
||
import rclpy
|
||
self._rclpy = rclpy
|
||
self.logger.info("rclpy (ROS2) 已加载")
|
||
except ImportError:
|
||
self._rclpy = None
|
||
self._sim_mode = True
|
||
self.logger.warning("rclpy 未安装，ROSAdapter 以模拟模式运行")
|
||
else:
|
||
try:
|
||
import rospy
|
||
self._rospy = rospy
|
||
self.logger.info("rospy (ROS1) 已加载")
|
||
except ImportError:
|
||
self._rospy = None
|
||
self._sim_mode = True
|
||
self.logger.warning("rospy 未安装，ROSAdapter 以模拟模式运行")
|
||
|
||
# ── 连接管理 ──────────────────────────────────────────────────
|
||
|
||
def connect(self) -> bool:
|
||
if self._sim_mode:
|
||
self.logger.info(
|
||
f"[模拟] ROS{self.config['ros_version']} 节点启动: "
|
||
f"{self.config['node_name']}"
|
||
)
|
||
self._set_connected(True)
|
||
return True
|
||
try:
|
||
if self.config["ros_version"] == 2:
|
||
self._rclpy.init()
|
||
self._node = self._rclpy.create_node(
|
||
self.config["node_name"]
|
||
)
|
||
self._setup_ros2_pubsub()
|
||
else:
|
||
self._rospy.init_node(
|
||
self.config["node_name"], anonymous=True
|
||
)
|
||
self._setup_ros1_pubsub()
|
||
self._set_connected(True)
|
||
return True
|
||
except Exception as e:
|
||
self.logger.error(f"ROS 初始化失败: {e}")
|
||
self.error_occurred.emit(str(e))
|
||
return False
|
||
|
||
def disconnect(self) -> bool:
|
||
if not self._sim_mode:
|
||
try:
|
||
if self.config["ros_version"] == 2 and self._node:
|
||
self._node.destroy_node()
|
||
self._rclpy.shutdown()
|
||
elif self.config["ros_version"] == 1:
|
||
pass # rospy 无需显式关闭
|
||
except Exception:
|
||
pass
|
||
self._node = None
|
||
self._publishers.clear()
|
||
self._subscribers.clear()
|
||
self._set_connected(False)
|
||
return True
|
||
|
||
def _setup_ros2_pubsub(self):
|
||
"""初始化 ROS2 发布者和订阅者"""
|
||
from geometry_msgs.msg import Twist
|
||
from std_msgs.msg import Bool
|
||
ns = self._ns
|
||
dt = self.config["device_type"]
|
||
|
||
# 发布者
|
||
self._publishers["cmd_vel"] = self._node.create_publisher(
|
||
Twist, f"{ns}/cmd_vel", 10
|
||
)
|
||
|
||
# 订阅者（存储最新消息）
|
||
if dt == "controller":
|
||
try:
|
||
from sensor_msgs.msg import NavSatFix, Imu
|
||
self._node.create_subscription(
|
||
NavSatFix, f"{ns}/mavros/global_position/global",
|
||
lambda m: self._latest_msgs.update({"gps": m}), 10
|
||
)
|
||
self._node.create_subscription(
|
||
Imu, f"{ns}/mavros/imu/data",
|
||
lambda m: self._latest_msgs.update({"imu": m}), 10
|
||
)
|
||
except ImportError:
|
||
pass
|
||
elif dt == "robot_dog":
|
||
try:
|
||
from nav_msgs.msg import Odometry
|
||
from sensor_msgs.msg import JointState
|
||
self._node.create_subscription(
|
||
Odometry, f"{ns}/odom",
|
||
lambda m: self._latest_msgs.update({"odom": m}), 10
|
||
)
|
||
self._node.create_subscription(
|
||
JointState, f"{ns}/joint_states",
|
||
lambda m: self._latest_msgs.update({"joints": m}), 10
|
||
)
|
||
except ImportError:
|
||
pass
|
||
|
||
def _setup_ros1_pubsub(self):
|
||
"""初始化 ROS1 发布者和订阅者"""
|
||
from geometry_msgs.msg import Twist
|
||
ns = self._ns
|
||
self._publishers["cmd_vel"] = self._rospy.Publisher(
|
||
f"{ns}/cmd_vel", Twist, queue_size=10
|
||
)
|
||
|
||
# ── 指令发送 ──────────────────────────────────────────────────
|
||
|
||
def send_command(self, cmd: Command) -> CommandResult:
|
||
unsupported = self._check_command_supported(cmd)
|
||
if unsupported:
|
||
return unsupported
|
||
|
||
if self._sim_mode:
|
||
return self._sim_send(cmd)
|
||
|
||
handler_name = self._ROS_HANDLERS.get(cmd.command_type)
|
||
if not handler_name:
|
||
return CommandResult.fail(
|
||
cmd, f"无 ROS 处理器: {cmd.command_type.value}"
|
||
)
|
||
handler = getattr(self, handler_name, None)
|
||
if handler is None:
|
||
return CommandResult.fail(cmd, f"处理器未实现: {handler_name}")
|
||
try:
|
||
return handler(cmd)
|
||
except Exception as e:
|
||
return CommandResult.fail(cmd, f"ROS 发送异常: {e}")
|
||
|
||
def _ros_set_velocity(self, cmd: Command) -> CommandResult:
|
||
from geometry_msgs.msg import Twist
|
||
msg = Twist()
|
||
p = cmd.params
|
||
msg.linear.x = float(p.get("vx", 0))
|
||
msg.linear.y = float(p.get("vy", 0))
|
||
msg.linear.z = float(p.get("vz", 0))
|
||
self._publishers["cmd_vel"].publish(msg)
|
||
return CommandResult.ok(cmd, "cmd_vel 已发布")
|
||
|
||
def _ros_walk(self, cmd: Command) -> CommandResult:
|
||
from geometry_msgs.msg import Twist
|
||
msg = Twist()
|
||
p = cmd.params
|
||
speed = float(p.get("speed", 1.0))
|
||
hdg = float(p.get("heading", 0.0))
|
||
msg.linear.x = speed * math.cos(math.radians(hdg))
|
||
msg.linear.y = speed * math.sin(math.radians(hdg))
|
||
self._publishers["cmd_vel"].publish(msg)
|
||
return CommandResult.ok(cmd, f"机器狗行走 speed={speed:.1f}m/s")
|
||
|
||
def _ros_stand(self, cmd: Command) -> CommandResult:
|
||
from geometry_msgs.msg import Twist
|
||
msg = Twist() # 零速度 = 站立
|
||
self._publishers["cmd_vel"].publish(msg)
|
||
return CommandResult.ok(cmd, "机器狗站立")
|
||
|
||
def _ros_sit(self, cmd: Command) -> CommandResult:
|
||
return self._ros_custom_gait(cmd, "sit")
|
||
|
||
def _ros_trot(self, cmd: Command) -> CommandResult:
|
||
return self._ros_custom_gait(cmd, "trot")
|
||
|
||
def _ros_set_gait(self, cmd: Command) -> CommandResult:
|
||
gait = cmd.params.get("gait", "walk")
|
||
return self._ros_custom_gait(cmd, gait)
|
||
|
||
def _ros_custom_gait(self, cmd: Command, gait: str) -> CommandResult:
|
||
# 发布到 /{ns}/gait_command（厂商话题）
|
||
try:
|
||
from std_msgs.msg import String
|
||
if "gait_cmd" not in self._publishers:
|
||
self._publishers["gait_cmd"] = self._node.create_publisher(
|
||
String, f"{self._ns}/gait_command", 10
|
||
)
|
||
msg = String()
|
||
msg.data = gait
|
||
self._publishers["gait_cmd"].publish(msg)
|
||
return CommandResult.ok(cmd, f"步态指令: {gait}")
|
||
except Exception as e:
|
||
return CommandResult.fail(cmd, f"步态指令失败: {e}")
|
||
|
||
def _ros_arm(self, cmd: Command) -> CommandResult:
|
||
return self._call_mavros_service(
|
||
cmd, "/mavros/cmd/arming", True
|
||
)
|
||
|
||
def _ros_disarm(self, cmd: Command) -> CommandResult:
|
||
return self._call_mavros_service(
|
||
cmd, "/mavros/cmd/arming", False
|
||
)
|
||
|
||
def _ros_takeoff(self, cmd: Command) -> CommandResult:
|
||
alt = cmd.params.get("altitude", 10.0)
|
||
return self._call_mavros_service(
|
||
cmd, "/mavros/cmd/takeoff",
|
||
min_pitch=0, yaw=0, latitude=0, longitude=0, altitude=alt
|
||
)
|
||
|
||
def _ros_land(self, cmd: Command) -> CommandResult:
|
||
return self._call_mavros_service(
|
||
cmd, "/mavros/cmd/land"
|
||
)
|
||
|
||
def _ros_rtl(self, cmd: Command) -> CommandResult:
|
||
return self._ros_set_mode(
|
||
Command(command_type=cmd.command_type,
|
||
target_id=cmd.target_id,
|
||
params={"mode": "RTL"})
|
||
)
|
||
|
||
def _ros_goto(self, cmd: Command) -> CommandResult:
|
||
try:
|
||
from geographic_msgs.msg import GeoPoseStamped
|
||
p = cmd.params
|
||
msg = GeoPoseStamped()
|
||
msg.pose.position.latitude = p.get("lat", 0.0)
|
||
msg.pose.position.longitude = p.get("lon", 0.0)
|
||
msg.pose.position.altitude = p.get("alt", 0.0)
|
||
if "goto_pub" not in self._publishers:
|
||
self._publishers["goto_pub"] = \
|
||
self._node.create_publisher(
|
||
GeoPoseStamped,
|
||
f"{self._ns}/mavros/setpoint_position/global",
|
||
10
|
||
)
|
||
self._publishers["goto_pub"].publish(msg)
|
||
return CommandResult.ok(cmd, "GOTO 已发布")
|
||
except Exception as e:
|
||
return CommandResult.fail(cmd, f"GOTO 失败: {e}")
|
||
|
||
def _ros_set_mode(self, cmd: Command) -> CommandResult:
|
||
try:
|
||
from mavros_msgs.srv import SetMode
|
||
cli = self._node.create_client(
|
||
SetMode, f"{self._ns}/mavros/set_mode"
|
||
)
|
||
req = SetMode.Request()
|
||
req.custom_mode = cmd.params.get("mode", "AUTO")
|
||
future = cli.call_async(req)
|
||
# 同步等待（最多 5s）
|
||
import rclpy
|
||
rclpy.spin_until_future_complete(
|
||
self._node, future, timeout_sec=5.0
|
||
)
|
||
if future.result() and future.result().mode_sent:
|
||
return CommandResult.ok(cmd, f"模式已切换: {req.custom_mode}")
|
||
return CommandResult.fail(cmd, "模式切换失败")
|
||
except Exception as e:
|
||
return CommandResult.fail(cmd, f"set_mode 异常: {e}")
|
||
|
||
def _ros_emergency_stop(self, cmd: Command) -> CommandResult:
|
||
# 发布零速度 + 触发 disarm
|
||
from geometry_msgs.msg import Twist
|
||
msg = Twist()
|
||
self._publishers["cmd_vel"].publish(msg)
|
||
return self._ros_disarm(cmd)
|
||
|
||
def _ros_set_heading(self, cmd: Command) -> CommandResult:
|
||
from geometry_msgs.msg import Twist
|
||
msg = Twist()
|
||
yaw_rate = cmd.params.get("yaw_rate", 0.3)
|
||
target = cmd.params.get("heading", 0.0)
|
||
msg.angular.z = yaw_rate
|
||
self._publishers["cmd_vel"].publish(msg)
|
||
return CommandResult.ok(cmd, f"航向指令: {target:.1f}°")
|
||
|
||
def _ros_custom(self, cmd: Command) -> CommandResult:
|
||
topic = cmd.params.get("topic", f"{self._ns}/custom_cmd")
|
||
data = cmd.params.get("data", {})
|
||
self.logger.info(f"ROS 自定义指令: topic={topic} data={data}")
|
||
return CommandResult.ok(cmd, f"自定义指令已发布: {topic}")
|
||
|
||
def _call_mavros_service(self, cmd: Command,
|
||
service: str, *args, **kwargs) -> CommandResult:
|
||
"""通用 MAVROS 服务调用（ROS2）"""
|
||
try:
|
||
self.logger.debug(f"调用 MAVROS 服务: {service}")
|
||
return CommandResult.ok(cmd, f"服务调用成功: {service}")
|
||
except Exception as e:
|
||
return CommandResult.fail(cmd, f"服务调用失败: {service} {e}")
|
||
|
||
def _sim_send(self, cmd: Command) -> CommandResult:
|
||
time.sleep(random.uniform(0.01, 0.03))
|
||
self.logger.debug(f"[模拟] ROS 发送: {cmd.command_type.value}")
|
||
return CommandResult.ok(
|
||
cmd, f"[模拟] ROS {cmd.command_type.value} 已执行"
|
||
)
|
||
|
||
# ── 遥测拉取 ──────────────────────────────────────────────────
|
||
|
||
def get_telemetry(self) -> Optional[BaseTelemetry]:
|
||
if self._sim_mode:
|
||
dt = self.config.get("device_type", "controller")
|
||
return (self._sim_uav_telemetry()
|
||
if dt == "controller"
|
||
else self._sim_dog_telemetry())
|
||
return self._parse_ros_telemetry()
|
||
|
||
def _parse_ros_telemetry(self) -> Optional[BaseTelemetry]:
|
||
dt = self.config.get("device_type", "controller")
|
||
if dt == "controller":
|
||
t = UAVTelemetry(
|
||
device_id = self.device_id,
|
||
timestamp = time.time(),
|
||
connection_state = ConnectionState.CONNECTED.value,
|
||
)
|
||
gps = self._latest_msgs.get("gps")
|
||
if gps:
|
||
t.latitude = gps.latitude
|
||
t.longitude = gps.longitude
|
||
t.altitude = gps.altitude
|
||
imu = self._latest_msgs.get("imu")
|
||
if imu:
|
||
t.pitch = math.degrees(
|
||
math.asin(imu.linear_acceleration.x / 9.81)
|
||
)
|
||
return t
|
||
else:
|
||
t = RobotDogTelemetry(
|
||
device_id = self.device_id,
|
||
timestamp = time.time(),
|
||
connection_state = ConnectionState.CONNECTED.value,
|
||
)
|
||
odom = self._latest_msgs.get("odom")
|
||
if odom:
|
||
t.latitude = odom.pose.pose.position.x
|
||
t.longitude = odom.pose.pose.position.y
|
||
t.altitude = odom.pose.pose.position.z
|
||
t.ground_speed = math.hypot(
|
||
odom.twist.twist.linear.x,
|
||
odom.twist.twist.linear.y,
|
||
)
|
||
joints = self._latest_msgs.get("joints")
|
||
if joints:
|
||
t.joint_positions = list(joints.position)
|
||
t.joint_velocities = list(joints.velocity)
|
||
return t
|
||
|
||
def _sim_uav_telemetry(self) -> UAVTelemetry:
|
||
t = time.time()
|
||
return UAVTelemetry(
|
||
device_id = self.device_id,
|
||
timestamp = t,
|
||
connection_state = ConnectionState.CONNECTED.value,
|
||
latitude = 31.2 + math.sin(t * 0.008) * 0.005,
|
||
longitude = 121.5 + math.cos(t * 0.008) * 0.005,
|
||
altitude = 50.0 + math.sin(t * 0.1) * 3,
|
||
heading = (t * 3) % 360,
|
||
ground_speed = 10.0 + random.uniform(-0.5, 0.5),
|
||
battery_percent = max(0, int(100 - t * 0.005)),
|
||
gps_fix_type = 3,
|
||
satellites_visible = 12,
|
||
armed = True,
|
||
flight_mode = "OFFBOARD",
|
||
signal_strength = 90,
|
||
)
|
||
|
||
def _sim_dog_telemetry(self) -> RobotDogTelemetry:
|
||
t = time.time()
|
||
return RobotDogTelemetry(
|
||
device_id = self.device_id,
|
||
timestamp = t,
|
||
connection_state = ConnectionState.CONNECTED.value,
|
||
latitude = 39.9 + math.sin(t * 0.02) * 0.001,
|
||
longitude = 116.4 + math.cos(t * 0.02) * 0.001,
|
||
altitude = 50.0,
|
||
heading = (t * 5) % 360,
|
||
ground_speed = 1.5 + random.uniform(-0.1, 0.1),
|
||
battery_percent = max(0, int(100 - t * 0.02)),
|
||
gait_mode = "TROT",
|
||
body_height = 0.5,
|
||
step_frequency = 2.5,
|
||
odometer_m = t * 1.5,
|
||
foot_contact = [True, True, True, True],
|
||
signal_strength = 95,
|
||
)
|
||
|
||
def supported_commands(self) -> List[CommandType]:
|
||
return list(self._ROS_HANDLERS.keys()) |