import math import time import sys import os sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing from utils.detect_dual_track_lines import detect_dual_track_lines def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=False, mode=11, gait_id=26, step_height=[0.06, 0.06], estimated_track_distance=1.0, detect_height=0.4, detection_interval=0.2): """ 控制机器狗仅使用Y轴移动调整到双轨道线的中间位置 参数: ctrl: Robot_Ctrl 对象，包含里程计信息 msg: robot_control_cmd_lcmt 对象，用于发送命令 max_time: 最大调整时间(秒)，默认为15秒 max_deviation: 允许的最大偏差(像素)，当偏差小于此值时认为已居中，默认为8像素 observe: 是否输出中间状态信息和可视化结果，默认为False mode: 控制模式，默认为11 gait_id: 步态ID，默认为26 step_height: 抬腿高度，默认为[0.06, 0.06] estimated_track_distance: 估计的机器人到轨道的距离(米)，用于计算实际偏差，默认为1.0米 detect_height: 检测高度，默认为0.4 detection_interval: 检测间隔时间(秒)，默认为0.2秒 返回: bool: 是否成功调整到中心位置 """ section("开始双轨道居中", "轨道居中") # 设置移动命令基本参数 msg.mode = mode msg.gait_id = gait_id msg.duration = 0 # wait next cmd msg.step_height = step_height # 记录起始时间 start_time = time.time() last_detection_time = 0 # 记录上次检测时间 # 记录起始位置 start_position = list(ctrl.odo_msg.xyz) if observe: debug(f"起始位置: {start_position}", "位置") # 在起点放置绿色标记 if hasattr(ctrl, 'place_marker'): ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True) # 根据RGB相机参数，计算像素偏差到实际距离的转换 # 相机水平视场角FOV = 1.46608弧度（约84度） # 图像宽度 = 1920像素 camera_hfov = 1.46608 # 水平视场角（弧度） image_width = 1920 # 图像宽度（像素） # 计算像素到角度的转换比例 pixels_per_radian = image_width / camera_hfov # 统计变量 detection_success_count = 0 detection_total_count = 0 # 稳定计数器 - 连续几次在中心位置才认为稳定 stable_count = 0 required_stable_count = 2 # 变量记录最后有效的偏差，用于处理检测失败的情况 last_valid_deviation = 0 # 开始调整循环 while time.time() - start_time < max_time: # 获取当前图像 image = ctrl.image_processor.get_current_image() # 检测双轨道线 detection_total_count += 1 center_info, left_info, right_info = detect_dual_track_lines(image, observe=observe, save_log=True) if center_info is not None: detection_success_count += 1 # 获取当前偏差 current_deviation = center_info["deviation"] last_valid_deviation = current_deviation # 记录有效偏差 # 转换像素偏差到实际距离偏差 # 1. 像素偏差转换为弧度 deviation_rad = current_deviation / pixels_per_radian # 2. 根据估计距离计算实际偏差（单位：米） actual_deviation = math.tan(deviation_rad) * estimated_track_distance if observe: debug(f"偏差: {current_deviation:.1f}px", "偏差") debug(f"实际距离偏差: {actual_deviation:.3f}m (距离估计: {estimated_track_distance:.1f}m)", "实际偏差") # 判断是否已经居中 if abs(current_deviation) <= max_deviation: stable_count += 1 if observe: info(f"已接近中心，稳定计数: {stable_count}/{required_stable_count}", "居中") info(f"距离偏差: {actual_deviation:.3f}m", "居中") # 即使在稳定过程中，也要保持小幅调整以保持居中 if stable_count < required_stable_count: # 直接根据实际偏差移动 msg.vel_des = [0, actual_deviation, 0] ctrl.Send_cmd(msg) if stable_count >= required_stable_count: # 已经稳定在中心位置 if observe: success(f"成功居中，最终偏差: {current_deviation:.1f}px ({actual_deviation:.3f}m)", "完成") break else: # 不在中心，重置稳定计数 stable_count = 0 # 直接根据计算出的实际偏差距离进行移动 # 为了避免移动过猛，可以限制最大移动距离 max_move_distance = 0.35 # 最大移动距离（米） move_distance = actual_deviation if abs(move_distance) > max_move_distance: move_distance = max_move_distance * (1 if move_distance > 0 else -1) if observe: debug(f"横向移动距离: {move_distance:.3f}m", "控制") # 设置速度命令 - 只使用y轴移动，不前进和转向 msg.vel_des = [0, move_distance, 0] # [前进速度, 侧向速度, 角速度] # 发送命令 msg.life_count += 1 ctrl.Send_cmd(msg) else: warning("未检测到双轨道线", "警告") # 如果已经有了一些有效的检测，使用最后一次有效的偏差继续移动 if detection_success_count > 0: # 使用最后一次有效偏差值计算移动距离 deviation_rad = last_valid_deviation / pixels_per_radian move_distance = math.tan(deviation_rad) * estimated_track_distance * 0.5 # 缩小系数，避免过度移动 # 限制最大移动距离 max_move_distance = 0.15 move_distance = max(-max_move_distance, min(max_move_distance, move_distance)) msg.vel_des = [0, move_distance, 0] msg.life_count += 1 ctrl.Send_cmd(msg) if observe: warning(f"使用最后偏差值继续移动: {move_distance:.3f}m", "暂停") else: # 如果一开始就没有检测到，可以尝试小范围搜索 if detection_total_count < 10: if detection_total_count % 2 == 0: # 向右搜索 msg.vel_des = [0, -0.08, 0] else: # 向左搜索 msg.vel_des = [0, 0.08, 0] msg.life_count += 1 ctrl.Send_cmd(msg) if observe: debug("搜索轨道线...", "搜索") else: # 超过一定次数仍未检测到，停止搜索 msg.vel_des = [0, 0, 0] msg.life_count += 1 ctrl.Send_cmd(msg) if observe: error("无法检测到轨道线，放弃调整", "失败") break # 短暂延时 time.sleep(0.05) # 停止移动 ctrl.base_msg.stop() # 计算最终位置与起始位置的变化 final_position = ctrl.odo_msg.xyz dx = final_position[0] - start_position[0] dy = final_position[1] - start_position[1] if observe: # 在终点放置红色标记 if hasattr(ctrl, 'place_marker'): ctrl.place_marker(final_position[0], final_position[1], final_position[2] if len(final_position) > 2 else 0.0, 'red', observe=True) info(f"横向移动距离: {abs(dy):.3f}米", "统计") # 显示检测成功率 if detection_total_count > 0: detection_rate = (detection_success_count / detection_total_count) * 100 info(f"轨迹检测成功率: {detection_rate:.1f}% ({detection_success_count}/{detection_total_count})", "统计") # 判断是否成功 success = False if time.time() - start_time >= max_time: if observe: warning("超过最大调整时间", "超时") else: # 如果因为已稳定在中心而退出循环，则认为成功 if stable_count >= required_stable_count: success = True return success def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_time=15, observe=False, mode=11, gait_id=26, step_height=[0.06, 0.06], estimated_track_distance=1.0, detection_interval=0.2): """ 先居中到双轨道线中间，然后沿轨道线行走指定距离 参数: ctrl: Robot_Ctrl 对象，包含里程计信息 msg: robot_control_cmd_lcmt 对象，用于发送命令 distance: 目标前进距离(米) speed: 前进速度(米/秒)，默认为0.5米/秒 max_centering_time: 最大居中调整时间(秒)，默认为15秒 observe: 是否输出中间状态信息和可视化结果，默认为False mode: 控制模式，默认为11 gait_id: 步态ID，默认为26 step_height: 抬腿高度，默认为[0.06, 0.06] estimated_track_distance: 估计的机器人到轨道的距离(米)，用于计算实际偏差，默认为1.0米 detection_interval: 检测间隔时间(秒)，默认为0.2秒 返回: bool: 是否成功完成居中和轨道跟随 """ section("开始双轨道居中和跟随", "轨道任务") # 第一步：居中到轨道中间 if observe: info("步骤1: 调整到轨道中间", "居中") centering_success = center_on_dual_tracks( ctrl, msg, max_time=max_centering_time, observe=observe, mode=mode, gait_id=gait_id, step_height=step_height, estimated_track_distance=estimated_track_distance, detection_interval=detection_interval ) if not centering_success: if observe: error("居中调整失败，无法继续跟随轨道", "失败") return False # 第二步：沿轨道跟随指定距离 if observe: info(f"步骤2: 沿轨道前进 {distance:.2f}米", "跟随") # 导入轨道跟随函数 from base_move.follow_dual_tracks import follow_dual_tracks following_success = follow_dual_tracks( ctrl, msg, speed=speed, target_distance=distance, observe=observe, mode=mode, gait_id=gait_id, step_height=step_height ) if observe: if following_success: success("成功完成轨道居中和跟随任务", "完成") else: warning("轨道跟随未完全成功", "警告") return following_success