From 6429e3cd5ef0b8ba870fdac9e6f0944c9a29a678 Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Thu, 15 May 2025 12:08:49 +0000 Subject: [PATCH] $ 5 --- base_move/move_base_hori_line.py | 51 ++++++++------------------------ task_1/task_1.py | 6 ++-- utils/detect_track.py | 1 + 3 files changed, 17 insertions(+), 41 deletions(-) diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index ea40cb1..a3f9ad2 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -27,6 +27,7 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): image = ctrl.image_processor.get_current_image() while attempts < max_attempts and not aligned: + print(f"尝试次数: {attempts+1}/{max_attempts}") # 检测横向线边缘 edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0) @@ -60,37 +61,6 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): if observe: print(f"需要旋转的角度: {angle_to_rotate:.2f}度") - - # 可视化横向线和校准角度 - if isinstance(image, str): - img = cv2.imread(image) - else: - img = image.copy() - - height, width = img.shape[:2] - center_x = width // 2 - - # 画出检测到的横向线 - line_length = 200 - end_x = edge_point[0] + line_length - end_y = int(edge_point[1] + slope * line_length) - start_x = edge_point[0] - line_length - start_y = int(edge_point[1] - slope * line_length) - cv2.line(img, (start_x, start_y), (end_x, end_y), (0, 255, 0), 2) - - # 画出水平线（目标线） - horizontal_y = edge_point[1] - cv2.line(img, (center_x - line_length, horizontal_y), - (center_x + line_length, horizontal_y), (0, 0, 255), 2) - - # 标记角度 - cv2.putText(img, f"当前斜率: {slope:.4f}", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) - cv2.putText(img, f"旋转角度: {angle_to_rotate:.2f}°", (10, 70), - cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) - - cv2.imshow("校准旋转分析", img) - cv2.waitKey(1500 if observe else 1) # 执行旋转 # 如果角度很小，增加一个小的偏移以确保旋转足够 @@ -247,7 +217,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): """ # 首先校准到水平 print("校准到横向线水平") - aligned = align_to_horizontal_line(ctrl, msg, observe=False) + aligned = align_to_horizontal_line(ctrl, msg, observe=observe) if not aligned: print("无法校准到横向线水平，停止移动") @@ -256,7 +226,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): # 检测横向线 # image = cv2.imread("current_image.jpg") # TEST image = ctrl.image_processor.get_current_image() - edge_point, edge_info = detect_horizontal_track_edge(image, observe=False) + edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe) if edge_point is None or edge_info is None: print("无法检测到横向线，停止移动") @@ -349,7 +319,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): # 如果没有提供图像处理器或图像验证失败，则使用里程计数据判断 return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功 -def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left=True, observe=False): +def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left=True, observe=False): """ 对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。 参数: @@ -363,14 +333,14 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left """ # 1. 对准横线 print("校准到横向线水平") - aligned = align_to_horizontal_line(ctrl, msg, observe=False) + aligned = align_to_horizontal_line(ctrl, msg, observe=observe) if not aligned: print("无法校准到横向线水平，停止动作") return False # 2. 检测横线并计算距离 image = ctrl.image_processor.get_current_image() - edge_point, edge_info = detect_horizontal_track_edge(image, observe=False) + edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe) if edge_point is None or edge_info is None: print("无法检测到横向线，停止动作") return False @@ -391,8 +361,11 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left # 3. 计算圆弧运动参数 angle_rad = math.radians(angle_deg) # 设定角速度（rad/s），可根据实际调整 - w = 0.4 if left else -0.4 # 左转为正，右转为负 - v = w * r # 线速度，正负号与角速度方向一致 + + base_w = 0.6 + + w = base_w if left else -base_w # 左转为正，右转为负 + v = abs(w * r) # 线速度，正负号与角速度方向一致 t = abs(angle_rad / w) # 运动时间，取绝对值保证为正 if observe: @@ -402,7 +375,7 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left msg.mode = 11 msg.gait_id = 26 msg.vel_des = [v, 0, w] # [前进速度, 侧向速度, 角速度] - msg.duration = int((t + 2) * 1000) # 加1秒余量 + msg.duration = int((t+1) * 1000) # 加1秒余量 msg.step_height = [0.06, 0.06] msg.life_count += 1 diff --git a/task_1/task_1.py b/task_1/task_1.py index babc848..80aad72 100644 --- a/task_1/task_1.py +++ b/task_1/task_1.py @@ -7,15 +7,17 @@ import os sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from base_move.move_base_hori_line import move_to_hori_line, arc_turn_around_hori_line -observe = True +observe = False def run_task_1(ctrl, msg): print('Running task 1...') # v2 arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=False, observe=observe) + + return True - move_to_hori_line(ctrl, msg, distance=1, observe=observe) + move_to_hori_line(ctrl, msg, target_distance=1, observe=observe) arc_turn_around_hori_line(ctrl, msg, angle_deg=180, left=True, observe=observe) diff --git a/utils/detect_track.py b/utils/detect_track.py index 705cfd1..9654d1e 100644 --- a/utils/detect_track.py +++ b/utils/detect_track.py @@ -5,6 +5,7 @@ from sklearn.metrics import silhouette_score from sklearn import linear_model def detect_horizontal_track_edge(image, observe=False, delay=1000): + observe = False # TSET """ 检测正前方横向黄色赛道的边缘，并返回y值最大的边缘点