From 0bb157240e665ed5e5044bd5f27419c562a6bce7 Mon Sep 17 00:00:00 2001 From: havoc420ubuntu <2993167370@qq.com> Date: Sat, 31 May 2025 22:01:48 +0000 Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=8F=20Adjust=20movement=20parameters?= =?UTF-8?q?=20in=20task=202=20and=20task=202.5=20for=20improved=20executio?= =?UTF-8?q?n,=20and=20enhance=20monitoring=20in=20task=203=20by=20adding?= =?UTF-8?q?=20target=20y-coordinate=20checks=20and=20refining=20output=20m?= =?UTF-8?q?essages.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- task_2/task_2.py | 2 +- task_2_5/task_2_5.py | 4 +-- task_3/task_3.py | 82 +++++++++++++------------------------------- 3 files changed, 26 insertions(+), 62 deletions(-) diff --git a/task_2/task_2.py b/task_2/task_2.py index 58ad678..90e15a7 100644 --- a/task_2/task_2.py +++ b/task_2/task_2.py @@ -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]) diff --git a/task_2_5/task_2_5.py b/task_2_5/task_2_5.py index 860b771..85bbab1 100644 --- a/task_2_5/task_2_5.py +++ b/task_2_5/task_2_5.py @@ -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) diff --git a/task_3/task_3.py b/task_3/task_3.py index 375b1db..8b40265 100644 --- a/task_3/task_3.py +++ b/task_3/task_3.py @@ -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秒", "站立")