S3client方法使用不当导致线程大量WAIT_CLOSE

背景

后端服务中有用到OSS的对象存储服务,完成文件上传操作,其中有这样一个场景:

问题定位

  1. 刚开始发现这个问题以为是线程池不够用,通过调整线程池大小,发现服务能支持时间长点,但压测一段时间发现还是会卡死,线程被打满。
  2. 后端又怀疑是不是使用@Asnyc线程嵌套导致的,去掉改成同步,问题依然存在
  3. 然后就排查代码看是不是那块资源未释放(查了好几遍没发现问题,该close的资源都close了)
  4. 后面有浮现了几次后发现,每次上传1000文件,就会有1000个线程 CLOSE_WAIT 就很奇怪,线程死活不关闭,然后就针对OSS相关代码做排查,一行一行把oss相关注释后,发现getFileSize()去掉后,再没有线程 CLOSE_WAIT 情况,就是这家伙惹的祸。。。。定位完毕(而时间已经是凌晨2点多了),欲哭无泪呀。OSS还有这个坑。血的教训。
/**
     * 获取文件大小
     *
     * @param fileURL   文件的url(标准oss地址)
     */
    public Long getFileSize(String fileURL) {
        // 解析bucketName
        String bucketName = getBucketName(fileURL);
        // 解析objectName
        String objectName = getObjectName(bucketName, fileURL);
        return s3client.getObject(bucketName, bucketName).getObjectMetadata().getInstanceLength();
    }

问题就处在 s3client.getObject(bucketName, bucketName).getObjectMetadata().getInstanceLength(); 这行代码。

oss SDK获取文件大小,应该调用getMetaData方法,代码里调用的getObject().getMetaData,相当于下载文件但是仅获取http头,OSS服务侧任务数据传输已完毕然后就断开连接了,本地获取到了文件流但是没有读取,此时就会导致CLOSE_WAIT,对应的tcp连接recv-q队列有值,send-q队列大小为0,表示应用已获取了数据但是还没来得及获取远程就关闭了连接,该连接不会再进入CLOSED状态,非CLOSED状态的连接不会被复用,连接一直不释放进而引发连接池打满的情况

解决方案

/**
     * 获取文件大小
     *
     * @param fileURL   文件的url(标准oss地址)
     */
    public Long getFileSize(String fileURL) {
        // 解析bucketName
        String bucketName = getBucketName(fileURL);
        // 解析objectName
        String objectName = getObjectName(bucketName, fileURL);
        return s3client.getObjectMetadata(bucketName, objectName).getInstanceLength();
    }

感悟

后面再用三方sdk的时候,特别是这种使用到线程池先关的,一定要做好压测,针对用到的每一个方法多看看源码和底层实现,做好资源回收,做好资源回收,做好资源回收!!!

