while retry_count < 3 and self.active:  
    try:  
        self.dwa_client.update_configuration(dwa_params)  
        break  
    except Exception as e:  
        retry_count += 1  
        rospy.sleep(0.5)  # 间隔0.5秒重试  

def set_zoneA_params(self):
    """设置A区域（碎石地形）的导航参数"""
    # 创建参数字典
    params_A = {
        'max_vel_x': 2.0,           # 降低最大线速度，适应碎石地形
        'min_vel_x': 0.0,
        'max_vel_theta': 1.0,
        'min_in_place_vel_theta': 0.4,
        'acc_lim_x': 2.0,           # 增大加速度限制
        'acc_lim_theta': 3.2,
        'xy_goal_tolerance': 0.2,
        'yaw_goal_tolerance': 0.1,
        'vx_samples': 30,           # 增加速度采样点，提升路径平滑度
        'inflation_radius': 0.3,    # 设置避障半径
        'path_distance_bias': 32.0,
        'goal_distance_bias': 20.0,
        'occdist_scale': 0.02,
        'cost_scaling_factor': 3.0   # 代价地图权重
    }
    
    # 应用参数
    self._apply_params(params_A)
    rospy.loginfo("已应用A区域（碎石地形）参数")

def set_zoneD_params(self):
    """设置D区域（雷区）的导航参数"""
    # 创建参数字典
    params_D = {
        'max_vel_x': 0.6,           # 严格限制线速度，确保安全
        'min_vel_x': 0.0,
        'max_vel_theta': 0.5,
        'min_in_place_vel_theta': 0.2,
        'acc_lim_x': 0.5,           # 低加速度限制，避免急动作
        'acc_lim_theta': 1.0,
        'xy_goal_tolerance': 0.1,
        'yaw_goal_tolerance': 0.05,
        'vx_samples': 20,
        'inflation_radius': 0.8,    # 最大避障半径，确保安全距离
        'path_distance_bias': 32.0,
        'goal_distance_bias': 20.0,
        'occdist_scale': 0.5,       # 提高障碍物距离权重
        'cost_scaling_factor': 10.0, # 最高代价地图权重
        'publish_cost_grid': True    # 开启代价网格可视化
    }
    
    # 应用参数
    self._apply_params(params_D)
    rospy.loginfo("已应用D区域（雷区）参数")

def _apply_params(self, params):
    """应用导航参数，包含三阶段重试机制"""
    retry_count = 0
    while retry_count < 3 and self.active:  # 三阶段重试机制
        try:
            # 获取当前支持的参数集合
            dwa_supported = self.dwa_client.get_configuration()
            dwa_params = {}
            
            # 过滤不支持的参数
            for key, value in params.items():
                if key in dwa_supported:
                    dwa_params[key] = value
                else:
                    rospy.logwarn(f"参数 {key} 不被当前硬件支持，已跳过")
            
            # 更新配置
            self.dwa_client.update_configuration(dwa_params)
            break
        except Exception as e:
            retry_count += 1
            rospy.logwarn(f"参数设置失败，正在重试 ({retry_count}/3): {e}")
            rospy.sleep(0.5)  # 间隔0.5秒重试
    
    if retry_count == 3:
        rospy.logerr("参数设置失败，将使用默认参数")

def detect(self, image_path=None, conf_threshold=0.5):
        # 初始化结果字典
        result_counts = {name: 0 for name in class_names.values()}

        # 获取检测图像
        if image_path is None:
            # 自动拍照模式
            cap = cv2.VideoCapture(0)
            if not cap.isOpened():
                return "无法打开摄像头", ""

            ret, frame = cap.read()
            cap.release()
            if not ret:
                return "拍照失败", ""

            # 保存原始照片
            timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
            captured_image_path = "detect_%s.jpg" % timestamp
            cv2.imwrite(captured_image_path, frame)
            image_to_detect = captured_image_path
        elif image_path in ["A", "B", "C", "D"]:
            cap = cv2.VideoCapture(0)
            if not cap.isOpened():
                return "无法打开摄像头", ""

            ret, frame = cap.read()
            cap.release()
            if not ret:
                return "拍照失败", ""

            # 保存原始照片
            captured_image_path = "%s.jpg" % image_path
            cv2.imwrite(captured_image_path, frame)
            image_to_detect = captured_image_path
        else:
            image_to_detect = image_path

        # 执行检测（使用YOLO自带的绘图功能）
        results = model.predict(
            source=image_to_detect,
            conf=conf_threshold,
            save=True,  # 自动保存结果图片
            save_txt=False,
            save_conf=False,
            exist_ok=True,
        )

        # 统计结果（只统计置信度大于阈值的检测）
        for r in results:
            for box in r.boxes:
                if float(box.conf) >= conf_threshold:
                    class_id = int(box.cls)
                    class_name = class_names.get(class_id, "unknown")
                    result_counts[class_name] += 1

        # 生成结果字符串
        result_str = "图片 {} 中检测到：".format(image_to_detect)
        for name, count in result_counts.items():
            if count > 0:
                result_str += "{}{}人，".format(name, count)
        result_str = result_str.rstrip("，")

        # 获取YOLO自动保存的结果图片路径
        result_image_path = "runs/{}".format(image_to_detect.split("/")[-1])
        if image_path is None:  # 如果是摄像头拍摄的
            result_image_path = "runs/{}".format(captured_image_path)
        print(result_str)
        print("结果图片已保存到: {0}".format(result_image_path))
        return result_str, result_image_path

