警告:Initalizing 'AppDelegate * __strong' with an expression of incompatible type ''id<UIApplication..

现象:使用ARC时,获取全局的AppDelegate会有警告Initalizing 'AppDelegate * __strong' with an expression of incompatible type ''id<UIApplicationDelegate>"。

原因:类型不一致

解决办法:强制类型转换

实例:

修改前:

AppDelegate *app = [UIApplication sharedApplication].delegate;

修改后:即(强制转换)

AppDelegate *app = (AppDelegate*)[UIApplication sharedApplication].delegate;


回到问题的log,在andorid 8 中,扫描后的wifi热点结果没有上报到wifi界面 对于这个错误,如何fix 08-07 17:22:47.057 1123 1123 I wpa_supplicant: module_name = [seekwave] 08-07 17:22:47.061 1123 1123 I wpa_supplicant: found [seekwave] 08-07 17:22:47.061 1123 1123 I wpa_supplicant: wpa_param_len = 4 08-07 17:22:47.062 1123 1123 I wpa_supplicant: wpa_param_len = 4 | wpa_param[0] = /vendor/bin/hw/wpa_supplicant 08-07 17:22:47.062 1123 1123 I wpa_supplicant: wpa_param_len = 4 | wpa_param[1] = -O/data/vendor/wifi/wpa/sockets 08-07 17:22:47.062 1123 1123 I wpa_supplicant: wpa_param_len = 4 | wpa_param[2] = -puse_p2p_group_interface=1 08-07 17:22:47.062 1123 1123 I wpa_supplicant: wpa_param_len = 4 | wpa_param[3] = -g@android:wpa_wlan0 08-07 17:22:47.066 1082 1082 D AndroidRuntime: Calling main entry com.android.commands.pm.Pm 08-07 17:22:47.081 1082 1082 I app_process: System.exit called, status: 0 08-07 17:22:47.081 1082 1082 I AndroidRuntime: VM exiting with result code 0. 08-07 17:22:47.092 1123 1123 I wpa_supplicant: Processing hidl events on FD 6 08-07 17:22:47.103 402 520 I system_server: Looking for service android.hardware.wifi.supplicant@1.0::ISupplicant/defa ult 08-07 17:22:47.105 1123 1123 I wpa_supplicant: Successfully initialized wpa_supplicant 08-07 17:22:47.105 1020 1092 V MediaProvider: pruneThumbnails 08-07 17:22:47.110 402 520 I system_server: Starting thread pool. 08-07 17:22:47.115 402 520 E SupplicantStaIfaceHal: Got zero HIDL supplicant ifaces. Stopping supplicant HIDL startup. 08-07 17:22:47.115 402 520 E SupplicantStaIfaceHal: initalizing ISupplicantIfaces failed.
最新发布
08-26
import cv2 import numpy as np import zmq import time import threading from enum import Enum import os #!/usr/bin/env python3 import sys sys.path.append("/home/xiaoyuan/diffusion_policy_graspping/src/arm_control") from hdx_fuse_sdk.hdx_config import HDXConfig #from hdx_fuse_sdk.blev2 import BLEClientHandler from hdx_fuse_sdk.blev3 import BLEClientHandler #from hdx_fuse_sdk.goal_nav import MoveBaseClient from hdx_fuse_sdk.robot_actuator import HDXRobot import pyrealsense2 as rs class Single_Arm_Inference: def __init__(self, port_name='/dev/ttyUSB0'): self.hdxRobot = HDXRobot(port_name) def get_joints_position(self): # self.hdxRobot.common_write(HDXConfig.ADDR__MODE_SELECTION, HDXConfig.CURRENT_MODE, byte_length=1) # 设置力矩模式 self.hdxRobot.set_torque(flag=True) # 获取关节数据 joints_pos = self.hdxRobot.get_robot_joints() joints_vel = self.hdxRobot.get_robot_velocity() return joints_pos class Double_Arm_Inference: def __init__(self, right_port_name='/dev/ttyUSB0', left_port_name='/dev/ttyUSB1'): self.right_arm = Single_Arm_Inference(right_port_name) self.right_dex = BLEClientHandler(ble_address = "C4:23:12:07:15:B5") self.left_arm = Single_Arm_Inference(left_port_name) self.left_dex = BLEClientHandler(ble_address = "C4:23:12:07:14:15") class CameraThread(threading.Thread): def __init__(self): super().__init__() self.pipeline = rs.pipeline() config = rs.config() config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) self.pipeline.start(config) self.latest_frame = None self.lock = threading.Lock() self.running = True self.daemon = True # 设置为守护线程,主程序退出时自动结束 def run(self): while self.running: try: frames = self.pipeline.wait_for_frames(timeout_ms=1000) color_frame = frames.get_color_frame() if color_frame: color_image = np.asanyarray(color_frame.get_data()) #color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) with self.lock: self.latest_frame = color_image except Exception as e: print(f"Camera error: {e}") time.sleep(0.1) def get_latest_frame(self): with self.lock: return self.latest_frame.copy() if self.latest_frame is not None else None def stop(self): self.running = False self.join() self.pipeline.stop() # 定义机器人观测数据结构 class RobotObsShape: HEAD_IMAGE_SHAPE = (480, 640, 3) HEAD_IMAGE_SIZE = HEAD_IMAGE_SHAPE[0] * HEAD_IMAGE_SHAPE[1] * HEAD_IMAGE_SHAPE[2] WRIST_IMAGE_SHAPE = (480, 640, 3) WRIST_IMAGE_SIZE = WRIST_IMAGE_SHAPE[0] * WRIST_IMAGE_SHAPE[1] * WRIST_IMAGE_SHAPE[2] STATE_SHAPE = (7,) STATE_SIZE = STATE_SHAPE[0] * 4 # float32占4字节 CHUNK_SIZE = HEAD_IMAGE_SIZE + WRIST_IMAGE_SIZE + STATE_SIZE DOF = 7 # 自由度 NUM_ACTIONS = 8 # 动作数量 ACTIONS_SHAPE = (NUM_ACTIONS, DOF) ACTIONS_SIZE = NUM_ACTIONS * DOF * 4 # 总动作数据大小 # 机器人状态枚举 class RobotState(str, Enum): IDLE = "IDLE" INITALIZING = "INITALIZING" ACTING = "ACTING" FINISHED = "FINISHED" class Client: def __init__(self, port=15558): self.context = zmq.Context() self.socket = self.context.socket(zmq.REQ) self.socket.connect(f"tcp://192.168.31.101:{port}") self.socket.RCVTIMEO = 20000 # 设置接收超时为1秒 self.running = True self.state = RobotState.IDLE print(f"Client connected to port {port}") # 启动相机线程 self.camera_thread = CameraThread() self.camera_thread.start() # 等待相机初始化 while self.camera_thread.get_latest_frame() is None and self.running: print("等待相机初始化...") time.sleep(0.5) def create_message(self): """创建符合协议的消息字节流""" # 确保图像尺寸正确 if self.head_img.shape != RobotObsShape.HEAD_IMAGE_SHAPE: self.head_img = cv2.resize(self.head_img, (RobotObsShape.HEAD_IMAGE_SHAPE[1], RobotObsShape.HEAD_IMAGE_SHAPE[0])) if self.wrist_img.shape != RobotObsShape.WRIST_IMAGE_SHAPE: self.wrist_img = cv2.resize(self.wrist_img, (RobotObsShape.WRIST_IMAGE_SHAPE[1], RobotObsShape.WRIST_IMAGE_SHAPE[0])) # 创建消息缓冲区 buffer = bytearray(RobotObsShape.CHUNK_SIZE) # 填充头部图像数据 head_bytes = self.head_img.tobytes() buffer[:len(head_bytes)] = head_bytes # 填充腕部图像数据 start = RobotObsShape.HEAD_IMAGE_SIZE end = start + RobotObsShape.WRIST_IMAGE_SIZE wrist_bytes = self.wrist_img.tobytes() buffer[start:end] = wrist_bytes # 填充关节角度数据 start = end end = start + RobotObsShape.STATE_SIZE buffer[start:end] = self.joint_angles.tobytes() return bytes(buffer) def parse_actions(self, action_bytes): """解析接收到的动作数据""" # 验证消息长度 if len(action_bytes) != RobotObsShape.ACTIONS_SIZE: return None actions = np.frombuffer(action_bytes, dtype=np.float32) return actions.reshape(RobotObsShape.ACTIONS_SHAPE) def run(self): """主循环:发送观察数据并接收动作""" iteration = 0 # 读取开始几张图像 等待相机稳定输出图像 frame = self.camera_thread.get_latest_frame() if frame is None: time.sleep(0.1) self.head_img = frame self.wrist_img = frame # 使用相同图像 self.joint_angles = np.array(yuanyouarm.left_arm.get_joints_position()) while self.running: # 获取图像 - 每次循环都更新 # 从相机线程获取最新帧 frame = self.camera_thread.get_latest_frame() if frame is None: time.sleep(0.1) continue self.head_img = frame self.wrist_img = frame # 使用相同图像 self.joint_angles = np.array(yuanyouarm.left_arm.get_joints_position()) #self.joint_angles = yuanyouarm.left_arm.get_joints_position() # print(f"joints1 pos: {yuanyouarm.left_arm.get_joints_position()}") # print(f"joints2 pos: {self.joint_angles}") # 显示图像 - 每次循环都显示 cv2.imshow("Head Camera", self.head_img) # 处理按键事件 key = cv2.waitKey(1) & 0xFF if key == ord('q'): self.running = False break elif key == ord('a'): self.state = RobotState.ACTING print("State changed to ACTING") elif key == ord('i'): self.state = RobotState.INITALIZING print("State changed to INITIALIZING") elif key == ord('f'): self.state = RobotState.FINISHED print("State changed to FINISHED") elif key == ord('s'): cv2.imwrite("saved_images/head_image.jpg", self.head_img) cv2.imwrite("saved_images/wrist_image.jpg", self.wrist_img) print("Images saved to saved_images/ directory") # 状态处理 if self.state == RobotState.INITALIZING: # 初始关节角度 min_read_flag, min_result_list = yuanyouarm.left_arm.hdxRobot.common_read(HDXConfig.ADDR_MIN_POSITION, byte_length=4, flag_unsigned=False) self.joint_angles = np.array([254.021978021978,39.32967032967033,17.208791208791208,291.4725274725275,119.56043956043956,75.05494505494505]) goal_Position = yuanyouarm.left_arm.hdxRobot._angle_to_value(self.joint_angles.tolist(),min_result_list) yuanyouarm.left_arm.hdxRobot.move_trackless(goal_Position) self.state = RobotState.IDLE # 修正:正确更新状态 print("Initialization complete") elif self.state == RobotState.ACTING: iteration += 1 message = self.create_message() try: print(f"Sending observation #{iteration}...") self.socket.send(message) print(1) # 接收动作响应 try: print(2) action_bytes = self.socket.recv() print(3) except zmq.Again: print(4) print("Receive timeout, retrying...") continue print(5) actions = self.parse_actions(action_bytes) if actions is not None: print("\nReceived actions:") for i, action in enumerate(actions): min_read_flag, min_result_list = yuanyouarm.left_arm.hdxRobot.common_read(HDXConfig.ADDR_MIN_POSITION, byte_length=4, flag_unsigned=False) self.joint_angles = action[:6] goal_Position = yuanyouarm.left_arm.hdxRobot._angle_to_value(self.joint_angles.tolist(),min_result_list) yuanyouarm.left_arm.hdxRobot.move_trackless(goal_Position) #time.sleep(0.2) if action[6] >= 0.95: gripper = "CLOSE" else: gripper = "OPEN" print(f"Action {i+1}: Joints={self.joint_angles}, Gripper={gripper}") # 如果夹爪关闭,结束动作 if gripper == "CLOSE": self.state = RobotState.FINISHED except Exception as e: print(f"Error occurred: {e}") self.state = RobotState.FINISHED elif self.state == RobotState.FINISHED: print("Task finished") self.running = False elif self.state == RobotState.IDLE: print("Idle state. Press 'a' to start sending messages.") cv2.destroyAllWindows() self.socket.close() self.context.term() self.camera_thread.stop() print("Client shutdown complete.") if __name__ == "__main__": yuanyouarm = Double_Arm_Inference() yuanyouarm.left_arm.hdxRobot.common_write(HDXConfig.ADDR__MODE_SELECTION, HDXConfig.POSITION_MODE, byte_length=HDXConfig.LEN_MODE_SELECTION) client = Client() client.run() # if __name__ == "__main__": # pipeline = rs.pipeline() # config = rs.config() # config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) # pipeline.start(config) # try: # yuanyouarm = Double_Arm_Inference() # yuanyouarm.left_arm.hdxRobot.common_write(HDXConfig.ADDR__MODE_SELECTION, HDXConfig.POSITION_MODE, byte_length=HDXConfig.LEN_MODE_SELECTION) # client = Client() # client.run() # print("Client shutdown complete.") # finally: # pipeline.stop() socket发送和接收出了问题
07-19
import datetime import pathlib import threading import time import hydra import numpy as np import torch import zmq # ZeroMQ用于进程间通信 from typing import Optional import dill # 用于Python对象序列化 from omegaconf import OmegaConf # 配置管理库 # 导入自定义模块 from agent.schema import RobotState, RobotObsShape # 机器人状态和观察数据结构,消息定义发送 from agent.utils import ( dict_apply, # 递归应用函数到字典中的张量 interpolate_image_batch, # 批量调整图像尺寸并进行归一化 ) from controller.policy.dexgraspvla_controller import DexGraspVLAController # 抓取控制策略 import logging import cv2 import os # 获取当前脚本所在目录 current_dir = os.path.dirname(os.path.abspath(__file__)) # 构建相对路径 pretrain_path = os.path.join(current_dir, "..", "..", "..", "dinov2", "checkpoints","dinov2_vitb14_pretrain.pth") # 初始化日志系统 def log_init(): """配置并初始化日志记录器""" logger = logging.getLogger(__name__) logger.setLevel(logging.DEBUG) # 设置日志级别 # 定义日志格式 format = ( "[%(asctime)s %(levelname)s %(filename)s %(funcName)s:%(lineno)d] %(message)s" ) handler = logging.StreamHandler() # 控制台日志处理器 handler.setLevel(logging.DEBUG) formatter = logging.Formatter(format, datefmt="%Y-%m-%d %H:%M:%S") handler.setFormatter(formatter) logger.addHandler(handler) logger.info("Logger inited") return logger logger = log_init() # 全局日志记录器 class Robot: """机器人控制主类,负责状态管理、观察获取和动作执行""" def __init__(self, config): """ 根据配置初始化机器人系统 参数: config: 包含以下关键字段的配置对象 - controller_checkpoint_path: 控制器模型路径 - device: 计算设备 (cpu/cuda) - port: ZeroMQ通信端口 - executions_per_action_chunk: 每个动作块执行次数 """ # 加载预训练控制器模型 checkpoint_path = config.controller_checkpoint_path payload = torch.load(checkpoint_path, pickle_module=dill) # 使用dill加载模型 #使用 `torch.load` 加载模型检查点,但指定了 `pickle_module=dill`。这通常是因为模型保存时使用了 `dill` 库而不是标准的 `pickle` 库。`dill` 可以序列化更广泛的对象,包括一些 `pickle` 无法处理的函数、lambda 表达式等。 # 更新模型配置中的本地权重路径 # payload["cfg"]["policy"]["obs_encoder"]["model_config"]["head"][ # "local_weights_path" # ] = "/home/fishros/.cache/torch/hub/checkpoints/dinov2_vitb14_pretrain.pth" payload["cfg"]["policy"]["obs_encoder"]["model_config"]["head"][ "local_weights_path" ] = pretrain_path cfg = payload["cfg"] cls = hydra.utils.get_class(cfg._target_) # 动态获取类 workspace = cls(cfg) # 创建工作空间实例 workspace.load_payload(payload, exclude_keys=None, include_keys=None) # 加载模型权重 # 初始化控制器 self.controller: DexGraspVLAController self.controller = workspace.model # 配置参数 self.device = config.device self.executions_per_action_chunk = config.executions_per_action_chunk # 设置模型为评估模式并转移到指定设备 self.controller.eval() print(f'!! torch.cuda.is_available()={torch.cuda.is_available()}') self.controller.to(self.device if torch.cuda.is_available() else "cpu") # 传感器数据初始化 self.head_image = None # 头部摄像头图像 self.wrist_image = None # 腕部摄像头图像 self.proprioception = None # 本体感知数据(关节角度等) # ZeroMQ通信设置 self.context = zmq.Context() self.socket = self.context.socket(zmq.REP) # 应答模式socket self.port = config.port self.socket.bind(f"tcp://*:{self.port}") # 绑定到指定端口 # 状态管理 self.state = RobotState.IDLE # 初始状态为空闲 # 重置动作和监听线程 self.resetting_action = None self.listening_thread = threading.Thread(target=self.listening_mannual) self.listening_thread.start() # 启动用户输入监听线程 def _parse_obs(self, message: bytes) -> Optional[dict]: """解析从socket接收的二进制观察数据""" # 验证消息长度 if len(message) != RobotObsShape.CHUNK_SIZE: logger.error( f"Invalid message size, required {RobotObsShape.CHUNK_SIZE} bytes" ) return None # 解析头部摄像头图像数据 (uint8数组) head_image = np.frombuffer( message.buffer[: RobotObsShape.HEAD_IMAGE_SIZE], dtype=np.uint8, ).reshape(RobotObsShape.HEAD_IMAGE_SHAPE) # 解析腕部摄像头图像数据 wrist_image = np.frombuffer( message.buffer[RobotObsShape.HEAD_IMAGE_SIZE : RobotObsShape.HEAD_IMAGE_SIZE+ RobotObsShape.WRIST_IMAGE_SIZE], dtype=np.uint8, ).reshape(RobotObsShape.WRIST_IMAGE_SHAPE) # 解析本体感知数据 (float32数组) proprioception = np.frombuffer( message.buffer[-RobotObsShape.STATE_SIZE :], dtype=np.float32, ).reshape(RobotObsShape.STATE_SHAPE) logger.info("Received head_image, wrist_image, and joint_angle") return { "head_image": head_image, "wrist_image": wrist_image, "proprioception": proprioception, } def listening_mannual(self) -> None: """监听用户输入线程函数,用于手动控制状态""" logger.info("Robot is listening...") while True: user_input = input("Press <Enter> or <q> to quit: ") if user_input == "q": self.state = RobotState.FINISHED # 退出程序 elif user_input == "i": self.state = RobotState.INITALIZING # 初始化状态 elif user_input == "r": self.state = RobotState.RESETTING # 重置状态 elif user_input == "f": self.state = RobotState.FINISHED # 结束状态 else: logger.info("Invalid input. Please press <Enter> or <q>.") def _initialize(self) -> None: """初始化机器人到准备抓取位置""" assert self.state == RobotState.INITALIZING logger.info("Initializing robot...") # 实际实现中这里会包含机械臂的初始化移动 self.state = RobotState.ACTING # 进入执行状态 logger.info("Robot initialized") def _reset_socket(self) -> None: """重置ZeroMQ socket连接""" logger.info("Resetting socket...") self.socket.close() self.context.term() # 重新创建socket self.context = zmq.Context() self.socket = self.context.socket(zmq.REP) self.socket.bind(f"tcp://*:{self.port}") logger.info("Socket reset") def _reset(self) -> None: """任务完成后重置机器人到初始位置""" assert self.state == RobotState.RESETTING logger.info("Resetting robot...") # 实际实现中这里会包含机械臂的复位移动 self.state = RobotState.ACTING logger.info("Robot reset") def _get_obs(self) -> Optional[dict]: """获取并预处理观察数据""" logger.info("Waiting for obs...") message = self.socket.recv(copy=False) # 接收观察数据 obs = self._parse_obs(message) if obs is None: self._reset_socket() # 解析失败时重置socket return None # 更新传感器数据 self.head_image = obs["head_image"] self.wrist_image = obs["wrist_image"] self.proprioception = obs["proprioception"] #self.head_image = cv2.imread("/home/fishros/hdx/tool/dataset_ori/imgs/0_130.jpg") #self.wrist_image = cv2.imread("/home/fishros/hdx/tool/dataset_ori/imgs/0_130.jpg") #self.proprioception = np.array([ 244.02, 39.33, 17.21, 291.47, 119.56, 75.05, 0.8], dtype=np.float32) #self.proprioception = np.array([ 188.07692307692307,47.12087912087912,-3.1868131868131866,311.56043956043953,156.26373626373626,64.46153846153847,1], dtype=np.float32) # 图像预处理 (插值和维度转换) rgb_head = interpolate_image_batch(self.head_image[None, ...]).unsqueeze(0) rgb_wrist = interpolate_image_batch(self.wrist_image[None, ...]).unsqueeze(0) logger.info("Robot state updated") return { "rgb": rgb_head, # (1,1,3,H,W) "right_cam_img": rgb_wrist, # (1,1,3,H,W) "right_state": torch.from_numpy(self.proprioception) .unsqueeze(0) .unsqueeze(0), # (1,1,6) } def act(self, obs: dict) -> bool: """使用控制器模型预测并发送动作""" # 将观察数据转移到模型设备 obs = dict_apply(obs, lambda x: x.to(self.controller.device)) # 模型推理 (无梯度计算) with torch.no_grad(): actions = self.controller.predict_action(obs_dict=obs) # (B,64,action_dim) # 处理动作数据 n_latency_steps = 3 # 延迟补偿步数 actions = ( actions.detach() .cpu() .numpy()[ 0, n_latency_steps : self.executions_per_action_chunk + n_latency_steps ] # (executions_per_action_chunk, action_dim) ) # 通过socket发送动作 logger.info(f"Sent action {actions}") self.socket.send(actions.tobytes()) return True def step(self) -> bool: """单步执行:获取观察->执行动作""" logger.info("Waiting for obs...") obs = self._get_obs() if obs is None: logger.error("Broken obs") return False logger.info("Robot state updated, acting...") if not self.act(obs): logger.error("Failed to send action") return False logger.info("Action sent, waiting for next obs...") return True def run(self) -> None: """机器人主控制循环""" logger.info("Robot loop starting...") assert self.state == RobotState.IDLE self.state = RobotState.INITALIZING # 状态机主循环 while True: logger.info(f"run loop with robot state: {self.state}") if self.state == RobotState.INITALIZING: self._initialize() elif self.state == RobotState.RESETTING: self._reset() elif self.state == RobotState.ACTING: self.step() # 执行主要控制循环 elif self.state == RobotState.FINISHED: logger.info("Robot loop finished, waiting for next command") # 可在此处添加等待新指令的逻辑 else: logger.error("Robot loop in unknown state.") break # OmegaConf解析器注册 def now_resolver(pattern: str): """处理${now:}时间格式化的解析器""" return datetime.now().strftime(pattern) # 注册自定义解析器 OmegaConf.register_new_resolver("now", now_resolver, replace=True) OmegaConf.register_new_resolver("eval", eval, replace=True) @hydra.main(version_base=None,config_path="config", config_name=pathlib.Path(__file__).stem) def main(cfg): """程序入口点:初始化并运行机器人""" robot = Robot(cfg) robot.run() if __name__ == "__main__": main()这是发送actions的服务端,主要为8组关节角和夹爪状态,我这边接收端的消息解析有点问题, 输出的action都是none def parse_actions(self, action_bytes): """解析接收到的动作数据""" # 验证消息长度 if len(action_bytes) != RobotObsShape.ACTIONS_SHAPE: return None actions = np.frombuffer(action_bytes, dtype=np.float32) return actions.reshape(RobotObsShape.ACTIONS_SHAPE)
07-19
import cv2 import numpy as np import zmq import time import threading from enum import Enum import os # 定义机器人观测数据结构 class RobotObsShape: HEAD_IMAGE_SHAPE = (480, 640, 3) HEAD_IMAGE_SIZE = HEAD_IMAGE_SHAPE[0] * HEAD_IMAGE_SHAPE[1] * HEAD_IMAGE_SHAPE[2] WRIST_IMAGE_SHAPE = (480, 640, 3) WRIST_IMAGE_SIZE = WRIST_IMAGE_SHAPE[0] * WRIST_IMAGE_SHAPE[1] * WRIST_IMAGE_SHAPE[2] STATE_SHAPE = (7,) STATE_SIZE = STATE_SHAPE[0] * 4 # float32占4字节 CHUNK_SIZE = HEAD_IMAGE_SIZE + WRIST_IMAGE_SIZE + STATE_SIZE DOF = 7 # 自由度 NUM_ACTIONS = 8 # 动作数量 ACTIONS_SHAPE = (NUM_ACTIONS, DOF) ACTIONS_SIZE = NUM_ACTIONS * DOF * 4 # 总动作数据大小 # 机器人状态枚举 class RobotState(str, Enum): IDLE = "IDLE" INITALIZING = "INITALIZING" ACTING = "ACTING" FINISHED = "FINISHED" def create_test_image(height, width): """创建测试图像:彩色渐变""" img = np.zeros((height, width, 3), dtype=np.uint8) for i in range(height): for j in range(width): img[i, j] = [ int(255 * j / width), # 红色通道:从左到右渐变 int(255 * i / height), # 绿色通道:从上到下渐变 150 # 蓝色通道:固定值 ] return img class Client: def __init__(self, port=15558): self.context = zmq.Context() self.socket = self.context.socket(zmq.REQ) self.socket.connect(f"tcp://192.168.2.211:{port}") self.socket.RCVTIMEO = 1000 # 设置接收超时为1秒 self.running = True self.state = RobotState.IDLE print(f"Client connected to port {port}") # # 创建测试图像 # self.head_img = create_test_image(*RobotObsShape.HEAD_IMAGE_SHAPE[:2]) # self.wrist_img = create_test_image(*RobotObsShape.WRIST_IMAGE_SHAPE[:2]) # # 初始关节角度 (6个关节角度 + 夹爪状态) # self.joint_angles = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], dtype=np.float32) self.head_img = cv2.imread("/home/fishros/hdx/tool/dataset_ori/imgs/0_0.jpg") self.wrist_img = cv2.imread("/home/fishros/hdx/tool/dataset_ori/imgs/0_0.jpg") # 初始关节角度 self.joint_angles = np.array([244.021978021978,39.32967032967033,17.208791208791208,291.4725274725275,119.56043956043956,75.05494505494505,0], dtype=np.float32) # 启动键盘监听线程 # self.listening_thread = threading.Thread(target=self.listen_keyboard) # self.listening_thread.daemon = True # self.listening_thread.start() # 创建保存图像的目录 # os.makedirs("saved_images", exist_ok=True) # def listen_keyboard(self): # """监听键盘输入线程""" # print("Keyboard listener started. Commands: [q]退出, [s]保存图片, [a]发送消息,[i]初始化关节") # while self.running: # cmd = input("> ").lower() # if cmd == 'q': # self.running = False # print("Exiting...") # elif cmd == 's': # # 保存当前图像 # cv2.imwrite("saved_images/head_image.jpg", self.head_img) # cv2.imwrite("saved_images/wrist_image.jpg", self.wrist_img) # print("Images saved to saved_images/ directory") # elif cmd == 'a': # self.state = RobotState.ACTING # print("State changed to ACTING") # elif cmd == 'i': # self.state = RobotState.INITALIZING # print("State changed to INITIALIZING") # elif cmd == 'f': # self.state = RobotState.FINISHED # print("State changed to FINISHED") def create_message(self): """创建符合协议的消息字节流""" # 确保图像尺寸正确 if self.head_img.shape != RobotObsShape.HEAD_IMAGE_SHAPE: self.head_img = cv2.resize(self.head_img, (RobotObsShape.HEAD_IMAGE_SHAPE[1], RobotObsShape.HEAD_IMAGE_SHAPE[0])) if self.wrist_img.shape != RobotObsShape.WRIST_IMAGE_SHAPE: self.wrist_img = cv2.resize(self.wrist_img, (RobotObsShape.WRIST_IMAGE_SHAPE[1], RobotObsShape.WRIST_IMAGE_SHAPE[0])) # 创建消息缓冲区 buffer = bytearray(RobotObsShape.CHUNK_SIZE) # 填充头部图像数据 head_bytes = self.head_img.tobytes() buffer[:len(head_bytes)] = head_bytes # 填充腕部图像数据 start = RobotObsShape.HEAD_IMAGE_SIZE end = start + RobotObsShape.WRIST_IMAGE_SIZE wrist_bytes = self.wrist_img.tobytes() buffer[start:end] = wrist_bytes # 填充关节角度数据 start = end end = start + RobotObsShape.STATE_SIZE buffer[start:end] = self.joint_angles.tobytes() return bytes(buffer) def parse_actions(self, action_bytes): """解析接收到的动作数据""" # 验证消息长度 if len(action_bytes) != RobotObsShape.ACTIONS_SHAPE: return None actions = np.frombuffer(action_bytes, dtype=np.float32) return actions.reshape(RobotObsShape.ACTIONS_SHAPE) def run(self): """主循环:发送观察数据并接收动作""" iteration = 0 while self.running: # 在图像上添加状态信息 # status_text = f"Iteration: {iteration} | State: {self.state}" # cv2.putText(self.head_img, status_text, (10, 30), # cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) # 显示图像 cv2.imshow("Head Camera", self.head_img) #cv2.imshow("Wrist Camera", self.wrist_img) # 处理按键事件(关键修改:放在主循环中) key = cv2.waitKey(1) & 0xFF if key == ord('q'): self.running = False break elif key == ord('a'): self.state = RobotState.ACTING print("State changed to ACTING") elif key == ord('i'): self.state = RobotState.INITALIZING print("State changed to INITIALIZING") elif key == ord('f'): self.state = RobotState.FINISHED print("State changed to FINISHED") elif key == ord('s'): cv2.imwrite("saved_images/head_image.jpg", self.head_img) cv2.imwrite("saved_images/wrist_image.jpg", self.wrist_img) print("Images saved to saved_images/ directory") # 状态处理 if self.state == RobotState.INITALIZING: # 初始关节角度 (6个关节角度 + 夹爪状态) #self.joint_angles = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], dtype=np.float32) # 创建新的测试图像 #self.head_img = create_test_image(*RobotObsShape.HEAD_IMAGE_SHAPE[:2]) #self.wrist_img = create_test_image(*RobotObsShape.WRIST_IMAGE_SHAPE[:2]) self.head_img = cv2.imread("/home/fishros/hdx/tool/dataset_ori/imgs/0_0.jpg") self.wrist_img = cv2.imread("/home/fishros/hdx/tool/dataset_ori/imgs/0_0.jpg") # 初始关节角度 self.joint_angles = np.array([244.021978021978,39.32967032967033,17.208791208791208,291.4725274725275,119.56043956043956,75.05494505494505,0], dtype=np.float32) time.sleep(0.1) # 避免过于频繁的更新 elif self.state == RobotState.ACTING: iteration += 1 message = self.create_message() try: print(f"Sending observation #{iteration}...") self.socket.send(message) # 接收动作响应 try: action_bytes = self.socket.recv() except zmq.Again: print("Receive timeout, retrying...") continue actions = self.parse_actions(action_bytes) print("\nReceived actions:") for i, action in enumerate(actions): joint_angles = action[:6] if action[6] < 0.95: gripper = "OPEN" else: gripper = "CLOSE" self.state = RobotState.FINISHED # 修正:使用赋值操作符= print(f"Action {i+1}: Joints={joint_angles}, Gripper={gripper}") # 更新关节角度 (模拟运动) self.joint_angles[:6] += np.random.uniform(-5, 5, 6) self.joint_angles[:6] = np.clip(self.joint_angles[:6], -180, 180) # 每8次迭代切换一次夹爪状态 if iteration % 8 == 0: self.joint_angles[6] = 1.0 - self.joint_angles[6] except Exception as e: print(f"Error occurred: {e}") self.running = False elif self.state == RobotState.FINISHED: # 任务完成状态,显示消息但不发送数据 print("Task finished. Press 'i' to initialize or 'a' to start again.") time.sleep(0.5) # 避免过于频繁的更新 elif self.state == RobotState.IDLE: # 空闲状态,显示消息但不发送数据 print("Idle state. Press 'a' to start sending messages.") time.sleep(0.5) # 避免过于频繁的更新 cv2.destroyAllWindows() self.socket.close() self.context.term() if __name__ == "__main__": client = Client() client.run() print("Client shutdown complete.")接受数据时报错(hdx) (hdx) fishros@fishros-linux:~/hdx/DexGraspVLA/agent$ python hdx_obs_output_socket.py Client connected to port 15558 Idle state. Press 'a' to start sending messages. Idle state. Press 'a' to start sending messages. Idle state. Press 'a' to start sending messages. Idle state. Press 'a' to start sending messages. Idle state. Press 'a' to start sending messages. Idle state. Press 'a' to start sending messages. Idle state. Press 'a' to start sending messages. Idle state. Press 'a' to start sending messages. State changed to ACTING Sending observation #1... Received actions: Error occurred: 'NoneType' object is not iterable Client shutdown complete.
07-19
from enum import Enum from typing import Literal, Union, Optional from pydantic import BaseModel, Field import numpy as np class Role(str, Enum): """Message role options""" SYSTEM = "system" USER = "user" ASSISTANT = "assistant" TOOL = "tool" ROLE_VALUES = tuple(role.value for role in Role) ROLE_TYPE = Literal[ROLE_VALUES] # type: ignore class Function(BaseModel): name: str arguments: str class ToolCall(BaseModel): """Represents a tool/function call in message""" id: str type: str = "function" function: Function class Message(BaseModel): """Represents a chat message in the conversation""" role: ROLE_TYPE = Field(...) # type: ignore content: Optional[str] = Field(default=None) tool_calls: Optional[list[ToolCall]] = Field(default=None) name: Optional[str] = Field(default=None) # 对话者的名字 tool_call_id: Optional[str] = Field(default=None) base64_image: Optional[str] = Field(default=None) def to_dict(self) -> dict: """Convert message to dictionary format""" message = {"role": self.role} if self.content is not None: message["content"] = self.content if self.tool_calls is not None: message["tool_calls"] = [tool_call.dict() for tool_call in self.tool_calls] if self.name is not None: message["name"] = self.name if self.tool_call_id is not None: message["tool_call_id"] = self.tool_call_id if self.base64_image is not None: message["base64_image"] = self.base64_image return message @classmethod def user_message( cls, content: str, base64_image: Optional[str] = None ) -> "Message": """Create a user message""" return cls(role=Role.USER, content=content, base64_image=base64_image) @classmethod def system_message(cls, content: str) -> "Message": """Create a system message""" return cls(role=Role.SYSTEM, content=content) @classmethod def assistant_message( cls, content: Optional[str] = None, base64_image: Optional[str] = None ) -> "Message": """Create an assistant message""" return cls(role=Role.ASSISTANT, content=content, base64_image=base64_image) @classmethod def tool_message( cls, content: str, name, tool_call_id: str, base64_image: Optional[str] = None ) -> "Message": """Create a tool message""" return cls( role=Role.TOOL, content=content, name=name, tool_call_id=tool_call_id, base64_image=base64_image, ) @classmethod def from_tool_calls( cls, tool_calls: list, content: Union[str, list[str]] = "", base64_image: Optional[str] = None, **kwargs, ): """Create ToolCallsMessage from raw tool calls. Args: tool_calls: Raw tool calls from LLM content: Optional message content base64_image: Optional base64 encoded image """ formatted_calls = [ {"id": call.id, "function": call.function.model_dump(), "type": "function"} for call in tool_calls ] return cls( role=Role.ASSISTANT, content=content, tool_calls=formatted_calls, base64_image=base64_image, **kwargs, ) class Memory(BaseModel): messages: list[Message] = Field(default_factory=list) max_messages: int = Field(default=100) def add_message(self, message: Message) -> None: """Add a message to memory""" self.messages.append(message) # Optional: Implement message limit if len(self.messages) > self.max_messages: self.messages = self.messages[-self.max_messages :] def add_messages(self, messages: list[Message]) -> None: """Add multiple messages to memory""" self.messages.extend(messages) def clear(self) -> None: """Clear all messages""" self.messages.clear() def get_recent_messages(self, n: int) -> list[Message]: """Get n most recent messages""" return self.messages[-n:] def to_dict_list(self) -> list[dict]: """Convert messages to list of dicts""" return [msg.to_dict() for msg in self.messages] class RobotRequestType(np.uint8, Enum): REQUEST = np.uint8(0) class RobotObsShape: """发送一个占位的字节,0 代表请求。接受数据格式为 (head_image, wrist_image, joint_angle) head_image: np.ndarray of shape (480, 640, 3), of dtype np.uint8 wrist_image: np.ndarray of shape (360, 480, 3), of dtype np.uint8 joint_angle: np.ndarray of shape (6,), of dtype np.float32 """ HEAD_IMAGE_SHAPE = (480, 640, 3) HEAD_IMAGE_SIZE = HEAD_IMAGE_SHAPE[0] * HEAD_IMAGE_SHAPE[1] * HEAD_IMAGE_SHAPE[2] WRIST_IMAGE_SHAPE = (360, 480, 3) WRIST_IMAGE_SIZE = ( WRIST_IMAGE_SHAPE[0] * WRIST_IMAGE_SHAPE[1] * WRIST_IMAGE_SHAPE[2] ) STATE_SHAPE = (7,) STATE_SIZE = STATE_SHAPE[0] * 4 # float32 requires 4 bytes CHUNK_SIZE = HEAD_IMAGE_SIZE + WRIST_IMAGE_SIZE + STATE_SIZE DOF = 7 ACTION_SIZE = DOF * 4 NUM_ACTIONS = 6 TOTAL_NUM_ACTIONS = NUM_ACTIONS * DOF TOTAL_ACTIONS_SIZE = TOTAL_NUM_ACTIONS * 4 # float32 requires 4 bytes ACTIONS_SHAPE = (NUM_ACTIONS, DOF) class RobotState(str, Enum): """Robot state options""" IDLE = "IDLE" INITALIZING = "INITALIZING" RESETTING = "RESETTING" ACTING = "ACTING" FINISHED = "FINISHED" 全文注释
07-16
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值