Update TIME_SLEEP in main.py for longer pauses, enhance task 2 with new movement commands and logging, refine task 3 to include standing duration, and adjust task 4's movement distance for better performance. Improve task 5 by ensuring unloading and loading sections are executed correctly.
This commit is contained in:
parent
f61fcea650
commit
68cae0c85a
22
main.py
22
main.py
@ -37,7 +37,7 @@ from base_move.go_to_xy import go_to_x_v2, go_to_y_v2
|
||||
from utils.log_helper import info
|
||||
|
||||
pass_marker = True
|
||||
TIME_SLEEP = 3000
|
||||
TIME_SLEEP = 5000
|
||||
|
||||
def main():
|
||||
rclpy.init() # 新增：在主程序中统一初始化 ROS 2 上下文
|
||||
@ -51,18 +51,18 @@ def main():
|
||||
Ctrl.base_msg.stop() # BUG 垃圾指令 for eat
|
||||
|
||||
# time.sleep(100) # TEST,
|
||||
# run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP)
|
||||
run_task_1(Ctrl, msg, time_sleep=TIME_SLEEP)
|
||||
|
||||
# arrow_direction = run_task_2(Ctrl, msg, xy_flag=False)
|
||||
arrow_direction = 'right' # TEST
|
||||
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, time_sleep=TIME_SLEEP)
|
||||
if arrow_direction == 'left':
|
||||
run_task_4(Ctrl, msg)
|
||||
else:
|
||||
run_task_3(Ctrl, msg, time_sleep=TIME_SLEEP)
|
||||
|
||||
turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
|
||||
run_task_5(Ctrl, msg, direction=arrow_direction, time_sleep=TIME_SLEEP)
|
||||
@ -74,7 +74,7 @@ def main():
|
||||
|
||||
run_task_2_5_back(Ctrl, msg, direction=arrow_direction)
|
||||
|
||||
run_task_2_back(Ctrl, msg)
|
||||
run_task_2_back(Ctrl, msg, xy_flag=False)
|
||||
|
||||
run_task_1_back(Ctrl, msg, time_sleep=TIME_SLEEP)
|
||||
|
||||
|
||||
@ -13,6 +13,8 @@ from base_move.turn_degree import turn_degree, turn_degree_twice, turn_degree_v2
|
||||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||||
from utils.decode_arrow import detect_arrow_direction, visualize_arrow_detection
|
||||
|
||||
from base_move.go_straight import go_straight, go_lateral
|
||||
|
||||
from base_move.move_base_hori_line import (
|
||||
arc_turn_around_hori_line,
|
||||
go_straight_by_hori_line,
|
||||
@ -292,8 +294,13 @@ def run_task_2(ctrl, msg, xy_flag=False):
|
||||
# 返回检测到的箭头方向
|
||||
return arrow_direction
|
||||
|
||||
def run_task_2_back(ctrl, msg):
|
||||
def run_task_2_back(ctrl, msg,xy_flag=False):
|
||||
turn_degree_v2(ctrl, msg, 0, absolute=True)
|
||||
go_straight(ctrl, msg, distance=0.9, speed=0.20, mode=11, gait_id=3, step_height=[0.21, 0.21])
|
||||
go_to_xy(ctrl, msg, 0.72, -11.1, speed=0.5, observe=True)
|
||||
print(ctrl.odo_msg.xyz[0], 'ctrl.odo_msg.xyz[0]')
|
||||
print(ctrl.odo_msg.xyz[1], 'ctrl.odo_msg.xyz[1]')
|
||||
print(ctrl.odo_msg.rpy[2], 'ctrl.odo_msg.rpy[2]')
|
||||
turn_degree_v2(ctrl, msg, -106.5, absolute=True)
|
||||
|
||||
print('角度为',ctrl.odo_msg.rpy[2])
|
||||
@ -325,6 +332,19 @@ def run_task_2_back(ctrl, msg):
|
||||
(0.4,0,0,0,0,0,1.5),
|
||||
] # [vel_x, vel_z] 对应 [左转, 右转, 左转]
|
||||
|
||||
gotoxy=[
|
||||
(0.76,-11),
|
||||
(0.71,-11),
|
||||
(1.12,-12),
|
||||
(1.49,-12.1),
|
||||
(1.1,-13.5),
|
||||
(0.74,-13.3),
|
||||
(0.68,-14.5),
|
||||
(0.97,-14.3),
|
||||
(1.69,-15.5),
|
||||
(1.59,-15.6),
|
||||
]
|
||||
|
||||
for i, (vel_x, vel_y, vel_z ,rpy_x,rpy_y,rpy_z,t) in enumerate(directions):
|
||||
# 设置转向命令
|
||||
msg.mode = 11 # Locomotion
|
||||
@ -335,6 +355,12 @@ def run_task_2_back(ctrl, msg):
|
||||
msg.step_height = [0.03, 0.03]
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
if xy_flag:
|
||||
go_to_xy(ctrl, msg, gotoxy[i][0], gotoxy[i][1], speed=0.5, observe=True)
|
||||
print('角度为',ctrl.odo_msg.rpy[2])
|
||||
print('x为',ctrl.odo_msg.xyz[0])
|
||||
print('y为',ctrl.odo_msg.xyz[1])
|
||||
print('z为',ctrl.odo_msg.xyz[2])
|
||||
|
||||
# 打印当前方向
|
||||
print(f"开始 {text[i]} ")
|
||||
|
||||
@ -437,9 +437,27 @@ def run_task_3(ctrl, msg, time_sleep=5000):
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
info("开始原地站立3秒", "站立")
|
||||
# time.sleep(time_sleep / 1000)
|
||||
time.sleep(time_sleep / 1000)
|
||||
info("完成原地站立", "站立")
|
||||
|
||||
|
||||
def run_task_3_back(ctrl, msg):
|
||||
return
|
||||
def run_task_3_back(ctrl, msg, time_sleep=5000):
|
||||
go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3)
|
||||
|
||||
# 原地站立3秒
|
||||
section("原地站立3秒", "站立")
|
||||
msg.mode = 12
|
||||
msg.gait_id = 0
|
||||
msg.duration = 0
|
||||
msg.step_height = [0.06, 0.06]
|
||||
msg.vel_des = [0, 0, 0]
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
info("开始原地站立3秒", "站立")
|
||||
time.sleep(time_sleep / 1000)
|
||||
info("完成原地站立", "站立")
|
||||
|
||||
section('任务3-1：up', "开始")
|
||||
pass_up_down(ctrl, msg)
|
||||
|
||||
@ -17,6 +17,7 @@ from utils.detect_dual_track_lines import detect_dual_track_lines
|
||||
from base_move.move_base_hori_line import calculate_distance_to_line
|
||||
from task_4.pass_bar import pass_bar
|
||||
from base_move.center_on_dual_tracks import center_on_dual_tracks
|
||||
from base_move.go_to_xy import go_to_xy, go_to_xy_v2
|
||||
|
||||
# 创建本模块特定的日志记录器
|
||||
logger = get_logger("任务4")
|
||||
@ -54,7 +55,7 @@ def run_task_4_back(ctrl, msg):
|
||||
ctrl.reset_offset()
|
||||
time.sleep(0.5)
|
||||
|
||||
go_straight(ctrl, msg, distance=3, speed=1, observe=True)
|
||||
go_straight(ctrl, msg, distance=2.5, speed=1, observe=True)
|
||||
center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False)
|
||||
|
||||
# 向右移动0.5秒
|
||||
@ -64,7 +65,7 @@ def run_task_4_back(ctrl, msg):
|
||||
turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
|
||||
|
||||
section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测")
|
||||
go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.35, speed=0.3)
|
||||
go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.35, speed=0.3, max_distance=1)
|
||||
|
||||
section('任务4-2：通过栏杆', "移动")
|
||||
turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
|
||||
@ -87,13 +88,20 @@ def run_task_4_back(ctrl, msg):
|
||||
camera_height = 0.355 # 单位: 米 # INFO from TF-tree
|
||||
edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=True, save_log=True)
|
||||
current_distance = calculate_distance_to_line(edge_info, camera_height, observe=True)
|
||||
# if current_distance < 1.5:
|
||||
# go_straight(ctrl, msg, distance=current_distance, speed=0.20, mode=11, gait_id=3, step_height=[0.21, 0.21])
|
||||
# else:
|
||||
# go_to_xy(ctrl, msg, 0, -10, speed=0.5, observe=True)
|
||||
go_straight(ctrl, msg, distance=current_distance, speed=0.20, mode=11, gait_id=3, step_height=[0.21, 0.21])
|
||||
|
||||
print(ctrl.odo_msg.xyz[0], 'ctrl.odo_msg.xyz[0]')
|
||||
print(ctrl.odo_msg.xyz[1], 'ctrl.odo_msg.xyz[1]')
|
||||
print(ctrl.odo_msg.rpy[2], 'ctrl.odo_msg.rpy[2]')
|
||||
|
||||
def go_straight_until_sky_ratio_below(ctrl, msg,
|
||||
sky_ratio_threshold=0.2,
|
||||
step_distance=0.5,
|
||||
max_distance=5,
|
||||
max_distance=3,
|
||||
speed=0.3
|
||||
):
|
||||
"""
|
||||
|
||||
@ -237,9 +237,9 @@ def run_task_5(ctrl, msg, direction='left', observe=False, time_sleep=5000):
|
||||
else:
|
||||
move_to_hori_line(ctrl, msg, target_distance=2, observe=observe, detect_func_version=4)
|
||||
|
||||
# section('任务5-3：卸货', "卸货")
|
||||
# ctrl.base_msg.lie_down(wait_time=time_sleep)
|
||||
# ctrl.base_msg.stand_up()
|
||||
section('任务5-3：卸货', "卸货")
|
||||
ctrl.base_msg.lie_down(wait_time=time_sleep)
|
||||
ctrl.base_msg.stand_up()
|
||||
|
||||
section('任务5-4：返回', "移动")
|
||||
go_straight(ctrl, msg, distance=-1, speed=0.5, observe=observe)
|
||||
@ -252,9 +252,9 @@ def run_task_5(ctrl, msg, direction='left', observe=False, time_sleep=5000):
|
||||
section('任务5-6：转弯', "转弯")
|
||||
arc_turn_around_hori_line(ctrl, msg, angle_deg=-85, target_distance=0.3, observe=observe, no_end_reset=True)
|
||||
|
||||
# section('任务5-5：上货', "卸货")
|
||||
# ctrl.base_msg.lie_down(wait_time=SLEEP_TIME)
|
||||
# ctrl.base_msg.stand_up()
|
||||
section('任务5-5：上货', "卸货")
|
||||
ctrl.base_msg.lie_down(wait_time=SLEEP_TIME)
|
||||
ctrl.base_msg.stand_up()
|
||||
|
||||
section('任务5-7：返回', "移动")
|
||||
turn_degree_v2(ctrl, msg, degree=-90, absolute=True)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user