diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 7f3677f..313e0c5 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -31,12 +31,11 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): attempts = 0 aligned = False image = ctrl.image_processor.get_current_image() - last_angle_to_rotate = 0 - accumulated_angle = 0 # 添加累积角度跟踪 - max_total_rotation = 45 # 防止过度累积旋转 - angle_accuracy_threshold = 2.0 # 改为更宽松的阈值 + accumulated_angle = 0 + max_total_rotation = 45 + angle_accuracy_threshold = 2.0 - # 记录初始位置，用于可能的回退 + # 记录初始位置 initial_yaw = ctrl.odo_msg.rpy[2] while attempts < max_attempts and not aligned: @@ -52,15 +51,14 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): # 尝试小幅度摇头寻找横线 if attempts < max_attempts - 1: - small_angle = 5 * (1 if attempts % 2 == 0 else -1) # 左右交替摇头 + small_angle = 5 * (1 if attempts % 2 == 0 else -1) info(f"尝试摇头 {small_angle}度 寻找横线", "校准") turn_degree(ctrl, msg, small_angle, absolute=False) - time.sleep(0.5) # 等待稳定 + time.sleep(0.5) attempts += 1 continue - # 获取检测到的斜率和其他信息 slope = edge_info["slope"] is_horizontal = edge_info["is_horizontal"] @@ -68,82 +66,63 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): debug(f"检测到横向线，斜率: {slope:.6f}", "检测") info(f"是否足够水平: {is_horizontal}", "检测") - # 如果已经水平，则无需旋转 if is_horizontal: success("横向线已经水平，无需校准", "完成") return True # 计算需要旋转的角度 - # 斜率 = tan(θ)，因此 θ = arctan(斜率) angle_rad = math.atan(slope) angle_deg = math.degrees(angle_rad) + angle_to_rotate = -angle_deg # 取负值使旋转方向正确 - # 调整角度方向 - # 正的斜率意味着线条从左到右上升，需要逆时针旋转校正 - # 负的斜率意味着线条从左到右下降，需要顺时针旋转校正 - # 注意旋转方向: 顺时针为负角度，逆时针为正角度 - raw_angle_to_rotate = -angle_deg # 取负值使旋转方向正确 + # 设置最小旋转阈值 + if abs(angle_to_rotate) < 0.5: # 降低最小旋转阈值 + angle_to_rotate = math.copysign(0.5, angle_to_rotate) - # 动态调整补偿系数：对于反复校准同一方向的情况，增加补偿 - if last_angle_to_rotate * raw_angle_to_rotate > 0: # 如果方向一致 - # 原来的角度太小，需要更大的补偿 - compensation_factor = 1.3 + min(0.3, attempts * 0.1) # 随着尝试次数增加补偿 - else: - # 方向不一致，可能过度补偿，降低系数 - compensation_factor = 0.8 + # 限制单次旋转角度 + angle_to_rotate = max(-15, min(15, angle_to_rotate)) - # 应用补偿 - angle_to_rotate = raw_angle_to_rotate * compensation_factor - - # 避免角度过小导致效果不明显 - if abs(angle_to_rotate) < 2.0: - # 对小角度增加最小旋转阈值 - angle_to_rotate = math.copysign(2.0, angle_to_rotate) - - # 限制单次旋转角度，避免过度旋转 - angle_to_rotate = max(-20, min(20, angle_to_rotate)) - - # 检查累积旋转是否过大 + # 检查累积旋转 if abs(accumulated_angle + angle_to_rotate) > max_total_rotation: warning(f"累积旋转角度({accumulated_angle + angle_to_rotate:.2f}°)过大，限制旋转", "警告") - # 如果过大，只旋转一半的角度 angle_to_rotate = angle_to_rotate / 2 if observe: - info(f"原始角度: {raw_angle_to_rotate:.2f}度, 补偿后: {angle_to_rotate:.2f}度", "角度") + info(f"计算旋转角度: {angle_to_rotate:.2f}度", "角度") info(f"当前累积旋转: {accumulated_angle:.2f}度", "累积") +<<<<<<< HEAD # 使用turn_degree函数执行旋转，增加精度参数 info(f"旋转角度: {angle_to_rotate:.2f}度", "旋转") turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False, precision=True) info(f"旋转结果: {turn_success}", "旋转结果") # 等待稳定 +======= + # 执行旋转 + turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False, precision=True) +>>>>>>> 6b15e9be908cb3e330a29e326449540f312dec4e time.sleep(0.3) - # 旋转后实际改变的角度 + # 计算实际旋转角度 current_yaw = ctrl.odo_msg.rpy[2] actual_rotation = math.degrees(current_yaw - initial_yaw - math.radians(accumulated_angle)) - # 规范化到 -180 到 180 度范围 + # 规范化角度 while actual_rotation > 180: actual_rotation -= 360 while actual_rotation < -180: actual_rotation += 360 - # 更新累积角度 accumulated_angle += actual_rotation if observe: info(f"请求旋转: {angle_to_rotate:.2f}度, 实际旋转: {actual_rotation:.2f}度", "旋转") info(f"旋转结果: {'成功' if turn_success else '失败'}", "成功" if turn_success else "失败") - # 增加尝试次数 attempts += 1 - last_angle_to_rotate = angle_to_rotate - # 判断成功条件：基于当前检测到的斜率，而不仅是运动控制的结果 - # 重新获取图像和检测横线 + # 检查校准结果 edge_point_after, edge_info_after = detect_horizontal_track_edge( ctrl.image_processor.get_current_image(), observe=observe, @@ -158,28 +137,25 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): if observe: info(f"校准后斜率: {current_slope:.6f}, 角度: {current_angle_deg:.2f}度", "检测") - # 使用更宽松的阈值判断成功 if abs(current_angle_deg) < angle_accuracy_threshold: success(f"校准成功，当前角度: {current_angle_deg:.2f}度", "完成") aligned = True else: warning(f"校准后角度仍超出阈值: {abs(current_angle_deg):.2f}° > {angle_accuracy_threshold}°", "警告") - # 如果到达最大尝试次数且未对齐，但最后一次接近成功，也返回成功 + # 最后一次尝试的妥协处理 if attempts == max_attempts and not aligned and edge_info_after: current_slope = edge_info_after["slope"] current_angle_deg = math.degrees(math.atan(current_slope)) - # 如果接近水平，放宽标准接受这个结果 - if abs(current_angle_deg) < angle_accuracy_threshold * 1.5: # 允许1.5倍阈值 + if abs(current_angle_deg) < angle_accuracy_threshold * 1.5: warning(f"达到最大尝试次数，接受近似结果，当前角度: {current_angle_deg:.2f}度", "妥协") aligned = True - # 如果无法对齐但已累积大量旋转，尝试恢复到最佳状态 + # 恢复最佳状态的处理 if not aligned and abs(accumulated_angle) > 15: warning(f"校准失败，累积旋转{accumulated_angle:.2f}度，尝试恢复到最佳状态", "恢复") - # 重新检测一次 best_edge_point, best_edge_info = detect_horizontal_track_edge( ctrl.image_processor.get_current_image(), observe=observe, @@ -190,7 +166,6 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): best_slope = best_edge_info["slope"] best_angle_deg = math.degrees(math.atan(best_slope)) - # 如果当前状态已经是最好的，不进行调整 if abs(best_angle_deg) < angle_accuracy_threshold * 1.5: info(f"当前状态已经接近水平，角度: {best_angle_deg:.2f}度", "保持") return True diff --git a/logs/robot_2025-05-18.log b/logs/robot_2025-05-18.log deleted file mode 100644 index b008187..0000000 --- a/logs/robot_2025-05-18.log +++ /dev/null @@ -1,25 +0,0 @@ -2025-05-18 16:58:17 | ERROR | utils.log_helper - ❌ 左侧区域未检测到垂直线 -2025-05-18 16:58:45 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 -2025-05-18 16:58:46 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 -2025-05-18 16:58:47 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码 -2025-05-18 16:58:48 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 -2025-05-18 16:58:49 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 17 条直线 -2025-05-18 16:58:50 | ERROR | utils.log_helper - ❌ 左侧区域未检测到垂直线 -2025-05-18 16:59:14 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 -2025-05-18 16:59:15 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 -2025-05-18 16:59:16 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码 -2025-05-18 16:59:17 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 -2025-05-18 16:59:18 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 26 条直线 -2025-05-18 16:59:19 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 2 条垂直线 -2025-05-18 16:59:20 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点 -2025-05-18 16:59:21 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250518_165921_631983.jpg -2025-05-18 16:59:21 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250518_165921_631983', 'tracking_point': (95, 1077), 'ground_intersection': (91, 1080), 'distance_to_left': 216.5, 'slope': -0.7530864197530864, 'line_mid_x': 216.5} -2025-05-18 16:59:34 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 -2025-05-18 16:59:35 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 -2025-05-18 16:59:36 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码 -2025-05-18 16:59:37 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 -2025-05-18 16:59:38 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 26 条直线 -2025-05-18 16:59:39 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 2 条垂直线 -2025-05-18 16:59:40 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点 -2025-05-18 16:59:41 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250518_165941_364168.jpg -2025-05-18 16:59:41 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250518_165941_364168', 'tracking_point': (95, 1077), 'ground_intersection': (91, 1080), 'distance_to_left': 216.5, 'slope': -0.7530864197530864, 'line_mid_x': 216.5} diff --git a/task_1/task_1.py b/task_1/task_1.py index ee362cc..3618bac 100644 --- a/task_1/task_1.py +++ b/task_1/task_1.py @@ -8,7 +8,8 @@ sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from base_move.move_base_hori_line import ( arc_turn_around_hori_line, go_straight_by_hori_line, - move_to_hori_line + move_to_hori_line, + align_to_horizontal_line ) from base_move.go_straight import go_straight from base_move.turn_degree import turn_degree @@ -22,6 +23,11 @@ observe = True def run_task_1(ctrl, msg): section('任务1：寻找横线并绕行', "启动") info('开始执行任务1...', "启动") + + # # TEST better align + # aligned = align_to_horizontal_line(ctrl, msg, observe=observe) + # print(aligned) + # return # v2 section('任务1-1：转弯并扫描QR码', "移动") @@ -74,8 +80,6 @@ def run_task_1(ctrl, msg): else: go_straight(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe) - return - section('任务1-5：模拟装货', "停止") info('机器人躺下，模拟装货过程', "信息") start_time = time.time() @@ -98,7 +102,7 @@ def run_task_1(ctrl, msg): ctrl=ctrl, msg=msg, radius=res['radius'] * 2, - angle_deg=85, + angle_deg=90, # pass_align=True, observe=observe diff --git a/utils/detect_track.py b/utils/detect_track.py index 5ec3b64..360bb6b 100644 --- a/utils/detect_track.py +++ b/utils/detect_track.py @@ -335,7 +335,7 @@ def detect_horizontal_track_edge(image, observe=False, delay=1000, save_log=True "points_count": len(selected_points), # 该组中点的数量 "intersection_point": intersection_point, # 中线与横向线的交点 "distance_to_bottom": distance_to_bottom, # 交点到图像底部的距离 - "points": selected_points # 添加选定的点组 + # "points": selected_points # 添加选定的点组 } return bottom_edge_point, edge_info