diff --git a/src/drone_perception/perceptive_drone_explorer.py b/src/drone_perception/perceptive_drone_explorer.py index 94a2e5f633..1a01ec04aa 100644 --- a/src/drone_perception/perceptive_drone_explorer.py +++ b/src/drone_perception/perceptive_drone_explorer.py @@ -2501,31 +2501,119 @@ def _update_info_window(self, perception: PerceptionResult): except Exception as e: self.logger.warning(f"⚠️ 更新信息窗口时出错: {e}") - def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarray] = None) -> Tuple[List[RedObject], np.ndarray]: - red_objects = [] + def _detect_color_objects_generic( + self, + image: np.ndarray, + color_type: str, + depth_array: Optional[np.ndarray] = None + ) -> Tuple[List[Any], np.ndarray]: + """ + 通用的颜色物体检测函数 + + Args: + image: 输入图像 + color_type: 颜色类型 ('red', 'blue', 'black') + depth_array: 深度数组(可选) + + Returns: + (检测到的物体列表, 标记后的图像) + """ + # 颜色类型配置映射 + COLOR_CONFIG = { + 'red': { + 'detection_config': config.PERCEPTION['RED_OBJECT_DETECTION'], + 'color_range_config': config.CAMERA['RED_COLOR_RANGE'], + 'object_class': RedObject, + 'object_list_attr': 'red_objects', + 'id_counter_attr': 'red_object_id_counter', + 'memory_time_attr': 'red_object_memory_time', + 'stats_key': 'red_objects_detected', + 'log_emoji': '🔴', + 'log_name': '红色物体', + 'mark_color': (0, 100, 255), # BGR格式 + 'label_prefix': 'R:', + 'record_func': 'record_red_object', + 'record_enabled_config': 'RECORD_RED_OBJECTS', + 'has_dual_range': True, # 红色需要两个颜色范围 + 'is_same_func': self._is_same_object, + }, + 'blue': { + 'detection_config': config.PERCEPTION['BLUE_OBJECT_DETECTION'], + 'color_range_config': config.CAMERA['BLUE_COLOR_RANGE'], + 'object_class': BlueObject, + 'object_list_attr': 'blue_objects', + 'id_counter_attr': 'blue_object_id_counter', + 'memory_time_attr': 'blue_object_memory_time', + 'stats_key': 'blue_objects_detected', + 'log_emoji': '🔵', + 'log_name': '蓝色物体', + 'mark_color': (255, 100, 0), # BGR格式 + 'label_prefix': 'B:', + 'record_func': 'record_blue_object', + 'record_enabled_config': 'RECORD_BLUE_OBJECTS', + 'has_dual_range': False, + 'is_same_func': self._is_same_object_blue, + }, + 'black': { + 'detection_config': config.PERCEPTION['BLACK_OBJECT_DETECTION'], + 'color_range_config': config.CAMERA['BLACK_COLOR_RANGE'], + 'object_class': BlackObject, + 'object_list_attr': 'black_objects', + 'id_counter_attr': 'black_object_id_counter', + 'memory_time_attr': 'black_object_memory_time', + 'stats_key': 'black_objects_detected', + 'log_emoji': '⚫', + 'log_name': '黑色物体', + 'mark_color': (128, 128, 128), # BGR格式 + 'label_prefix': 'K:', + 'record_func': 'record_black_object', + 'record_enabled_config': 'RECORD_BLACK_OBJECTS', + 'has_dual_range': False, + 'is_same_func': self._is_same_object_black, + } + } + + if color_type not in COLOR_CONFIG: + raise ValueError(f"不支持的颜色类型: {color_type},支持的类型: {list(COLOR_CONFIG.keys())}") + + cfg = COLOR_CONFIG[color_type] + detected_objects = [] marked_image = image.copy() if image is not None else None - - if not config.PERCEPTION['RED_OBJECT_DETECTION']['ENABLED'] or image is None: - return red_objects, marked_image - + + # 检查是否启用检测 + if not cfg['detection_config']['ENABLED'] or image is None: + return detected_objects, marked_image + try: + # 转换为HSV颜色空间 hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) - - lower_red1 = np.array(config.CAMERA['RED_COLOR_RANGE']['LOWER1']) - upper_red1 = np.array(config.CAMERA['RED_COLOR_RANGE']['UPPER1']) - lower_red2 = np.array(config.CAMERA['RED_COLOR_RANGE']['LOWER2']) - upper_red2 = np.array(config.CAMERA['RED_COLOR_RANGE']['UPPER2']) - - mask1 = cv2.inRange(hsv, lower_red1, upper_red1) - mask2 = cv2.inRange(hsv, lower_red2, upper_red2) - red_mask = cv2.bitwise_or(mask1, mask2) - + + # 创建颜色掩码(红色需要特殊处理,有两个范围) + if cfg['has_dual_range']: + # 红色:两个颜色范围 + lower1 = np.array(cfg['color_range_config']['LOWER1']) + upper1 = np.array(cfg['color_range_config']['UPPER1']) + lower2 = np.array(cfg['color_range_config']['LOWER2']) + upper2 = np.array(cfg['color_range_config']['UPPER2']) + + mask1 = cv2.inRange(hsv, lower1, upper1) + mask2 = cv2.inRange(hsv, lower2, upper2) + color_mask = cv2.bitwise_or(mask1, mask2) + else: + # 其他颜色:单个颜色范围 + lower = np.array(cfg['color_range_config']['LOWER']) + upper = np.array(cfg['color_range_config']['UPPER']) + color_mask = cv2.inRange(hsv, lower, upper) + + # 形态学操作 kernel = np.ones((5, 5), np.uint8) - red_mask = cv2.morphologyEx(red_mask, cv2.MORPH_CLOSE, kernel) - red_mask = cv2.morphologyEx(red_mask, cv2.MORPH_OPEN, kernel) - - contours, _ = cv2.findContours(red_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - + color_mask = cv2.morphologyEx(color_mask, cv2.MORPH_CLOSE, kernel) + color_mask = cv2.morphologyEx(color_mask, cv2.MORPH_OPEN, kernel) + + # 查找轮廓 + contours, _ = cv2.findContours(color_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + # 获取无人机状态 try: state = self.client.getMultirotorState(vehicle_name=self.drone_name) drone_pos = state.kinematics_estimated.position @@ -2534,47 +2622,56 @@ def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarra except: drone_pos = None yaw = 0.0 - + + # 获取配置参数 + min_area = cfg['detection_config']['MIN_AREA'] + max_area = cfg['detection_config']['MAX_AREA'] + existing_objects = getattr(self, cfg['object_list_attr']) + id_counter_attr = cfg['id_counter_attr'] + + # 处理每个轮廓 for contour in contours: area = cv2.contourArea(contour) - min_area = config.PERCEPTION['RED_OBJECT_DETECTION']['MIN_AREA'] - max_area = config.PERCEPTION['RED_OBJECT_DETECTION']['MAX_AREA'] - + if min_area <= area <= max_area: x, y, w, h = cv2.boundingRect(contour) center_x = x + w // 2 center_y = y + h // 2 - + + # 计算置信度 aspect_ratio = w / h if h > 0 else 1.0 confidence = min(1.0, area / 1000.0) * (1.0 / (1.0 + abs(aspect_ratio - 1.0))) - + + # 计算世界坐标位置 world_pos = None if drone_pos is not None and depth_array is not None: try: if 0 <= center_y < depth_array.shape[0] and 0 <= center_x < depth_array.shape[1]: distance = depth_array[center_y, center_x] - + if 0.5 < distance < 50.0: height, width = depth_array.shape fov_h = math.radians(90) - + pixel_angle_x = (center_x - width/2) / (width/2) * (fov_h/2) pixel_angle_y = (center_y - height/2) / (height/2) * (fov_h/2) - + z = distance x_rel = z * math.tan(pixel_angle_x) y_rel = z * math.tan(pixel_angle_y) - + world_x = x_rel * math.cos(yaw) - y_rel * math.sin(yaw) + drone_pos.x_val world_y = x_rel * math.sin(yaw) + y_rel * math.cos(yaw) + drone_pos.y_val world_z = drone_pos.z_val - + world_pos = (world_x, world_y, world_z) except: pass - - red_object = RedObject( - id=self.red_object_id_counter, + + # 创建物体对象 + current_id = getattr(self, id_counter_attr) + color_object = cfg['object_class']( + id=current_id, position=world_pos if world_pos else (0.0, 0.0, 0.0), pixel_position=(center_x, center_y), size=area, @@ -2583,308 +2680,80 @@ def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarra last_seen=time.time(), visited=False ) - + + # 检查是否是已存在的物体 is_new_object = True - for existing_obj in self.red_objects: - if self._is_same_object(red_object, existing_obj): + for existing_obj in existing_objects: + if cfg['is_same_func'](color_object, existing_obj): existing_obj.last_seen = time.time() - existing_obj.pixel_position = red_object.pixel_position + existing_obj.pixel_position = color_object.pixel_position existing_obj.confidence = max(existing_obj.confidence, confidence) if world_pos: existing_obj.position = world_pos - red_object = existing_obj + color_object = existing_obj is_new_object = False break - + + # 如果是新物体,增加ID计数器和统计 if is_new_object: - self.red_object_id_counter += 1 - red_objects.append(red_object) - self.stats['red_objects_detected'] += 1 - self.logger.info(f"🔴 检测到红色物体 #{red_object.id} (置信度: {confidence:.2f})") - - if self.data_logger and config.DATA_RECORDING['RECORD_RED_OBJECTS']: - self.data_logger.record_red_object(red_object) + setattr(self, id_counter_attr, current_id + 1) + detected_objects.append(color_object) + self.stats[cfg['stats_key']] += 1 + self.logger.info(f"{cfg['log_emoji']} 检测到{cfg['log_name']} #{color_object.id} (置信度: {confidence:.2f})") + + # 记录到数据日志 + if self.data_logger and config.DATA_RECORDING.get(cfg['record_enabled_config'], False): + record_func = getattr(self.data_logger, cfg['record_func'], None) + if record_func: + record_func(color_object) else: - red_objects.append(red_object) - + detected_objects.append(color_object) + + # 在图像上标记物体 if marked_image is not None: - color = (0, 100, 255) - if red_object.visited: - color = (0, 200, 0) - - cv2.rectangle(marked_image, (x, y), (x+w, y+h), color, 2) - cv2.circle(marked_image, (center_x, center_y), 5, color, -1) - - label = f"R:{red_object.id} ({confidence:.2f})" + mark_color = cfg['mark_color'] + if color_object.visited: + mark_color = (0, 200, 0) # 已访问的物体显示为绿色 + + cv2.rectangle(marked_image, (x, y), (x+w, y+h), mark_color, 2) + cv2.circle(marked_image, (center_x, center_y), 5, mark_color, -1) + + label = f"{cfg['label_prefix']}{color_object.id} ({confidence:.2f})" cv2.putText(marked_image, label, (x, y-10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - + cv2.FONT_HERSHEY_SIMPLEX, 0.5, mark_color, 1) + + # 清理过期的物体记忆 current_time = time.time() - self.red_objects = [obj for obj in self.red_objects - if current_time - obj.last_seen < self.red_object_memory_time] - - visited_count = sum(1 for obj in self.red_objects if obj.visited) - if len(red_objects) > 0: - self.logger.debug(f"🔴 当前红色物体: {len(self.red_objects)}个, 已访问: {visited_count}个") - + memory_time = getattr(self, cfg['memory_time_attr']) + filtered_objects = [obj for obj in existing_objects + if current_time - obj.last_seen < memory_time] + setattr(self, cfg['object_list_attr'], filtered_objects) + + # 更新统计信息 + visited_count = sum(1 for obj in filtered_objects if obj.visited) + if len(detected_objects) > 0: + self.logger.debug(f"{cfg['log_emoji']} 当前{cfg['log_name']}: {len(filtered_objects)}个, 已访问: {visited_count}个") + except Exception as e: - self.logger.warning(f"⚠️ 红色物体检测失败: {e}") + cfg = COLOR_CONFIG[color_type] + self.logger.warning(f"⚠️ {cfg['log_name']}检测失败: {e}") + + return detected_objects, marked_image - return red_objects, marked_image + def _detect_red_objects(self, image: np.ndarray, depth_array: Optional[np.ndarray] = None) -> Tuple[List[RedObject], np.ndarray]: + """检测红色物体 - 使用通用检测函数""" + objects, marked_image = self._detect_color_objects_generic(image, 'red', depth_array) + return objects, marked_image def _detect_blue_objects(self, image: np.ndarray, depth_array: Optional[np.ndarray] = None) -> Tuple[List[BlueObject], np.ndarray]: - blue_objects = [] - marked_image = image.copy() if image is not None else None - - if not config.PERCEPTION['BLUE_OBJECT_DETECTION']['ENABLED'] or image is None: - return blue_objects, marked_image - - try: - hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) - - lower_blue = np.array(config.CAMERA['BLUE_COLOR_RANGE']['LOWER']) - upper_blue = np.array(config.CAMERA['BLUE_COLOR_RANGE']['UPPER']) - - blue_mask = cv2.inRange(hsv, lower_blue, upper_blue) - - kernel = np.ones((5, 5), np.uint8) - blue_mask = cv2.morphologyEx(blue_mask, cv2.MORPH_CLOSE, kernel) - blue_mask = cv2.morphologyEx(blue_mask, cv2.MORPH_OPEN, kernel) - - contours, _ = cv2.findContours(blue_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - drone_pos = state.kinematics_estimated.position - orientation = state.kinematics_estimated.orientation - roll, pitch, yaw = airsim.to_eularian_angles(orientation) - except: - drone_pos = None - yaw = 0.0 - - for contour in contours: - area = cv2.contourArea(contour) - min_area = config.PERCEPTION['BLUE_OBJECT_DETECTION']['MIN_AREA'] - max_area = config.PERCEPTION['BLUE_OBJECT_DETECTION']['MAX_AREA'] - - if min_area <= area <= max_area: - x, y, w, h = cv2.boundingRect(contour) - center_x = x + w // 2 - center_y = y + h // 2 - - aspect_ratio = w / h if h > 0 else 1.0 - confidence = min(1.0, area / 1000.0) * (1.0 / (1.0 + abs(aspect_ratio - 1.0))) - - world_pos = None - if drone_pos is not None and depth_array is not None: - try: - if 0 <= center_y < depth_array.shape[0] and 0 <= center_x < depth_array.shape[1]: - distance = depth_array[center_y, center_x] - - if 0.5 < distance < 50.0: - height, width = depth_array.shape - fov_h = math.radians(90) - - pixel_angle_x = (center_x - width/2) / (width/2) * (fov_h/2) - pixel_angle_y = (center_y - height/2) / (height/2) * (fov_h/2) - - z = distance - x_rel = z * math.tan(pixel_angle_x) - y_rel = z * math.tan(pixel_angle_y) - - world_x = x_rel * math.cos(yaw) - y_rel * math.sin(yaw) + drone_pos.x_val - world_y = x_rel * math.sin(yaw) + y_rel * math.cos(yaw) + drone_pos.y_val - world_z = drone_pos.z_val - - world_pos = (world_x, world_y, world_z) - except: - pass - - blue_object = BlueObject( - id=self.blue_object_id_counter, - position=world_pos if world_pos else (0.0, 0.0, 0.0), - pixel_position=(center_x, center_y), - size=area, - confidence=confidence, - timestamp=time.time(), - last_seen=time.time(), - visited=False - ) - - is_new_object = True - for existing_obj in self.blue_objects: - if self._is_same_object_blue(blue_object, existing_obj): - existing_obj.last_seen = time.time() - existing_obj.pixel_position = blue_object.pixel_position - existing_obj.confidence = max(existing_obj.confidence, confidence) - if world_pos: - existing_obj.position = world_pos - blue_object = existing_obj - is_new_object = False - break - - if is_new_object: - self.blue_object_id_counter += 1 - blue_objects.append(blue_object) - self.stats['blue_objects_detected'] += 1 - self.logger.info(f"🔵 检测到蓝色物体 #{blue_object.id} (置信度: {confidence:.2f})") - - if self.data_logger and config.DATA_RECORDING['RECORD_BLUE_OBJECTS']: - self.data_logger.record_blue_object(blue_object) - else: - blue_objects.append(blue_object) - - if marked_image is not None: - color = (255, 100, 0) - if blue_object.visited: - color = (0, 200, 0) - - cv2.rectangle(marked_image, (x, y), (x+w, y+h), color, 2) - cv2.circle(marked_image, (center_x, center_y), 5, color, -1) - - label = f"B:{blue_object.id} ({confidence:.2f})" - cv2.putText(marked_image, label, (x, y-10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - - current_time = time.time() - self.blue_objects = [obj for obj in self.blue_objects - if current_time - obj.last_seen < self.blue_object_memory_time] - - visited_count = sum(1 for obj in self.blue_objects if obj.visited) - if len(blue_objects) > 0: - self.logger.debug(f"🔵 当前蓝色物体: {len(self.blue_objects)}个, 已访问: {visited_count}个") - - except Exception as e: - self.logger.warning(f"⚠️ 蓝色物体检测失败: {e}") - - return blue_objects, marked_image + """检测蓝色物体 - 使用通用检测函数""" + objects, marked_image = self._detect_color_objects_generic(image, 'blue', depth_array) + return objects, marked_image def _detect_black_objects(self, image: np.ndarray, depth_array: Optional[np.ndarray] = None) -> Tuple[List[BlackObject], np.ndarray]: - black_objects = [] - marked_image = image.copy() if image is not None else None - - if not config.PERCEPTION['BLACK_OBJECT_DETECTION']['ENABLED'] or image is None: - return black_objects, marked_image - - try: - hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) - - lower_black = np.array(config.CAMERA['BLACK_COLOR_RANGE']['LOWER']) - upper_black = np.array(config.CAMERA['BLACK_COLOR_RANGE']['UPPER']) - - black_mask = cv2.inRange(hsv, lower_black, upper_black) - - kernel = np.ones((5, 5), np.uint8) - black_mask = cv2.morphologyEx(black_mask, cv2.MORPH_CLOSE, kernel) - black_mask = cv2.morphologyEx(black_mask, cv2.MORPH_OPEN, kernel) - - contours, _ = cv2.findContours(black_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - - try: - state = self.client.getMultirotorState(vehicle_name=self.drone_name) - drone_pos = state.kinematics_estimated.position - orientation = state.kinematics_estimated.orientation - roll, pitch, yaw = airsim.to_eularian_angles(orientation) - except: - drone_pos = None - yaw = 0.0 - - for contour in contours: - area = cv2.contourArea(contour) - min_area = config.PERCEPTION['BLACK_OBJECT_DETECTION']['MIN_AREA'] - max_area = config.PERCEPTION['BLACK_OBJECT_DETECTION']['MAX_AREA'] - - if min_area <= area <= max_area: - x, y, w, h = cv2.boundingRect(contour) - center_x = x + w // 2 - center_y = y + h // 2 - - aspect_ratio = w / h if h > 0 else 1.0 - confidence = min(1.0, area / 1000.0) * (1.0 / (1.0 + abs(aspect_ratio - 1.0))) - - world_pos = None - if drone_pos is not None and depth_array is not None: - try: - if 0 <= center_y < depth_array.shape[0] and 0 <= center_x < depth_array.shape[1]: - distance = depth_array[center_y, center_x] - - if 0.5 < distance < 50.0: - height, width = depth_array.shape - fov_h = math.radians(90) - - pixel_angle_x = (center_x - width/2) / (width/2) * (fov_h/2) - pixel_angle_y = (center_y - height/2) / (height/2) * (fov_h/2) - - z = distance - x_rel = z * math.tan(pixel_angle_x) - y_rel = z * math.tan(pixel_angle_y) - - world_x = x_rel * math.cos(yaw) - y_rel * math.sin(yaw) + drone_pos.x_val - world_y = x_rel * math.sin(yaw) + y_rel * math.cos(yaw) + drone_pos.y_val - world_z = drone_pos.z_val - - world_pos = (world_x, world_y, world_z) - except: - pass - - black_object = BlackObject( - id=self.black_object_id_counter, - position=world_pos if world_pos else (0.0, 0.0, 0.0), - pixel_position=(center_x, center_y), - size=area, - confidence=confidence, - timestamp=time.time(), - last_seen=time.time(), - visited=False - ) - - is_new_object = True - for existing_obj in self.black_objects: - if self._is_same_object_black(black_object, existing_obj): - existing_obj.last_seen = time.time() - existing_obj.pixel_position = black_object.pixel_position - existing_obj.confidence = max(existing_obj.confidence, confidence) - if world_pos: - existing_obj.position = world_pos - black_object = existing_obj - is_new_object = False - break - - if is_new_object: - self.black_object_id_counter += 1 - black_objects.append(black_object) - self.stats['black_objects_detected'] += 1 - self.logger.info(f"⚫ 检测到黑色物体 #{black_object.id} (置信度: {confidence:.2f})") - - if self.data_logger and config.DATA_RECORDING['RECORD_BLACK_OBJECTS']: - self.data_logger.record_black_object(black_object) - else: - black_objects.append(black_object) - - if marked_image is not None: - color = (128, 128, 128) - if black_object.visited: - color = (0, 200, 0) - - cv2.rectangle(marked_image, (x, y), (x+w, y+h), color, 2) - cv2.circle(marked_image, (center_x, center_y), 5, color, -1) - - label = f"K:{black_object.id} ({confidence:.2f})" - cv2.putText(marked_image, label, (x, y-10), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - - current_time = time.time() - self.black_objects = [obj for obj in self.black_objects - if current_time - obj.last_seen < self.black_object_memory_time] - - visited_count = sum(1 for obj in self.black_objects if obj.visited) - if len(black_objects) > 0: - self.logger.debug(f"⚫ 当前黑色物体: {len(self.black_objects)}个, 已访问: {visited_count}个") - - except Exception as e: - self.logger.warning(f"⚠️ 黑色物体检测失败: {e}") - - return black_objects, marked_image + """检测黑色物体 - 使用通用检测函数""" + objects, marked_image = self._detect_color_objects_generic(image, 'black', depth_array) + return objects, marked_image def _is_same_object(self, obj1: RedObject, obj2: RedObject, distance_threshold=2.0) -> bool: if obj1.position != (0.0, 0.0, 0.0) and obj2.position != (0.0, 0.0, 0.0):