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