def execute_mission(self, goals, zone_type="A"):
    """执行导航任务序列，包含参数调整和徘徊脱困机制
    
    Args:
        goals: 导航目标点列表
        zone_type: 区域类型，用于加载对应参数
        
    Returns:
        任务是否成功完成
    """
    # 根据区域类型设置参数
    if zone_type == "A":
        self.set_zoneA_params()
    elif zone_type == "B":
        self.set_zoneB_params()
    elif zone_type == "C":
        self.set_zoneC_params()
    elif zone_type == "D":
        self.set_zoneD_params()
    else:
        rospy.logwarn(f"未知区域类型: {zone_type}，使用默认参数")
    
    for goal in goals:
        rospy.loginfo(f"导航至目标点: {goal.target_pose.pose.position.x}, {goal.target_pose.pose.position.y}")
        
        # 清理代价地图
        self.clear_costmaps()
        
        # 发送导航目标
        self.nav_client.send_goal(goal)
        
        # 记录开始时间和位置
        start_time = rospy.Time.now().to_sec()
        last_check_time = start_time
        last_position = self.get_current_pose()
        hover_start_time = None
        inflation_reduced = False
        
        # 等待导航完成或超时
        while not self.nav_client.wait_for_result(rospy.Duration(0.5)) and self.active:
            current_time = rospy.Time.now().to_sec()
            
            # 每5秒检查一次徘徊状态
            if current_time - last_check_time > 5.0:
                current_position = self.get_current_pose()
                distance_moved = self.distance(last_position, current_position)
                
                # 徘徊检测：如果5秒内移动距离小于0.1米，判定为徘徊
                if distance_moved < 0.1:
                    if hover_start_time is None:
                        hover_start_time = current_time
                        rospy.logwarn("检测到徘徊，开始计时...")
                    
                    # 徘徊持续20秒，执行脱困策略
                    if current_time - hover_start_time > 20.0:
                        rospy.logwarn("徘徊超过20秒，执行脱困策略")
                        
                        # 如果还未减小膨胀半径，则设置为0强行通过
                        if not inflation_reduced:
                            rospy.logwarn("减小膨胀半径为0，尝试强行通过")
                            self.dwa_client.update_configuration({'inflation_radius': 0.0})
                            inflation_reduced = True
                            hover_start_time = current_time  # 重置徘徊计时
                        else:
                            # 取消当前目标
                            self.nav_client.cancel_goal()
                            rospy.logwarn("取消当前导航目标")
                            
                            # 清理代价地图
                            self.clear_costmaps()
                            rospy.loginfo("清理代价地图")
                            
                            # 原地旋转，获取新的环境信息
                            self.rotate_in_place(duration=3.0)
                            rospy.loginfo("执行原地旋转，重新获取环境信息")
                            
                            # 重新发送目标
                            self.nav_client.send_goal(goal)
                            rospy.loginfo("重新发送导航目标")
                            
                            # 重置状态
                            hover_start_time = None
                            inflation_reduced = False
                else:
                    # 移动正常，重置徘徊状态
                    hover_start_time = None
                    
                    # 如果之前减小了膨胀半径，现在恢复
                    if inflation_reduced:
                        if zone_type == "A":
                            self.dwa_client.update_configuration({'inflation_radius': 0.3})
                        elif zone_type == "B":
                            self.dwa_client.update_configuration({'inflation_radius': 0.4})
                        elif zone_type == "C":
                            self.dwa_client.update_configuration({'inflation_radius': 0.7})
                        elif zone_type == "D":
                            self.dwa_client.update_configuration({'inflation_radius': 0.8})
                        inflation_reduced = False
                        rospy.loginfo("恢复正常膨胀半径")
                
                # 更新上次检查时间和位置
                last_check_time = current_time
                last_position = current_position
            
            # 导航超时处理（50秒）
            if current_time - start_time > 50.0:
                rospy.logwarn("导航超时，取消当前目标")
                self.nav_client.cancel_goal()
                return False
        
        # 检查导航结果
        if self.nav_client.get_state() == GoalStatus.SUCCEEDED:
            rospy.loginfo("成功到达目标点")
        else:
            rospy.logwarn(f"导航失败，状态码: {self.nav_client.get_state()}")
            return False
    
    return True

