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_straight_with_qrcode 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 base_move.move_base_hori_line import ( detect_horizontal_track_edge, detect_horizontal_track_edge_v2, detect_horizontal_track_edge_v3, calculate_distance_to_line, move_to_hori_line, arc_turn_around_hori_line ) from base_move.center_on_dual_tracks import center_on_dual_tracks # from base_move.follow_dual_tracks import follow_dual_tracks def go_straight_to_horizontal_line_with_qr(ctrl, msg, target_distance=0.5, speed=0.5, max_distance=10, detect_func_version=2, qr_check_interval=0.3, observe=False): """ 控制机器人直线行走，直到与横向线距离为指定值，同时识别路径上的二维码 参数: ctrl: Robot_Ctrl 对象，包含里程计信息 msg: robot_control_cmd_lcmt 对象，用于发送命令 target_distance: 与横向线的目标距离(米)，默认为0.5米 speed: 行走速度(米/秒)，默认为0.5米/秒 max_distance: 最大行走距离(米)，超过此距离将停止，默认为10米 detect_func_version: 检测横线的函数版本，默认为2 qr_check_interval: 检查二维码的时间间隔(秒)，默认为0.3秒 observe: 是否输出中间状态信息，默认为False 返回: tuple: (是否成功, 二维码结果, 额外信息字典) """ # 返回结果字典，包含过程中的状态信息 res = { 'qr_result': None, 'success': False, 'distance_moved': 0, 'target_reached': False } # 启动异步QR码扫描 qr_result = None try: ctrl.image_processor.start_async_scan(interval=qr_check_interval) if observe: info("已启动异步QR码扫描", "扫描") except Exception as e: if observe: error(f"启动QR码扫描失败: {e}", "失败") # 获取起始位置 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) # 设置移动命令 msg.mode = 11 # Locomotion模式 msg.gait_id = 26 # 自变频步态 msg.vel_des = [speed, 0, 0] # [前进速度, 侧向速度, 角速度] msg.duration = 0 # wait next cmd msg.step_height = [0.06, 0.06] # 抬腿高度 # 开始移动 msg.life_count += 1 ctrl.Send_cmd(msg) # 记录起始时间和上次QR码检查时间 start_time = time.time() last_qr_check_time = start_time # 相机高度 (单位: 米) camera_height = 0.355 # 存储上一次检测到的横线距离 last_hori_line_distance = None # 主循环：持续移动直到达到目标或最大距离 distance_moved = 0 timeout = max_distance / speed + 5 # 超时时间 while distance_moved < max_distance 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) # 检测横向线 image = ctrl.image_processor.get_current_image() if detect_func_version == 1: edge_point, edge_info = detect_horizontal_track_edge(image, observe=False, save_log=True) elif detect_func_version == 2: edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=False, save_log=True) elif detect_func_version == 3: edge_point, edge_info = detect_horizontal_track_edge_v3(image, observe=False, save_log=True) else: edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=False, save_log=True) # 如果检测到横向线 if edge_point is not None and edge_info is not None: # 计算到横向线的距离 current_distance = calculate_distance_to_line(edge_info, camera_height, observe=False) last_hori_line_distance = current_distance if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 info(f"检测到横向线，距离: {current_distance:.3f}米，目标距离: {target_distance:.3f}米", "检测") # 判断是否达到目标距离 if current_distance <= target_distance + 0.1: if observe: success(f"已达到目标距离，当前距离: {current_distance:.3f}米", "完成") # 平滑停止 if hasattr(ctrl.base_msg, 'stop_smooth'): ctrl.base_msg.stop_smooth() else: ctrl.base_msg.stop() res['success'] = True res['target_reached'] = True res['final_distance'] = current_distance break # 根据距离动态调整速度 # 离目标越近，速度越慢 if current_distance - target_distance < 2.0: # 线性降低速度 speed_factor = min(1.0, max(0.3, (current_distance - target_distance) / 2.0)) new_speed = speed * speed_factor if observe and abs(new_speed - msg.vel_des[0]) > 0.05: info(f"调整速度: {msg.vel_des[0]:.2f} -> {new_speed:.2f} 米/秒", "速度") msg.vel_des[0] = new_speed msg.life_count += 1 ctrl.Send_cmd(msg) # 检查QR码扫描结果 current_time = time.time() if current_time - last_qr_check_time >= qr_check_interval: qr_data, scan_time = ctrl.image_processor.get_last_qr_result() if qr_data and scan_time > start_time: qr_result = qr_data res['qr_result'] = qr_result if observe: success(f"扫描到QR码: {qr_data}", "扫描") last_qr_check_time = current_time # 输出调试信息 if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 debug(f"已移动: {distance_moved:.3f}米, 最大距离: {max_distance:.3f}米", "移动") time.sleep(0.05) # 控制循环频率 # 停止移动 if not res['target_reached']: if observe: if distance_moved >= max_distance: warning(f"已达到最大移动距离 {max_distance:.3f}米，但未检测到横向线", "停止") else: warning("超时停止", "停止") ctrl.base_msg.stop() res['success'] = False # 停止异步QR码扫描 ctrl.image_processor.stop_async_scan() # 最后一次检查QR码结果 qr_data, scan_time = ctrl.image_processor.get_last_qr_result() if qr_data and (qr_result is None or scan_time > last_qr_check_time): qr_result = qr_data res['qr_result'] = qr_result if observe: success(f"最终扫描到QR码: {qr_data}", "扫描") # 记录最终位置和移动距离 final_position = ctrl.odo_msg.xyz dx = final_position[0] - start_position[0] dy = final_position[1] - start_position[1] final_distance_moved = math.sqrt(dx*dx + dy*dy) res['distance_moved'] = final_distance_moved 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"移动完成，总距离: {final_distance_moved:.3f}米", "完成") if qr_result: info(f"扫描到的QR码: {qr_result}", "结果") return res['success'], qr_result, res def run_task_5(ctrl, msg, direction='left', observe=False): """ 走向卸货 """ center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False) section('任务5-1：直线移动并扫描二维码', "移动") # 最大移动距离为8米 max_distance = 2 # 开始移动并扫描二维码 go_success, res = go_straight_with_qrcode(ctrl, msg, distance=max_distance, speed=1, observe=observe) # 输出结果 if go_success: success("成功到达横线前指定距离", "完成") if res['qr_result']: info(f"扫描到二维码: {res['qr_result']}", "二维码") else: warning("未扫描到二维码", "二维码") else: error("未能成功到达横线前指定距离", "失败") section('任务5-2：移动到卸货点', "移动") center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False) if direction == 'right' and res['qr_result'] == 'B-2' or direction == 'left' and res['qr_result'] == 'B-1': # 直走 move_to_hori_line(ctrl, msg, target_distance=1.4, observe=observe, detect_func_version=4) else: move_to_hori_line(ctrl, msg, target_distance=2, observe=observe, detect_func_version=4) section('任务5-3：卸货', "卸货") ctrl.base_msg.lie_down(wait_time=3000) ctrl.base_msg.stand_up() # section('任务5-4：返回', "移动") go_straight(ctrl, msg, distance=-1, speed=0.5, observe=observe) move_to_hori_line(ctrl, msg, target_distance=1.3, observe=observe) section('任务5-5：转弯', "转弯") turn_degree_v2(ctrl, msg, degree=90) go_straight(ctrl, msg, distance=1.2, speed=0.5, observe=observe) section('任务5-6：转弯', "转弯") arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.3, observe=observe) # 返回移动和扫描结果 return go_success, res['qr_result']