import websocket import json import rel import threading import time import socket import re # 定义消息类型常量 kTypeUndefined = 0 # 未定义的 kNav = 1 # 导航 kStepCtrl = 2 # 单步控制 kSecondposEnter = 3 # 二次定位进入 kSecondposQuit = 4 # 二次定位退出 kCarry = 5 # 搬运动作 kWait = 6 # 等待 kCharge = 7 # 充电 class WooshWebSocketClient: def __init__(self, url, debug=False): self.subscription_events = {} # 订阅事件字典 self.subscription_data = {} # 订阅数据字典 self.subscription_lock = threading.Lock() # 订阅锁 self.thread = None # 线程对象 self.url = url # WebSocket URL self.ws_request = None # 请求WebSocket对象 self.ws_subscribe = None # 订阅WebSocket对象 self.subscriptions = {} # 订阅回调函数字典 self.debug = debug # 调试模式 websocket.enableTrace(self.debug) # 启用WebSocket调试 @staticmethod def on_open(ws): """WebSocket连接打开时的回调函数""" print("WebSocket connection opened") def on_message(self, ws, message): """WebSocket接收到消息时的回调函数""" message = json.loads(message) sn_num = message.get("sn") if sn_num is not None: with self.subscription_lock: self.subscription_data[sn_num] = message self.subscription_events[sn_num].set() else: self.exec_callback(message["type"], message) def exec_callback(self, message_type, message): """执行回调函数""" with self.subscription_lock: if message_type in self.subscriptions: try: self.subscriptions[message_type](message) except Exception as e: print(f"Error executing callback for {message_type}: {e}") @staticmethod def on_error(ws, error): """WebSocket发生错误时的回调函数""" print(f"WebSocket error: {error}") @staticmethod def on_close(ws, close_status, close_msg): """WebSocket连接关闭时的回调函数""" print("WebSocket connection closed") def connect(self): """连接WebSocket""" self.close() self.ws_subscribe = websocket.WebSocketApp(self.url, on_open=self.on_open, on_message=self.on_message, on_error=self.on_error, on_close=self.on_close) self.ws_request = websocket.WebSocketApp(self.url, on_open=self.on_open, on_message=self.on_message, on_error=self.on_error, on_close=self.on_close) # 后台 rel 调度执行 self.ws_subscribe.run_forever(dispatcher=rel, reconnect=10) self.ws_request.run_forever(dispatcher=rel, reconnect=10) self.thread = threading.Thread(target=rel.dispatch) self.thread.start() def close(self): """关闭WebSocket连接""" if self.ws_subscribe: self.ws_subscribe.close() if self.ws_request: self.ws_request.close() if self.thread: rel.abort() self.thread.join() def reconnect_test(self): """测试WebSocket连接是否正常""" match = re.match(r'ws://(\d+\.\d+\.\d+\.\d+):(\d+)/', self.url) ip_address = match.group(1) port = int(match.group(2)) try: s = socket.create_connection((ip_address, port), timeout=5) s.close() print(f"Connection to {ip_address}:{port} successful.") return True except socket.error as e: print(f"Error connecting to {ip_address}:{port}: {e}") return False def add_topic_callback(self, message_type: str, cb_func): """添加话题回调函数""" with self.subscription_lock: self.subscriptions[message_type] = cb_func def submit_subscriptions(self): """发送订阅请求""" msg = { "type": "woosh.Subscription", "body": { "sub": True, "topics": list(self.subscriptions.keys()) } } self.ws_subscribe.send(json.dumps(msg)) def remove_topic_callback(self, message_type): """移除话题回调函数""" with self.subscription_lock: if message_type in self.subscriptions: del self.subscriptions[message_type] def request(self, message_type, body=None, timeout=8, send_type='request'): """发送请求，根据 sn 序列号保证 req 和 resp 匹配""" sn_num = int(time.time()) subscription_event = threading.Event() with self.subscription_lock: self.subscription_events[sn_num] = subscription_event if body is None or type(body) is not dict: msg = {"type": message_type, "sn": sn_num} else: msg = {"type": message_type, "sn": sn_num, "body": body} if send_type == 'request': self.ws_request.send(json.dumps(msg)) if send_type == 'subscribe': self.ws_subscribe.send(json.dumps(msg)) if subscription_event.wait(timeout): with self.subscription_lock: subscription_data = self.subscription_data[sn_num] self.subscription_data[sn_num] = None del self.subscription_events[sn_num] if subscription_data.get('ok'): return subscription_data.get('body') else: print( f"Fail to get ok for response to {message_type} sn: {sn_num}") return None else: print( f"Timeout waiting for response to {message_type} sn: {sn_num}") return None def request_try(self, message_type, body=None, send_type='request', timeout=8): ret = False try: ret = self.request(message_type, body=body, timeout=timeout, send_type=send_type) except Exception as e: print(f"Error requesting {message_type}: {e}, body: {body}") if self.reconnect_test(): self.connect() finally: return ret class WooshApi(WooshWebSocketClient): def __init__(self, url, debug=False): super().__init__(url, debug) self.connect() # ================================================= # 机器人信息相关 # ================================================= def robot_info(self): """获取机器人信息""" return self.request_try("woosh.robot.RobotInfo") def robot_general(self): """获取机器人通用信息""" return self.request_try("woosh.robot.General") def robot_setting(self): """获取机器人设置信息""" return self.request_try("woosh.robot.Setting") def robot_state(self): """获取机器人状态信息""" return self.request_try("woosh.robot.RobotState") def robot_mode(self): """获取机器人模式信息""" return self.request_try("woosh.robot.Mode") def robot_pose_speed(self): """获取机器人姿态和速度信息""" return self.request_try("woosh.robot.PoseSpeed") def robot_battery(self, send_type='request'): """获取机器人电池信息""" return self.request_try(message_type="woosh.robot.Battery", send_type=send_type) def robot_network(self): """获取机器人网络信息""" return self.request_try("woosh.robot.Network") def robot_scene(self): """获取机器人场景信息""" return self.request_try("woosh.robot.Scene") def robot_task_proc(self): """获取机器人任务进程信息""" return self.request_try("woosh.robot.TaskProc") def robot_device_state(self): """获取机器人设备状态信息""" return self.request_try("woosh.robot.DeviceState") def robot_hardware_state(self): """获取机器人硬件状态信息""" return self.request_try("woosh.robot.HardwareState") def robot_operation_state(self): """获取机器人操作状态信息""" return self.request_try("woosh.robot.OperationState") def robot_model(self): """获取机器人模型信息""" return self.request_try("woosh.robot.Model") def robot_task_history(self): """获取机器人任务历史信息""" return self.request_try("woosh.robot.TaskHistory") def robot_status_code(self, robot_id): """获取机器人状态码信息""" body = {"robotId": robot_id} return self.request_try("woosh.robot.count.StatusCodes", body) def robot_abnormal_codes(self): """获取机器人异常码信息""" return self.request_try("woosh.robot.count.AbnormalCodes") def robot_is_impede(self): """判断机器人是否行驶路线被阻碍 kNavBitUndefined 0 未定义 kNarrow 1 狭窄通道 kGuide 2 引导到达 kInaLift 4 乘梯中 kImpede 8 阻碍 kQRCode 16 二维码 """ msg = self.robot_operation_state() if msg is None: return None return msg.get('nav') & 0b1000 == 8 if msg.get('nav') else None # ================================================= # 机器人配置相关 # ================================================= def setting_identity(self, name, robot_id=None): """设置机器人身份信息""" body = {"name": name} if robot_id is not None: body["robot_id"] = robot_id return self.request_try("woosh.robot.setting.Identity", body) def setting_auto_charge(self, allow=True): """设置机器人自动充电""" body = {"allow": allow} return self.request_try("woosh.robot.setting.AutoCharge", body) def setting_auto_park(self, allow=True): """设置机器人自动停车""" body = {"allow": allow} return self.request_try("woosh.robot.setting.AutoPark", body) def setting_power(self, alarm=10, low=20, idle=80, full=98): """设置机器人电量阈值""" body = {"alarm": alarm, "low": low, "idle": idle, "full": full} return self.request_try("woosh.robot.setting.Power", body) # ================================================= # 机器人场景地图相关 # ================================================= def map_scene_list(self): """获取场景列表""" return self.request_try("woosh.map.SceneList") def map_scene_data(self, scene_name): """获取场景数据""" body = {"name": scene_name} return self.request_try("woosh.map.SceneData", body) def map_download(self, scene_name): """下载场景地图""" return self.request_try("woosh.map.Download", body={"sceneName": scene_name}) def map_upload(self): """上传场景地图""" return self.request_try("woosh.map.Upload") # ================================================= # 机器人请求相关 # ================================================= def robot_switch_work_mode(self, mode: int): """切换工作模式 kWorkModeUndefined 0 未定义的 kDeployMode 1 部署模式 kTaskMode 2 任务模式 kScheduleMode 3 调度模式 """ return self.request_try("woosh.robot.SwitchWorkMode", {"mode": mode}) def robot_init_robot(self, body): """初始化机器人""" if body.get('pose') is None: return self.request_try("woosh.robot.InitRobot", {"isRecord": True}) else: return self.request_try("woosh.robot.InitRobot", body) def robot_set_robot_pose(self, x, y, theta): """设置机器人姿态""" return self.request_try("woosh.robot.SetRobotPose", {"pose": {"x": x, "y": y, "theta": theta}}) def robot_switch_control_mode(self, mode: int): """切换控制模式 kControlModeUndefined 0 未定义的 kControlModeManual 1 手动模式 kControlModeAuto 2 自动模式 """ return self.request_try("woosh.robot.SwitchControlMode", {"mode": mode}) def robot_switch_map(self, scene_name, map_name): """切换地图""" return self.request_try("woosh.robot.SwitchMap", {"sceneName": scene_name, "mapName": map_name}) def robot_speak(self, text): """机器人语音播报""" return self.request_try("woosh.robot.Speak", {"text": text}) def robot_exec_task(self, target: dict, poses: list): """执行任务 target: {"x": 0, "y": 0, "theta": 0} path: [{"x": 0, "y": 0, "theta": 0}, ...] """ task_id = int(time.time()) body = { "taskId": task_id, "type": kNav, "planPath": { "planPath": [ { "path": { "poses": poses }, "target": target } ] } } return self.request_try("woosh.robot.ExecTask", body) def robot_charge_task(self, mark_no=None): """执行充电任务""" task_id = int(time.time()) body = { "taskId": task_id, "type": 3, } if mark_no: body["markNo"] = mark_no return self.request_try("woosh.robot.ExecTask", body) def robot_go_to(self, x, y, theta): """机器人移动到指定位置""" target = {"x": x, "y": y, "theta": theta} poses = [{"x": x, "y": y, "theta": theta}] return self.robot_exec_task(target, poses) def robot_action_order(self, order=0): """ kOrderUndefined 0 未定义的 kStart 1 开始(弃用) kPause 2 暂停 kContinue 3 继续 kCancel 4 取消 kRecover 5 恢复(单机任务有效) kWaitBreak 6 等待打断 kTmCtrl 7 交通管制 kReleaseCtrl 8 解除管制 """ return self.request_try("woosh.robot.ActionOrder", {"order": order}) def robot_plan_nav_path(self, start: dict, end: dict, tolerance=0): """规划导航路径 start: {"x": 0, "y": 0, "theta": 0} end: {"x": 0, "y": 0, "theta": 0} tolerance: 允许绕行范围, 单位为米 """ body = { "start": start, "end": end, "tolerance": tolerance } return self.request_try("woosh.robot.PlanNavPath", body) def robot_get_nav_path(self): """获取导航路径""" return self.request_try("woosh.robot.NavPath", body={}) def robot_twist(self, linear, angular): """机器人运动 linear: 线速度, 单位为米/秒 angular: 角速度, 单位为弧度/秒 """ body = { "linear": linear, "angular": angular } return self.request_try("woosh.robot.Twist", body) def robot_occupancy(self, occupy: bool): """设置机器人占用状态""" body = { "occupy": occupy } return self.request_try("woosh.robot.SetOccupancy", body) def robot_open_hotspot(self, order_type): """打开机器人热点""" body = { "order": 5, "enable": order_type } return self.request("woosh.robot.RobotWiFi", body) def robot_step_control(self, direction, distance, speed, action=1, avoid=0): """机器人单步控制 direction: 方向 (right, left, ahead, back, rotate) distance: 距离, 单位为米 speed: 速度, 单位为米/秒 action: 动作类型 avoid: 是否避障 """ mode_mapping = { "right": 3, "left": 3, "ahead": 1, "back": 1, "rotate": 2 } mode = mode_mapping.get(direction, 0) distance = abs(distance) direction_to_multiplier = {"right": -1, "left": 1, "ahead": 1, "back": -1} distance *= direction_to_multiplier.get(direction, 1) speed = 0.15 if speed >= 0.2 else speed body = { "step_control": { "steps": [ { "mode": mode, "value": distance, "speed": speed } ], "avoid": avoid, "action": action } } return self.request("woosh.ros.CallAction", body) def robot_lift_control(self, direction, height): """机器人升降控制 direction: 方向 (down, up) height: 高度, 单位为米 """ height = abs(height) direction_to_multiplier = {"down": -1, "up": 1} height *= direction_to_multiplier.get(direction, 1) body = { "lift_control2": { "height": height, "action": 1 } } return self.request("woosh.ros.CallAction", body)