Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' of ssh://120.27.199.238:222/Havoc420mac/mi-task into main

Browse Source
This commit is contained in:
havoc420ubuntu 2025-05-19 06:26:54 +00:00
commit 5d586c2999
4 changed files with 34 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
base_move/move_base_hori_line.py
View File

@ -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

25
logs/robot_2025-05-18.log
View File

@ -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}

12
task_1/task_1.py
View File

@ -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

2
utils/detect_track.py
View File

@ -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