mi-task/task_5/task_5.py

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

SLEEP_TIME = 3000


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, time_sleep=5000):
    """
    走向卸货
    """
    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_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=time_sleep)
    # 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.30, observe=observe)

    section('任务5-5：转弯', "转弯")
    turn_degree_v2(ctrl, msg, degree=179, absolute=True)
    go_straight(ctrl, msg, distance=1.4, speed=0.6, observe=observe)

    section('任务5-6：转弯', "转弯")
    arc_turn_around_hori_line(ctrl, msg, angle_deg=-85, target_distance=0.3, observe=observe, no_end_reset=True)

    # section('任务5-5：上货', "卸货")
    # ctrl.base_msg.lie_down(wait_time=SLEEP_TIME)
    # ctrl.base_msg.stand_up()

    section('任务5-7：返回', "移动")
    turn_degree_v2(ctrl, msg, degree=-90, absolute=True)

    # 返回移动和扫描结果
    return go_success, res['qr_result']