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

🎏 v0.1 base task-1

Browse Source
This commit is contained in:
havoc420ubuntu 2025-05-16 11:35:53 +00:00
parent 59961b9905
commit 35e8cc9858
2 changed files with 42 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
base_move/move_base_hori_line.py
View File

@ -327,7 +327,8 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distance=0.2, def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distance=0.2,
pass_align=False, pass_align=False,
scan_qrcode=False, qr_check_interval=0.3, scan_qrcode=False, qr_check_interval=0.3,
observe=False,): observe=False,
bad_big_angle_corret=False):
""" """
对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。 对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。
参数: 参数:
@ -340,6 +341,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
observe: 是否打印调试信息 observe: 是否打印调试信息
scan_qrcode: 是否在旋转过程中扫描QR码，默认为False scan_qrcode: 是否在旋转过程中扫描QR码，默认为False
qr_check_interval: QR码检查间隔时间(秒)，默认为0.3秒 qr_check_interval: QR码检查间隔时间(秒)，默认为0.3秒
bad_big_angle_corret: 是否对大角度错误进行修正，默认为False # TODO clear
返回: 返回:
bool或元组: 如果scan_qrcode为False，返回bool表示是否成功完成操作； bool或元组: 如果scan_qrcode为False，返回bool表示是否成功完成操作；
如果scan_qrcode为True，返回(bool, str)元组，表示(是否成功完成操作, QR码扫描结果) 如果scan_qrcode为True，返回(bool, str)元组，表示(是否成功完成操作, QR码扫描结果)
@ -468,8 +471,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
start_position = list(ctrl.odo_msg.xyz) start_position = list(ctrl.odo_msg.xyz)
# 在起点放置蓝色标记 # 在起点放置蓝色标记
if observe and hasattr(ctrl, 'place_marker'): # TEST 获取当前姿态
# 获取当前姿态
yaw = ctrl.odo_msg.rpy[2] yaw = ctrl.odo_msg.rpy[2]
ctrl.place_marker(start_position[0], start_position[1], ctrl.place_marker(start_position[0], start_position[1],
start_position[2] if len(start_position) > 2 else 0.0, start_position[2] if len(start_position) > 2 else 0.0,
@ -729,7 +731,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
final_angle_deg = math.degrees(final_angle_turned) final_angle_deg = math.degrees(final_angle_turned)
# 这里使用原始目标角度进行比较 # 这里使用原始目标角度进行比较
if not bad_big_angle_corret:
target_angle_deg = angle_deg if left else -angle_deg target_angle_deg = angle_deg if left else -angle_deg
else:
target_angle_deg = -angle_deg if left else angle_deg
if observe: if observe:
# 输出停止过程中角度变化 # 输出停止过程中角度变化

56
task_1/task_1.py
View File

@ -9,7 +9,7 @@ from base_move.move_base_hori_line import move_to_hori_line, arc_turn_around_hor
from utils.detect_track import detect_horizontal_track_edge from utils.detect_track import detect_horizontal_track_edge
from base_move.move_base_hori_line import calculate_distance_to_line from base_move.move_base_hori_line import calculate_distance_to_line
observe = True observe = False
def run_task_1(ctrl, msg): def run_task_1(ctrl, msg):
print('🐶 Running task 1...') print('🐶 Running task 1...')
@ -17,43 +17,43 @@ def run_task_1(ctrl, msg):
# v2 # v2
print('😺 task 1 - 1') print('😺 task 1 - 1')
# 在 arc_turn_around_hori_line 中启用 QR 码扫描 # 在 arc_turn_around_hori_line 中启用 QR 码扫描
# turn_success, qr_result = arc_turn_around_hori_line( turn_success, qr_result = arc_turn_around_hori_line(
# ctrl=ctrl, ctrl=ctrl,
# msg=msg, msg=msg,
# angle_deg=85, angle_deg=85,
# left=False, left=False,
# observe=observe, observe=observe,
# scan_qrcode=True, # 启用 QR 码扫描 scan_qrcode=True, # 启用 QR 码扫描
# qr_check_interval=0.3 # 每 0.3 秒检查一次扫描结果 qr_check_interval=0.3 # 每 0.3 秒检查一次扫描结果
# ) )
# return
# if qr_result: if qr_result:
# print(f"🎯 在任务 1-1 中成功扫描到 QR 码: {qr_result}") print(f"🎯 在任务 1-1 中成功扫描到 QR 码: {qr_result}")
# else: else:
# print("⚠️ 任务 1-1 中未能扫描到任何 QR 码") print("⚠️ 任务 1-1 中未能扫描到任何 QR 码")
# print('😺 task 1 - 2') print('😺 task 1 - 2')
# # 执行常规的移动操作，不需要 QR 码扫描 # 执行常规的移动操作，不需要 QR 码扫描
# move_to_hori_line(ctrl, msg, target_distance=1, observe=observe) move_to_hori_line(ctrl, msg, target_distance=1, observe=observe)
print('😺 task 1 - 3') print('😺 task 1 - 3')
qr_result = "A-1" # direction = True if qr_result == 'A-1' else False
direction = True if qr_result == 'A-1' else False # TODO
turn_success = arc_turn_around_hori_line( turn_success = arc_turn_around_hori_line(
ctrl=ctrl, ctrl=ctrl,
msg=msg, msg=msg,
angle_deg=180, angle_deg=170,
target_distance=0.5, target_distance=0.4, # TODO 优化这里的参数
left=direction, left=False, # direction,
# pass_align=True, pass_align=True,
observe=observe, observe=observe,
# TODO clear
bad_big_angle_corret=True
) )
return
print('😺 task 1 - 4') print('😺 task 1 - 4')
move_to_hori_line(ctrl, msg, observe=observe) # move_to_hori_line(ctrl, msg, target_distance=0.6, observe=observe)
print('😺 task 1 - 5 休眠，模拟装货')