Refactor main execution flow by adding support for task 3 back functionality and adjusting task 1 and task 2 parameters. Update task 2.5 back execution logic and improve movement commands for better control. Set observe flag in track detection functions for consistency.
This commit is contained in:
parent
8ef996ee94
commit
4200fb8aeb
38
main.py
38
main.py
@ -22,7 +22,7 @@ from utils.base_msg import BaseMsg
|
||||
|
||||
from task_1.task_1 import run_task_1
|
||||
from task_2_5.task_2_5 import run_task_2_5
|
||||
from task_3.task_3 import run_task_3
|
||||
from task_3.task_3 import run_task_3, run_task_3_back
|
||||
from task_1.task_1 import run_task_1, run_task_1_back
|
||||
from task_2.task_2 import run_task_2, run_task_2_back
|
||||
from task_2_5.task_2_5 import run_task_2_5, run_task_2_5_back
|
||||
@ -43,37 +43,33 @@ def main():
|
||||
msg = robot_control_cmd_lcmt()
|
||||
|
||||
try:
|
||||
print("Recovery stand")
|
||||
info("Recovery stand", "info")
|
||||
Ctrl.base_msg.stand_up()
|
||||
Ctrl.base_msg.stop() # BUG 垃圾指令 for eat
|
||||
|
||||
# time.sleep(100) # TEST,
|
||||
run_task_1(Ctrl, msg)
|
||||
# run_task_1(Ctrl, msg)
|
||||
|
||||
arrow_direction = run_task_2(Ctrl, msg, xy_flag=False)
|
||||
# arrow_direction = run_task_2(Ctrl, msg, xy_flag=False)
|
||||
# arrow_direction = 'right' # TEST
|
||||
|
||||
info(f"识别到箭头方向: {arrow_direction}", "info")
|
||||
run_task_2_5(Ctrl, msg, direction=arrow_direction)
|
||||
# info(f"识别到箭头方向: {arrow_direction}", "info")
|
||||
# run_task_2_5(Ctrl, msg, direction=arrow_direction)
|
||||
|
||||
if arrow_direction == 'left':
|
||||
run_task_4(Ctrl, msg)
|
||||
else:
|
||||
run_task_3(Ctrl, msg)
|
||||
# if arrow_direction == 'left':
|
||||
# run_task_4(Ctrl, msg)
|
||||
# else:
|
||||
# run_task_3(Ctrl, msg)
|
||||
|
||||
turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
|
||||
run_task_5(Ctrl, msg, direction=arrow_direction)
|
||||
# turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
|
||||
# run_task_5(Ctrl, msg, direction=arrow_direction)
|
||||
|
||||
if arrow_direction == 'left':
|
||||
run_task_3_back(Ctrl, msg)
|
||||
else:
|
||||
run_task_4_back(Ctrl, msg)
|
||||
# if arrow_direction == 'left':
|
||||
# run_task_3_back(Ctrl, msg)
|
||||
# else:
|
||||
# run_task_4_back(Ctrl, msg)
|
||||
|
||||
return
|
||||
|
||||
run_task_5(Ctrl, msg)
|
||||
|
||||
run_task_2_5_back(Ctrl, msg, direction=arrow_direction)
|
||||
# run_task_2_5_back(Ctrl, msg, direction=arrow_direction)
|
||||
|
||||
run_task_2_back(Ctrl, msg)
|
||||
|
||||
|
||||
@ -13,6 +13,8 @@ from base_move.move_base_hori_line import (
|
||||
)
|
||||
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
|
||||
|
||||
# 创建本模块特定的日志记录器
|
||||
@ -134,11 +136,11 @@ def run_task_1(ctrl, msg):
|
||||
|
||||
def run_task_1_back(ctrl, msg):
|
||||
section('任务1-11: 返回', "移动")
|
||||
|
||||
go_straight(ctrl, msg, distance=0.2, observe=observe)
|
||||
turn_degree_v2(ctrl, msg, -90, absolute=True)
|
||||
|
||||
section('任务1-11: 直线移动', "移动")
|
||||
move_to_hori_line(ctrl, msg, target_distance=0.4, observe=observe)
|
||||
move_to_hori_line(ctrl, msg, target_distance=0.2, observe=observe)
|
||||
|
||||
section('任务1-12: 90度旋转', "旋转")
|
||||
turn_success, res = arc_turn_around_hori_line(
|
||||
@ -150,13 +152,14 @@ def run_task_1_back(ctrl, msg):
|
||||
max_radius=0.4,
|
||||
pass_align=True,
|
||||
observe=observe,
|
||||
no_end_reset=True,
|
||||
)
|
||||
|
||||
return
|
||||
turn_degree_v2(ctrl, msg, degree=90, absolute=True)
|
||||
|
||||
section('任务1-13: 直线移动', "移动")
|
||||
move_distance = 0.35
|
||||
go_straight_by_hori_line(ctrl, msg, distance=move_distance, observe=observe)
|
||||
move_distance = 0.5
|
||||
go_straight(ctrl, msg, distance=move_distance, observe=observe)
|
||||
|
||||
section('任务1-14: 模拟装货', "停止")
|
||||
info('机器人躺下，模拟装货过程', "信息")
|
||||
@ -169,27 +172,26 @@ def run_task_1_back(ctrl, msg):
|
||||
ctrl.base_msg.stand_up()
|
||||
|
||||
section('任务1-16: 返回', "移动")
|
||||
go_straight(ctrl, msg, distance=-(move_distance + res['radius'] + 0.3), observe=observe)
|
||||
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=90,
|
||||
radius=0.4,
|
||||
angle_deg=-85,
|
||||
radius=res['radius'] * 2,
|
||||
pass_align=True,
|
||||
observe=observe,
|
||||
)
|
||||
|
||||
section('任务1-18: 直线移动', "移动")
|
||||
move_to_hori_line(ctrl, msg, target_distance=0.35, observe=observe)
|
||||
move_to_hori_line(ctrl, msg, target_distance=0.4, observe=observe)
|
||||
|
||||
section('任务1-19: 90度旋转', "旋转")
|
||||
turn_success, res = arc_turn_around_hori_line(
|
||||
ctrl=ctrl,
|
||||
msg=msg,
|
||||
angle_deg=90,
|
||||
radius=0.4,
|
||||
)
|
||||
turn_degree_v2(ctrl, msg, degree=0, absolute=True)
|
||||
|
||||
go_to_xy(ctrl, msg, target_x=-0.2, target_y=0, observe=observe)
|
||||
|
||||
success("任务1-back完成", "完成")
|
||||
@ -249,7 +249,6 @@ def run_task_2(ctrl, msg, xy_flag=False):
|
||||
return arrow_direction
|
||||
|
||||
def run_task_2_back(ctrl, msg):
|
||||
# go_to_xy(ctrl, msg, 0.2, 1, speed=0.5, observe=True)
|
||||
go_to_xy(ctrl, msg, 0.7, 5, speed=0.5, observe=True)
|
||||
turn_degree_v2(ctrl, msg, -106.5, absolute=True)
|
||||
|
||||
|
||||
@ -6,7 +6,7 @@ import os
|
||||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
|
||||
from base_move.turn_degree import turn_degree, turn_degree_v2
|
||||
from base_move.go_to_xy import go_straight
|
||||
from base_move.go_to_xy import go_straight, go_to_x_v2, go_to_y_v2
|
||||
from base_move.move_base_hori_line import arc_turn_around_hori_line, align_to_horizontal_line
|
||||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||||
|
||||
@ -50,23 +50,6 @@ def run_task_2_5(Ctrl, msg, direction='left'):
|
||||
|
||||
def run_task_2_5_back(Ctrl, msg, direction='left'):
|
||||
section('任务2.5-back：预备进入任务3', "启动")
|
||||
turn_degree_v2(Ctrl, msg, degree=-90, absolute=True)
|
||||
|
||||
arc_turn_around_hori_line(
|
||||
Ctrl,
|
||||
msg,
|
||||
angle_deg=90 if direction == 'left' else -90,
|
||||
target_distance=0.3,
|
||||
detect_func_version=2,
|
||||
observe=observe,
|
||||
)
|
||||
|
||||
turn_degree(Ctrl, msg, degree=90, absolute=True)
|
||||
|
||||
# arc_turn_around_hori_line(
|
||||
# Ctrl,
|
||||
# msg,
|
||||
# angle_deg=70,
|
||||
# target_distance=0.5,
|
||||
# pass_align=True,
|
||||
# )
|
||||
|
||||
go_to_x_v2(Ctrl, msg, target_x=0.8, observe=observe)
|
||||
|
||||
@ -362,7 +362,7 @@ def detect_horizontal_track_edge_v2(image, observe=False, delay=1000, save_log=T
|
||||
edge_point: 赛道前方边缘点的坐标 (x, y)
|
||||
edge_info: 边缘信息字典
|
||||
"""
|
||||
# observe = False # TEST
|
||||
observe = False # TEST
|
||||
# 如果输入是字符串（文件路径），则加载图像
|
||||
if isinstance(image, str):
|
||||
img = cv2.imread(image)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user