import time import sys import os import toml import copy import math import lcm import numpy as np import cv2 import tempfile # 添加父目录到路径，以便能够导入utils sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) # 添加当前目录到路径，确保可以找到local文件 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, 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 # 创建本模块特定的日志记录器 logger = get_logger("任务3") observe = True robot_cmd = { 'mode':0, 'gait_id':0, 'contact':0, 'life_count':0, 'vel_des':[0.0, 0.0, 0.0], 'rpy_des':[0.0, 0.0, 0.0], 'pos_des':[0.0, 0.0, 0.0], 'acc_des':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'ctrl_point':[0.0, 0.0, 0.0], 'foot_pose':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], 'step_height':[0.0, 0.0], 'value':0, 'duration':0 } 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") full_steps = {'step':[robot_cmd]} k =0 for i in steps['step']: cmd = copy.deepcopy(robot_cmd) cmd['duration'] = i['duration'] if i['type'] == 'usergait': cmd['mode'] = 11 # LOCOMOTION cmd['gait_id'] = 110 # USERGAIT cmd['vel_des'] = i['body_vel_des'] cmd['rpy_des'] = i['body_pos_des'][0:3] cmd['pos_des'] = i['body_pos_des'][3:6] cmd['foot_pose'][0:2] = i['landing_pos_des'][0:2] cmd['foot_pose'][2:4] = i['landing_pos_des'][3:5] cmd['foot_pose'][4:6] = i['landing_pos_des'][6:8] cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11] cmd['step_height'][0] = math.ceil(i['step_height'][0] * 1e3) + math.ceil(i['step_height'][1] * 1e3) * 1e3 cmd['step_height'][1] = math.ceil(i['step_height'][2] * 1e3) + math.ceil(i['step_height'][3] * 1e3) * 1e3 cmd['acc_des'] = i['weight'] cmd['value'] = i['use_mpc_traj'] cmd['contact'] = math.floor(i['landing_gain'] * 1e1) cmd['ctrl_point'][2] = i['mu'] if k == 0: full_steps['step'] = [cmd] else: full_steps['step'].append(cmd) k=k+1 f = open("./task_3/Gait_Params_up_full.toml", 'w') f.write("# Gait Params\n") f.writelines(toml.dumps(full_steps)) f.close() # pre 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() ctrl.lc_s.publish("user_gait_file", usergait_msg.encode()) time.sleep(0.5) usergait_msg.data = file_obj_gait_params.read() ctrl.lc_s.publish("user_gait_file", usergait_msg.encode()) time.sleep(0.1) file_obj_gait_def.close() file_obj_gait_params.close() msg.mode = 62 msg.value = 0 msg.contact = 15 msg.gait_id = 110 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 = 320 # 最大循环次数，作为安全保障 min_iterations = 250 # 最小循环次数，作为安全保障 # 姿态判断参数 pitch_threshold = 0.05 # 俯仰角阈值（弧度） angular_rate_threshold = 0.03 # 角速度阈值（弧度/秒） # 阶段控制 climbing_detected = False # 是否检测到正在爬坡 # 高度变化记录 height_window = [] pitch_window = [] window_size = 8 # 记录起始姿态和高度 start_height = ctrl.odo_msg.xyz[2] info(f"开始监测爬坡过程，初始高度: {start_height:.3f}", "监测") for i in range(max_iterations): # 发送控制命令维持心跳 ctrl.Send_cmd(msg) # 获取当前状态数据 vz = ctrl.odo_msg.vxyz[2] # Z轴速度 current_height = ctrl.odo_msg.xyz[2] # 当前高度 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: 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: 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 time.sleep(0.2) except KeyboardInterrupt: msg.mode = 7 #PureDamper before KeyboardInterrupt: msg.gait_id = 0 msg.duration = 0 msg.life_count += 1 ctrl.Send_cmd(msg) pass section('任务3-2：x = 2', "开始") turn_degree_v2(ctrl, msg, 90, absolute=True) center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, detect_height=0.3) go_straight(ctrl, msg, distance=0.5, speed=0.5, observe=True) time.sleep(1) section('任务3-3：down', "完成") try: steps = toml.load("./task_3/Gait_Params_down.toml") full_steps = {'step':[robot_cmd]} k = 0 for i in steps['step']: cmd = copy.deepcopy(robot_cmd) cmd['duration'] = i['duration'] if i['type'] == 'usergait': cmd['mode'] = 11 # LOCOMOTION cmd['gait_id'] = 110 # USERGAIT cmd['vel_des'] = i['body_vel_des'] cmd['rpy_des'] = i['body_pos_des'][0:3] cmd['pos_des'] = i['body_pos_des'][3:6] cmd['foot_pose'][0:2] = i['landing_pos_des'][0:2] cmd['foot_pose'][2:4] = i['landing_pos_des'][3:5] cmd['foot_pose'][4:6] = i['landing_pos_des'][6:8] cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11] cmd['step_height'][0] = math.ceil(i['step_height'][0] * 1e3) + math.ceil(i['step_height'][1] * 1e3) * 1e3 cmd['step_height'][1] = math.ceil(i['step_height'][2] * 1e3) + math.ceil(i['step_height'][3] * 1e3) * 1e3 cmd['acc_des'] = i['weight'] cmd['value'] = i['use_mpc_traj'] cmd['contact'] = math.floor(i['landing_gain'] * 1e1) cmd['ctrl_point'][2] = i['mu'] if k == 0: full_steps['step'] = [cmd] else: full_steps['step'].append(cmd) k=k+1 f = open("./task_3/Gait_Params_down_full.toml", 'w') f.write("# Gait Params\n") f.writelines(toml.dumps(full_steps)) f.close() # pre 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() ctrl.lc_s.publish("user_gait_file", usergait_msg.encode()) time.sleep(0.5) usergait_msg.data = file_obj_gait_params.read() ctrl.lc_s.publish("user_gait_file", usergait_msg.encode()) time.sleep(0.1) file_obj_gait_def.close() file_obj_gait_params.close() msg.mode = 62 msg.value = 0 msg.contact = 15 msg.gait_id = 110 msg.duration = 1000 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 # 最小循环次数，确保有足够的时间开始动作 start_height = ctrl.odo_msg.xyz[2] # 记录起始高度 # 姿态判断参数 pitch_threshold = 0.05 # 俯仰角阈值（弧度） angular_rate_threshold = 0.03 # 角速度阈值（弧度/秒） # 阶段控制 descending_detected = False # 是否检测到正在下坡 flat_ground_detected = False # 是否检测到已到达平地 # 高度变化记录 height_window = [] pitch_window = [] window_size = 8 info(f"开始监测下坡过程，初始高度: {start_height}", "监测") for i in range(max_iterations): # 发送控制命令维持心跳 ctrl.Send_cmd(msg) # 获取当前状态数据 vz = ctrl.odo_msg.vxyz[2] # Z轴速度 current_height = ctrl.odo_msg.xyz[2] # 当前高度 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} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.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 time.sleep(0.2) if not flat_ground_detected: info(f"达到最大循环次数，未能明确检测到到达平地。当前高度: {ctrl.odo_msg.xyz[2]:.3f}", "警告") except KeyboardInterrupt: msg.mode = 7 #PureDamper before KeyboardInterrupt: msg.gait_id = 0 msg.duration = 0 msg.life_count += 1 ctrl.Send_cmd(msg) pass def go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3, max_time=30, observe=True): """ 控制机器人直走，直到摄像头检测到黄色区域比例超过指定阈值后开始下降时停止 参数: ctrl: Robot_Ctrl 对象，包含里程计信息 msg: robot_control_cmd_lcmt 对象，用于发送命令 yellow_ratio_threshold: 黄色区域占比阈值(0-1之间的浮点数)，默认为0.15 speed: 前进速度(米/秒)，默认为0.3米/秒 max_time: 最大行走时间(秒)，默认为30秒 observe: 是否输出中间状态信息和可视化结果，默认为True 返回: bool: 是否成功检测到黄色区域并停止 """ section("开始直行寻找黄色区域", "黄色检测") # 设置移动命令基本参数 msg.mode = 11 # Locomotion模式 msg.gait_id = 26 # 自变频步态 msg.duration = 0 # wait next cmd msg.step_height = [0.06, 0.06] # 抬腿高度 msg.vel_des = [speed, 0, 0] # [前进速度, 侧向速度, 角速度] # 记录起始时间和位置 start_time = time.time() start_position = list(ctrl.odo_msg.xyz) if observe: info(f"开始寻找黄色区域，阈值: {yellow_ratio_threshold:.2%}", "启动") debug(f"起始位置: {start_position}", "位置") # 检测间隔 check_interval = 0.3 # 秒 last_check_time = 0 # 黄色区域监测变量 yellow_peak_detected = False # 是否检测到峰值 yellow_decreasing = False # 是否开始下降 max_yellow_ratio = 0.0 # 记录最大黄色区域占比 yellow_ratio_history = [] # 记录黄色区域占比历史 history_window_size = 5 # 历史窗口大小，用于平滑处理 try: # 开始移动 msg.life_count += 1 ctrl.Send_cmd(msg) # 持续检测黄色区域 while time.time() - start_time < max_time and not yellow_decreasing: current_time = time.time() # 定期发送移动命令保持移动状态 if current_time - last_check_time >= check_interval: # 获取当前图像并保存到临时文件 current_image = ctrl.image_processor.get_current_image() # 创建临时文件保存图像 with tempfile.NamedTemporaryFile(suffix='.jpg', delete=False) as temp_file: temp_filename = temp_file.name cv2.imwrite(temp_filename, current_image) try: # 分析图像中的黄色区域 yellow_ratio = analyze_yellow_area_ratio(temp_filename, debug=False, save_result=False) # 添加到历史记录 yellow_ratio_history.append(yellow_ratio) if len(yellow_ratio_history) > history_window_size: yellow_ratio_history.pop(0) # 计算平滑后的当前黄色占比（使用最近几次的平均值以减少噪声） current_smooth_ratio = sum(yellow_ratio_history) / len(yellow_ratio_history) # 计算已移动距离（仅用于显示） current_position = ctrl.odo_msg.xyz dx = current_position[0] - start_position[0] dy = current_position[1] - start_position[1] distance_moved = math.sqrt(dx*dx + dy*dy) if observe: info(f"当前黄色区域占比: {yellow_ratio:.2%}, 平滑值: {current_smooth_ratio:.2%}, 已移动: {distance_moved:.2f}米", "检测") # 检测是否达到阈值（开始监测峰值） if current_smooth_ratio >= yellow_ratio_threshold: # 更新最大值 if current_smooth_ratio > max_yellow_ratio: max_yellow_ratio = current_smooth_ratio if not yellow_peak_detected: yellow_peak_detected = True if observe: info(f"黄色区域占比超过阈值，开始监测峰值", "检测") # 检测是否开始下降 elif yellow_peak_detected and current_smooth_ratio < max_yellow_ratio * 0.9: # 下降到峰值的90%以下认为开始下降 yellow_decreasing = True if observe: success(f"检测到黄色区域占比开始下降，峰值: {max_yellow_ratio:.2%}, 当前: {current_smooth_ratio:.2%}", "检测") finally: # 删除临时文件 try: os.unlink(temp_filename) except: pass # 更新心跳 msg.life_count += 1 ctrl.Send_cmd(msg) last_check_time = current_time # 小间隔 time.sleep(0.05) # 平滑停止 if yellow_decreasing: if observe: info("开始平滑停止", "停止") # 先降低速度再停止，实现平滑停止 slowdown_steps = 5 for i in range(slowdown_steps, 0, -1): slowdown_factor = i / slowdown_steps msg.vel_des = [speed * slowdown_factor, 0, 0] msg.life_count += 1 ctrl.Send_cmd(msg) time.sleep(0.1) # 完全停止 ctrl.base_msg.stop() # 计算最终移动距离 final_position = ctrl.odo_msg.xyz dx = final_position[0] - start_position[0] dy = final_position[1] - start_position[1] final_distance = math.sqrt(dx*dx + dy*dy) if observe: if yellow_decreasing: success(f"成功检测到黄色区域峰值并停止，峰值占比: {max_yellow_ratio:.2%}, 总移动距离: {final_distance:.2f}米", "完成") else: warning(f"未能在限定时间内检测到黄色区域峰值，总移动距离: {final_distance:.2f}米", "超时") return yellow_decreasing except KeyboardInterrupt: # 处理键盘中断 ctrl.base_msg.stop() if observe: warning("操作被用户中断", "中断") return False except Exception as e: # 处理其他异常 ctrl.base_msg.stop() if observe: error(f"发生错误: {str(e)}", "错误") return False def run_task_3(ctrl, msg, time_sleep=5000): 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) turn_degree_v2(ctrl, msg, 90, absolute=True) center_on_dual_tracks(ctrl, msg, max_time=15, 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) # 原地站立3秒 section("原地站立3秒", "站立") msg.mode = 11 # Locomotion模式 msg.gait_id = 26 # 自变频步态 msg.duration = 0 # wait next cmd 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("完成原地站立", "站立") def run_task_3_back(ctrl, msg): return