110 lines
3.5 KiB
Python
110 lines
3.5 KiB
Python
import time
|
||
import sys
|
||
import os
|
||
import math
|
||
|
||
# 添加父目录到路径，以便能够导入utils
|
||
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
|
||
)
|
||
from base_move.go_straight import go_straight
|
||
from base_move.turn_degree import turn_degree
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
|
||
# 创建本模块特定的日志记录器
|
||
logger = get_logger("任务1")
|
||
|
||
observe = True
|
||
|
||
def run_task_1(ctrl, msg):
|
||
section('任务1：寻找横线并绕行', "启动")
|
||
info('开始执行任务1...', "启动")
|
||
|
||
# v2
|
||
section('任务1-1：转弯并扫描QR码', "移动")
|
||
# 在 arc_turn_around_hori_line 中启用 QR 码扫描
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=-85, # 负值表示右转
|
||
observe=observe,
|
||
scan_qrcode=True, # 启用 QR 码扫描
|
||
qr_check_interval=0.3 # 每 0.3 秒检查一次扫描结果
|
||
)
|
||
|
||
if res and res['qr_result']:
|
||
success(f"在任务1-1中成功扫描到QR码: {res['qr_result']}", "扫描")
|
||
else:
|
||
warning("任务1-1中未能扫描到任何QR码", "警告")
|
||
|
||
section('任务1-2：移动到横线', "移动")
|
||
# 执行常规的移动操作，不需要 QR 码扫描
|
||
move_to_hori_line(ctrl, msg, target_distance=0.8, observe=observe)
|
||
|
||
section('任务1-3：转弯', "旋转")
|
||
direction = False # if res['qr_result'] == 'A-2' else True
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=-170, # 负值表示右转
|
||
target_distance=0.5, # TODO 优化这里的参数
|
||
pass_align=True,
|
||
observe=observe,
|
||
# TODO clear
|
||
bad_big_angle_corret=True
|
||
)
|
||
|
||
if turn_success:
|
||
success("任务1-3完成", "完成")
|
||
else:
|
||
warning("任务1-3失败", "警告")
|
||
return
|
||
|
||
section('任务1-4：直线移动', "移动")
|
||
move_distance = 0.3
|
||
if direction:
|
||
# TODO 这里的校准不一定好用，需要优化
|
||
go_straight_by_hori_line(ctrl, msg, distance=move_distance, speed=0.5, observe=observe)
|
||
else:
|
||
go_straight(ctrl, msg, distance=move_distance, speed=0.5, observe=observe)
|
||
|
||
section('任务1-5：模拟装货', "停止")
|
||
info('机器人躺下，模拟装货过程', "信息")
|
||
start_time = time.time()
|
||
ctrl.base_msg.lie_down(wait_time=3000) # TODO 比赛时改成 5s
|
||
elapsed = time.time() - start_time
|
||
timing("装货过程", elapsed)
|
||
|
||
# 站起来
|
||
info('机器人站起来，准备继续任务', "信息")
|
||
ctrl.base_msg.stand_up()
|
||
|
||
section('任务1-6：返回', "移动")
|
||
go_straight(ctrl, msg, distance=-(move_distance + res['radius']), speed=0.5, observe=observe)
|
||
|
||
# turn and back
|
||
turn_degree(ctrl, msg, degree=-90, absolute=True)
|
||
|
||
section('任务1-7：90度转弯', "旋转")
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=85, # 正值表示左转
|
||
observe=observe
|
||
)
|
||
|
||
section('任务1-8：直线移动', "移动")
|
||
move_to_hori_line(ctrl, msg, target_distance=0.4, observe=observe)
|
||
|
||
section('任务1-9：90度旋转', "旋转")
|
||
turn_degree(ctrl, msg, degree=90, absolute=True)
|
||
|
||
section('任务1-10: y校准，准备 task-2', "移动")
|
||
# TODO
|
||
|
||
success("任务1完成", "完成")
|
||
|