🎏 Adjust movement parameters in task 2 and task 2.5 for improved execution, and enhance monitoring in task 3 by adding target y-coordinate checks and refining output messages.
This commit is contained in:
parent
56c4131fa4
commit
0bb157240e
@ -293,7 +293,7 @@ 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.7, 5, speed=0.5, observe=True)
|
||||
go_to_xy(ctrl, msg, 0.7, 4.8, speed=0.5, observe=True)
|
||||
turn_degree_v2(ctrl, msg, -106.5, absolute=True)
|
||||
|
||||
print('角度为',ctrl.odo_msg.rpy[2])
|
||||
|
||||
@ -28,7 +28,7 @@ def run_task_2_5(Ctrl, msg, direction='left'):
|
||||
target_distance=0.2,
|
||||
detect_func_version=3,
|
||||
pass_align=True,
|
||||
radius=0.55,
|
||||
radius=0.6,
|
||||
observe=observe,
|
||||
no_end_reset=True,
|
||||
)
|
||||
@ -40,4 +40,4 @@ def run_task_2_5_back(Ctrl, msg, direction='left'):
|
||||
section('任务2.5-back：预备进入任务3', "启动")
|
||||
turn_degree_v2(Ctrl, msg, degree=0, absolute=True)
|
||||
go_to_x_v2(Ctrl, msg, target_x=1, observe=observe)
|
||||
go_to_y_v2(Ctrl, msg, target_y=5.5, speed=0.5, observe=True)
|
||||
go_to_y_v2(Ctrl, msg, target_y=5.2, speed=0.5, observe=True)
|
||||
|
||||
@ -101,8 +101,9 @@ def pass_up_down(ctrl, msg):
|
||||
stable_threshold = 8 # 连续15次检测z轴不再增加则认为已经停止
|
||||
z_speed_threshold = 0.01 # z轴速度阈值，小于这个值认为已经停止爬升
|
||||
climb_speed_threshold = 0.05 # 检测到开始爬坡的速度阈值
|
||||
max_iterations = 230 # 最大循环次数，作为安全保障
|
||||
max_iterations = 280 # 最大循环次数，作为安全保障
|
||||
min_iterations = 170 # 最小循环次数，作为安全保障
|
||||
y_target = 7.5 # 目标y坐标，达到此值时停止
|
||||
|
||||
# 姿态判断参数
|
||||
pitch_threshold = 0.05 # 俯仰角阈值（弧度）
|
||||
@ -118,7 +119,8 @@ def pass_up_down(ctrl, msg):
|
||||
|
||||
# 记录起始姿态和高度
|
||||
start_height = ctrl.odo_msg.xyz[2]
|
||||
info(f"开始监测爬坡过程，初始高度: {start_height:.3f}", "监测")
|
||||
start_y = ctrl.odo_msg.xyz[1]
|
||||
info(f"开始监测爬坡过程，初始高度: {start_height:.3f}, 初始Y坐标: {start_y:.3f}", "监测")
|
||||
|
||||
for i in range(max_iterations):
|
||||
# 发送控制命令维持心跳
|
||||
@ -127,6 +129,7 @@ def pass_up_down(ctrl, msg):
|
||||
# 获取当前状态数据
|
||||
vz = ctrl.odo_msg.vxyz[2] # Z轴速度
|
||||
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速度
|
||||
@ -140,38 +143,17 @@ def pass_up_down(ctrl, msg):
|
||||
|
||||
# 每10次迭代打印一次当前信息
|
||||
if i % 10 == 0:
|
||||
info(f"Step:{i} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
|
||||
info(f"Step:{i} 当前Y坐标={current_y:.3f}, 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}", "监测")
|
||||
|
||||
# 检测是否开始爬坡阶段
|
||||
if not climbing_detected and vz > climb_speed_threshold:
|
||||
climbing_detected = True
|
||||
info(f"检测到开始爬坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测")
|
||||
|
||||
# 多条件判断是否完成爬坡
|
||||
if i > min_iterations and climbing_detected and len(height_window) == window_size:
|
||||
# 计算高度和俯仰角的稳定性
|
||||
height_std = np.std(height_window) # 高度标准差
|
||||
pitch_std = np.std(pitch_window) # 俯仰角标准差
|
||||
|
||||
# 多条件综合判断
|
||||
position_stable = abs(vz) < z_speed_threshold # 垂直速度稳定
|
||||
attitude_stable = abs(current_pitch) < pitch_threshold # 俯仰角接近水平
|
||||
angular_rate_stable = abs(pitch_rate) < angular_rate_threshold # 角速度稳定
|
||||
height_stable = height_std < 0.01 # 高度变化小
|
||||
pitch_stable = pitch_std < 0.01 # 俯仰角变化小
|
||||
vbody_stable = abs(vbody_z) < 0.01 # 机体Z方向速度稳定
|
||||
|
||||
# 综合判断条件
|
||||
if (position_stable and attitude_stable and angular_rate_stable) or \
|
||||
(position_stable and height_stable and pitch_stable) or \
|
||||
(vbody_stable and attitude_stable and height_stable):
|
||||
stable_count += 1
|
||||
if stable_count >= stable_threshold:
|
||||
info(f"检测到已完成爬坡：\n - Z轴速度: {vz:.3f}\n - 俯仰角: {current_pitch:.3f}\n - 角速度: {pitch_rate:.3f}\n - 当前高度: {current_height:.3f}\n - 上升了: {current_height - start_height:.3f}米", "监测")
|
||||
break
|
||||
else:
|
||||
# 重置稳定计数
|
||||
stable_count = 0
|
||||
# 检查是否达到目标y坐标
|
||||
if current_y >= y_target:
|
||||
info(f"已达到目标Y坐标: {current_y:.3f}，停止爬坡", "监测")
|
||||
break
|
||||
|
||||
time.sleep(0.2)
|
||||
except KeyboardInterrupt:
|
||||
@ -185,7 +167,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.5, speed=0.5, observe=True)
|
||||
go_straight(ctrl, msg, distance=0.3, speed=0.5, observe=True)
|
||||
time.sleep(1)
|
||||
|
||||
section('任务3-3：down', "完成")
|
||||
@ -249,6 +231,8 @@ def pass_up_down(ctrl, msg):
|
||||
max_iterations = 110 # 最大循环次数，作为安全保障
|
||||
min_iterations = 80 # 最小循环次数，确保有足够的时间开始动作
|
||||
start_height = ctrl.odo_msg.xyz[2] # 记录起始高度
|
||||
start_y = ctrl.odo_msg.xyz[1] # 记录起始y坐标
|
||||
y_target = 10.3 # 目标y坐标，达到此值时停止
|
||||
|
||||
# 姿态判断参数
|
||||
pitch_threshold = 0.05 # 俯仰角阈值（弧度）
|
||||
@ -263,7 +247,7 @@ def pass_up_down(ctrl, msg):
|
||||
pitch_window = []
|
||||
window_size = 8
|
||||
|
||||
info(f"开始监测下坡过程，初始高度: {start_height}", "监测")
|
||||
info(f"开始监测下坡过程，初始高度: {start_height}, 初始Y坐标: {start_y:.3f}", "监测")
|
||||
|
||||
for i in range(max_iterations):
|
||||
# 发送控制命令维持心跳
|
||||
@ -272,6 +256,7 @@ def pass_up_down(ctrl, msg):
|
||||
# 获取当前状态数据
|
||||
vz = ctrl.odo_msg.vxyz[2] # Z轴速度
|
||||
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速度
|
||||
@ -285,44 +270,23 @@ def pass_up_down(ctrl, msg):
|
||||
|
||||
# 每10次迭代打印一次当前信息
|
||||
if observe and i % 10 == 0:
|
||||
info(f"Step:{i} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
|
||||
info(f"Step:{i} 当前Y坐标={current_y:.3f}, 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}", "监测")
|
||||
|
||||
# 检测是否开始下坡阶段
|
||||
if not descending_detected and vz < descent_speed_threshold:
|
||||
descending_detected = True
|
||||
info(f"检测到开始下坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测")
|
||||
|
||||
# 多条件判断是否到达平地
|
||||
if i > min_iterations and descending_detected and len(height_window) == window_size:
|
||||
# 计算高度和俯仰角的稳定性
|
||||
height_std = np.std(height_window) # 高度标准差
|
||||
pitch_std = np.std(pitch_window) # 俯仰角标准差
|
||||
|
||||
# 多条件综合判断
|
||||
position_stable = abs(vz) < z_speed_threshold # 垂直速度稳定
|
||||
attitude_stable = abs(current_pitch) < pitch_threshold # 俯仰角接近水平
|
||||
angular_rate_stable = abs(pitch_rate) < angular_rate_threshold # 角速度稳定
|
||||
height_stable = height_std < 0.01 # 高度变化小
|
||||
pitch_stable = pitch_std < 0.01 # 俯仰角变化小
|
||||
vbody_stable = abs(vbody_z) < 0.01 # 机体Z方向速度稳定
|
||||
|
||||
# 综合判断条件
|
||||
if (position_stable and attitude_stable and angular_rate_stable) or \
|
||||
(position_stable and height_stable and pitch_stable) or \
|
||||
(vbody_stable and attitude_stable and height_stable):
|
||||
stable_count += 1
|
||||
if stable_count >= stable_threshold:
|
||||
info(f"检测到已到达平地：\n - Z轴速度: {vz:.3f}\n - 俯仰角: {current_pitch:.3f}\n - 角速度: {pitch_rate:.3f}\n - 高度: {current_height:.3f}\n - 下降了: {start_height - current_height:.3f}米", "监测")
|
||||
flat_ground_detected = True
|
||||
break
|
||||
else:
|
||||
# 重置稳定计数
|
||||
stable_count = 0
|
||||
# 检查是否达到目标y坐标
|
||||
if current_y >= y_target:
|
||||
info(f"已达到目标Y坐标: {current_y:.3f}，停止下坡", "监测")
|
||||
flat_ground_detected = True
|
||||
break
|
||||
|
||||
time.sleep(0.2)
|
||||
|
||||
if not flat_ground_detected:
|
||||
info(f"达到最大循环次数，未能明确检测到到达平地。当前高度: {ctrl.odo_msg.xyz[2]:.3f}", "警告")
|
||||
info(f"达到最大循环次数，未能到达目标Y坐标。当前Y坐标: {ctrl.odo_msg.xyz[1]:.3f}", "警告")
|
||||
except KeyboardInterrupt:
|
||||
msg.mode = 7 #PureDamper before KeyboardInterrupt:
|
||||
msg.gait_id = 0
|
||||
@ -503,7 +467,7 @@ def run_task_3(ctrl, msg, time_sleep=5000):
|
||||
center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False)
|
||||
|
||||
section('任务3-2：yellow stop', "开始")
|
||||
# go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3)
|
||||
go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3)
|
||||
|
||||
# 原地站立3秒
|
||||
section("原地站立3秒", "站立")
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user