import time import sys import os import cv2 import numpy as np import math # 添加父目录到路径，以便能够导入utils sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from base_move.turn_degree import turn_degree, turn_degree_v2 from base_move.go_straight import go_straight, go_lateral from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing from utils.gray_sky_analyzer import analyze_gray_sky_ratio from utils.detect_track import detect_horizontal_track_edge from utils.detect_dual_track_lines import detect_dual_track_lines from base_move.move_base_hori_line import calculate_distance_to_line from task_4.pass_bar import pass_bar # 创建本模块特定的日志记录器 logger = get_logger("任务4") def run_task_4(ctrl, msg): section('任务4-1：直线移动', "移动") # 设置机器人运动模式为快步跑 msg.mode = 11 # 运动模式 msg.gait_id = 3 # 步态ID（快步跑） msg.vel_des = [0.35, 0, 0] # 期望速度 msg.pos_des = [ 0, 0, 0] msg.duration = 0 # 零时长表示持续运动，直到接收到新命令 msg.step_height = [0.21, 0.21] # 持续运动时摆动腿的离地高度 msg.life_count += 1 ctrl.Send_cmd(msg) time.sleep(5) # 持续5秒钟 section('任务4-2：移动直到灰色天空比例小于阈值', "天空检测") go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2) section('任务4-3：通过栏杆', "移动") pass_bar(ctrl, msg) def run_task_4_back(ctrl, msg): """ 参数: ctrl: Robot_Ctrl对象 msg: 控制消息对象 image_processor: 可选的图像处理器实例 """ turn_degree_v2(ctrl, msg, degree=-90, absolute=True) # center_on_dual_tracks(ctrl, msg, max_time=30, observe=False, stone_path_mode=False) # 向右移动0.5秒 section('任务4-回程：向右移动', "移动") # go_lateral(ctrl, msg, distance=-0.3, speed=0.1, observe=True) # DEBUG section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测") go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.35, speed=0.5) section('任务4-2：通过栏杆', "移动") turn_degree_v2(ctrl, msg, degree=-90, absolute=True) pass_bar(ctrl, msg) section('任务4-3：stone', "移动") go_straight(ctrl, msg, distance=1, speed=2) # Use enhanced calibration for better Y-axis correction on stone path go_straight_with_enhanced_calibration(ctrl, msg, distance=4.5, speed=0.35, mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True) section('任务4-3：前进直到遇到黄线 - 石板路', "移动") # 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处 # 获取相机高度 camera_height = 0.355 # 单位: 米 # INFO from TF-tree edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=True, save_log=True) current_distance = calculate_distance_to_line(edge_info, camera_height, observe=True) go_straight(ctrl, msg, distance=current_distance, speed=0.20, mode=11, gait_id=3, step_height=[0.21, 0.21]) def go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2, step_distance=0.5, max_distance=2, speed=0.3 ): """ 控制机器人沿直线行走，直到灰色天空比例低于指定阈值 参数: ctrl: Robot_Ctrl对象 msg: 控制命令消息对象 sky_ratio_threshold: 灰色天空比例阈值，当检测到的比例低于此值时停止 step_distance: 每次移动的步长(米) max_distance: 最大移动距离(米)，防止无限前进 speed: 移动速度(米/秒) 返回: bool: 是否成功找到天空比例低于阈值的位置 """ total_distance = 0 success_flag = False # 设置移动命令 msg.mode = 11 # Locomotion模式 msg.gait_id = 26 # 自变频步态 msg.step_height = [0.06, 0.06] # 抬腿高度 while total_distance < max_distance: # 获取当前图像 current_image = ctrl.image_processor.get_current_image() if current_image is None: warning("无法获取图像，等待...", "图像") time.sleep(0.5) continue # 保存当前图像用于分析 temp_image_path = "/tmp/current_sky_image.jpg" cv2.imwrite(temp_image_path, current_image) # 分析灰色天空比例 try: sky_ratio = analyze_gray_sky_ratio(temp_image_path) info(f"当前灰色天空比例: {sky_ratio:.2%}", "分析") # 如果天空比例高于阈值，停止移动 if sky_ratio < sky_ratio_threshold: success(f"检测到灰色天空比例({sky_ratio:.2%})低于阈值({sky_ratio_threshold:.2%})，停止移动", "完成") success_flag = True break except Exception as e: error(f"分析图像时出错: {e}", "错误") # 继续前进一段距离 info(f"继续前进 {step_distance} 米...", "移动") # 设置移动速度和方向 msg.vel_des = [speed, 0, 0] # [前进速度, 侧向速度, 角速度] msg.duration = 0 # wait next cmd msg.life_count += 1 # 发送命令 ctrl.Send_cmd(msg) # 估算前进时间 move_time = step_distance / speed time.sleep(move_time) # 累计移动距离 total_distance += step_distance info(f"已移动总距离: {total_distance:.2f} 米", "距离") # 平滑停止 if hasattr(ctrl.base_msg, 'stop_smooth'): ctrl.base_msg.stop_smooth() else: ctrl.base_msg.stop() if not success_flag and total_distance >= max_distance: warning(f"已达到最大移动距离 {max_distance} 米，但未找到天空比例小于 {sky_ratio_threshold:.2%} 的位置", "超时") return success_flag def go_straight_with_enhanced_calibration(ctrl, msg, distance, speed=0.5, observe=False, mode=11, gait_id=3, step_height=[0.21, 0.21]): """ 控制机器人在石板路上沿直线行走，使用视觉校准和姿态传感器融合来保持直线 参数: ctrl: Robot_Ctrl 对象 msg: 控制消息对象 distance: 要行走的距离(米) speed: 行走速度(米/秒) observe: 是否输出调试信息 mode: 运动模式 gait_id: 步态ID step_height: 摆动腿高度 返回: bool: 是否成功完成 """ section("开始石板路增强直线移动", "石板路移动") # 参数验证 if abs(distance) < 0.01: info("距离太短，无需移动", "信息") return True # 检查相机是否可用 if not hasattr(ctrl, 'image_processor') or not hasattr(ctrl.image_processor, 'get_current_image'): warning("机器人控制器没有提供图像处理器，将使用姿态传感器辅助校准", "警告") # 限制速度范围 speed = min(max(abs(speed), 0.1), 1.0) # 确定前进或后退方向 forward = distance > 0 move_speed = speed if forward else -speed abs_distance = abs(distance) # 获取起始位置和姿态 start_position = list(ctrl.odo_msg.xyz) start_yaw = ctrl.odo_msg.rpy[2] # 记录起始朝向 if observe: debug(f"起始位置: {start_position}", "位置") info(f"开始石板路增强直线移动，距离: {abs_distance:.3f}米，速度: {abs(move_speed):.2f}米/秒", "移动") # 设置移动命令 msg.mode = mode msg.gait_id = gait_id msg.step_height = step_height msg.duration = 0 # wait next cmd # 根据需要移动的距离动态调整移动速度 if abs_distance > 1.0: actual_speed = move_speed # 距离较远时用设定速度 else: actual_speed = move_speed * 0.8 # 较近距离略微降速 # 设置移动速度和方向 msg.vel_des = [actual_speed, 0, 0] # [前进速度, 侧向速度, 角速度] msg.life_count += 1 # 发送命令 ctrl.Send_cmd(msg) # 估算移动时间，但实际上会通过里程计控制 estimated_time = abs_distance / abs(actual_speed) timeout = estimated_time + 5 # 增加超时时间 # 使用里程计进行实时监控移动距离 distance_moved = 0 start_time = time.time() last_check_time = start_time position_check_interval = 0.1 # 位置检查间隔(秒) # 计算移动方向单位向量（世界坐标系下） direction_vector = [math.cos(start_yaw), math.sin(start_yaw)] if not forward: direction_vector = [-direction_vector[0], -direction_vector[1]] # 视觉跟踪相关变量 vision_check_interval = 0.2 # 视觉检查间隔(秒) last_vision_check = start_time vision_correction_gain = 0.006 # 视觉修正增益系数 # 用于滤波的队列 deviation_queue = [] filter_size = 5 last_valid_deviation = 0 # PID控制参数 - 用于角度修正 kp_angle = 0.6 # 比例系数 ki_angle = 0.02 # 积分系数 kd_angle = 0.1 # 微分系数 # PID控制变量 angle_error_integral = 0 last_angle_error = 0 # 偏移量累计 - 用于检测持续偏移 y_offset_accumulator = 0 # 动态调整参数 slow_down_ratio = 0.85 # 当移动到目标距离的85%时开始减速 completion_threshold = 0.95 # 当移动到目标距离的95%时停止 # 监控移动过程 while distance_moved < abs_distance * completion_threshold and time.time() - start_time < timeout: current_time = time.time() # 按固定间隔检查位置 if current_time - last_check_time >= position_check_interval: # 获取当前位置和朝向 current_position = ctrl.odo_msg.xyz current_yaw = ctrl.odo_msg.rpy[2] # 计算在移动方向上的位移 dx = current_position[0] - start_position[0] dy = current_position[1] - start_position[1] # 计算在初始方向上的投影距离（实际前进距离） distance_moved = dx * direction_vector[0] + dy * direction_vector[1] distance_moved = abs(distance_moved) # 确保距离为正值 # 计算垂直于移动方向的偏移量（y方向偏移） y_offset = -dx * direction_vector[1] + dy * direction_vector[0] # 累积y方向偏移量，检测持续偏移趋势 y_offset_accumulator = y_offset_accumulator * 0.8 + y_offset * 0.2 # 根据前进或后退确定期望方向 expected_direction = start_yaw if forward else (start_yaw + math.pi) % (2 * math.pi) # 使用IMU朝向数据计算角度偏差 yaw_error = current_yaw - expected_direction # 角度归一化 while yaw_error > math.pi: yaw_error -= 2 * math.pi while yaw_error < -math.pi: yaw_error += 2 * math.pi # 使用PID控制计算角速度修正 # 比例项 p_control = kp_angle * yaw_error # 积分项 (带衰减) angle_error_integral = angle_error_integral * 0.9 + yaw_error angle_error_integral = max(-1.0, min(1.0, angle_error_integral)) # 限制积分范围 i_control = ki_angle * angle_error_integral # 微分项 d_control = kd_angle * (yaw_error - last_angle_error) last_angle_error = yaw_error # 计算总的角速度控制量 angular_correction = -(p_control + i_control + d_control) # 限制最大角速度修正 angular_correction = max(min(angular_correction, 0.3), -0.3) # 根据持续的y偏移趋势增加侧向校正 lateral_correction = 0 if abs(y_offset_accumulator) > 0.05: # 如果累积偏移超过5厘米 lateral_correction = -y_offset_accumulator * 0.8 # 反向校正 lateral_correction = max(min(lateral_correction, 0.15), -0.15) # 限制最大侧向速度 if observe and abs(lateral_correction) > 0.05: warning(f"累积Y偏移校正: {y_offset_accumulator:.3f}米，应用侧向速度 {lateral_correction:.3f}m/s", "偏移") # 计算完成比例 completion_ratio = distance_moved / abs_distance # 根据距离完成情况调整速度 if completion_ratio > slow_down_ratio: # 计算减速系数 slow_factor = 1.0 - (completion_ratio - slow_down_ratio) / (1.0 - slow_down_ratio) # 确保不会减速太多 slow_factor = max(0.3, slow_factor) new_speed = actual_speed * slow_factor if observe and abs(new_speed - msg.vel_des[0]) > 0.05: info(f"减速: {msg.vel_des[0]:.2f} -> {new_speed:.2f} 米/秒 (完成: {completion_ratio*100:.1f}%)", "移动") actual_speed = new_speed # 应用修正 - 同时应用角速度和侧向速度修正 msg.vel_des = [actual_speed, lateral_correction, angular_correction] msg.life_count += 1 ctrl.Send_cmd(msg) if observe and current_time % 1.0 < position_check_interval: debug(f"已移动: {distance_moved:.3f}米, 目标: {abs_distance:.3f}米 (完成: {completion_ratio*100:.1f}%)", "距离") debug(f"Y偏移: {y_offset:.3f}米, 累积偏移: {y_offset_accumulator:.3f}米", "偏移") debug(f"朝向偏差: {math.degrees(yaw_error):.1f}度, 角速度修正: {angular_correction:.3f}rad/s", "角度") debug(f"PID: P={p_control:.3f}, I={i_control:.3f}, D={d_control:.3f}", "控制") # 更新检查时间 last_check_time = current_time # 定期进行视觉检查和修正 if hasattr(ctrl, 'image_processor') and current_time - last_vision_check >= vision_check_interval: try: # 获取当前相机帧 frame = ctrl.image_processor.get_current_image() if frame is not None: # 检测轨道线 - 使用特定的石板路模式 center_info, _, _ = detect_dual_track_lines( frame, observe=False, save_log=False, stone_path_mode=True) # 如果成功检测到轨道线，使用它进行修正 if center_info is not None: # 获取当前偏差 current_deviation = center_info["deviation"] last_valid_deviation = current_deviation # 添加到队列进行滤波 deviation_queue.append(current_deviation) if len(deviation_queue) > filter_size: deviation_queue.pop(0) # 计算滤波后的偏差值 (去除最大和最小值后的平均) if len(deviation_queue) >= 3: filtered_deviations = sorted(deviation_queue)[1:-1] if len(deviation_queue) > 2 else deviation_queue filtered_deviation = sum(filtered_deviations) / len(filtered_deviations) else: filtered_deviation = current_deviation # 计算视觉侧向修正速度 vision_lateral_correction = -filtered_deviation * vision_correction_gain # 限制最大侧向速度修正 vision_lateral_correction = max(min(vision_lateral_correction, 0.2), -0.2) # 与当前的侧向校正进行融合 (加权平均) if msg.vel_des[1] != 0: # 如果已经有侧向校正，与视觉校正进行融合 fused_lateral = msg.vel_des[1] * 0.3 + vision_lateral_correction * 0.7 else: # 如果没有侧向校正，直接使用视觉校正 fused_lateral = vision_lateral_correction if observe and abs(vision_lateral_correction) > 0.05: warning(f"视觉修正: 偏差 {filtered_deviation:.1f}像素，应用侧向速度 {vision_lateral_correction:.3f}m/s", "视觉") # 应用视觉修正，保留当前前进速度和角速度 msg.vel_des = [msg.vel_des[0], fused_lateral, msg.vel_des[2]] msg.life_count += 1 ctrl.Send_cmd(msg) if observe and current_time % 1.0 < vision_check_interval: debug(f"视觉检测: 原始偏差 {current_deviation:.1f}像素, 滤波后 {filtered_deviation:.1f}像素", "视觉") debug(f"融合侧向速度: {fused_lateral:.3f}m/s", "视觉") except Exception as e: if observe: error(f"视觉检测异常: {e}", "错误") # 更新视觉检查时间 last_vision_check = current_time # 短暂延时 time.sleep(0.01) # 平滑停止 slowdown_steps = 5 for i in range(slowdown_steps, 0, -1): slowdown_factor = i / slowdown_steps msg.vel_des = [actual_speed * slowdown_factor, 0, 0] msg.life_count += 1 ctrl.Send_cmd(msg) time.sleep(0.1) # 最后完全停止 if hasattr(ctrl.base_msg, 'stop_smooth'): ctrl.base_msg.stop_smooth() else: ctrl.base_msg.stop() # 获取最终位置和实际移动距离 final_position = ctrl.odo_msg.xyz dx = final_position[0] - start_position[0] dy = final_position[1] - start_position[1] actual_distance = math.sqrt(dx*dx + dy*dy) # 计算最终y方向偏移 final_y_offset = -dx * direction_vector[1] + dy * direction_vector[0] if observe: success(f"石板路增强直线移动完成，实际移动距离: {actual_distance:.3f}米", "完成") info(f"最终Y方向偏移: {final_y_offset:.3f}米", "偏移") # 判断是否成功完成 distance_error = abs(actual_distance - abs_distance) y_offset_error = abs(final_y_offset) go_success = distance_error < 0.1 and y_offset_error < 0.1 # 距离误差和y偏移都小于10厘米视为成功 if observe: if go_success: success(f"移动成功", "成功") else: warning(f"移动不够精确，距离误差: {distance_error:.3f}米, Y偏移: {y_offset_error:.3f}米", "警告") return go_success