264 lines
11 KiB
Python
264 lines
11 KiB
Python
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.2, 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']
|