207 lines
6.9 KiB
Python
Executable File
207 lines
6.9 KiB
Python
Executable File
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,
|
||
align_to_horizontal_line
|
||
)
|
||
from base_move.go_straight import go_straight
|
||
from base_move.turn_degree import turn_degree, turn_degree_twice, turn_degree_v2
|
||
from base_move.center_on_dual_tracks import center_on_dual_tracks
|
||
from base_move.go_to_xy import go_to_xy
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
from utils.speech_demo import speak
|
||
# 创建本模块特定的日志记录器
|
||
logger = get_logger("任务1")
|
||
|
||
observe = False # 控制是否输出更多调试信息
|
||
direction = True
|
||
|
||
def run_task_1(ctrl, msg, time_sleep=5000):
|
||
global direction
|
||
section('任务1：寻找横线并绕行', "启动")
|
||
info('开始执行任务1...', "启动")
|
||
|
||
# TEST better align
|
||
# aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
|
||
# print(aligned)
|
||
# go_straight(ctrl, msg, -10, observe=observe)
|
||
# return
|
||
|
||
# v2
|
||
section('任务1-1：转弯并扫描QR码', "移动")
|
||
# 在 arc_turn_around_hori_line 中启用 QR 码扫描
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
pass_align=True, # TEST 测试用，直接以 1.5 m 半径右转。
|
||
radius=1.5,
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=-85, # 负值表示右转 # INFO 目前来看，这个 -85 + 内部的微调，跟镜像里不断测试出来的效果好了很多，暂时可用。
|
||
#
|
||
observe=False, # INFO 暂时关闭观察
|
||
scan_qrcode=True, # 启用 QR 码扫描
|
||
qr_check_interval=0.3 # 每 0.3 秒检查一次扫描结果
|
||
)
|
||
|
||
if res and res.get('qr_result'):
|
||
success(f"在任务1-1中成功扫描到QR码: {res.get('qr_result')}", "扫描")
|
||
else:
|
||
warning("任务1-1中未能扫描到任何QR码", "警告")
|
||
|
||
section('任务1-2：移动到横线', "移动")
|
||
# 执行常规的移动操作
|
||
move_to_hori_line(ctrl, msg, target_distance=1.1, observe=observe)
|
||
|
||
section('任务1-3：转弯', "旋转")
|
||
direction = False if res.get('qr_result') == 'A-1' else True # TODO 需要检查一下，这个方向是否正确
|
||
|
||
# if direction == False :speak("A 区库位 1")
|
||
# else:speak("A 区库位 2")
|
||
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=-170 if direction else 170, # TODO need check
|
||
target_distance=0.6,
|
||
min_radius=0.3,
|
||
max_radius=0.4,
|
||
#
|
||
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
|
||
move_speed = 0.5
|
||
if not direction:
|
||
# TODO to check
|
||
# go_straight_by_hori_line(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)
|
||
turn_degree_v2(ctrl, msg, degree=90, absolute=True, precision=True)
|
||
go_straight(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)
|
||
else:
|
||
go_straight(ctrl, msg, distance=move_distance, speed=move_speed, observe=observe)
|
||
|
||
section('任务1-5：模拟装货', "停止")
|
||
info('机器人躺下，模拟装货过程', "信息")
|
||
start_time = time.time()
|
||
ctrl.base_msg.lie_down(wait_time=time_sleep)
|
||
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'] + 0.3), observe=observe)
|
||
|
||
# turn and back
|
||
# turn_degree_twice(ctrl, msg, degree=0, absolute=True, precision=True)
|
||
turn_degree_v2(ctrl, msg, degree=0, absolute=True, precision=True)
|
||
|
||
# add
|
||
go_straight(ctrl, msg, distance=0.3, observe=observe)
|
||
|
||
section('任务1-7：90度转弯', "旋转")
|
||
radius = res['radius'] * 2 + 0.1
|
||
info(f"任务1-7: 转弯半径: {radius}", "信息")
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
radius=radius,
|
||
angle_deg=85 if direction else -85,
|
||
#
|
||
pass_align=True,
|
||
observe=observe,
|
||
no_end_reset=True,
|
||
)
|
||
|
||
section('任务1-8：直线移动', "移动")
|
||
move_to_hori_line(ctrl, msg, target_distance=0.3, observe=observe)
|
||
|
||
section('任务1-9：90度旋转', "旋转")
|
||
turn_degree_v2(ctrl, msg, degree=0, absolute=True, precision=True)
|
||
|
||
section('任务1-10: y校准，准备 task-2', "移动")
|
||
# TODO
|
||
|
||
success("任务1完成", "完成")
|
||
|
||
|
||
def run_task_1_back(ctrl, msg, time_sleep=5000):
|
||
section('任务1-11: 返回', "移动")
|
||
go_straight(ctrl, msg, distance=0.2, observe=observe)
|
||
turn_degree_v2(ctrl, msg, -90, absolute=True) # 不确定 odo 效果如何；
|
||
|
||
section('任务1-11: 直线移动', "移动")
|
||
move_to_hori_line(ctrl, msg, target_distance=0.2, observe=observe)
|
||
|
||
section('任务1-12: 180度旋转', "旋转")
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=170 if direction else -170,
|
||
target_distance=0.6,
|
||
min_radius=0.3,
|
||
max_radius=0.4,
|
||
pass_align=True,
|
||
observe=observe,
|
||
no_end_reset=True,
|
||
)
|
||
|
||
turn_degree_v2(ctrl, msg, degree=90, absolute=True)
|
||
|
||
section('任务1-13: 直线移动', "移动")
|
||
move_distance = 0.5
|
||
go_straight(ctrl, msg, distance=move_distance, observe=observe)
|
||
|
||
section('任务1-14: 模拟装货', "停止")
|
||
info('机器人躺下，模拟装货过程', "信息")
|
||
start_time = time.time()
|
||
ctrl.base_msg.lie_down(wait_time=time_sleep)
|
||
elapsed = time.time() - start_time
|
||
timing("装货过程", elapsed)
|
||
|
||
section('任务1-15: 站起来', "移动")
|
||
ctrl.base_msg.stand_up()
|
||
|
||
section('任务1-16: 返回', "移动")
|
||
go_straight(ctrl, msg, distance=-(move_distance + res['radius']), observe=observe)
|
||
|
||
turn_degree_v2(ctrl, msg, degree=179, absolute=True)
|
||
|
||
section('任务1-17: 90度转弯', "旋转")
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=-85 if direction else 85,
|
||
radius=res['radius'] * 2,
|
||
pass_align=True,
|
||
observe=observe,
|
||
)
|
||
|
||
section('任务1-18: 直线移动', "移动")
|
||
move_to_hori_line(ctrl, msg, target_distance=0.4, observe=observe)
|
||
|
||
section('任务1-19: 90度旋转', "旋转")
|
||
turn_degree_v2(ctrl, msg, degree=0, absolute=True)
|
||
|
||
go_straight(ctrl, msg, distance=-1.3, observe=observe)
|
||
# go_to_xy(ctrl, msg, target_x=-0.2, target_y=0, observe=observe) # TEST
|
||
|
||
success("任务1-back完成", "完成")
|