def rotate_in_place(self, duration=3.0, speed=0.5):
    """原地旋转，用于获取新的环境信息
    
    Args:
        duration: 旋转持续时间（秒）
        speed: 旋转速度（弧度/秒）
    """
    cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
    twist = Twist()
    twist.angular.z = speed
    
    start_time = rospy.Time.now()
    rate = rospy.Rate(10)  # 10Hz
    
    while (rospy.Time.now() - start_time).to_sec() < duration and not rospy.is_shutdown():
        cmd_vel_pub.publish(twist)
        rate.sleep()
    
    # 停止旋转
    twist.angular.z = 0.0
    cmd_vel_pub.publish(twist)

def clear_costmaps(self):
    """清理导航代价地图"""
    try:
        rospy.wait_for_service('/move_base/clear_costmaps', timeout=2.0)
        clear_costmaps_srv = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
        clear_costmaps_srv()
        rospy.loginfo("代价地图已清理")
    except (rospy.ServiceException, rospy.ROSException) as e:
        rospy.logerr(f"清理代价地图失败: {e}")

def signal_handler(self, sig, frame):
    """处理Ctrl+C信号，实现紧急停止"""
    rospy.loginfo("接收到终止信号，执行紧急停止...")
    
    # 发布零速度指令停止机器人
    cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
    stop_cmd = Twist()
    stop_cmd.linear.x = 0.0
    stop_cmd.linear.y = 0.0
    stop_cmd.linear.z = 0.0
    stop_cmd.angular.x = 0.0
    stop_cmd.angular.y = 0.0
    stop_cmd.angular.z = 0.0
    
    # 连续发送5次停止指令，确保接收
    for _ in range(5):
        cmd_vel_pub.publish(stop_cmd)
        rospy.sleep(0.01)  # 10ms间隔
    
    # 取消所有导航目标
    if self.nav_client:
        self.nav_client.cancel_all_goals()
    
    # 标记系统为非活动状态
    self.active = False
    
    rospy.loginfo("机器人已安全停止，系统即将退出")
    rospy.signal_shutdown("用户中断")

def handle_request(self, req):
    """处理救援任务请求
    
    Args:
        req: 包含任务区域的请求对象
        
    Returns:
        任务执行结果
    """
    rospy.loginfo(f"收到救援任务请求: {req.data}")
    
    # 取消当前任务（如果有）
    if self.nav_client.get_state() in [GoalStatus.ACTIVE, GoalStatus.PENDING]:
        self.nav_client.cancel_all_goals()
        rospy.loginfo("已取消当前任务")
    
    # 根据请求区域执行对应任务
    if req.data == "A":
        self.processA()
        return String("A区任务执行完毕")
    elif req.data == "B":
        self.processB()
        return String("B区任务执行完毕")
    elif req.data == "C":
        self.processC()
        return String("C区任务执行完毕")
    elif req.data == "D":
        self.processD()
        return String("D区任务执行完毕")
    else:
        return String(f"未知区域: {req.data}")

def processA(self):
    """A区救援任务"""
    rospy.loginfo("开始执行A区救援任务")
    goals = [
        self.create_goal(1.5, 2.1, 0, 1),  # 到达A点
    ]
    if self.execute_mission(goals, "A"):
        rospy.loginfo("到达A区，正在接收伤员...")
        self.detect("test1.jpg", 0.5)
        time.sleep(3)
        
        # 返回营地
        rospy.loginfo("A区任务完成，返回营地")
        return_goals = [
            self.create_goal(0, 0, 0, 1),  # 返回营地
        ]
        self.execute_mission(return_goals, "A")
    else:
        rospy.logerr("A区任务失败")