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() 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 climb_speed_threshold = 0.05 # 检测到开始爬坡的速度阈值 max_iterations = 450 # 最大循环次数，作为安全保障 y_target = 9.3 # 目标y坐标，达到此值时停止 # 阶段控制 climbing_detected = False # 是否检测到正在爬坡 # 记录起始姿态和高度 start_height = ctrl.odo_msg.xyz[2] start_y = ctrl.odo_msg.xyz[1] info(f"开始监测爬坡过程，初始高度: {start_height:.3f}, 初始Y坐标: {start_y:.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_y = ctrl.odo_msg.xyz[1] # 当前y坐标 current_pitch = ctrl.odo_msg.rpy[1] # 当前俯仰角 # 每10次迭代打印一次当前信息 if i % 10 == 0: 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}", "监测") # 检查是否达到目标y坐标 if current_y >= y_target: info(f"已达到目标Y坐标: {current_y:.3f}，停止爬坡", "监测") break 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) go_lateral(ctrl, msg, distance=0.1, speed=0.5, observe=True) center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False, detect_height=0.3) go_straight(ctrl, msg, distance=0.6, 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 # 参数设置 descent_speed_threshold = -0.05 # 检测到开始下坡的速度阈值（负值表示下降） max_iterations = 200 # 最大循环次数，作为安全保障 start_height = ctrl.odo_msg.xyz[2] # 记录起始高度 start_y = ctrl.odo_msg.xyz[1] # 记录起始y坐标 y_target = 11.6 # 目标y坐标，达到此值时停止 # 阶段控制 descending_detected = False # 是否检测到正在下坡 flat_ground_detected = False # 是否检测到已到达平地 info(f"开始监测下坡过程，初始高度: {start_height}, 初始Y坐标: {start_y:.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_y = ctrl.odo_msg.xyz[1] # 当前y坐标 current_pitch = ctrl.odo_msg.rpy[1] # 当前俯仰角 # 每10次迭代打印一次当前信息 if observe and i % 10 == 0: 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}", "监测") # 检查是否达到目标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"达到最大循环次数，未能到达目标Y坐标。当前Y坐标: {ctrl.odo_msg.xyz[1]:.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_straight(ctrl, msg, distance=0.1, speed=0.5, observe=True) section('任务3-1：up', "开始") pass_up_down(ctrl, msg) turn_degree_v2(ctrl, msg, 90, absolute=True) go_lateral(ctrl, msg, distance=0.2, speed=0.5, observe=True) 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) # 原地站立3秒 section("原地站立5秒", "站立") msg.mode = 12 msg.gait_id = 0 msg.duration = 0 msg.step_height = [0.06, 0.06] msg.vel_des = [0, 0, 0] msg.life_count += 1 ctrl.Send_cmd(msg) info("开始原地站立5秒", "站立") time.sleep(time_sleep / 1000) info("完成原地站立", "站立") def run_task_3_back(ctrl, msg, time_sleep=5000): go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3) # 原地站立3秒 section("原地站立5秒", "站立") msg.mode = 12 msg.gait_id = 0 msg.duration = 0 msg.step_height = [0.06, 0.06] msg.vel_des = [0, 0, 0] msg.life_count += 1 ctrl.Send_cmd(msg) info("开始原地站立5秒", "站立") time.sleep(time_sleep / 1000) info("完成原地站立", "站立") section('任务3-1：up', "开始") pass_up_down(ctrl, msg)