根据您提供的课件截图（涉及 ROS 通信、图像队列、PyTorch 预处理）以及您的具体需求（识别正方体/球体、尺寸>3cm、自动追踪抓取），我为您编写了一个完整的 Python 代码框架。 这段代码**融合了您课件中的 ROS 架构**，并补充了具体的**OpenCV 几何识别算法**（用于识别球体和正方体）以及**机械臂控制逻辑**。 ### 代码设计思路 1. **架构复用**：保留了截图中的 `rospy.Subscriber`、`image_queue` 和 `image_proc` 循环结构。 2. **视觉识别**：使用 OpenCV 传统算法（无需训练模型，即插即用）。 * **球体**：使用霍夫圆变换 (`HoughCircles`) 或 轮廓圆形度检测。 * **正方体**：使用轮廓多边形近似 (`approxPolyDP`)，检测是否有 4 个顶点。 * **尺寸过滤**：通过像素直径/面积估算实际物理尺寸，过滤掉小于 3cm 的物体。 3. **控制逻辑**：模拟了坐标转换和机械臂抓取动作。 ### Python 代码实现 ```python #!/usr/bin/env python3 # -*- coding: utf-8 -*- import rospy import cv2 import numpy as np import queue import time from sensor_msgs.msg import Image from cv_bridge import CvBridge # ================= 配置参数 ================= # 假设相机高度和像素比例，用于估算实际尺寸 (需要根据实际标定修改) PIXELS_PER_CM = 30 # 例如：1cm 对应 30 像素 MIN_SIZE_CM = 3.0 # 最小抓取尺寸 3cm CAMERA_HEIGHT = 40 # 相机距离桌面的高度 cm class SmartSorter: def __init__(self): # 1. 初始化 ROS 节点 rospy.init_node('smart_sorter_node', anonymous=True) # 2. 初始化图像桥接和队列 (参考截图 5.3.1 & 5.4) self.bridge = CvBridge() self.image_queue = queue.Queue(maxsize=1) # 3. 订阅摄像头话题 (参考截图 5.3.1) # 注意：话题名称需与实际相机驱动一致，通常是 /usb_cam/image_raw 或 /camera/color/image_raw self.image_sub = rospy.Subscriber('/usb_cam/image_rect_color', Image, self.image_callback, queue_size=1) # 4. 状态变量 (参考截图 5.4) self.state = {'fps': 0.0, 'tic': time.time()} self.running = True # 5. 模拟机械臂连接 self.arm_connected = True print("系统初始化完成，等待图像...") def image_callback(self, msg): """ROS 图像回调函数，将图像放入队列""" try: # 如果队列满了，先获取旧的，防止阻塞 if self.image_queue.full(): try: self.image_queue.get_nowait() except queue.Empty: pass self.image_queue.put(msg) except Exception as e: rospy.logerr(f"图像回调错误：{e}") def detect_geometry(self, frame): """ 核心识别逻辑：识别正方体和球体 返回：(形状类型，中心坐标 x, y, 估算尺寸 cm) """ gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) blurred = cv2.medianBlur(gray, 5) height, width = frame.shape[:2] detected_objects = [] # --- 1. 识别球体 (使用霍夫圆变换) --- circles = cv2.HoughCircles(blurred, cv2.HOUGH_GRADIENT, dp=1, minDist=50, param1=50, param2=50, minRadius=20, maxRadius=200) if circles is not None: circles = np.round(circles[0, :]).astype("int") for (x, y, r) in circles: # 估算尺寸：直径 = 2 * 半径 size_cm = (2 * r) / PIXELS_PER_CM if size_cm >= MIN_SIZE_CM: detected_objects.append(('Sphere', x, y, size_cm)) # 绘制绿色圆圈表示球体 cv2.circle(frame, (x, y), r, (0, 255, 0), 2) cv2.putText(frame, f"Sphere: {size_cm:.1f}cm", (x-20, y-20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) # --- 2. 识别正方体 (使用轮廓检测 + 多边形近似) --- # 边缘检测 edges = cv2.Canny(blurred, 50, 150) contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) for cnt in contours: area = cv2.contourArea(cnt) if area > 1000: # 过滤噪点 # 多边形近似 epsilon = 0.04 * cv2.arcLength(cnt, True) approx = cv2.approxPolyDP(cnt, epsilon, True) # 正方体在 2D 投影通常表现为四边形 if len(approx) == 4: # 计算外接圆半径来估算大小 ((x, y), radius) = cv2.minEnclosingCircle(cnt) size_cm = (2 * radius) / PIXELS_PER_CM if size_cm >= MIN_SIZE_CM: # 进一步验证是否为正方形 (长宽比接近 1) x,y,w,h = cv2.boundingRect(approx) aspect_ratio = float(w)/h if 0.8 <= aspect_ratio <= 1.2: detected_objects.append(('Cube', int(x+w/2), int(y+h/2), size_cm)) # 绘制蓝色矩形表示正方体 cv2.drawContours(frame, [approx], 0, (255, 0, 0), 2) cv2.putText(frame, f"Cube: {size_cm:.1f}cm", (x, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2) return detected_objects def move_arm_and_grab(self, shape_type, x, y, size): """ 模拟机械臂追踪和抓取逻辑 实际使用时需替换为真实的机械臂控制 SDK (如 moveit, pymodbus 等) """ print(f">>> 发现目标：{shape_type}, 尺寸：{size:.2f}cm, 像素坐标：({x}, {y})") # 1. 坐标转换 (像素 -> 机械臂基座坐标) # 这里需要一个手眼标定矩阵，此处简化为线性映射 # 假设图像中心对应机械臂基座中心 img_center_x, img_center_y = 320, 240 # 假设分辨率 640x480 offset_x = (x - img_center_x) * 0.1 # 比例系数 offset_y = (y - img_center_y) * 0.1 target_x = offset_x target_y = offset_y target_z = CAMERA_HEIGHT - size # 简单的高度估算 print(f" -> 转换后机械臂坐标：X={target_x:.2f}, Y={target_y:.2f}, Z={target_z:.2f}") # 2. 执行动作 if self.arm_connected: print(" -> [动作] 机械臂移动中...") time.sleep(1) # 模拟移动时间 print(" -> [动作] 到达目标位置") time.sleep(0.5) print(" -> [动作] 闭合夹爪 (抓取)") time.sleep(0.5) print(" -> [动作] 抬起机械臂") time.sleep(1) print(" -> [动作] 放置到分拣区") else: print(" -> [警告] 机械臂未连接，仅模拟逻辑") def image_proc(self): """ 图像处理主函数 (参考截图 5.4) """ if not self.image_queue.empty(): try: # 1. 获取图像 ros_image = self.image_queue.get(block=False) # 2. 转换为 OpenCV 格式 frame = self.bridge.imgmsg_to_cv2(ros_image, "bgr8") # 3. 识别物体 objects = self.detect_geometry(frame) # 4. 如果有识别到物体，执行抓取 (这里策略是抓取第一个识别到的) if objects: # 按尺寸从大到小排序，优先抓大的 objects.sort(key=lambda k: k[3], reverse=True) target = objects[0] self.move_arm_and_grab(target[0], target[1], target[2], target[3]) # 5. 显示图像 (调试用) cv2.imshow("Smart Sorting View", frame) cv2.waitKey(1) # 6. 计算 FPS (参考截图 5.4) toc = time.time() curr_fps = 1.0 / (toc - self.state['tic']) if (toc - self.state['tic']) > 0 else 0 self.state['fps'] = curr_fps if self.state['fps'] == 0.0 else (self.state['fps'] * 0.95 + curr_fps * 0.05) self.state['tic'] = toc except queue.Empty: pass except Exception as e: rospy.logerr(f"处理错误：{e}") def run(self): """主循环 (参考截图 5.3.2)""" rate = rospy.Rate(30) # 30Hz while not rospy.is_shutdown() and self.running: try: self.image_proc() rate.sleep() except KeyboardInterrupt: print("用户中断，退出程序") self.running = False break cv2.destroyAllWindows() if __name__ == '__main__': try: sorter = SmartSorter() sorter.run() except rospy.ROSInterruptException: pass ``` ### 代码关键点解析（对应您的课件） 1. **ROS 订阅 (`rospy.Subscriber`)**： * 代码第 33 行完全对应您截图 5.3.1 中的 `image_sub = rospy.Subscriber(...)`。 * 它监听摄像头话题，将数据放入 `image_queue`。 2. **图像预处理与队列 (`image_queue`)**： * 代码第 36 行和 58 行使用了 `queue.Queue`，对应截图 5.4 中的 `image_queue.get(block=True)`。这保证了图像处理的线程安全，不会丢帧。 3. **识别算法 (`detect_geometry`)**： * **球体识别**：使用了 `cv2.HoughCircles`。这是识别球体最经典的方法。 * **正方体识别**：使用了 `cv2.approxPolyDP`。通过检测轮廓是否有 4 个顶点且长宽比接近 1:1 来判断正方体。 * **尺寸过滤**：代码第 93 行和 115 行 `if size_cm >= MIN_SIZE_CM` 实现了您要求的“大小大于三厘米”的过滤逻辑。 4. **主循环 (`run`)**： * 对应截图 5.3.2 的 `while True` 结构，包含 `try-except` 块来捕获 `KeyboardInterrupt`，确保程序能安全退出。 ### 如何使用 1. **环境准备**：确保已安装 `rospy`, `cv2` (opencv-python), `numpy`。 2. **相机标定**：代码中的 `PIXELS_PER_CM = 30` 是一个估算值。您需要拿一个已知 3cm 的物体放在相机前，测量其像素直径，然后更新这个参数，否则尺寸判断会不准。 3. **话题名称**：检查第 33 行 `/usb_cam/image_rect_color` 是否与您实际相机的话题一致（可以使用 `rostopic list` 查看）。 4. **硬件对接**：`move_arm_and_grab` 函数目前是打印日志。您需要在此处填入您机械臂的具体控制代码（例如发送串口指令或调用 MoveIt! 接口）。