import math import time import cv2 import sys import os sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from utils.detect_track import detect_horizontal_track_edge from base_move.turn_degree import turn_degree def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): """ 控制机器人旋转到横向线水平的位置 参数: ctrl: Robot_Ctrl 对象，包含里程计信息 msg: robot_control_cmd_lcmt 对象，用于发送命令 observe: 是否输出中间状态信息和可视化结果，默认为False max_attempts: 最大尝试次数，默认为3次 返回: bool: 是否成功校准 """ attempts = 0 aligned = False image = ctrl.image_processor.get_current_image() while attempts < max_attempts and not aligned: print(f"尝试次数: {attempts+1}/{max_attempts}") # 检测横向线边缘 edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0) if edge_point is None or edge_info is None: print("未检测到横向线，无法进行校准") return False # 获取检测到的斜率和其他信息 slope = edge_info["slope"] is_horizontal = edge_info["is_horizontal"] if observe: print(f"检测到横向线，斜率: {slope:.6f}") print(f"是否足够水平: {is_horizontal}") # 如果已经水平，则无需旋转 if is_horizontal: print("横向线已经水平，无需校准") return True # 计算需要旋转的角度 # 斜率 = tan(θ)，因此 θ = arctan(斜率) angle_rad = math.atan(slope) angle_deg = math.degrees(angle_rad) # 调整角度方向 # 正的斜率意味着线条从左到右上升，需要逆时针旋转校正 # 负的斜率意味着线条从左到右下降，需要顺时针旋转校正 # 注意旋转方向: 顺时针为负角度，逆时针为正角度 angle_to_rotate = -angle_deg # 取负值使旋转方向正确 if observe: print(f"需要旋转的角度: {angle_to_rotate:.2f}度") # 执行旋转 # 如果角度很小，增加一个小的偏移以确保旋转足够 if abs(angle_to_rotate) < 3.0: angle_to_rotate *= 1.5 # 对小角度进行放大以确保效果 # 限制旋转角度，避免过度旋转 angle_to_rotate = max(-30, min(30, angle_to_rotate)) # 使用turn_degree函数执行旋转 turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False) if observe: print(f"旋转结果: {'成功' if turn_success else '失败'}") # 增加尝试次数 attempts += 1 # 在旋转后重新获取图像，这里需要调用获取图像的函数 # 代码中没有提供获取实时图像的方法，假设每次外部会更新image参数 # 检查是否已经对齐 # 对于实际应用，应该在旋转后重新捕获图像并检测横向线 # 这里简单地根据旋转是否成功和旋转角度是否足够小来判断 if turn_success and abs(angle_to_rotate) < 5.0: aligned = True return aligned def calculate_distance_to_line(edge_info, camera_height, camera_tilt_angle_deg=0, observe=False): """ 根据相机参数和图像中横线位置计算相机到横线的实际距离 几何模型说明: 1. 相机位于高度camera_height处，向下倾斜camera_tilt_angle_deg度 2. 图像底部对应相机视场的下边缘，横线在图像中的位置通过像素坐标确定 3. 计算相机视线到横线的角度，然后使用三角函数计算实际距离 参数: edge_info: 边缘信息字典，包含distance_to_bottom等信息 camera_height: 相机高度(米) camera_tilt_angle_deg: 相机向下倾斜的角度（度） observe: 是否打印中间计算值 返回: float: 到横向线的X轴水平距离(米) """ if edge_info is None or "distance_to_bottom" not in edge_info: return None # 1. 获取图像中交点到底部的距离(像素) distance_in_pixels = edge_info["distance_to_bottom"] if observe: print(f"图像中交点到底部的像素距离: {distance_in_pixels}") # 2. 获取相机参数 horizontal_fov_rad = 1.46608 # 水平视场角（弧度）约84度 image_height_px = 1080 # 图像高度（像素） image_width_px = 1920 # 图像宽度（像素） # 3. 计算垂直视场角 aspect_ratio = image_width_px / image_height_px # 宽高比 vertical_fov_rad = horizontal_fov_rad / aspect_ratio # 垂直视场角(弧度) vertical_fov_deg = math.degrees(vertical_fov_rad) # 垂直视场角(度) if observe: print(f"相机参数: 水平FOV={math.degrees(horizontal_fov_rad):.1f}°, 垂直FOV={vertical_fov_deg:.1f}°") print(f"图像尺寸: {image_width_px}x{image_height_px}, 宽高比: {aspect_ratio:.2f}") # 4. 直接计算视线角度 # 计算图像底部到相机视场中心的角度 half_vfov_rad = vertical_fov_rad / 2 # 计算图像底部到横线的角度比例 # 比例 = 底部到横线的像素距离 / 图像总高度 pixel_ratio = distance_in_pixels / image_height_px # 计算从图像底部到横线的角度 bottom_to_line_angle_rad = pixel_ratio * vertical_fov_rad # 计算从相机视场中心到横线的角度 # 负值表示横线在视场中心以下，正值表示在中心以上 center_to_line_angle_rad = bottom_to_line_angle_rad - half_vfov_rad # 考虑相机倾斜角度 # 相机向下倾斜为正值，此时视场中心相对水平线向下 camera_tilt_rad = math.radians(camera_tilt_angle_deg) # 计算横线相对于水平面的视线角度 # 负值表示视线向下看到横线，正值表示视线向上看到横线 view_angle_rad = center_to_line_angle_rad - camera_tilt_rad if observe: print(f"视场角度关系:") print(f" - 图像底部到横线角度: {math.degrees(bottom_to_line_angle_rad):.2f}°") print(f" - 视场中心到横线角度: {math.degrees(center_to_line_angle_rad):.2f}°") print(f" - 相机倾斜角度: {camera_tilt_angle_deg}°") print(f" - 最终视线角度: {math.degrees(view_angle_rad):.2f}° ({'向下' if view_angle_rad < 0 else '向上'})") # 5. 防止除零错误或异常值 # 确保视线角度不接近于0(水平视线无法确定地面交点) min_angle_rad = 0.01 # 约0.57度 if abs(view_angle_rad) < min_angle_rad: if observe: print(f"视线角度过小({math.degrees(view_angle_rad):.2f}°)，使用最小角度: {math.degrees(min_angle_rad):.2f}°") view_angle_rad = -min_angle_rad # 设为向下的最小角度 # 6. 计算水平距离 # 仅当视线向下时计算地面距离 if view_angle_rad < 0: # 视线向下 # 基本几何关系: 水平距离 = 高度 / tan(视线向下的角度) # 注意角度为负，所以需要取负 ground_distance = camera_height / math.tan(-view_angle_rad) if observe: print(f"计算公式: 距离 = 相机高度({camera_height}米) / tan(|视线角度|({abs(math.degrees(view_angle_rad)):.2f}°))") print(f"计算结果: 距离 = {ground_distance:.3f}米") else: # 视线平行或向上，无法确定地面交点 if observe: print(f"视线向上或水平，无法计算地面距离") return 0.5 # 返回一个默认值 # 7. 应用校正和限制 # 可选的校正因子(通过实验校准) correction_factor = 1.0 distance = ground_distance * correction_factor # 设置合理的范围限制 min_distance = 0.1 # 最小距离(米) # 限制结果在合理范围内 final_distance = max(min_distance, distance) if observe and final_distance != distance: print(f"应用范围限制: 原始距离 {distance:.3f}米 -> 最终距离 {final_distance:.3f}米") elif observe: print(f"最终距离: {final_distance:.3f}米") return final_distance def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): """ 控制机器人校准并移动到横向线前的指定距离 参数: ctrl: Robot_Ctrl 对象，包含里程计信息 msg: robot_control_cmd_lcmt 对象，用于发送命令 target_distance: 目标位置与横向线的距离(米)，默认为0.5米 observe: 是否输出中间状态信息和可视化结果，默认为False 返回: bool: 是否成功到达目标位置 """ # 首先校准到水平 print("校准到横向线水平") aligned = align_to_horizontal_line(ctrl, msg, observe=observe) if not aligned: print("无法校准到横向线水平，停止移动") return False # 检测横向线 # image = cv2.imread("current_image.jpg") # TEST image = ctrl.image_processor.get_current_image() edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe) if edge_point is None or edge_info is None: print("无法检测到横向线，停止移动") return False # 获取相机高度 camera_height = 0.355 # 单位: 米 # INFO from TF-tree # 计算当前距离 current_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe) if current_distance is None: print("无法计算到横向线的距离，停止移动") return False if observe: print(f"当前距离: {current_distance:.3f}米, 目标距离: {target_distance:.3f}米") # 计算需要移动的距离 distance_to_move = current_distance - target_distance if abs(distance_to_move) < 0.05: # 如果已经很接近目标距离 print("已经达到目标距离，无需移动") return True # 设置移动命令 msg.mode = 11 # Locomotion模式 msg.gait_id = 26 # 自变频步态 # 移动方向设置 forward = distance_to_move > 0 # 判断是前进还是后退 # 设置移动速度 move_speed = 1 # 米/秒 if forward: msg.vel_des = [move_speed, 0, 0] # 设置前进速度 else: msg.vel_des = [-move_speed, 0, 0] # 设置后退速度 # 获取起始位置 start_position = list(ctrl.odo_msg.xyz) # 转换为列表，因为xyz是元组 if observe: print(f"起始位置: {start_position}") print(f"开始移动: {'前进' if forward else '后退'} {abs(distance_to_move):.3f}米") # 在起点放置绿色标记 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) # 估算移动时间，但实际上会通过里程计控制 move_time = abs(distance_to_move) / move_speed msg.duration = int((move_time + 2) * 1000) # 加一点余量，确保有足够时间移动 msg.step_height = [0.06, 0.06] # 抬腿高度 msg.life_count += 1 # 发送命令 ctrl.Send_cmd(msg) # 使用里程计进行实时监控移动距离 distance_moved = 0 start_time = time.time() timeout = move_time + 1 # 超时时间设置为预计移动时间加1秒 # 监控移动距离，但不执行减速（改用stop_smooth） while distance_moved < abs(distance_to_move) * 0.95 and time.time() - start_time < timeout: # 计算已移动距离 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 and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 print(f"已移动: {distance_moved:.3f}米, 目标: {abs(distance_to_move):.3f}米") time.sleep(0.05) # 小间隔检查位置 # 使用平滑停止方法 if forward: current_vel = [move_speed, 0, 0] else: current_vel = [-move_speed, 0, 0] # 使用平滑停止 ctrl.base_msg.stop_smooth(current_vel=current_vel) if observe: print(f"移动完成，平稳停止，通过里程计计算的移动距离: {distance_moved:.3f}米") # 在终点放置红色标记 end_position = list(ctrl.odo_msg.xyz) if hasattr(ctrl, 'place_marker'): ctrl.place_marker(end_position[0], end_position[1], end_position[2] if len(end_position) > 2 else 0.0, 'red', observe=True) # 如果没有提供图像处理器或图像验证失败，则使用里程计数据判断 return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功 def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left=True, observe=False): """ 对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。 参数: ctrl: Robot_Ctrl对象 msg: robot_control_cmd_lcmt对象 target_distance: 横线前目标保持距离，默认0.5米 angle_deg: 旋转角度，支持90或180度 left: True为左转，False为右转 observe: 是否打印调试信息 返回: bool: 是否成功完成动作 """ # 1. 对准横线 print("校准到横向线水平") aligned = align_to_horizontal_line(ctrl, msg, observe=observe) if not aligned: print("无法校准到横向线水平，停止动作") return False # 2. 检测横线并计算距离 image = ctrl.image_processor.get_current_image() edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe) if edge_point is None or edge_info is None: print("无法检测到横向线，停止动作") return False camera_height = 0.355 # 单位: 米 r = calculate_distance_to_line(edge_info, camera_height, observe=observe) # 减去目标距离 r -= target_distance if r is None: print("无法计算到横向线的距离，停止动作") return False if observe: print(f"当前距离: {r:.3f}米") # 3. 计算圆弧运动参数 angle_rad = math.radians(angle_deg) # 设定角速度（rad/s），可根据实际调整 base_w = 0.6 w = base_w if left else -base_w # 左转为正，右转为负 v = abs(w * r) # 线速度，正负号与角速度方向一致 t = abs(angle_rad / w) # 运动时间，取绝对值保证为正 if observe: print(f"圆弧半径: {r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s, 运动时间: {t:.2f}s") # 4. 发送圆弧运动指令 msg.mode = 11 msg.gait_id = 26 msg.vel_des = [v, 0, w] # [前进速度, 侧向速度, 角速度] msg.duration = int((t + 2) * 1000) # 加2秒余量 msg.step_height = [0.06, 0.06] msg.life_count += 1 ctrl.Send_cmd(msg) # 记录起始角度和位置 start_yaw = ctrl.odo_msg.rpy[2] start_position = list(ctrl.odo_msg.xyz) # 计算圆弧长度 arc_length = abs(angle_rad * r) # 进入循环，实时判断 distance_moved = 0 angle_turned = 0 start_time = time.time() timeout = t + 2 # 给个超时保护 # 初始速度 current_v = v current_w = w # 旋转精度监控 target_angle = angle_rad if left else -angle_rad # 目标角度（弧度） slowdown_threshold = 0.85 # 当达到目标角度的85%时开始减速 # 监控旋转进度并自行控制减速 while abs(angle_turned) < abs(angle_rad) * 0.98 and time.time() - start_time < timeout: # 计算已移动距离 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) # 计算已旋转角度 current_yaw = ctrl.odo_msg.rpy[2] angle_turned = current_yaw - start_yaw # 角度归一化处理 while angle_turned > math.pi: angle_turned -= 2 * math.pi while angle_turned < -math.pi: angle_turned += 2 * math.pi # 计算角度完成比例 completion_ratio = abs(angle_turned) / abs(angle_rad) # 当完成一定比例时开始减速 if completion_ratio > slowdown_threshold: # 剩余比例作为速度系数 speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold) # 确保速度系数不会太小 speed_factor = max(0.3, speed_factor) # 应用减速 current_v = v * speed_factor current_w = w * speed_factor # 更新速度命令 msg.mode = 11 msg.gait_id = 26 msg.vel_des = [current_v, 0, current_w] msg.duration = 200 # 短时间更新 msg.step_height = [0.06, 0.06] msg.life_count += 1 ctrl.Send_cmd(msg) if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 print(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度因子: {speed_factor:.2f}, 当前角速度: {current_w:.3f}rad/s") else: if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 print(f"已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度") time.sleep(0.05) # 直接发送停止命令，不使用平滑停止 msg.mode = 11 msg.gait_id = 26 msg.vel_des = [0, 0, 0] # 零速度 msg.duration = 200 msg.step_height = [0.06, 0.06] msg.life_count += 1 ctrl.Send_cmd(msg) # 停下来后的最终角度 final_yaw = ctrl.odo_msg.rpy[2] final_angle_turned = final_yaw - start_yaw # 角度归一化处理 while final_angle_turned > math.pi: final_angle_turned -= 2 * math.pi while final_angle_turned < -math.pi: final_angle_turned += 2 * math.pi final_angle_deg = math.degrees(final_angle_turned) target_angle_deg = angle_deg if left else -angle_deg if observe: print(f"旋转完成，目标角度: {target_angle_deg:.2f}度, 实际角度: {final_angle_deg:.2f}度") # 检查是否需要微调 angle_error = abs(final_angle_deg - target_angle_deg) if angle_error > 3.0: # 如果误差超过3度 if observe: print(f"角度误差: {angle_error:.2f}度，进行微调") # 计算需要微调的角度 adjust_angle = target_angle_deg - final_angle_deg # 使用turn_degree函数进行微调 turn_degree(ctrl, msg, adjust_angle, absolute=False) if observe: print("圆弧转弯完成") return True # 用法示例 if __name__ == "__main__": move_to_hori_line(None, None, observe=True) # 90度左转 arc_turn_around_hori_line(None, None, angle_deg=90, left=True, observe=True) # 180度右转 arc_turn_around_hori_line(None, None, angle_deg=180, left=False, observe=True)