Remove Gait_Params_up_full.toml file and update task_3.py and task_4.py to enhance navigation and control. Integrate go_lateral function for lateral movement, improve task execution with center_on_dual_tracks, and streamline file publishing for gait parameters. Update detection logic for horizontal lines and adjust iteration thresholds for better performance.
This commit is contained in:
parent
ba0994a82b
commit
b65b3e5b2e
File diff suppressed because it is too large
Load Diff
@ -1179,9 +1179,10 @@ def go_straight_until_hori_line(ctrl, msg, target_distance=0.5, max_distance=5.0
|
||||
edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=observe, save_log=True)
|
||||
elif detect_func_version == 3:
|
||||
edge_point, edge_info = detect_horizontal_track_edge_v3(image, observe=observe, save_log=True)
|
||||
elif detect_func_version == 4:
|
||||
edge_point, edge_info = detect_furthest_horizontal_intersection(image, observe=observe, save_log=True)
|
||||
else:
|
||||
error("未指定检测版本，请检查参数", "失败")
|
||||
edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True)
|
||||
|
||||
if edge_point is not None and edge_info is not None:
|
||||
line_detected = True
|
||||
@ -1197,7 +1198,7 @@ def go_straight_until_hori_line(ctrl, msg, target_distance=0.5, max_distance=5.0
|
||||
info(f"检测到横向线，当前距离: {current_distance:.3f}米, 目标距离: {target_distance:.3f}米", "距离")
|
||||
|
||||
# 判断是否达到目标距离
|
||||
if current_distance <= target_distance + 0.1: # 允许0.1米的误差
|
||||
if current_distance <= target_distance + 0.1: # additional_distance 额外距离
|
||||
success(f"已达到目标距离，当前距离: {current_distance:.3f}米", "完成")
|
||||
break
|
||||
|
||||
@ -1270,6 +1271,10 @@ def go_straight_until_hori_line(ctrl, msg, target_distance=0.5, max_distance=5.0
|
||||
edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=observe, save_log=True)
|
||||
elif detect_func_version == 3:
|
||||
edge_point, edge_info = detect_horizontal_track_edge_v3(image, observe=observe, save_log=True)
|
||||
elif detect_func_version == 4:
|
||||
edge_point, edge_info = detect_furthest_horizontal_intersection(image, observe=observe, save_log=True)
|
||||
else:
|
||||
error("未指定检测版本，请检查参数", "失败")
|
||||
|
||||
if edge_point is not None and edge_info is not None:
|
||||
final_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe)
|
||||
|
||||
@ -16,8 +16,9 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)))
|
||||
|
||||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||||
from base_move.turn_degree import turn_degree, turn_degree_v2
|
||||
from base_move.go_straight import go_straight
|
||||
from base_move.go_straight import go_straight, go_lateral
|
||||
from base_move.go_to_xy import go_to_x_v2
|
||||
from base_move.center_on_dual_tracks import center_on_dual_tracks
|
||||
from file_send_lcmt import file_send_lcmt
|
||||
from utils.yellow_area_analyzer import analyze_yellow_area_ratio
|
||||
|
||||
@ -41,7 +42,7 @@ robot_cmd = {
|
||||
|
||||
def pass_up_down(ctrl, msg):
|
||||
usergait_msg = file_send_lcmt()
|
||||
lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
|
||||
# lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
|
||||
|
||||
try:
|
||||
steps = toml.load("./task_3/Gait_Params_up.toml")
|
||||
@ -80,10 +81,10 @@ def pass_up_down(ctrl, msg):
|
||||
file_obj_gait_def = open("./task_3/Gait_Def_up.toml",'r')
|
||||
file_obj_gait_params = open("./task_3/Gait_Params_up_full.toml",'r')
|
||||
usergait_msg.data = file_obj_gait_def.read()
|
||||
lcm_usergait.publish("user_gait_file",usergait_msg.encode())
|
||||
ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
|
||||
time.sleep(0.5)
|
||||
usergait_msg.data = file_obj_gait_params.read()
|
||||
lcm_usergait.publish("user_gait_file",usergait_msg.encode())
|
||||
ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
|
||||
time.sleep(0.1)
|
||||
file_obj_gait_def.close()
|
||||
file_obj_gait_params.close()
|
||||
@ -100,8 +101,8 @@ def pass_up_down(ctrl, msg):
|
||||
stable_threshold = 8 # 连续15次检测z轴不再增加则认为已经停止
|
||||
z_speed_threshold = 0.01 # z轴速度阈值，小于这个值认为已经停止爬升
|
||||
climb_speed_threshold = 0.05 # 检测到开始爬坡的速度阈值
|
||||
max_iterations = 250 # 最大循环次数，作为安全保障
|
||||
min_iterations = 150 # 最小循环次数，作为安全保障
|
||||
max_iterations = 320 # 最大循环次数，作为安全保障
|
||||
min_iterations = 250 # 最小循环次数，作为安全保障
|
||||
|
||||
# 姿态判断参数
|
||||
pitch_threshold = 0.05 # 俯仰角阈值（弧度）
|
||||
@ -139,7 +140,7 @@ def pass_up_down(ctrl, msg):
|
||||
|
||||
# 每10次迭代打印一次当前信息
|
||||
if i % 10 == 0:
|
||||
info(f"当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
|
||||
info(f"Step:{i} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
|
||||
|
||||
# 检测是否开始爬坡阶段
|
||||
if not climbing_detected and vz > climb_speed_threshold:
|
||||
@ -189,6 +190,8 @@ def pass_up_down(ctrl, msg):
|
||||
# msg.vel_des = [0, 0.2, 0] # [前进速度, 侧向速度, 角速度]
|
||||
# msg.life_count += 1
|
||||
# ctrl.Send_cmd(msg)
|
||||
turn_degree_v2(ctrl, msg, 90, absolute=True)
|
||||
go_straight(ctrl, msg, distance=0.5, speed=0.5, observe=True)
|
||||
time.sleep(1)
|
||||
# go_to_x_v2(ctrl, msg, 2, speed=0.5, precision=True, observe=True)
|
||||
|
||||
@ -230,10 +233,10 @@ def pass_up_down(ctrl, msg):
|
||||
file_obj_gait_def = open("./task_3/Gait_Def_up.toml",'r')
|
||||
file_obj_gait_params = open("./task_3/Gait_Params_down_full.toml",'r')
|
||||
usergait_msg.data = file_obj_gait_def.read()
|
||||
lcm_usergait.publish("user_gait_file",usergait_msg.encode())
|
||||
ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
|
||||
time.sleep(0.5)
|
||||
usergait_msg.data = file_obj_gait_params.read()
|
||||
lcm_usergait.publish("user_gait_file",usergait_msg.encode())
|
||||
ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
|
||||
time.sleep(0.1)
|
||||
file_obj_gait_def.close()
|
||||
file_obj_gait_params.close()
|
||||
@ -250,8 +253,8 @@ def pass_up_down(ctrl, msg):
|
||||
stable_threshold = 10 # 连续10次检测z轴速度接近零则认为已经到达平地
|
||||
z_speed_threshold = 0.005 # z轴速度阈值，小于这个值认为已经停止下降
|
||||
descent_speed_threshold = -0.05 # 检测到开始下坡的速度阈值（负值表示下降）
|
||||
max_iterations = 250 # 最大循环次数，作为安全保障
|
||||
min_iterations = 150 # 最小循环次数，确保有足够的时间开始动作
|
||||
max_iterations = 120 # 最大循环次数，作为安全保障
|
||||
min_iterations = 80 # 最小循环次数，确保有足够的时间开始动作
|
||||
start_height = ctrl.odo_msg.xyz[2] # 记录起始高度
|
||||
|
||||
# 姿态判断参数
|
||||
@ -289,7 +292,7 @@ def pass_up_down(ctrl, msg):
|
||||
|
||||
# 每10次迭代打印一次当前信息
|
||||
if observe and i % 10 == 0:
|
||||
info(f"当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
|
||||
info(f"Step:{i} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
|
||||
|
||||
# 检测是否开始下坡阶段
|
||||
if not descending_detected and vz < descent_speed_threshold:
|
||||
@ -495,13 +498,18 @@ def go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3, max_
|
||||
|
||||
|
||||
def run_task_3(ctrl, msg):
|
||||
section('任务3：步态切换', "启动")
|
||||
section('任务3：上下坡', "启动")
|
||||
info('开始执行任务3...', "启动")
|
||||
|
||||
turn_degree_v2(ctrl, msg, 90, absolute=True)
|
||||
go_lateral(ctrl, msg, distance=0.2, speed=0.1, observe=True)
|
||||
|
||||
# section('任务3-1：up', "开始")
|
||||
# pass_up_down(ctrl, msg)
|
||||
section('任务3-1：up', "开始")
|
||||
pass_up_down(ctrl, msg)
|
||||
|
||||
section('任务3-2：center on dual tracks', "开始")
|
||||
turn_degree_v2(ctrl, msg, 90, absolute=True)
|
||||
center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, stone_path_mode=False)
|
||||
|
||||
section('任务3-2：yellow stop', "开始")
|
||||
go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3)
|
||||
|
||||
@ -11,10 +11,8 @@ from base_move.turn_degree import turn_degree, turn_degree_v2
|
||||
from base_move.go_straight import go_straight, go_lateral
|
||||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||||
from utils.gray_sky_analyzer import analyze_gray_sky_ratio
|
||||
from base_move.move_base_hori_line import go_straight_until_hori_line
|
||||
from base_move.follow_dual_tracks import go_straight_with_visual_track
|
||||
from base_move.center_on_dual_tracks import center_on_dual_tracks
|
||||
|
||||
from utils.detect_track import detect_horizontal_track_edge
|
||||
from base_move.move_base_hori_line import calculate_distance_to_line
|
||||
from task_4.pass_bar import pass_bar
|
||||
|
||||
# 创建本模块特定的日志记录器
|
||||
@ -52,7 +50,7 @@ def run_task_4_back(ctrl, msg):
|
||||
|
||||
# 向右移动0.5秒
|
||||
section('任务4-回程：向右移动', "移动")
|
||||
go_lateral(ctrl, msg, distance=-0.3, speed=0.1, observe=True)
|
||||
go_lateral(ctrl, msg, distance=-0.3, speed=0.1, observe=True) # DEBUG
|
||||
|
||||
section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测")
|
||||
go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.35, speed=0.5)
|
||||
@ -62,29 +60,16 @@ def run_task_4_back(ctrl, msg):
|
||||
|
||||
section('任务4-3：stone', "移动")
|
||||
go_straight(ctrl, msg, distance=1, speed=2)
|
||||
# go_straight_with_visual_track(ctrl, msg, distance=4.5, observe=False,
|
||||
# mode=11,
|
||||
# speed=0.35,
|
||||
# gait_id=3,
|
||||
# step_height=[0.21, 0.21],
|
||||
# )
|
||||
|
||||
go_straight(ctrl, msg, distance=4.5, speed=0.35, mode=11, gait_id=3, step_height=[0.21, 0.21])
|
||||
|
||||
section('任务4-3：前进直到遇到黄线 - 石板路', "移动")
|
||||
# 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处
|
||||
go_straight_until_hori_line(
|
||||
ctrl,
|
||||
msg,
|
||||
target_distance=1, # 目标与黄线的距离(米)
|
||||
max_distance=6.0, # 最大搜索距离(米)
|
||||
speed=0.2, # 移动速度(米/秒)
|
||||
mode=11, # 运动模式
|
||||
gait_id=3, # 步态ID（快步跑）
|
||||
step_height=[0.21, 0.21], # 摆动腿离地高度
|
||||
observe=True # 显示调试信息
|
||||
)
|
||||
|
||||
go_straight(ctrl, msg, distance=0.8, speed=0.2, observe=True, mode=11, gait_id=3, step_height=[0.21, 0.21])
|
||||
# 获取相机高度
|
||||
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)
|
||||
go_straight(ctrl, msg, distance=current_distance, speed=0.20, mode=11, gait_id=3, step_height=[0.21, 0.21])
|
||||
|
||||
|
||||
def go_straight_until_sky_ratio_below(ctrl, msg,
|
||||
|
||||
@ -16,6 +16,7 @@ from base_move.move_base_hori_line import (
|
||||
detect_horizontal_track_edge, detect_horizontal_track_edge_v2, detect_horizontal_track_edge_v3,
|
||||
calculate_distance_to_line, move_to_hori_line, arc_turn_around_hori_line
|
||||
)
|
||||
from base_move.center_on_dual_tracks import center_on_dual_tracks
|
||||
# from base_move.follow_dual_tracks import follow_dual_tracks
|
||||
|
||||
|
||||
@ -209,6 +210,8 @@ def run_task_5(ctrl, msg, direction='left', observe=False):
|
||||
"""
|
||||
走向卸货
|
||||
"""
|
||||
|
||||
center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, stone_path_mode=False)
|
||||
section('任务5-1：直线移动并扫描二维码', "移动")
|
||||
|
||||
# 最大移动距离为8米
|
||||
@ -228,6 +231,7 @@ def run_task_5(ctrl, msg, direction='left', observe=False):
|
||||
error("未能成功到达横线前指定距离", "失败")
|
||||
|
||||
section('任务5-2：移动到卸货点', "移动")
|
||||
center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, stone_path_mode=False)
|
||||
if direction == 'right' and res['qr_result'] == 'B-2' or direction == 'left' and res['qr_result'] == 'B-1':
|
||||
# 直走
|
||||
move_to_hori_line(ctrl, msg, target_distance=1.4, observe=observe, detect_func_version=4)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user