import math import time import cv2 import numpy as np 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: # 检测横向线边缘 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 isinstance(image, str): img = cv2.imread(image) else: img = image.copy() height, width = img.shape[:2] center_x = width // 2 # 画出检测到的横向线 line_length = 200 end_x = edge_point[0] + line_length end_y = int(edge_point[1] + slope * line_length) start_x = edge_point[0] - line_length start_y = int(edge_point[1] - slope * line_length) cv2.line(img, (start_x, start_y), (end_x, end_y), (0, 255, 0), 2) # 画出水平线（目标线） horizontal_y = edge_point[1] cv2.line(img, (center_x - line_length, horizontal_y), (center_x + line_length, horizontal_y), (0, 0, 255), 2) # 标记角度 cv2.putText(img, f"当前斜率: {slope:.4f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) cv2.putText(img, f"旋转角度: {angle_to_rotate:.2f}°", (10, 70), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) cv2.imshow("校准旋转分析", img) cv2.waitKey(1500 if observe else 1) # 执行旋转 # 如果角度很小，增加一个小的偏移以确保旋转足够 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): """ 根据边缘信息和相机高度计算到横向线的实际距离 参数: edge_info: 边缘信息字典，包含distance_to_bottom等信息 camera_height: 相机高度(米) 返回: float: 到横向线的估计距离(米) """ if edge_info is None or "distance_to_bottom" not in edge_info: return None # 获取交点到图像底部的像素距离 distance_in_pixels = edge_info["distance_to_bottom"] # 计算比例系数 (需要根据实际场景标定) # 这里使用一个简化的算法，假设像素距离与实际距离成反比；实际应用中应该进行相机标定和测试以获得更准确的参数 # 假设FOV (视场角)为84度 (1.46608弧度), 图像高度为1080像素 fov_vertical = 1.46608 / 16 * 9 # 假设16:9的图像比例计算垂直FOV image_height = 1080 # 从robot.xacro中获取 # 计算每个像素对应的角度 angle_per_pixel = fov_vertical / image_height # 计算交点对应的角度 angle = angle_per_pixel * distance_in_pixels # 使用相机高度和角度计算距离 (使用正切函数) # 距离 = 相机高度 / tan(角度) distance = camera_height / math.tan(angle) return max(0.0, distance) # 确保距离是正数 def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False): """ 控制机器人校准并移动到横向线前的指定距离 参数: ctrl: Robot_Ctrl 对象，包含里程计信息 msg: robot_control_cmd_lcmt 对象，用于发送命令 target_distance: 目标与横向线的距离(米)，默认为0.1米 observe: 是否输出中间状态信息和可视化结果，默认为False 返回: bool: 是否成功到达目标位置 """ # 首先校准到水平 aligned = align_to_horizontal_line(ctrl, msg, observe=observe) if not aligned: print("无法校准到横向线水平，停止移动") return False # 检测横向线 edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_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) 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}米") # 估算移动时间，但实际上会通过里程计控制 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 + 5 # 超时时间设置为预计移动时间加5秒 while distance_moved < abs(distance_to_move) 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}米") # 如果已经接近目标距离，准备停止 if distance_moved >= abs(distance_to_move) * 0.95: break time.sleep(0.05) # 小间隔检查位置 # 发送停止命令 msg.vel_des = [0, 0, 0] msg.life_count += 1 ctrl.Send_cmd(msg) if observe: print(f"移动完成，通过里程计计算的移动距离: {distance_moved:.3f}米") # 如果没有提供图像处理器或图像验证失败，则使用里程计数据判断 return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功 # 用法示例 if __name__ == "__main__": # 这里需要替换为实际的控制器和消息对象 # ctrl = Robot_Ctrl() # msg = robot_control_cmd_lcmt() # image_processor = ImageProcessor() # 使用示例图像路径 image_path = "path/to/image.jpg" # 基本用法 # align_to_horizontal_line(ctrl, msg, image_path, observe=True) # move_to_hori_line(ctrl, msg, image_path, target_distance=0.1, observe=True) # 使用里程计和图像处理器进行更精确的定位 # 从图像处理器获取当前图像 # current_image = image_processor.get_image() # move_to_hori_line(ctrl, msg, current_image, target_distance=0.1, observe=True, image_processor=image_processor)