lyx@lyx-virtual-machine:~/Codroid_ROS2_WS/src/codroid_msgs/src$ python3 2.py [INFO] [1761226713.090949430] [s360_robot_full_control]: === S3 60机器人控制节点启动 === [INFO] [1761226713.206587598] [s360_robot_full_control]: ✅ MoveIt2接口初始化完成 [INFO] [1761226713.207614623] [s360_robot_full_control]: 📡 Socket服务器监听: 192.168.101.111:8888 [INFO] [1761226713.208277726] [s360_robot_full_control]: ✅ 系统初始化完成: 192.168.101.111:8888 [INFO] [1761226713.209199884] [s360_robot_full_control]: 🚀 节点进入运行状态,等待指令... [INFO] [1761226718.977626025] [s360_robot_full_control]: 📞 新客户端连接: (&#39;192.168.101.112&#39;, 58502) [INFO] [1761226718.994709784] [s360_robot_full_control]: 📥 接收指令: 类型=p2, 参数=[[0.4, 0.1, 0.2], [1.0, 0.0, 0.0, 0.0]] [FATAL] [1761226719.002192131] [s360_robot_full_control]: ❌ 节点运行异常: Failed to get number of ready entities for action client: wait set index for result client is out of bounds, at ./src/rcl_action/action_client.c:635 Traceback (most recent call last): File "/home/lyx/Codroid_ROS2_WS/src/codroid_msgs/src/2.py", line 491, in main executor.spin() File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 294, in spin self.spin_once() File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 806, in spin_once self._spin_once_impl(timeout_sec) File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 787, in _spin_once_impl handler, entity, node = self.wait_for_ready_callbacks( File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 723, in wait_for_ready_callbacks return next(self._cb_iter) File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 642, in _wait_for_ready_callbacks if wt in waitables and wt.is_ready(wait_set): File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/action/client.py", line 231, in is_ready ready_entities = self._client_handle.is_ready(wait_set) rclpy._rclpy_pybind11.RCLError: Failed to get number of ready entities for action client: wait set index for result client is out of bounds, at ./src/rcl_action/action_client.c:635 [INFO] [1761226719.002788258] [s360_robot_full_control]: 🛑 节点开始关闭,释放资源中... [INFO] [1761226719.003273958] [s360_robot_full_control]: ⏳ 等待Socket线程退出... [WARN] [1761226719.009362504] [s360_robot_full_control]: Joint states are not available yet! [INFO] [1761226719.010012613] [s360_robot_full_control]: Joint states are available now [INFO] [1761226719.072622565] [s360_robot_full_control]: 📋 收到规划轨迹: 37个点 [INFO] [1761226719.077731579] [s360_robot_full_control]: ⏳ 开始运动完成检测... [INFO] [1761226719.078853332] [s360_robot_full_control]: 📤 指令执行结果: None(客户端:(&#39;192.168.101.112&#39;, 58502)) Exception in thread Thread-2 (handle_client): Traceback (most recent call last): File "/usr/lib/python3.10/threading.py", line 1016, in _bootstrap_inner self.run() File "/usr/lib/python3.10/threading.py", line 953, in run self._target(*self._args, **self._kwargs) File "/home/lyx/Codroid_ROS2_WS/src/codroid_msgs/src/2.py", line 453, in handle_client self.get_logger().info(f"📞 客户端连接已断开: {client_socket.getpeername()}") OSError: [Errno 9] Bad file descriptor [INFO] [1761226719.981364921] [s360_robot_full_control]: 📡 Socket服务器已关闭 [INFO] [1761226719.982142669] [s360_robot_full_control]: 🗑️ 销毁MoveIt2资源... [INFO] [1761226719.988285992] [s360_robot_full_control]: ✅ 所有资源已释放 [INFO] [1761226720.018866697] [s360_robot_full_control]: ✅ 节点已完全关闭 #!/usr/bin/env python3 """ S3 60机器人控制节点 - 运动完成确认修复版 基于关节角度比较确认运动完成,到达目标后等待0.5s返回 修复:解决关节数据顺序问题、多线程资源竞争、节点关闭资源泄漏问题 """ import socket import json import time import traceback from threading import Thread, Lock import rclpy from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor from sensor_msgs.msg import JointState from moveit_msgs.msg import DisplayTrajectory from pymoveit2 import MoveIt2 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint # -------------------------- 配置参数 -------------------------- # 请根据你的机器人硬件配置调整以下参数 JOINT_NAMES = ["Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"] BASE_LINK_NAME = "base_link" END_EFFECTOR_NAME = "link6" MOVE_GROUP_ARM = "arm" PLANNER_ID = "RRTConnectkConfigDefault" SERVER_IP = "192.168.101.111" # 机器人控制节点IP SERVER_PORT = 8888 # 通信端口 MAX_VELOCITY = 0.5 # 最大速度(单位:rad/s) MAX_ACCELERATION = 0.5 # 最大加速度(单位:rad/s²) JOINT_TOLERANCE = 0.03 # 关节角度容差(单位:rad) MAX_POINTS = 5 # 轨迹最大采样点数 MOTION_TIMEOUT = 30.0 # 运动超时时间(单位:s) class S360RobotControlNode(Node): def __init__(self): super().__init__("s360_robot_full_control") self.get_logger().info("=== S3 60机器人控制节点启动 ===") # 状态管理(新增:is_shutting_down用于关闭状态标记) self.is_shutting_down = False self.joint_state_received = False # 关节状态是否已接收 self.current_joint_positions = {name: 0.0 for name in JOINT_NAMES} # 关节位置字典 # 线程锁(新增:解决多线程资源竞争) self.joints_lock = Lock() # 关节数据锁 self.trajectory_lock = Lock() # 轨迹数据锁 self.moveit_lock = Lock() # MoveIt2操作锁(核心修复) # 轨迹与运动相关变量 self.planned_trajectory = None # -------------------------- 子系统初始化 -------------------------- # 1. 轨迹发布器(控制机器人运动) self.trajectory_pub = self.create_publisher(JointTrajectory, &#39;RobotMove&#39;, 10) # 2. 轨迹监控器(接收MoveIt2规划结果) self.trajectory_sub = self.create_subscription( DisplayTrajectory, &#39;/display_planned_path&#39;, self.trajectory_callback, 10 ) # 3. 关节状态订阅器(获取机器人当前关节位置) self.joint_state_sub = self.create_subscription( JointState, &#39;joint_states&#39;, self.joint_state_callback, 10 ) # 4. MoveIt2初始化(运动规划核心) self.moveit2 = None self.init_moveit2() # 5. Socket服务器(接收外部控制指令) self.socket_thread = Thread( target=self.start_socket_server, daemon=True ) self.socket_thread.start() self.get_logger().info(f"✅ 系统初始化完成: {SERVER_IP}:{SERVER_PORT}") # ======================= 核心初始化方法 ======================= def init_moveit2(self): """初始化MoveIt2接口,建立与运动规划器的连接""" try: self.moveit2 = MoveIt2( node=self, joint_names=JOINT_NAMES, base_link_name=BASE_LINK_NAME, end_effector_name=END_EFFECTOR_NAME, group_name=MOVE_GROUP_ARM ) self.moveit2.planner_id = PLANNER_ID self.moveit2.max_velocity = MAX_VELOCITY self.moveit2.max_acceleration = MAX_ACCELERATION self.get_logger().info("✅ MoveIt2接口初始化完成") except Exception as e: self.get_logger().error(f"❌ MoveIt2初始化失败: {e}") self.moveit2 = None # ======================= 数据回调方法 ======================= def joint_state_callback(self, msg): """关节状态回调:更新当前关节位置(字典存储,解决顺序问题)""" try: # 建立“关节名-位置”映射,避免顺序错乱 joint_mapping = {name: position for name, position in zip(msg.name, msg.position)} with self.joints_lock: # 只更新已知关节的位置,过滤无效数据 for name in JOINT_NAMES: if name in joint_mapping: self.current_joint_positions[name] = joint_mapping[name] self.joint_state_received = True # 标记关节数据已就绪 except Exception as e: self.get_logger().error(f"❌ 关节状态处理异常: {e}\n{traceback.format_exc()}") def trajectory_callback(self, msg): """轨迹回调:接收MoveIt2规划的轨迹点""" try: if msg.trajectory and msg.trajectory[0].joint_trajectory.points: with self.trajectory_lock: self.planned_trajectory = msg.trajectory[0].joint_trajectory.points self.get_logger().info(f"📋 收到规划轨迹: {len(self.planned_trajectory)}个点") except Exception as e: self.get_logger().error(f"❌ 轨迹处理异常: {e}") # ======================= 数据工具方法 ======================= def get_current_joints_in_standard_order(self): """获取标准顺序(JOINT_NAMES)的当前关节位置列表""" with self.joints_lock: return [self.current_joint_positions[name] for name in JOINT_NAMES] def wait_for_planned_trajectory(self, timeout=5.0): """等待MoveIt2规划轨迹生成,超时返回None""" start_time = time.time() while rclpy.ok() and not self.is_shutting_down: with self.trajectory_lock: if self.planned_trajectory: return self.planned_trajectory if time.time() - start_time > timeout: self.get_logger().warning("⚠️ 等待轨迹超时") return None time.sleep(0.1) return None # ======================= 轨迹发布方法 ======================= def publish_joint_trajectory(self, target_joints): """发布单目标点轨迹(用于简单关节运动)""" try: traj_msg = JointTrajectory() traj_msg.joint_names = JOINT_NAMES point = JointTrajectoryPoint() point.positions = target_joints point.time_from_start = rclpy.duration.Duration(seconds=2.0).to_msg() traj_msg.points.append(point) self.trajectory_pub.publish(traj_msg) return True except Exception as e: self.get_logger().error(f"❌ 关节轨迹发布失败: {e}") return False def publish_planned_trajectory(self, trajectory_points): """发布MoveIt2规划的多段轨迹(采样后发布,避免点数过多)""" try: if not trajectory_points: self.get_logger().warning("⚠️ 无轨迹点可发布") return False traj_msg = JointTrajectory() traj_msg.joint_names = JOINT_NAMES # 采样轨迹点(最多MAX_POINTS个,平衡平滑度与实时性) if len(trajectory_points) > MAX_POINTS: step = max(1, len(trajectory_points) // MAX_POINTS) trajectory_points = [trajectory_points[i] for i in range(0, len(trajectory_points), step)][:MAX_POINTS] # 分配时间戳,确保运动平滑 total_time = 5.0 # 总运动时长(可根据需求调整) for i, point in enumerate(trajectory_points): traj_point = JointTrajectoryPoint() traj_point.positions = point.positions traj_point.time_from_start = rclpy.duration.Duration( seconds=total_time * (i / (len(trajectory_points) - 1)) ).to_msg() traj_msg.points.append(traj_point) self.trajectory_pub.publish(traj_msg) return True except Exception as e: self.get_logger().error(f"❌ 规划轨迹发布失败: {e}") return False def publish_smooth_trajectory(self, start_joints, target_joints, num_points=5): """发布平滑过渡轨迹(从起点到目标点的线性插值)""" try: traj_msg = JointTrajectory() traj_msg.joint_names = JOINT_NAMES num_points = min(num_points, MAX_POINTS) # 限制最大点数 total_time = 5.0 # 总运动时长 # 线性插值生成中间点 for i in range(num_points): ratio = i / (num_points - 1) if num_points > 1 else 1.0 point = JointTrajectoryPoint() point.positions = [ start + ratio * (target - start) for start, target in zip(start_joints, target_joints) ] point.time_from_start = rclpy.duration.Duration(seconds=total_time * ratio).to_msg() traj_msg.points.append(point) self.trajectory_pub.publish(traj_msg) return True except Exception as e: self.get_logger().error(f"❌ 平滑轨迹发布失败: {e}") return False # ======================= 运动控制核心方法 ======================= def execute_joint_motion(self, target_joints): """执行关节空间运动:根据目标关节角度控制机器人""" # 修复1:关闭状态下拒绝执行 if self.is_shutting_down: self.get_logger().error("❌ 节点正在关闭,拒绝关节运动指令") return False # 修复2:检查关节数据是否就绪 if not self.joint_state_received: self.get_logger().error("❌ 未收到关节状态数据,无法执行关节运动") return False # 基础参数校验 if not self.moveit2 or len(target_joints) != len(JOINT_NAMES): self.get_logger().error("❌ 关节运动参数无效(MoveIt2未就绪或关节数不匹配)") return False try: # 获取当前关节位置,用于生成平滑轨迹 current_joints = self.get_current_joints_in_standard_order() self.publish_smooth_trajectory(current_joints, target_joints) # 修复3:MoveIt2操作加锁,避免多线程竞争 with self.moveit_lock: self.moveit2.move_to_configuration(target_joints) # 等待运动完成(基于关节角度容差判断) return self.wait_for_motion_completion(target_joints, timeout=MOTION_TIMEOUT) except Exception as e: self.get_logger().error(f"❌ 关节运动异常: {e}\n{traceback.format_exc()}") return False def execute_pose_motion(self, target_pose): """执行位姿空间运动:根据目标位姿(位置+姿态)控制机器人""" # 修复1:关闭状态下拒绝执行 if self.is_shutting_down: self.get_logger().error("❌ 节点正在关闭,拒绝位姿运动指令") return False # 修复2:检查关节数据是否就绪 if not self.joint_state_received: self.get_logger().error("❌ 未收到关节状态数据,无法执行位姿运动") return False # 基础参数校验(target_pose格式:[[x,y,z], [x,y,z,w]]) if not self.moveit2 or len(target_pose) != 2 or len(target_pose[0]) != 3 or len(target_pose[1]) != 4: self.get_logger().error("❌ 位姿运动参数无效(格式错误或MoveIt2未就绪)") return False try: target_pos, target_quat = target_pose # 拆分位置和姿态 # 修复3:MoveIt2操作加锁,避免多线程竞争 with self.moveit_lock: # 1. 计算逆运动学(位姿→关节角度) ik_result = self.moveit2.compute_ik(target_pos, target_quat) if not ik_result: self.get_logger().error("❌ 逆运动学求解失败(目标位姿不可达)") return False # 2. 执行位姿运动 self.moveit2.move_to_pose(position=target_pos, quat_xyzw=target_quat, cartesian=False) # 3. 提取目标关节角度(用于运动完成判断) target_joints = self._extract_joint_positions_from_state(ik_result) if not target_joints: self.get_logger().error("❌ 无法提取目标关节角度,无法判断运动完成") return False # 4. 发布轨迹(优先用规划轨迹,其次用平滑轨迹) planned_trajectory = self.wait_for_planned_trajectory(timeout=5.0) if planned_trajectory: self.publish_planned_trajectory(planned_trajectory) else: current_joints = self.get_current_joints_in_standard_order() self.publish_smooth_trajectory(current_joints, target_joints) # 5. 等待运动完成 return self.wait_for_motion_completion(target_joints, timeout=MOTION_TIMEOUT) except Exception as e: self.get_logger().error(f"❌ 位姿运动异常: {e}\n{traceback.format_exc()}") return False def _extract_joint_positions_from_state(self, joint_state): """从JointState消息中提取标准顺序的关节位置""" if not joint_state or not hasattr(joint_state, &#39;position&#39;): self.get_logger().warning("⚠️ 无效的JointState消息,无法提取关节位置") return None try: position_map = {name: pos for name, pos in zip(joint_state.name, joint_state.position)} # 按JOINT_NAMES顺序提取,确保与目标顺序一致 return [position_map[name] for name in JOINT_NAMES if name in position_map] except Exception as e: self.get_logger().error(f"❌ 关节提取失败: {e}") return None # ======================= 运动完成判断方法 ======================= def wait_for_motion_completion(self, target_joints, timeout=30.0): """基于关节角度容差判断运动是否完成(稳定0.5s后确认)""" start_time = time.time() target_reached_time = None # 首次到达目标的时间 last_log_time = start_time # 上次打印日志的时间 stable_count = 0 # 稳定计数器(0.1s/次,5次=0.5s) self.get_logger().info("⏳ 开始运动完成检测...") while rclpy.ok() and not self.is_shutting_down: # 1. 检查超时 if time.time() - start_time > timeout: current_joints = self.get_current_joints_in_standard_order() errors = [abs(c - t) for c, t in zip(current_joints, target_joints)] self.get_logger().error( f"⚠️ 运动超时! 最大误差: {max(errors):.4f}rad\n" f"目标关节: {[round(t, 3) for t in target_joints]}\n" f"当前关节: {[round(c, 3) for c in current_joints]}" ) return False # 2. 获取当前关节位置 current_joints = self.get_current_joints_in_standard_order() # 3. 检查是否到达目标(所有关节在容差内) if self._joints_reached_target(current_joints, target_joints): if target_reached_time is None: target_reached_time = time.time() self.get_logger().info("🎯 首次到达目标位置,开始稳定检测...") stable_count += 1 # 稳定0.5s(5个检测周期)后确认完成 if stable_count >= 5: stable_time = time.time() - target_reached_time self.get_logger().info(f"✅ 运动完成! 稳定时间: {stable_time:.2f}s") return True else: # 未到达目标,重置稳定计数器 target_reached_time = None stable_count = 0 # 4. 每秒打印一次运动状态(避免日志刷屏) current_time = time.time() if current_time - last_log_time >= 1.0: last_log_time = current_time elapsed = current_time - start_time errors = [abs(c - t) for c, t in zip(current_joints, target_joints)] self.get_logger().info( f"⏱️ 运动中... 已耗时: {elapsed:.1f}s | " f"最大误差: {max(errors):.4f}rad | " f"稳定计数: {stable_count}/5" ) time.sleep(0.1) # 100ms检测周期,平衡实时性与资源占用 def _joints_reached_target(self, current_joints, target_joints): """检查所有关节是否在容差范围内到达目标位置""" return all(abs(c - t) <= JOINT_TOLERANCE for c, t in zip(current_joints, target_joints)) # ======================= 网络通信方法 ======================= def start_socket_server(self): """启动Socket服务器,接收外部控制指令(如p1/p2/p3)""" server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) # 允许端口复用 try: server_socket.bind((SERVER_IP, SERVER_PORT)) server_socket.listen(5) # 最大同时连接数 server_socket.settimeout(1.0) # 超时避免阻塞关闭流程 self.get_logger().info(f"📡 Socket服务器监听: {SERVER_IP}:{SERVER_PORT}") while rclpy.ok() and not self.is_shutting_down: try: # 接收客户端连接(非阻塞,超时后循环检查关闭状态) client_socket, addr = server_socket.accept() self.get_logger().info(f"📞 新客户端连接: {addr}") # 启动线程处理客户端请求(避免阻塞其他连接) Thread(target=self.handle_client, args=(client_socket,), daemon=True).start() except socket.timeout: continue # 超时无连接,继续循环检查关闭状态 except Exception as e: self.get_logger().error(f"❌ Socket连接异常: {e}") except Exception as e: self.get_logger().fatal(f"❌ Socket服务器启动失败: {e}") finally: server_socket.close() self.get_logger().info("📡 Socket服务器已关闭") def handle_client(self, client_socket): """处理单个客户端的控制指令(指令格式:cmd_type:json_params)""" try: # 接收指令(最大1024字节,避免数据包过大) data = client_socket.recv(1024).decode().strip() if not data: self.get_logger().warning("⚠️ 客户端发送空指令,断开连接") return # 解析指令格式(如"p2:[[0.4,0.1,0.2],[1.0,0.0,0.0,0.0]]") if &#39;:&#39; not in data: self.get_logger().error(f"❌ 无效指令格式: {data}(需符合&#39;cmd:params&#39;)") client_socket.sendall("False".encode()) return cmd_type, cmd_json = data.split(&#39;:&#39;, 1) cmd_params = json.loads(cmd_json) self.get_logger().info(f"📥 接收指令: 类型={cmd_type}, 参数={cmd_params}") # 执行对应指令 success = False if cmd_type == "p1": # p1指令:关节空间运动(参数:[joint1, joint2, ..., joint6]) success = self.execute_joint_motion(cmd_params) elif cmd_type in ["p2", "p3"]: # p2/p3指令:位姿空间运动(参数:[[x,y,z], [x,y,z,w]]) success = self.execute_pose_motion(cmd_params) else: self.get_logger().error(f"❌ 未知指令类型: {cmd_type}(支持p1/p2/p3)") # 向客户端返回执行结果(True/False) client_socket.sendall(str(success).encode()) self.get_logger().info(f"📤 指令执行结果: {success}(客户端:{client_socket.getpeername()})") except json.JSONDecodeError: self.get_logger().error(f"❌ JSON参数解析失败: {cmd_json}") client_socket.sendall("False".encode()) except Exception as e: self.get_logger().error(f"❌ 客户端处理异常: {e}\n{traceback.format_exc()}") client_socket.sendall("False".encode()) finally: client_socket.close() self.get_logger().info(f"📞 客户端连接已断开: {client_socket.getpeername()}") # ======================= 节点关闭方法 ======================= def shutdown(self): """优化关闭流程:确保线程退出、资源释放(核心修复)""" self.is_shutting_down = True self.get_logger().info("🛑 节点开始关闭,释放资源中...") # 1. 等待Socket线程退出(避免通信中强制关闭) if hasattr(self, &#39;socket_thread&#39;) and self.socket_thread.is_alive(): self.get_logger().info("⏳ 等待Socket线程退出...") self.socket_thread.join(timeout=2.0) # 最多等待2秒 if self.socket_thread.is_alive(): self.get_logger().warning("⚠️ Socket线程未正常退出,强制终止") # 2. 销毁MoveIt2资源(避免资源泄漏导致的"Destroyable"错误) if hasattr(self, &#39;moveit2&#39;) and self.moveit2 is not None: self.get_logger().info("🗑️ 销毁MoveIt2资源...") # 置空MoveIt2实例,触发垃圾回收 self.moveit2 = None # 3. 关闭发布器与订阅器(ROS2节点标准操作) self.destroy_publisher(self.trajectory_pub) self.destroy_subscription(self.trajectory_sub) self.destroy_subscription(self.joint_state_sub) self.get_logger().info("✅ 所有资源已释放") def main(): """节点主函数:初始化ROS2、启动执行器""" rclpy.init() node = S360RobotControlNode() # 使用多线程执行器,支持同时处理回调与Socket通信 executor = MultiThreadedExecutor(num_threads=4) executor.add_node(node) try: node.get_logger().info("🚀 节点进入运行状态,等待指令...") executor.spin() except KeyboardInterrupt: # 捕获Ctrl+C中断信号 node.get_logger().info("🛑 收到键盘中断信号(Ctrl+C)") except Exception as e: # 捕获其他未知异常 node.get_logger().fatal(f"❌ 节点运行异常: {e}\n{traceback.format_exc()}") finally: # 确保关闭流程执行 node.shutdown() node.destroy_node() rclpy.shutdown() node.get_logger().info("✅ 节点已完全关闭") if __name__ == "__main__": main() 怎么解决,给我完整的解决代码
10-24
#!/usr/bin/env python3 """ S3 60机器人控制节点 - 运动完成确认修复版 基于关节角度比较确认运动完成,到达目标后等待0.5s返回 修复:解决MoveIt2 Action Client执行器上下文冲突问题 修复:关节数据重排为标准顺序进行对比 """ import socket import json import time from threading import Thread, Lock import traceback import rclpy from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor from sensor_msgs.msg import JointState from moveit_msgs.msg import DisplayTrajectory from pymoveit2 import MoveIt2 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint # -------------------------- 配置参数 -------------------------- JOINT_NAMES = ["Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"] BASE_LINK_NAME = "base_link" END_EFFECTOR_NAME = "link6" MOVE_GROUP_ARM = "arm" PLANNER_ID = "RRTConnectkConfigDefault" SERVER_IP = "192.168.101.111" SERVER_PORT = 8888 MAX_VELOCITY = 0.5 MAX_ACCELERATION = 0.5 JOINT_TOLERANCE = 0.03 # 关节角度容差(弧度) MAX_POINTS = 5 # 最大点数限制 class S360RobotControlNode(Node): def __init__(self): super().__init__("s360_robot_full_control") self.get_logger().info("=== S3 60机器人控制节点启动 ===") # 状态管理 self.is_shutting_down = False self.current_joint_positions = [0.0] * len(JOINT_NAMES) self.joints_lock = Lock() self.joint_state_received = False # 标记是否收到过关节状态 self.planned_trajectory = None # 整合轨迹监控器功能 self.trajectory_lock = Lock() # 关节状态消息中的关节顺序(用于重排) self.joint_state_order = JOINT_NAMES.copy() # -------------------------- 整合子系统功能 -------------------------- # 1. 轨迹发布器(原RobotMovePublisher) self.trajectory_pub = self.create_publisher(JointTrajectory, &#39;RobotMove&#39;, 10) self.get_logger().info("✅ 轨迹发布器初始化完成") # 2. 轨迹监控器(原TrajectoryMonitor) self.trajectory_sub = self.create_subscription( DisplayTrajectory, &#39;/display_planned_path&#39;, self.trajectory_callback, 10 ) self.get_logger().info("✅ 轨迹监控器初始化完成") # 3. 关节状态订阅器 self.joint_state_sub = self.create_subscription( JointState, &#39;joint_states&#39;, self.joint_state_callback, 10 ) self.get_logger().info("✅ 关节状态订阅器初始化完成") # 4. MoveIt2初始化(延迟到执行器启动后,避免上下文冲突) self.moveit2 = None self.init_moveit2() # 5. Socket服务器 self.socket_thread = Thread( target=self.start_socket_server, daemon=True ) self.socket_thread.start() self.get_logger().info(f"✅ Socket服务器启动: {SERVER_IP}:{SERVER_PORT}") # -------------------------- 原RobotMovePublisher方法(整合) -------------------------- def publish_joint_trajectory(self, target_joints): """发布关节轨迹到RobotMove - 单点""" try: traj_msg = JointTrajectory() traj_msg.joint_names = JOINT_NAMES point = JointTrajectoryPoint() point.positions = target_joints point.time_from_start = rclpy.duration.Duration(seconds=2.0).to_msg() traj_msg.points.append(point) self.trajectory_pub.publish(traj_msg) self.get_logger().info(f"📢 发布关节轨迹到RobotMove: {[f&#39;{x:.3f}&#39; for x in target_joints]}") return True except Exception as e: self.get_logger().error(f"❌ 轨迹发布失败: {e}") return False def publish_planned_trajectory(self, trajectory_points): """发布MoveIt2规划的轨迹到RobotMove - 最多5个点""" try: if not trajectory_points: self.get_logger().error("❌ 轨迹点为空") return False traj_msg = JointTrajectory() traj_msg.joint_names = JOINT_NAMES # 限制最多5个点 if len(trajectory_points) > MAX_POINTS: step = len(trajectory_points) // MAX_POINTS sampled_points = [] for i in range(MAX_POINTS): idx = min(i * step, len(trajectory_points) - 1) sampled_points.append(trajectory_points[idx]) # 确保包含起点和终点 if len(trajectory_points) > 1: sampled_points[0] = trajectory_points[0] sampled_points[-1] = trajectory_points[-1] trajectory_points = sampled_points # 发布轨迹点 for i, point in enumerate(trajectory_points[:MAX_POINTS]): traj_point = JointTrajectoryPoint() traj_point.positions = point.positions # 重新计算时间戳(总时间2秒) total_time = 2.0 time_ratio = i / (len(trajectory_points[:MAX_POINTS]) - 1) if len(trajectory_points[:MAX_POINTS]) > 1 else 1.0 traj_point.time_from_start = rclpy.duration.Duration(seconds=total_time * time_ratio).to_msg() traj_msg.points.append(traj_point) self.trajectory_pub.publish(traj_msg) self.get_logger().info(f"📢 发布规划轨迹到RobotMove: {len(traj_msg.points)}个点") return True except Exception as e: self.get_logger().error(f"❌ 规划轨迹发布失败: {e}") return False def publish_smooth_trajectory(self, start_joints, target_joints, num_points=5): """发布平滑插值轨迹到RobotMove - 最多5个点""" try: traj_msg = JointTrajectory() traj_msg.joint_names = JOINT_NAMES # 确保不超过最大点数 num_points = min(num_points, MAX_POINTS) # 生成平滑插值点 for i in range(num_points): ratio = i / (num_points - 1) if num_points > 1 else 1.0 point = JointTrajectoryPoint() point.positions = [ start + ratio * (target - start) for start, target in zip(start_joints, target_joints) ] point.time_from_start = rclpy.duration.Duration(seconds=2.0 * ratio).to_msg() traj_msg.points.append(point) self.trajectory_pub.publish(traj_msg) self.get_logger().info(f"📢 发布平滑轨迹到RobotMove: {num_points}个点") return True except Exception as e: self.get_logger().error(f"❌ 平滑轨迹发布失败: {e}") return False # -------------------------- 原TrajectoryMonitor方法(整合) -------------------------- def trajectory_callback(self, msg): """处理MoveIt2规划的轨迹""" try: if msg.trajectory and len(msg.trajectory) > 0: joint_trajectory = msg.trajectory[0].joint_trajectory if joint_trajectory.points: with self.trajectory_lock: self.planned_trajectory = joint_trajectory.points self.get_logger().info(f"📋 收到规划轨迹: {len(joint_trajectory.points)}个点 (将采样为最多{MAX_POINTS}个点)") except Exception as e: self.get_logger().error(f"❌ 轨迹处理异常: {e}") def get_planned_trajectory(self): """获取规划的轨迹""" with self.trajectory_lock: return self.planned_trajectory def clear_trajectory(self): """清空轨迹缓存""" with self.trajectory_lock: self.planned_trajectory = None # -------------------------- 核心功能方法 -------------------------- def init_moveit2(self): """初始化MoveIt2(独立方法,确保上下文正确)""" try: self.moveit2 = MoveIt2( node=self, # 绑定到主节点,避免跨节点上下文冲突 joint_names=JOINT_NAMES, base_link_name=BASE_LINK_NAME, end_effector_name=END_EFFECTOR_NAME, group_name=MOVE_GROUP_ARM ) self.moveit2.planner_id = PLANNER_ID self.moveit2.max_velocity = MAX_VELOCITY self.moveit2.max_acceleration = MAX_ACCELERATION self.get_logger().info("✅ MoveIt2接口初始化完成") except Exception as e: self.get_logger().error(f"❌ MoveIt2初始化失败: {e}") self.moveit2 = None def joint_state_callback(self, msg): """关节状态回调 - 获取当前关节位置""" try: if all(joint in msg.name for joint in JOINT_NAMES): # 记录关节状态消息中的关节顺序 self.joint_state_order = msg.name with self.joints_lock: for i, name in enumerate(JOINT_NAMES): if name in msg.name: idx = msg.name.index(name) self.current_joint_positions[i] = msg.position[idx] self.joint_state_received = True except Exception as e: self.get_logger().warn(f"⚠️ 关节状态处理异常: {e}") def start_socket_server(self): """启动Socket服务器""" server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) server_socket.bind((SERVER_IP, SERVER_PORT)) server_socket.listen(5) server_socket.settimeout(1.0) self.get_logger().info(f"📡 Socket服务器就绪: {SERVER_IP}:{SERVER_PORT}") while rclpy.ok() and not self.is_shutting_down: try: client_socket, addr = server_socket.accept() self.get_logger().info(f"🔌 客户端连接: {addr}") Thread( target=self.handle_client, args=(client_socket,), daemon=True ).start() except socket.timeout: continue except Exception as e: if rclpy.ok() and not self.is_shutting_down: self.get_logger().error(f"❌ Socket接受异常: {e}") server_socket.close() def handle_client(self, client_socket): """处理客户端请求""" try: client_socket.settimeout(30.0) data = client_socket.recv(1024).decode().strip() if not data: return # 解析指令 if &#39;:&#39; in data: cmd_type, cmd_json = data.split(&#39;:&#39;, 1) cmd_params = json.loads(cmd_json) self.get_logger().info(f"📥 收到指令: {cmd_type}, 参数: {cmd_params}") # 执行对应指令 success = False if cmd_type == "p1": success = self.execute_joint_motion(cmd_params) elif cmd_type in ["p2", "p3"]: success = self.execute_pose_motion(cmd_params) else: self.get_logger().error(f"❌ 未知指令类型: {cmd_type}") # 发送响应 response = "True" if success else "False" client_socket.sendall(response.encode()) self.get_logger().info(f"📤 发送响应: {response}") except Exception as e: self.get_logger().error(f"❌ 客户端处理异常: {e}") try: client_socket.sendall("False".encode()) except: pass finally: try: client_socket.close() except: pass self.get_logger().info("🔌 客户端连接关闭") def execute_joint_motion(self, target_joints): """执行关节空间运动""" if not self.moveit2: self.get_logger().error("❌ MoveIt2未初始化") return False if len(target_joints) != len(JOINT_NAMES): self.get_logger().error(f"❌ 关节数不匹配: 期望{len(JOINT_NAMES)}, 收到{len(target_joints)}") return False try: self.get_logger().info(f"🔄 开始关节运动: {[f&#39;{x:.3f}&#39; for x in target_joints]}") # 获取当前关节状态 with self.joints_lock: current_joints = self.current_joint_positions.copy() # 发布平滑插值轨迹 self.publish_smooth_trajectory(current_joints, target_joints, num_points=5) # 执行MoveIt2运动(阻塞到规划完成) self.moveit2.move_to_configuration(target_joints) # 等待运动完成 success = self.wait_for_motion_completion(target_joints, timeout=30.0) if success: self.get_logger().info("✅ 关节运动完成") else: self.get_logger().warning("⚠️ 关节运动超时或未完全到达") return success except Exception as e: self.get_logger().error(f"❌ 关节运动执行异常: {e}") return False def execute_pose_motion(self, target_pose): """执行笛卡尔空间运动""" if not self.moveit2: self.get_logger().error("❌ MoveIt2未初始化") return False if len(target_pose) != 2 or len(target_pose[0]) != 3 or len(target_pose[1]) != 4: self.get_logger().error("❌ 位姿格式错误") return False try: target_pos, target_quat = target_pose self.get_logger().info(f"🔄 开始位姿运动: 位置={target_pos}, 姿态={target_quat}") # 清空之前的轨迹缓存 self.clear_trajectory() # 计算逆运动学获取目标关节位置 ik_result = self.moveit2.compute_ik(target_pos, target_quat) if not ik_result: self.get_logger().error("❌ 逆运动学求解失败") return False # 提取关节位置 target_joints = self._extract_joint_positions_from_state(ik_result) if not target_joints: self.get_logger().error("❌ 无法从逆运动学结果中提取关节位置") return False self.get_logger().info(f"🎯 逆运动学解算结果: {[f&#39;{x:.3f}&#39; for x in target_joints]}") # 执行MoveIt2位姿运动(非笛卡尔路径) self.moveit2.move_to_pose( position=target_pos, quat_xyzw=target_quat, cartesian=False ) # 等待规划完成并获取轨迹 planned_trajectory = self.wait_for_planned_trajectory(timeout=10.0) if planned_trajectory: self.publish_planned_trajectory(planned_trajectory) self.get_logger().info("✅ 成功发布MoveIt2规划轨迹到RobotMove(最多5个点)") else: self.get_logger().warning("⚠️ 无法获取规划轨迹,使用备用平滑轨迹") with self.joints_lock: current_joints = self.current_joint_positions.copy() self.publish_smooth_trajectory(current_joints, target_joints, num_points=5) # 等待运动完成 success = self.wait_for_motion_completion(target_joints, timeout=30.0) if success: self.get_logger().info("✅ 位姿运动完成") else: self.get_logger().warning("⚠️ 位姿运动超时") return success except Exception as e: self.get_logger().error(f"❌ 位姿运动执行异常: {e}") self.get_logger().error(f"详细错误: {traceback.format_exc()}") return False def _extract_joint_positions_from_state(self, joint_state): """从JointState消息中提取关节位置""" try: if not joint_state or not hasattr(joint_state, &#39;position&#39;): self.get_logger().error("❌ 无效的JointState对象") return None extracted_joints = [0.0] * len(JOINT_NAMES) success_count = 0 for i, joint_name in enumerate(JOINT_NAMES): if joint_name in joint_state.name: idx = joint_state.name.index(joint_name) extracted_joints[i] = joint_state.position[idx] success_count += 1 else: self.get_logger().warning(f"⚠️ 在JointState中未找到关节: {joint_name}") if success_count == len(JOINT_NAMES): return extracted_joints else: self.get_logger().error(f"❌ 只找到了 {success_count}/{len(JOINT_NAMES)} 个关节") return None except Exception as e: self.get_logger().error(f"❌ 提取关节位置失败: {e}") return None def wait_for_planned_trajectory(self, timeout=10.0): """等待并获取MoveIt2规划的轨迹""" start_time = time.time() while rclpy.ok() and time.time() - start_time < timeout: planned_trajectory = self.get_planned_trajectory() if planned_trajectory: return planned_trajectory time.sleep(0.1) return None def wait_for_motion_completion(self, target_joints, timeout=30.0): """基于关节角度比较确认运动完成,到达目标后等待0.5s返回""" if not self.joint_state_received: self.get_logger().warning("⚠️ 尚未收到关节状态数据,等待运动完成可能不准确") self.get_logger().info("⏳ 开始等待运动完成...") start_time = time.time() target_reached_time = None while rclpy.ok() and time.time() - start_time < timeout: # 获取当前关节位置 with self.joints_lock: current_joints = self.current_joint_positions.copy() # 检查是否到达目标位置 - 重排关节顺序到标准顺序 if self._joints_reached_target(current_joints, target_joints): if target_reached_time is None: target_reached_time = time.time() self.get_logger().info("🎯 首次到达目标位置,开始稳定等待...") else: # 稳定0.5秒后确认完成 if time.time() - target_reached_time >= 0.5: self.get_logger().info(f"✅ 运动完成!稳定时间: {time.time() - target_reached_time:.2f}s") return True else: # 未到达目标,重置计时 if target_reached_time is not None: self.get_logger().info("⚠️ 位置偏移,重置稳定计时") target_reached_time = None # 每5秒打印一次进度 elapsed = time.time() - start_time if elapsed % 5.0 < 0.1: max_error = self._calculate_max_joint_error(current_joints, target_joints) self.get_logger().info(f"⏱️ 等待中... 已等待{elapsed:.1f}s, 最大关节误差: {max_error:.4f}rad") time.sleep(0.1) # 超时处理 if target_reached_time is not None: stable_duration = time.time() - target_reached_time self.get_logger().warning(f"⚠️ 运动部分完成,稳定{stable_duration:.2f}s后超时") else: with self.joints_lock: current_joints = self.current_joint_positions.copy() max_error = self._calculate_max_joint_error(current_joints, target_joints) self.get_logger().warning(f"⚠️ 运动超时,最大关节误差: {max_error:.4f}rad") return False def _joints_reached_target(self, current_joints, target_joints): """检查所有关节是否在容差范围内到达目标""" # 重新排序当前关节数据到标准顺序 reordered_current = self._reorder_joints_to_standard(current_joints) if len(reordered_current) != len(target_joints): return False for current, target in zip(reordered_current, target_joints): if abs(current - target) > JOINT_TOLERANCE: return False return True def _calculate_max_joint_error(self, current_joints, target_joints): """计算最大关节误差""" # 重新排序当前关节数据到标准顺序 reordered_current = self._reorder_joints_to_standard(current_joints) if len(reordered_current) != len(target_joints): return float(&#39;inf&#39;) max_error = 0.0 for current, target in zip(reordered_current, target_joints): error = abs(current - target) if error > max_error: max_error = error return max_error def _reorder_joints_to_standard(self, joints_data): """ 将关节数据重排为标准顺序 ["Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"] 基于当前关节状态话题的实际顺序进行重排 """ try: # 如果关节状态消息中的顺序与标准顺序一致,直接返回 if self.joint_state_order == JOINT_NAMES: return joints_data # 否则,按照实际关节状态消息的顺序重排到标准顺序 if len(joints_data) == 6 and len(self.joint_state_order) == 6: # 创建从实际顺序到标准顺序的映射 reordered = [0.0] * 6 for i, joint_name in enumerate(JOINT_NAMES): if joint_name in self.joint_state_order: idx = self.joint_state_order.index(joint_name) reordered[i] = joints_data[idx] else: self.get_logger().warning(f"⚠️ 在关节状态消息中未找到关节: {joint_name}") reordered[i] = joints_data[i] # 使用原位置作为备用 # 如果检测到您提供的特定顺序 [&#39;Joint2&#39;,&#39;Joint3&#39;,&#39;Joint1&#39;,&#39;Joint4&#39;,&#39;Joint5&#39;,&#39;Joint6&#39;] if (len(self.joint_state_order) == 6 and self.joint_state_order[0] == "Joint2" and self.joint_state_order[1] == "Joint3" and self.joint_state_order[2] == "Joint1"): # 使用硬编码的重排逻辑确保正确性 reordered = [ joints_data[2], # Joint1 (在原数组索引2) joints_data[0], # Joint2 (在原数组索引0) joints_data[1], # Joint3 (在原数组索引1) joints_data[3], # Joint4 (在原数组索引3) joints_data[4], # Joint5 (在原数组索引4) joints_data[5] # Joint6 (在原数组索引5) ] return reordered else: self.get_logger().warning(f"⚠️ 关节数据长度不匹配: 数据{len(joints_data)}, 顺序{len(self.joint_state_order)}") return joints_data except Exception as e: self.get_logger().error(f"❌ 关节数据重排失败: {e}") return joints_data def shutdown(self): """关闭节点""" self.is_shutting_down = True self.get_logger().info("🛑 开始关闭节点...") def main(): rclpy.init() node = None try: node = S360RobotControlNode() # 执行器只添加主节点(关键修复:避免多节点上下文冲突) executor = MultiThreadedExecutor(num_threads=4) executor.add_node(node) node.get_logger().info("🚀 节点开始运行(按Ctrl+C退出)") executor.spin() except KeyboardInterrupt: node.get_logger().info("🛑 收到中断信号,关闭节点...") except Exception as e: if node: node.get_logger().error(f"❌ 节点运行异常: {e}") else: print(f"❌ 节点初始化异常: {e}") finally: if node: node.shutdown() node.destroy_node() rclpy.shutdown() print("✅ 节点已安全关闭") if __name__ == "__main__": main() name: - Joint2 - Joint3 - Joint1 - Joint4 - Joint5 - Joint6 position: - -0.11739741912 - 1.5494284625348966 - -0.0340351848 - -0.1387772676 - 1.5710593366799999 - -1.60547801652 velocity: [] effort: [] [INFO] [1761221208.042075563] [s360_robot_full_control]: ⏱️ 等待中... 已等待0.0s, 最大关节误差: 1.5621rad [INFO] [1761221213.085019225] [s360_robot_full_control]: ⏱️ 等待中... 已等待5.0s, 最大关节误差: 1.5621rad [INFO] [1761221218.132077655] [s360_robot_full_control]: ⏱️ 等待中... 已等待10.1s, 最大关节误差: 1.5621rad [INFO] [1761221223.078469150] [s360_robot_full_control]: ⏱️ 等待中... 已等待15.0s, 最大关节误差: 1.5621rad [INFO] [1761221228.111193687] [s360_robot_full_control]: ⏱️ 等待中... 已等待20.1s, 最大关节误差: 1.5621rad ^C[INFO] [1761221230.534367902] [s360_robot_full_control]: 🛑 收到中断信号,关闭节点... [INFO] [1761221208.035660440] [CodroidIO]: 监听回调 {"joint_names": ["Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"], "points": [{"positions": [-0.02599377318, 0.31809715146, 1.5268861447148965, 0.27448657488, 1.57111925778, -1.59747255756], "velocities": [], "accelerations": []}, {"positions": [-0.02682495239343273, 0.27309715146000013, 1.5292177054939136, 0.23178313590264085, 1.5711143145327557, -1.5983018962505733], "velocities": [], "accelerations": []}, {"positions": [-0.02931848988091759, 0.13809715973330774, 1.5362123874023041, 0.10367282682164504, 1.5710994847919322, -1.6007899121698186], "velocities": [], "accelerations": []}, {"positions": [-0.03240979190073338, -0.02926577312589769, 1.5448838729604328, -0.055148790743353965, 1.5710810999840064, -1.6038743689606618], "velocities": [], "accelerations": []}, {"positions": [-0.034037705688143, -0.1174009421117632, 1.5494503730328655, -0.13878600875551295, 1.5710714183410908, -1.6054986779748885], "velocities": [], "accelerations": []}]} 这是怎么回事,关节数据没有更新,已经到达目标的点位了依然没有返回true,借鉴下面代码中解决关节数据更新的方法解决上诉代码的问题 #!/usr/bin/env python3 """ 优化版:根据坐标类型自动区分控制方式 - 关节坐标 → 关节空间控制 - 笛卡尔坐标 → 笛卡尔空间控制 - 支持p1-p5等多种指令标识(仅作标识用) - 精简日志输出 """ from threading import Thread, Lock import socket import json import rclpy from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from pymoveit2 import MoveIt2 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy from moveit_msgs.msg import DisplayTrajectory from codroid_msgs.msg import RobotInfo import time import concurrent.futures # -------------------------- 配置参数 -------------------------- PYCHARM_PORT = 8888 JOINT_TOLERANCE = 0.03 STABLE_COUNT_THRESHOLD = 5 P2_PLAN_TIMEOUT = 10 P2_MOVE_TIMEOUT = 60 # 机器人参数 joint_names = ["Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"] base_link_name = "base_link" end_effector_name = "link6" MOVE_GROUP_ARM = "arm"#test_group # -------------------------- 轨迹发布类 -------------------------- class RobotMovePublisher(Node): def __init__(self): super().__init__(&#39;robot_move_publisher&#39;) self.qos_profile = QoSProfile( reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL, depth=10 ) self.pub = self.create_publisher(JointTrajectory, &#39;RobotMove&#39;, self.qos_profile) self.get_logger().info("轨迹发布器就绪") def publish_direct_joint(self, target_joints): """发布关节目标""" try: traj_msg = JointTrajectory() traj_msg.joint_names = joint_names point = JointTrajectoryPoint() point.positions = target_joints traj_msg.points.append(point) self.pub.publish(traj_msg) self.get_logger().info(f"发布关节目标") return True except Exception as e: self.get_logger().error(f"关节发布失败: {e}") return False def publish_smooth_trajectory(self, start_joints, end_joints): """发布平滑轨迹""" try: traj_msg = JointTrajectory() traj_msg.joint_names = joint_names # 5点平滑插值 for ratio in [0.0, 0.25, 0.5, 0.75, 1.0]: point = JointTrajectoryPoint() point.positions = [start + ratio * (end - start) for start, end in zip(start_joints, end_joints)] traj_msg.points.append(point) self.pub.publish(traj_msg) self.get_logger().info("发布平滑轨迹") return True except Exception as e: self.get_logger().error(f"轨迹发布失败: {e}") return False # -------------------------- 轨迹管理器 -------------------------- class TrajectoryManager(Node): def __init__(self): super().__init__(&#39;trajectory_manager&#39;) self.traj_pub = RobotMovePublisher() self.plan_end_joints = None self.traj_published = False self.sub = self.create_subscription( DisplayTrajectory, &#39;/display_planned_path&#39;, self.plan_callback, 10 ) self.get_logger().info("轨迹管理器就绪") def plan_callback(self, msg): """处理规划轨迹""" if not msg.trajectory or not msg.trajectory[0].joint_trajectory.points: return if not self.traj_published: joint_traj = msg.trajectory[0].joint_trajectory start_joints = list(joint_traj.points[0].positions) self.plan_end_joints = list(joint_traj.points[-1].positions) if self.traj_pub.publish_smooth_trajectory(start_joints, self.plan_end_joints): self.traj_published = True self.get_logger().info("轨迹发布完成") # -------------------------- 核心规划与通信类 -------------------------- class CommunicationPlanner(Node): def __init__(self): super().__init__(&#39;communication_planner&#39;) # 状态管理 self.real_joints = None self.joints_lock = Lock() # 子系统初始化 self.traj_manager = TrajectoryManager() self.callback_group = ReentrantCallbackGroup() self.moveit2 = MoveIt2( node=self, joint_names=joint_names, base_link_name=base_link_name, end_effector_name=end_effector_name, group_name=MOVE_GROUP_ARM, callback_group=self.callback_group, ) # MoveIt2参数配置 self.moveit2.max_velocity = 0.5 self.moveit2.max_acceleration = 0.5 self.moveit2.planner_id = "RRTConnectkConfigDefault" # 线程池 self.thread_pool = concurrent.futures.ThreadPoolExecutor(max_workers=4) # 订阅器 self.create_subscription( RobotInfo, &#39;/RobotInfo&#39;, self.robot_info_callback, 10 ) # 启动Socket服务器 self.socket_thread = Thread(target=self.start_socket_server, daemon=True) self.socket_thread.start() self.get_logger().info("系统就绪,等待指令...") def robot_info_callback(self, msg): """更新实时关节数据""" if msg.joint_positions and len(msg.joint_positions) == 6: with self.joints_lock: self.real_joints = msg.joint_positions def wait_for_real_joints(self, timeout=10.0): """等待关节数据就绪""" start_time = time.time() while rclpy.ok(): with self.joints_lock: if self.real_joints is not None: return True if time.time() - start_time > timeout: self.get_logger().error("关节数据获取超时") return False time.sleep(0.1) def is_joint_reached(self, target_joints): """检查关节是否到达目标""" with self.joints_lock: current = self.real_joints if current is None: return False # 检查所有关节误差 return all(abs(c - t) <= JOINT_TOLERANCE for c, t in zip(current, target_joints)) def identify_coordinate_type(self, data): """自动识别坐标类型""" if isinstance(data, list): if len(data) == 6 and all(isinstance(x, (int, float)) for x in data): return "joint" # 6关节坐标 elif (len(data) == 2 and isinstance(data[0], list) and isinstance(data[1], list) and len(data[0]) == 3 and len(data[1]) == 4): return "cartesian" # 笛卡尔坐标 return "unknown" def handle_joint_control(self, target_joints, command_id="unknown"): """处理关节空间控制""" self.get_logger().info(f"[{command_id}] 执行关节控制") if not self.wait_for_real_joints(): return False if not self.traj_manager.traj_pub.publish_direct_joint(target_joints): return False # 等待稳定到达 start_time = time.time() stable_count = 0 while rclpy.ok() and time.time() - start_time < 30: if self.is_joint_reached(target_joints): stable_count += 1 if stable_count >= STABLE_COUNT_THRESHOLD: self.get_logger().info(f"[{command_id}] 关节控制完成") return True else: stable_count = 0 time.sleep(0.1) self.get_logger().warning(f"[{command_id}] 关节控制超时") return False def _cartesian_planning_thread(self, target_pos, target_quat): """笛卡尔规划线程""" try: self.moveit2.move_to_pose(position=target_pos, quat_xyzw=target_quat) return True except Exception as e: self.get_logger().error(f"笛卡尔规划失败: {e}") return False def handle_cartesian_control(self, target_pos, target_quat, command_id="unknown"): """处理笛卡尔空间控制""" self.get_logger().info(f"[{command_id}] 执行笛卡尔控制") if not self.wait_for_real_joints(): return False # 重置状态 self.traj_manager.plan_end_joints = None self.traj_manager.traj_published = False # 设置规划起点 with self.joints_lock: self.moveit2.joint_value_target = self.real_joints self.moveit2.update_planning_scene() # 异步规划 planning_future = self.thread_pool.submit( self._cartesian_planning_thread, target_pos, target_quat ) try: if not planning_future.result(timeout=P2_PLAN_TIMEOUT): return False except concurrent.futures.TimeoutError: self.get_logger().error(f"[{command_id}] 笛卡尔规划超时") return False # 等待规划结果 start_time = time.time() while rclpy.ok() and self.traj_manager.plan_end_joints is None: if time.time() - start_time > P2_PLAN_TIMEOUT: self.get_logger().warning(f"[{command_id}] 未获取规划终点") return False time.sleep(0.1) target_joints = self.traj_manager.plan_end_joints return self._wait_for_joints_non_blocking(target_joints, command_id) def _wait_for_joints_non_blocking(self, target_joints, command_id="unknown"): """非阻塞等待关节到达""" start_time = time.time() stable_count = 0 while rclpy.ok() and time.time() - start_time < P2_MOVE_TIMEOUT: rclpy.spin_once(self, timeout_sec=0) if self.is_joint_reached(target_joints): stable_count += 1 if stable_count >= STABLE_COUNT_THRESHOLD: self.get_logger().info(f"[{command_id}] 笛卡尔控制完成") return True else: stable_count = 0 time.sleep(0.1) self.get_logger().warning(f"[{command_id}] 笛卡尔控制超时") return False def process_command(self, command_id, data): """根据坐标类型自动选择控制方式""" coord_type = self.identify_coordinate_type(data) if coord_type == "joint": return self.handle_joint_control(data, command_id) elif coord_type == "cartesian": if len(data) == 2: return self.handle_cartesian_control(data[0], data[1], command_id) else: self.get_logger().error(f"[{command_id}] 笛卡尔坐标格式错误") return False else: self.get_logger().error(f"[{command_id}] 无法识别的坐标格式") return False def start_socket_server(self): """Socket服务器主循环""" with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as server_socket: server_socket.bind(("0.0.0.0", PYCHARM_PORT)) server_socket.listen(1) self.get_logger().info(f"Socket服务启动,端口: {PYCHARM_PORT}") while rclpy.ok(): conn, addr = server_socket.accept() with conn: self.get_logger().info(f"客户端连接: {addr}") try: data = conn.recv(1024).decode("utf-8") if not data: continue self.get_logger().info(f"接收指令: {data}") result = False # 解析指令格式: p1:[data], p2:[data], p3:[data] 等 if ":" in data: command_id, json_str = data.split(":", 1) try: command_data = json.loads(json_str) result = self.process_command(command_id, command_data) except json.JSONDecodeError: self.get_logger().error("JSON解析失败") result = False else: # 如果没有指令标识,使用默认标识 try: coord_data = json.loads(data) result = self.process_command("default", coord_data) except json.JSONDecodeError: self.get_logger().error("数据格式错误") result = False conn.sendall(str(result).encode("utf-8")) self.get_logger().info(f"返回结果: {result}") except Exception as e: self.get_logger().error(f"指令处理异常: {e}") conn.sendall("error".encode("utf-8")) def shutdown(self): """清理资源""" if hasattr(self, &#39;thread_pool&#39;): self.thread_pool.shutdown(wait=False) # -------------------------- 程序入口 -------------------------- def main(): rclpy.init() planner = None try: planner = CommunicationPlanner() executor = rclpy.executors.MultiThreadedExecutor(4) executor.add_node(planner) executor.add_node(planner.traj_manager) executor.add_node(planner.traj_manager.traj_pub) executor.spin() except KeyboardInterrupt: planner.get_logger().info("程序终止") except Exception as e: if planner is not None: planner.get_logger().error(f"系统异常: {e}") else: print(f"系统异常: {e}") finally: if planner is not None: planner.shutdown() rclpy.shutdown() if __name__ == "__main__": main()
最新发布
10-24
`S3Client` 是 AWS SDK for Java 2.x 提供的用于操作 Amazon S3 的核心类,它是线程安全的、高性能的,并基于现代 Java 风格(如响应式流)设计。 下面详细介绍如何使用 `S3Client` 进行常见的 S3 操作。 --- ## ✅ 1. 添加依赖(Maven) 首先,在 `pom.xml` 中引入 AWS SDK for Java 2.x 的 S3 模块: ```xml <dependency> <groupId>software.amazon.awssdk</groupId> <artifactId>s3</artifactId> <version>2.21.40</version> <!-- 推荐使用最新稳定版 --> </dependency> ``` > 💡 版本号可查看 [AWS SDK for Java 2.x Releases](https://github.com/aws/aws-sdk-java-v2) --- ## ✅ 2. 创建 S3Client 实例 ```java import software.amazon.awssdk.auth.credentials.DefaultCredentialsProvider; import software.amazon.awssdk.regions.Region; import software.amazon.awssdk.services.s3.S3Client; public class S3Example { public static void main(String[] args) { S3Client s3 = S3Client.builder() .region(Region.US_EAST_1) // 设置区域 .credentialsProvider(DefaultCredentialsProvider.create()) // 使用默认凭证链 .build(); System.out.println("S3Client 已创建"); } } ``` ### 🔐 凭证说明: - `DefaultCredentialsProvider` 会按顺序查找: - 环境变量 (`AWS_ACCESS_KEY_ID`, `AWS_SECRET_ACCESS_KEY`) - `~/.aws/credentials` 文件 - IAM Role(EC2/ECS/Lambda) - 也可以手动指定静态密钥: ```java .credentialsProvider(StaticCredentialsProvider.create( AwsBasicCredentials.create("your-access-key", "your-secret-key") )) ``` --- ## ✅ 3. 常见操作示例 ### 📁 3.1 列出所有 Bucket ```java import software.amazon.awssdk.services.s3.model.ListBucketsResponse; ListBucketsResponse response = s3.listBuckets(); response.buckets().forEach(bucket -> System.out.println("Bucket: " + bucket.name()) ); ``` --- ### 📂 3.2 创建 Bucket ```java import software.amazon.awssdk.services.s3.model.CreateBucketRequest; import software.amazon.awssdk.services.s3.model.S3Exception; try { CreateBucketRequest request = CreateBucketRequest.builder() .bucket("my-unique-bucket-name-12345") .build(); s3.createBucket(request); System.out.println("Bucket created!"); } catch (S3Exception e) { System.err.println(e.awsErrorDetails().errorMessage()); } ``` > ⚠️ Bucket 名称必须全局唯一,且符合命名规则(小写、不能包含下划线等) --- ### 📤 3.3 上传文件(Put Object) ```java import software.amazon.awssdk.core.sync.RequestBody; import software.amazon.awssdk.services.s3.model.PutObjectRequest; PutObjectRequest putRequest = PutObjectRequest.builder() .bucket("my-unique-bucket-name-12345") .key("hello.txt") // 对象键(即路径) .contentType("text/plain") .build(); s3.putObject(putRequest, RequestBody.fromString("Hello from S3Client!")); System.out.println("File uploaded!"); ``` #### 上传本地文件: ```java import software.amazon.awssdk.core.sync.RequestBody; import java.nio.file.Paths; RequestBody body = RequestBody.fromFile(Paths.get("path/to/local/file.zip")); s3.putObject(PutObjectRequest.builder() .bucket("my-bucket") .key("file.zip") .build(), body); ``` --- ### 📥 3.4 下载文件(GetObject) ```java import software.amazon.awssdk.core.ResponseInputStream; import software.amazon.awssdk.services.s3.model.GetObjectRequest; import software.amazon.awssdk.services.s3.model.GetObjectResponse; GetObjectRequest getRequest = GetObjectRequest.builder() .bucket("my-unique-bucket-name-12345") .key("hello.txt") .build(); ResponseInputStream<GetObjectResponse> inputStream = s3.getObject(getRequest); String content = new String(inputStream.readAllBytes()); System.out.println("Content: " + content); inputStream.close(); ``` #### 下载到本地文件: ```java s3.getObject(GetObjectRequest.builder() .bucket("my-bucket") .key("file.zip") .build(), Paths.get("downloaded-file.zip")); // 自动保存到文件 ``` --- ### 🗑️ 3.5 删除对象 ```java import software.amazon.awssdk.services.s3.model.DeleteObjectRequest; DeleteObjectRequest deleteRequest = DeleteObjectRequest.builder() .bucket("my-unique-bucket-name-12345") .key("hello.txt") .build(); s3.deleteObject(deleteRequest); System.out.println("Object deleted!"); ``` --- ### 📋 3.6 列出 Bucket 中的对象 ```java import software.amazon.awssdk.services.s3.model.ListObjectsV2Request; import software.amazon.awssdk.services.s3.model.ListObjectsV2Response; import software.amazon.awssdk.services.s3.model.S3Object; ListObjectsV2Request listReq = ListObjectsV2Request.builder() .bucket("my-unique-bucket-name-12345") .maxKeys(10) .build(); ListObjectsV2Response listRes = s3.listObjectsV2(listReq); for (S3Object object : listRes.contents()) { System.out.println("Key: " + object.key() + ", Size: " + object.size()); } ``` --- ### 🔐 3.7 设置对象权限(例如公开读) ```java PutObjectRequest putRequest = PutObjectRequest.builder() .bucket("my-bucket") .key("public-image.jpg") .acl("public-read") // 设置 ACL .contentType("image/jpeg") .build(); s3.putObject(putRequest, RequestBody.fromFile(Paths.get("image.jpg"))); ``` --- ## ✅ 4. 异常处理 建议用 try-catch 包裹调用: ```java try { s3.getObject(...); } catch (S3Exception e) { System.err.printf("Error Code: %s%n", e.awsErrorDetails().errorCode()); System.err.printf("Message: %s%n", e.getMessage()); } ``` 常见错误码:`NoSuchBucket`, `NoSuchKey`, `AccessDenied`, `SignatureDoesNotMatch` 等。 --- ## ✅ 5. 关闭 S3Client(可选) 虽然 `S3Client` 内部资源会在 JVM 退出时自动释放,但推荐显式关闭: ```java s3.close(); ``` 或者使用 try-with-resources: ```java try (S3Client s3 = S3Client.builder().region(Region.US_EAST_1).build()) { // 使用 s3... } // 自动关闭 ``` --- ## ✅ 6. 配置客户端高级选项(可选) ```java S3Client s3 = S3Client.builder() .region(Region.US_WEST_2) .credentialsProvider(...) .httpClient(ApacheHttpClient.builder().maxConnections(50).build()) // 自定义 HTTP 客户端 .overrideConfiguration(cfg -> cfg.retryPolicy(RetryPolicy.defaultRetryPolicy())) // 重试策略 .serviceConfiguration(S3Configuration.builder() .checksumValidationEnabled(false) // 可禁用校验和验证提升性能 .build()) .build(); ``` --- ## ✅ 总结 | 功能 | 方法 | |------|------| | 创建客户端 | `S3Client.builder().build()` | | 上传文件 | `putObject()` | | 下载文件 | `getObject()` | | 列出对象 | `listObjectsV2()` | | 删除对象 | `deleteObject()` | | 创建桶 | `createBucket()` | | 异常处理 | 捕获 `S3Exception` | | 资源释放 | 调用 `.close()` | ---
评论 1
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值