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

feat(base_move): improve arc_turn_around_hori_line function

Browse Source 
- Add target_distance parameter to adjust the radius of the arc
- Modify the function to subtract target_distance from the calculated radius
- Update the velocity calculation to use the absolute value
- Increase the duration by
This commit is contained in:
havoc420ubuntu 2025-05-15 08:35:38 +00:00
parent ffc5d45fb0
commit e547885e98
2 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
base_move/move_base_hori_line.py
View File

@ -352,7 +352,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厘米，则认为成功 return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功
def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, observe=False): def arc_turn_around_hori_line(ctrl, msg, target_distance=0.5, angle_deg=90, left=True, observe=False):
""" """
对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。 对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。
参数: 参数:
@ -366,20 +366,24 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, observe=False)
""" """
# 1. 对准横线 # 1. 对准横线
print("校准到横向线水平") print("校准到横向线水平")
aligned = align_to_horizontal_line(ctrl, msg, observe=observe) aligned = align_to_horizontal_line(ctrl, msg, observe=False)
if not aligned: if not aligned:
print("无法校准到横向线水平，停止动作") print("无法校准到横向线水平，停止动作")
return False return False
# 2. 检测横线并计算距离 # 2. 检测横线并计算距离
image = ctrl.image_processor.get_current_image() image = ctrl.image_processor.get_current_image()
edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe) edge_point, edge_info = detect_horizontal_track_edge(image, observe=False)
if edge_point is None or edge_info is None: if edge_point is None or edge_info is None:
print("无法检测到横向线，停止动作") print("无法检测到横向线，停止动作")
return False return False
camera_height = 0.355 # 单位: 米 camera_height = 0.355 # 单位: 米
r = calculate_distance_to_line(edge_info, camera_height, observe=observe) r = calculate_distance_to_line(edge_info, camera_height, observe=observe)
# 减去目标距离
r -= target_distance
if r is None: if r is None:
print("无法计算到横向线的距离，停止动作") print("无法计算到横向线的距离，停止动作")
return False return False
@ -391,7 +395,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, observe=False)
angle_rad = math.radians(angle_deg) angle_rad = math.radians(angle_deg)
# 设定角速度（rad/s），可根据实际调整 # 设定角速度（rad/s），可根据实际调整
w = 0.4 if left else -0.4 # 左转为正，右转为负 w = 0.4 if left else -0.4 # 左转为正，右转为负
v = w * r # 线速度 v = abs(w * r) # 线速度
t = abs(angle_rad / w) # 运动时间 t = abs(angle_rad / w) # 运动时间
if observe: if observe:
@ -401,7 +405,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, observe=False)
msg.mode = 11 msg.mode = 11
msg.gait_id = 26 msg.gait_id = 26
msg.vel_des = [v, 0, w] # [前进速度, 侧向速度, 角速度] msg.vel_des = [v, 0, w] # [前进速度, 侧向速度, 角速度]
msg.duration = int((t + 1) * 1000) # 加1秒余量 msg.duration = int((t + 2) * 1000) # 加1秒余量
msg.step_height = [0.06, 0.06] msg.step_height = [0.06, 0.06]
msg.life_count += 1 msg.life_count += 1

5
task_1/task_1.py
View File

@ -5,7 +5,7 @@ import os
# 添加父目录到路径，以便能够导入utils # 添加父目录到路径，以便能够导入utils
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from base_move.move_base_hori_line import align_to_horizontal_line, move_to_hori_line from base_move.move_base_hori_line import move_to_hori_line, arc_turn_around_hori_line
observe = True observe = True
@ -13,7 +13,8 @@ def run_task_1(ctrl, msg):
print('Running task 1...') print('Running task 1...')
# v2 # v2
move_to_hori_line(ctrl, msg, observe=observe) arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=False, observe=observe)
# move_to_hori_line(ctrl, msg, observe=observe)