Refactor task 3 to enhance execution flow by removing unused parameters and adjusting movement commands. Update task 2.5 to include a new turning command and refine distance parameters for improved performance. Disable logging in dual track detection for cleaner output.
This commit is contained in:
parent
0bb157240e
commit
6015ab6552
@ -14,11 +14,9 @@ observe = True
|
||||
|
||||
def run_task_2_5(Ctrl, msg, direction='left'):
|
||||
section('任务2.5：预备进入任务3', "启动")
|
||||
turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
|
||||
# go_straight(Ctrl, msg, distance=-0.1, speed=0.5, observe=observe)
|
||||
|
||||
# TEST
|
||||
turn_degree_v2(Ctrl, msg, 90, absolute=observe)
|
||||
|
||||
section('任务2.5-1：第一次旋转', "移动")
|
||||
|
||||
turn_success, res = arc_turn_around_hori_line(
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -42,7 +42,6 @@ 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")
|
||||
|
||||
try:
|
||||
steps = toml.load("./task_3/Gait_Params_up.toml")
|
||||
@ -96,27 +95,13 @@ def pass_up_down(ctrl, msg):
|
||||
msg.duration = 1000
|
||||
msg.life_count += 1
|
||||
|
||||
# 参数设置
|
||||
stable_count = 0 # 用于计数z轴稳定的次数
|
||||
stable_threshold = 8 # 连续15次检测z轴不再增加则认为已经停止
|
||||
z_speed_threshold = 0.01 # z轴速度阈值，小于这个值认为已经停止爬升
|
||||
climb_speed_threshold = 0.05 # 检测到开始爬坡的速度阈值
|
||||
max_iterations = 280 # 最大循环次数，作为安全保障
|
||||
min_iterations = 170 # 最小循环次数，作为安全保障
|
||||
y_target = 7.5 # 目标y坐标，达到此值时停止
|
||||
|
||||
# 姿态判断参数
|
||||
pitch_threshold = 0.05 # 俯仰角阈值（弧度）
|
||||
angular_rate_threshold = 0.03 # 角速度阈值（弧度/秒）
|
||||
max_iterations = 400 # 最大循环次数，作为安全保障
|
||||
y_target = 7.8 # 目标y坐标，达到此值时停止
|
||||
|
||||
# 阶段控制
|
||||
climbing_detected = False # 是否检测到正在爬坡
|
||||
|
||||
# 高度变化记录
|
||||
height_window = []
|
||||
pitch_window = []
|
||||
window_size = 8
|
||||
|
||||
# 记录起始姿态和高度
|
||||
start_height = ctrl.odo_msg.xyz[2]
|
||||
start_y = ctrl.odo_msg.xyz[1]
|
||||
@ -131,15 +116,6 @@ def pass_up_down(ctrl, msg):
|
||||
current_height = ctrl.odo_msg.xyz[2] # 当前高度
|
||||
current_y = ctrl.odo_msg.xyz[1] # 当前y坐标
|
||||
current_pitch = ctrl.odo_msg.rpy[1] # 当前俯仰角
|
||||
pitch_rate = ctrl.odo_msg.omegaBody[1] # 俯仰角速度
|
||||
vbody_z = ctrl.odo_msg.vBody[2] # 机体坐标系Z速度
|
||||
|
||||
# 更新滑动窗口数据
|
||||
height_window.append(current_height)
|
||||
pitch_window.append(current_pitch)
|
||||
if len(height_window) > window_size:
|
||||
height_window.pop(0)
|
||||
pitch_window.pop(0)
|
||||
|
||||
# 每10次迭代打印一次当前信息
|
||||
if i % 10 == 0:
|
||||
@ -167,7 +143,7 @@ def pass_up_down(ctrl, msg):
|
||||
section('任务3-2：x = 2', "开始")
|
||||
turn_degree_v2(ctrl, msg, 90, absolute=True)
|
||||
center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False, detect_height=0.3)
|
||||
go_straight(ctrl, msg, distance=0.3, speed=0.5, observe=True)
|
||||
go_straight(ctrl, msg, distance=0.6, speed=0.5, observe=True)
|
||||
time.sleep(1)
|
||||
|
||||
section('任务3-3：down', "完成")
|
||||
@ -224,29 +200,16 @@ def pass_up_down(ctrl, msg):
|
||||
msg.life_count += 1
|
||||
|
||||
# 参数设置
|
||||
stable_count = 0 # 用于计数z轴稳定的次数
|
||||
stable_threshold = 10 # 连续10次检测z轴速度接近零则认为已经到达平地
|
||||
z_speed_threshold = 0.005 # z轴速度阈值，小于这个值认为已经停止下降
|
||||
descent_speed_threshold = -0.05 # 检测到开始下坡的速度阈值（负值表示下降）
|
||||
max_iterations = 110 # 最大循环次数，作为安全保障
|
||||
min_iterations = 80 # 最小循环次数，确保有足够的时间开始动作
|
||||
max_iterations = 200 # 最大循环次数，作为安全保障
|
||||
start_height = ctrl.odo_msg.xyz[2] # 记录起始高度
|
||||
start_y = ctrl.odo_msg.xyz[1] # 记录起始y坐标
|
||||
y_target = 10.3 # 目标y坐标，达到此值时停止
|
||||
|
||||
# 姿态判断参数
|
||||
pitch_threshold = 0.05 # 俯仰角阈值（弧度）
|
||||
angular_rate_threshold = 0.03 # 角速度阈值（弧度/秒）
|
||||
y_target = 10.2 # 目标y坐标，达到此值时停止
|
||||
|
||||
# 阶段控制
|
||||
descending_detected = False # 是否检测到正在下坡
|
||||
flat_ground_detected = False # 是否检测到已到达平地
|
||||
|
||||
# 高度变化记录
|
||||
height_window = []
|
||||
pitch_window = []
|
||||
window_size = 8
|
||||
|
||||
info(f"开始监测下坡过程，初始高度: {start_height}, 初始Y坐标: {start_y:.3f}", "监测")
|
||||
|
||||
for i in range(max_iterations):
|
||||
@ -258,16 +221,7 @@ def pass_up_down(ctrl, msg):
|
||||
current_height = ctrl.odo_msg.xyz[2] # 当前高度
|
||||
current_y = ctrl.odo_msg.xyz[1] # 当前y坐标
|
||||
current_pitch = ctrl.odo_msg.rpy[1] # 当前俯仰角
|
||||
pitch_rate = ctrl.odo_msg.omegaBody[1] # 俯仰角速度
|
||||
vbody_z = ctrl.odo_msg.vBody[2] # 机体坐标系Z速度
|
||||
|
||||
# 更新滑动窗口数据
|
||||
height_window.append(current_height)
|
||||
pitch_window.append(current_pitch)
|
||||
if len(height_window) > window_size:
|
||||
height_window.pop(0)
|
||||
pitch_window.pop(0)
|
||||
|
||||
|
||||
# 每10次迭代打印一次当前信息
|
||||
if observe and i % 10 == 0:
|
||||
info(f"Step:{i} 当前Y坐标={current_y:.3f}, 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}", "监测")
|
||||
@ -459,6 +413,7 @@ def run_task_3(ctrl, msg, time_sleep=5000):
|
||||
info('开始执行任务3...', "启动")
|
||||
|
||||
turn_degree_v2(ctrl, msg, 90, absolute=True)
|
||||
go_straight(ctrl, msg, distance=0.1, speed=0.5, observe=True)
|
||||
|
||||
section('任务3-1：up', "开始")
|
||||
pass_up_down(ctrl, msg)
|
||||
|
||||
@ -11,6 +11,7 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True,
|
||||
min_line_length=0.05,
|
||||
max_line_gap=40,
|
||||
detect_height=0.4):
|
||||
save_log = False
|
||||
"""
|
||||
检测左右两条平行的黄色轨道线，优化后能够更准确处理各种路况
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user