$ 5
This commit is contained in:
parent
c6f236ca92
commit
6429e3cd5e
@ -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
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
@ -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值最大的边缘点
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user