1305 lines
57 KiB
Python
1305 lines
57 KiB
Python
import math
|
||
import time
|
||
import cv2
|
||
import numpy as np
|
||
|
||
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, detect_horizontal_track_edge_v2, detect_horizontal_track_edge_v3,
|
||
detect_left_side_track
|
||
)
|
||
from utils.detect_furthest_intersection import detect_furthest_horizontal_intersection
|
||
from base_move.turn_degree import turn_degree
|
||
from base_move.go_straight import go_straight
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
|
||
# 创建此模块特定的日志记录器
|
||
logger = get_logger("横线移动")
|
||
|
||
def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3, detect_func_version=1):
|
||
"""
|
||
控制机器人旋转到横向线水平的位置
|
||
|
||
参数:
|
||
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()
|
||
accumulated_angle = 0
|
||
max_total_rotation = 45
|
||
angle_accuracy_threshold = 2.0
|
||
|
||
# 记录初始位置
|
||
initial_yaw = ctrl.odo_msg.rpy[2]
|
||
|
||
while attempts < max_attempts and not aligned:
|
||
section(f"尝试校准横线 {attempts+1}/{max_attempts}", "校准")
|
||
|
||
# 检测横向线边缘; 不同版本
|
||
if detect_func_version == 1:
|
||
edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0, save_log=True)
|
||
elif detect_func_version == 2:
|
||
edge_point, edge_info = detect_horizontal_track_edge_v2(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0, save_log=True)
|
||
elif detect_func_version == 3:
|
||
edge_point, edge_info = detect_horizontal_track_edge_v3(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0, save_log=True)
|
||
elif detect_func_version == 4:
|
||
edge_point, edge_info = detect_furthest_horizontal_intersection(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0, save_log=True)
|
||
else:
|
||
error("未指定检测版本，请检查参数", "失败")
|
||
return False
|
||
|
||
print('😺', edge_info)
|
||
|
||
if edge_point is None or edge_info is None:
|
||
error("未检测到横向线，无法进行校准", "失败")
|
||
|
||
# 尝试小幅度摇头寻找横线
|
||
# TODO 或者改成上下低头?
|
||
if attempts < max_attempts - 1:
|
||
small_angle = 5 * (1 if attempts % 2 == 0 else -1)
|
||
info(f"尝试摇头 {small_angle}度 寻找横线", "校准")
|
||
turn_degree(ctrl, msg, small_angle, absolute=False)
|
||
time.sleep(0.5)
|
||
|
||
attempts += 1
|
||
continue
|
||
|
||
slope = edge_info["slope"]
|
||
is_horizontal = edge_info["is_horizontal"]
|
||
|
||
if observe:
|
||
debug(f"检测到横向线，斜率: {slope:.6f}", "检测")
|
||
info(f"是否足够水平: {is_horizontal}", "检测")
|
||
|
||
if is_horizontal:
|
||
success("横向线已经水平，无需校准", "完成")
|
||
return True
|
||
|
||
# 计算需要旋转的角度
|
||
angle_rad = math.atan(slope)
|
||
angle_deg = math.degrees(angle_rad)
|
||
angle_to_rotate = -angle_deg # 取负值使旋转方向正确
|
||
|
||
# 设置最小旋转阈值
|
||
if abs(angle_to_rotate) < 0.5: # 降低最小旋转阈值
|
||
angle_to_rotate = math.copysign(0.5, angle_to_rotate)
|
||
|
||
# 检查累积旋转
|
||
if abs(accumulated_angle + angle_to_rotate) > max_total_rotation:
|
||
warning(f"累积旋转角度({accumulated_angle + angle_to_rotate:.2f}°)过大，限制旋转", "警告")
|
||
angle_to_rotate = angle_to_rotate / 2
|
||
|
||
if observe:
|
||
info(f"计算旋转角度: {angle_to_rotate:.2f}度", "角度")
|
||
info(f"当前累积旋转: {accumulated_angle:.2f}度", "累积")
|
||
|
||
# 使用turn_degree函数执行旋转，增加精度参数
|
||
info(f"旋转角度: {angle_to_rotate:.2f}度", "旋转")
|
||
turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False, precision=True)
|
||
info(f"旋转结果: {turn_success}", "旋转结果")
|
||
|
||
# 等待稳定
|
||
time.sleep(0.3)
|
||
|
||
# 计算实际旋转角度
|
||
current_yaw = ctrl.odo_msg.rpy[2]
|
||
actual_rotation = math.degrees(current_yaw - initial_yaw - math.radians(accumulated_angle))
|
||
|
||
# 规范化角度
|
||
while actual_rotation > 180:
|
||
actual_rotation -= 360
|
||
while actual_rotation < -180:
|
||
actual_rotation += 360
|
||
|
||
accumulated_angle += actual_rotation
|
||
|
||
if observe:
|
||
info(f"请求旋转: {angle_to_rotate:.2f}度, 实际旋转: {actual_rotation:.2f}度", "旋转")
|
||
info(f"旋转结果: {'成功' if turn_success else '失败'}", "成功" if turn_success else "失败")
|
||
|
||
attempts += 1
|
||
|
||
# 检查校准结果
|
||
if detect_func_version == 1:
|
||
edge_point_after, edge_info_after = detect_horizontal_track_edge(
|
||
ctrl.image_processor.get_current_image(),
|
||
observe=observe,
|
||
delay=1000 if observe else 0,
|
||
save_log=True
|
||
)
|
||
elif detect_func_version == 2:
|
||
edge_point_after, edge_info_after = detect_horizontal_track_edge_v2(
|
||
ctrl.image_processor.get_current_image(),
|
||
observe=observe,
|
||
delay=1000 if observe else 0,
|
||
save_log=True
|
||
)
|
||
|
||
if edge_info_after and "slope" in edge_info_after:
|
||
current_slope = edge_info_after["slope"]
|
||
current_angle_deg = math.degrees(math.atan(current_slope))
|
||
|
||
if observe:
|
||
info(f"校准后斜率: {current_slope:.6f}, 角度: {current_angle_deg:.2f}度", "检测")
|
||
|
||
if abs(current_angle_deg) < angle_accuracy_threshold:
|
||
success(f"校准成功，当前角度: {current_angle_deg:.2f}度", "完成")
|
||
aligned = True
|
||
else:
|
||
warning(f"校准后角度仍超出阈值: {abs(current_angle_deg):.2f}° > {angle_accuracy_threshold}°", "警告")
|
||
|
||
# 最后一次尝试的妥协处理
|
||
if attempts == max_attempts and not aligned and edge_info_after:
|
||
current_slope = edge_info_after["slope"]
|
||
current_angle_deg = math.degrees(math.atan(current_slope))
|
||
|
||
if abs(current_angle_deg) < angle_accuracy_threshold * 1.5:
|
||
warning(f"达到最大尝试次数，接受近似结果，当前角度: {current_angle_deg:.2f}度", "妥协")
|
||
aligned = True
|
||
|
||
# 恢复最佳状态的处理
|
||
if not aligned and abs(accumulated_angle) > 15:
|
||
warning(f"校准失败，累积旋转{accumulated_angle:.2f}度，尝试恢复到最佳状态", "恢复")
|
||
|
||
best_edge_point, best_edge_info = detect_horizontal_track_edge(
|
||
ctrl.image_processor.get_current_image(),
|
||
observe=observe,
|
||
save_log=True
|
||
)
|
||
|
||
if best_edge_info and "slope" in best_edge_info:
|
||
best_slope = best_edge_info["slope"]
|
||
best_angle_deg = math.degrees(math.atan(best_slope))
|
||
|
||
if abs(best_angle_deg) < angle_accuracy_threshold * 1.5:
|
||
info(f"当前状态已经接近水平，角度: {best_angle_deg:.2f}度", "保持")
|
||
return 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:
|
||
debug(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:
|
||
debug(f"相机参数: 水平FOV={math.degrees(horizontal_fov_rad):.1f}°, 垂直FOV={vertical_fov_deg:.1f}°", "计算")
|
||
debug(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:
|
||
debug("视场角度关系:", "计算")
|
||
debug(f" - 图像底部到横线角度: {math.degrees(bottom_to_line_angle_rad):.2f}°", "角度")
|
||
debug(f" - 视场中心到横线角度: {math.degrees(center_to_line_angle_rad):.2f}°", "角度")
|
||
debug(f" - 相机倾斜角度: {camera_tilt_angle_deg}°", "角度")
|
||
debug(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:
|
||
warning(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:
|
||
info(f"计算公式: 距离 = 相机高度({camera_height}米) / tan(|视线角度|({abs(math.degrees(view_angle_rad)):.2f}°))", "计算")
|
||
info(f"计算结果: 距离 = {ground_distance:.3f}米", "距离")
|
||
else: # 视线平行或向上，无法确定地面交点
|
||
if observe:
|
||
warning(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:
|
||
warning(f"应用范围限制: 原始距离 {distance:.3f}米 -> 最终距离 {final_distance:.3f}米", "警告")
|
||
elif observe:
|
||
info(f"最终距离: {final_distance:.3f}米", "距离")
|
||
|
||
return final_distance
|
||
|
||
def go_straight_by_hori_line(ctrl, msg, distance=0.5, speed=0.5, observe=False):
|
||
"""
|
||
根据横向线位置，控制机器人移动到指定距离
|
||
# INFO 本质就是封装了一下 go-straight，但是需要先校准到水平
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
distance: 目标位置与横向线的距离(米)，默认为0.5米
|
||
speed: 移动速度(米/秒)，默认为0.5米/秒
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
"""
|
||
# 首先校准到水平
|
||
section("校准到横向线水平", "校准")
|
||
aligned = align_to_horizontal_line(ctrl, msg, detect_func_version=2, observe=observe)
|
||
|
||
if not aligned:
|
||
error("无法校准到横向线水平，停止移动", "停止")
|
||
return False
|
||
else:
|
||
success("校准完成", "完成")
|
||
|
||
go_straight(ctrl, msg, distance=distance, speed=speed, observe=observe)
|
||
|
||
def move_to_hori_line(ctrl, msg, target_distance=0.5, absolute_deg=200, observe=False,
|
||
detect_func_version=1,
|
||
):
|
||
"""
|
||
控制机器人校准并移动到横向线前的指定距离
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
target_distance: 目标位置与横向线的距离(米)，默认为0.5米
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
|
||
返回:
|
||
bool: 是否成功到达目标位置
|
||
"""
|
||
# 首先校准到水平
|
||
section("校准到横向线水平", "校准")
|
||
if absolute_deg < 200:
|
||
aligned = turn_degree(ctrl, msg, absolute_deg, absolute=True)
|
||
aligned = turn_degree(ctrl, msg, absolute_deg, absolute=True)
|
||
else:
|
||
aligned = align_to_horizontal_line(ctrl, msg, observe=observe, detect_func_version=detect_func_version)
|
||
|
||
if not aligned:
|
||
error("无法校准到横向线水平，停止移动", "停止")
|
||
return False
|
||
|
||
# 检测横向线
|
||
image = ctrl.image_processor.get_current_image()
|
||
if detect_func_version == 1:
|
||
edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True)
|
||
elif detect_func_version == 2:
|
||
edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=observe, save_log=True)
|
||
elif detect_func_version == 3:
|
||
edge_point, edge_info = detect_horizontal_track_edge_v3(image, observe=observe, save_log=True)
|
||
elif detect_func_version == 4:
|
||
edge_point, edge_info = detect_furthest_horizontal_intersection(image, observe=observe, save_log=True)
|
||
|
||
if edge_point is None or edge_info is None:
|
||
error("无法检测到横向线，停止移动", "停止")
|
||
return False
|
||
print(f"edge_info: {edge_info}")
|
||
|
||
# 获取相机高度
|
||
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:
|
||
error("无法计算到横向线的距离，停止移动", "停止")
|
||
return False
|
||
|
||
if observe:
|
||
info(f"当前距离: {current_distance:.3f}米, 目标距离: {target_distance:.3f}米", "距离")
|
||
|
||
# 计算需要移动的距离
|
||
distance_to_move = current_distance - target_distance
|
||
|
||
if abs(distance_to_move) < 0.05: # 如果已经很接近目标距离
|
||
success("已经达到目标距离，无需移动", "完成")
|
||
return True
|
||
|
||
# 设置移动命令
|
||
msg.mode = 11 # Locomotion模式
|
||
msg.gait_id = 26 # 自变频步态
|
||
|
||
# 移动方向设置
|
||
forward = distance_to_move > 0 # 判断是前进还是后退
|
||
|
||
# 设置移动速度
|
||
# 根据需要移动的距离动态调整移动速度
|
||
if abs(distance_to_move) > 1.0:
|
||
move_speed = 1.0 # 距离较远时用最大速度
|
||
elif abs(distance_to_move) > 0.5:
|
||
move_speed = 0.6 # 中等距离用中等速度
|
||
elif abs(distance_to_move) > 0.2:
|
||
move_speed = 0.3 # 较近距离用较慢速度
|
||
else:
|
||
move_speed = 0.15 # 非常接近时用最慢速度
|
||
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:
|
||
debug(f"起始位置: {start_position}", "位置")
|
||
info(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 = 0 # wait next cmd
|
||
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 + 2 # 增加超时时间为预计移动时间加2秒
|
||
|
||
# 监控移动距离，增加移动距离的系数
|
||
while distance_moved < abs(distance_to_move) * 1.05 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秒左右打印一次
|
||
debug(f"已移动: {distance_moved:.3f}米, 目标: {abs(distance_to_move):.3f}米", "移动")
|
||
|
||
time.sleep(0.05) # 小间隔检查位置
|
||
|
||
# 使用平滑停止
|
||
# ctrl.base_msg.stop_smooth()
|
||
ctrl.base_msg.stop()
|
||
|
||
if observe:
|
||
success(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.15 # 如果误差小于15厘米，则认为成功
|
||
|
||
def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.2,
|
||
pass_align=False,
|
||
# radius
|
||
radius=None,
|
||
min_radius=0.1,
|
||
max_radius=5,
|
||
# mode
|
||
detect_func_version=1,
|
||
# Qrcode
|
||
scan_qrcode=False,
|
||
qr_check_interval=0.3,
|
||
# additional
|
||
observe=False,
|
||
bad_big_angle_corret=False,
|
||
no_end_reset=False,):
|
||
"""
|
||
对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。
|
||
参数:
|
||
ctrl: Robot_Ctrl对象
|
||
msg: robot_control_cmd_lcmt对象
|
||
target_distance: 横线前目标保持距离，默认0.5米
|
||
angle_deg: 旋转角度，正值为左转，负值为右转
|
||
pass_align: 是否跳过对准步骤，默认为False
|
||
observe: 是否打印调试信息
|
||
scan_qrcode: 是否在旋转过程中扫描QR码，默认为False
|
||
qr_check_interval: QR码检查间隔时间(秒)，默认为0.3秒
|
||
|
||
bad_big_angle_corret: 是否对大角度错误进行修正，默认为False # TODO clear
|
||
no_end_reset: 是否在旋转完成后不进行位置重置，默认为False
|
||
返回:
|
||
bool或元组: 如果scan_qrcode为False，返回bool表示是否成功完成操作；
|
||
如果scan_qrcode为True，返回(bool, str)元组，表示(是否成功完成操作, QR码扫描结果)
|
||
"""
|
||
|
||
# 返回此任务的中间状态
|
||
res = {}
|
||
|
||
# 启动异步QR码扫描（如果需要）
|
||
qr_result = None
|
||
if scan_qrcode:
|
||
# 确保image_processor存在
|
||
if not hasattr(ctrl, 'image_processor') or ctrl.image_processor is None:
|
||
warning("无法启用QR码扫描，image_processor不存在", "警告")
|
||
scan_qrcode = False
|
||
else:
|
||
# 启动异步扫描
|
||
try:
|
||
ctrl.image_processor.start_async_scan(interval=0.2)
|
||
info("已启动异步QR码扫描", "扫描")
|
||
except Exception as e:
|
||
error(f"启动QR码扫描失败: {e}", "失败")
|
||
scan_qrcode = False
|
||
|
||
# 1. 对准横线
|
||
if not pass_align:
|
||
section("校准到横向线水平", "校准")
|
||
aligned = align_to_horizontal_line(ctrl, msg, detect_func_version=detect_func_version, observe=observe)
|
||
if not aligned:
|
||
error("无法校准到横向线水平，停止动作", "停止")
|
||
if scan_qrcode:
|
||
ctrl.image_processor.stop_async_scan()
|
||
return False, res
|
||
return False, res
|
||
else:
|
||
info("跳过对准步骤", "信息")
|
||
|
||
# 2. 检测横线并计算距离
|
||
r = radius
|
||
if r is None:
|
||
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:
|
||
error("无法检测到横向线，停止动作", "停止")
|
||
if scan_qrcode:
|
||
ctrl.image_processor.stop_async_scan()
|
||
return False, res
|
||
return False, res
|
||
|
||
camera_height = 0.355 # 单位: 米
|
||
r = calculate_distance_to_line(edge_info, camera_height, observe=observe)
|
||
if r is None:
|
||
error("无法计算到横向线的距离，停止动作", "停止")
|
||
if scan_qrcode:
|
||
ctrl.image_processor.stop_async_scan()
|
||
return False, res
|
||
return False, res
|
||
|
||
# 减去目标距离
|
||
r -= target_distance
|
||
res['radius'] = r
|
||
|
||
if observe:
|
||
info(f"当前距离: {r:.3f}米", "距离")
|
||
|
||
# 3. 计算圆弧运动参数
|
||
# 根据角度大小调整时间补偿系数
|
||
abs_angle_deg = abs(angle_deg)
|
||
if abs_angle_deg <= 70:
|
||
time_compensation = 0.78 # 对70度及以下角度使用更小的补偿系数
|
||
elif abs_angle_deg <= 90:
|
||
time_compensation = 0.85 # 90度左右使用稍大的系数
|
||
else:
|
||
# 对180度旋转，增加补偿系数，使旋转时间更长
|
||
time_compensation = 1.4 # 增加40%的时间
|
||
|
||
# 调整实际目标角度，针对不同角度应用不同的缩放比例
|
||
actual_angle_deg = angle_deg
|
||
if abs_angle_deg <= 70:
|
||
actual_angle_deg = angle_deg * 0.85 # 70度及以下角度减少到85%
|
||
elif abs_angle_deg <= 90:
|
||
actual_angle_deg = angle_deg * 0.9 # 90度减少到90%
|
||
elif abs_angle_deg >= 170: # 处理大角度旋转，特别是180度情况
|
||
actual_angle_deg = angle_deg # 对于180度旋转，使用完整角度，不进行缩放
|
||
|
||
if observe and actual_angle_deg != angle_deg:
|
||
info(f"应用角度补偿: 目标{angle_deg}度 -> 实际指令{actual_angle_deg:.1f}度", "角度")
|
||
|
||
angle_rad = math.radians(actual_angle_deg)
|
||
|
||
# 设定角速度（rad/s），角度的符号决定了转向方向
|
||
base_w = 0.8
|
||
w = math.copysign(base_w, angle_deg) # 保持角速度与角度同号
|
||
|
||
# 确保r有一个最小值，避免速度过低
|
||
effective_r = min(max_radius, max(r, min_radius))
|
||
|
||
# 线速度，必须与角速度和半径的关系一致：v = w * r
|
||
# 这样才能保证走圆弧
|
||
v = abs(w * effective_r) # 线速度，正负号与角速度方向一致
|
||
|
||
info(f"开始移动圆弧，半径: {effective_r:.3f}米, 线速度: {v:.3f}m/s, 角速度: {w:.3f}rad/s")
|
||
if observe:
|
||
if effective_r != r:
|
||
warning(f"注意: 实际半径 {r:.3f}米，使用半径 {effective_r:.3f}米 计算速度", "警告")
|
||
|
||
t = abs(angle_rad / w) # 运动时间，取绝对值保证为正
|
||
|
||
# 应用时间补偿系数
|
||
t *= time_compensation
|
||
|
||
if observe:
|
||
print(f"圆弧半径: {effective_r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s")
|
||
print(f"理论运动时间: {abs(angle_rad / w):.2f}s, 应用补偿系数{time_compensation}后: {t:.2f}s")
|
||
|
||
# 4. 发送圆弧运动指令
|
||
msg.mode = 11
|
||
msg.gait_id = 26
|
||
msg.vel_des = [v, 0, w] # [前进速度, 侧向速度, 角速度]
|
||
msg.duration = 0 # wait next
|
||
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)
|
||
|
||
# DEBUG 在起点放置蓝色标记
|
||
if observe:
|
||
yaw = ctrl.odo_msg.rpy[2]
|
||
ctrl.place_marker(start_position[0], start_position[1],
|
||
start_position[2] if len(start_position) > 2 else 0.0,
|
||
'blue', observe=True)
|
||
info(f"在起点位置放置蓝色标记: {start_position}", "位置")
|
||
|
||
# 计算圆心位置 - 相对于机器人前进方向的左侧或右侧
|
||
# 圆心与机器人当前位置的距离就是圆弧半径
|
||
# 角度为正(左转)时，圆心在左侧；角度为负(右转)时，圆心在右侧
|
||
circle_side = 1 if angle_deg > 0 else -1
|
||
center_x = start_position[0] - effective_r * math.sin(yaw) * circle_side
|
||
center_y = start_position[1] + effective_r * math.cos(yaw) * circle_side
|
||
center_z = start_position[2] if len(start_position) > 2 else 0.0
|
||
|
||
# 在圆心放置绿色标记
|
||
ctrl.place_marker(center_x, center_y, center_z, 'green', observe=True)
|
||
info(f"在旋转圆心放置绿色标记: [{center_x:.3f}, {center_y:.3f}, {center_z:.3f}]", "位置")
|
||
|
||
# 计算目标点的位置 - 旋转后的位置
|
||
# 使用几何关系：目标点是从起点绕圆心旋转angle_rad弧度后的位置
|
||
target_angle = yaw + angle_rad # 直接使用angle_rad的正负确定方向
|
||
# 计算从圆心到目标点的向量
|
||
target_x = center_x + effective_r * math.sin(target_angle) * circle_side
|
||
target_y = center_y - effective_r * math.cos(target_angle) * circle_side
|
||
target_z = start_position[2] if len(start_position) > 2 else 0.0
|
||
|
||
# 在目标点放置红色标记
|
||
ctrl.place_marker(target_x, target_y, target_z, 'red', observe=True)
|
||
info(f"在目标位置放置红色标记: [{target_x:.3f}, {target_y:.3f}, {target_z:.3f}]", "位置")
|
||
|
||
# 进入循环，实时判断
|
||
distance_moved = 0
|
||
angle_turned = 0
|
||
start_time = time.time()
|
||
timeout = t + 2 # 给个超时保护
|
||
last_qr_check_time = 0
|
||
|
||
# 初始速度
|
||
current_v = v
|
||
current_w = w
|
||
|
||
# 对小角度提前开始减速
|
||
if abs_angle_deg <= 70:
|
||
slowdown_threshold = 0.65 # 当达到目标角度的65%时开始减速
|
||
elif abs_angle_deg >= 170: # 针对大角度(如180度)采用更晚的减速时机
|
||
slowdown_threshold = 0.7 # 当达到目标角度的70%时才开始减速
|
||
else:
|
||
slowdown_threshold = 0.75 # 对于大角度，75%时开始减速（原来是80%）
|
||
|
||
# 根据角度大小调整旋转停止阈值
|
||
if abs_angle_deg >= 170:
|
||
rotation_completion_threshold = 0.85 # 大角度旋转提前停止，避免惯性导致过度旋转
|
||
else:
|
||
rotation_completion_threshold = 0.95 # 默认旋转到95%时停止
|
||
|
||
# 监控旋转进度并自行控制减速
|
||
# 对于180度旋转使用特殊处理
|
||
if abs_angle_deg >= 170:
|
||
# 简化旋转逻辑，直接持续旋转一段时间
|
||
rotation_time = t # 使用计算的理论时间
|
||
if observe:
|
||
info(f"大角度旋转({abs_angle_deg}度)，将持续旋转 {rotation_time:.2f} 秒", "旋转")
|
||
|
||
# 发送固定时间的旋转命令
|
||
start_time = time.time()
|
||
|
||
# 前段使用全速，但减少前段旋转时间比例，减少过度旋转
|
||
first_segment_time = rotation_time * 0.65 # 原来是0.7，减少到0.65
|
||
elapsed_time = 0
|
||
|
||
while elapsed_time < first_segment_time and time.time() - start_time < rotation_time + 2: # 加2秒保护
|
||
elapsed_time = time.time() - start_time
|
||
|
||
# 计算已旋转角度（仅用于打印）
|
||
if observe and time.time() % 0.5 < 0.02:
|
||
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
|
||
debug(f"全速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 已用时间: {elapsed_time:.2f}s/{rotation_time:.2f}s", "旋转")
|
||
|
||
# 检查QR码扫描结果（如果启用）
|
||
if scan_qrcode:
|
||
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
|
||
success(f"在旋转过程中扫描到QR码: {qr_data}", "扫描")
|
||
last_qr_check_time = current_time
|
||
|
||
time.sleep(0.05)
|
||
|
||
# 后段减速，减少减速幅度，保持更高速度
|
||
if observe:
|
||
info(f"进入减速阶段", "旋转")
|
||
|
||
# 减速到70%，保持前进速度和角速度的比例关系
|
||
reduced_w = w * 0.7 # 原来是0.5
|
||
reduced_v = abs(reduced_w * effective_r) # 维持圆弧关系
|
||
|
||
msg.mode = 11
|
||
msg.gait_id = 26
|
||
msg.vel_des = [reduced_v, 0, reduced_w] # 维持圆弧关系
|
||
msg.duration = 0
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 继续旋转直到总时间结束，减少总旋转时间
|
||
total_rotation_time = rotation_time * 1.15 # 原来是1.2，减少到1.15
|
||
while time.time() - start_time < total_rotation_time:
|
||
# 计算已旋转角度（仅用于打印）
|
||
if observe and time.time() % 0.5 < 0.02:
|
||
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
|
||
debug(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 已用时间: {time.time() - start_time:.2f}s/{total_rotation_time:.2f}s", "旋转")
|
||
|
||
# 检查QR码扫描结果（如果启用）
|
||
if scan_qrcode:
|
||
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
|
||
success(f"在旋转过程中扫描到QR码: {qr_data}", "扫描")
|
||
last_qr_check_time = current_time
|
||
|
||
time.sleep(0.05)
|
||
|
||
# 停止
|
||
ctrl.base_msg.stop()
|
||
else:
|
||
# 非180度的常规旋转逻辑保持不变
|
||
while abs(angle_turned) < abs(angle_rad) * rotation_completion_threshold 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
|
||
|
||
# 检查QR码扫描结果（如果启用）
|
||
if scan_qrcode:
|
||
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
|
||
print(f"🔍 在旋转过程中扫描到QR码: {qr_data}")
|
||
last_qr_check_time = current_time
|
||
|
||
# 计算角度完成比例
|
||
completion_ratio = abs(angle_turned) / abs(angle_rad)
|
||
|
||
# 当完成一定比例时开始减速
|
||
if completion_ratio > slowdown_threshold:
|
||
# 实现多阶段减速，特别针对大角度旋转
|
||
if abs_angle_deg >= 170:
|
||
if completion_ratio < 0.7: # 第一阶段：60%-70%
|
||
speed_factor = 0.65 # 原来是0.7，减少到0.65
|
||
elif completion_ratio < 0.8: # 第二阶段：70%-80%
|
||
speed_factor = 0.35 # 原来是0.4，减少到0.35
|
||
else: # 第三阶段：80%以上
|
||
speed_factor = 0.15 # 原来是0.2，减少到0.15
|
||
else:
|
||
# 标准减速逻辑保持不变
|
||
# 剩余比例作为速度系数
|
||
speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold)
|
||
|
||
# 根据角度调整最小速度因子
|
||
min_speed_factor = 0.15 # 默认最小速度因子
|
||
# 确保速度系数不会太小，但可以更慢一些
|
||
speed_factor = max(min_speed_factor, 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)
|
||
|
||
# 针对180度旋转，在减速命令后稍作等待，以确保减速效果
|
||
if abs_angle_deg >= 170:
|
||
time.sleep(0.03) # 短暂延迟，让减速命令更有效
|
||
|
||
if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次
|
||
debug(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秒左右打印一次
|
||
debug(f"已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度", "旋转")
|
||
|
||
time.sleep(0.05)
|
||
|
||
if observe:
|
||
info(f"旋转过程结束，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]", "停止")
|
||
debug(f'旋转结束角度: {math.degrees(ctrl.odo_msg.rpy[2]):.2f}°', "角度")
|
||
ctrl.base_msg.stop()
|
||
if observe:
|
||
debug(f'停止角度: {math.degrees(ctrl.odo_msg.rpy[2]):.2f}°', "角度")
|
||
|
||
# 记录停止前的角度
|
||
pre_stop_yaw = ctrl.odo_msg.rpy[2]
|
||
|
||
# 等待机器人完全停止
|
||
time.sleep(0.5)
|
||
|
||
# 停下来后的最终角度
|
||
final_yaw = ctrl.odo_msg.rpy[2]
|
||
final_angle_turned = final_yaw - start_yaw
|
||
|
||
# 记录实际结束位置
|
||
end_position = list(ctrl.odo_msg.xyz)
|
||
|
||
# 在实际结束位置放置黄色标记，与预计的目标位置进行对比
|
||
if observe and hasattr(ctrl, 'place_marker'):
|
||
ctrl.place_marker(end_position[0], end_position[1],
|
||
end_position[2] if len(end_position) > 2 else 0.0,
|
||
'yellow', observe=True)
|
||
info(f"在实际结束位置放置黄色标记: {end_position}", "位置")
|
||
|
||
# 计算实际位置与预期位置的偏差
|
||
position_error = math.sqrt((end_position[0] - target_x)**2 +
|
||
(end_position[1] - target_y)**2)
|
||
info(f"位置误差: {position_error:.3f}米", "距离")
|
||
|
||
# 角度归一化处理
|
||
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)
|
||
|
||
# 这里直接使用原始目标角度
|
||
if not bad_big_angle_corret:
|
||
target_angle_deg = angle_deg
|
||
else:
|
||
target_angle_deg = -angle_deg
|
||
|
||
if observe:
|
||
# 输出停止过程中角度变化
|
||
stop_angle_diff = math.degrees(final_yaw - pre_stop_yaw)
|
||
while stop_angle_diff > 180:
|
||
stop_angle_diff -= 360
|
||
while stop_angle_diff < -180:
|
||
stop_angle_diff += 360
|
||
debug(f"停止过程中旋转了: {stop_angle_diff:.2f}度", "角度")
|
||
info(f"旋转完成，目标角度: {target_angle_deg:.2f}度, 实际角度: {final_angle_deg:.2f}度", "角度")
|
||
|
||
# 检查是否需要微调
|
||
angle_error = abs(final_angle_deg - target_angle_deg)
|
||
|
||
# 针对不同角度使用不同的微调阈值
|
||
adjustment_threshold = 3.0 # 默认微调阈值
|
||
if abs_angle_deg >= 170:
|
||
adjustment_threshold = 10.0 # 大角度旋转使用更大的微调阈值，因为初步旋转已经确保大致方向正确
|
||
|
||
if angle_error > adjustment_threshold and not no_end_reset: # 如果误差超过阈值
|
||
if observe:
|
||
warning(f"角度误差: {angle_error:.2f}度，进行微调", "角度")
|
||
|
||
# 计算需要微调的角度
|
||
adjust_angle = target_angle_deg - final_angle_deg
|
||
|
||
# 增加补偿系数，因为实际旋转常会超出理论计算值
|
||
if abs_angle_deg <= 70:
|
||
compensation_factor = 0.55 # 比之前更保守，从0.65降至0.55
|
||
elif abs_angle_deg >= 170: # 单独处理180度附近的大角度
|
||
# 动态调整补偿因子，更保守处理
|
||
if angle_error > 60: # 如果误差超过60度
|
||
compensation_factor = 0.4 # 从0.5降至0.4
|
||
elif angle_error > 30: # 如果误差超过30度
|
||
compensation_factor = 0.35 # 从0.4降至0.35
|
||
else:
|
||
compensation_factor = 0.25 # 从0.3降至0.25，更保守
|
||
else:
|
||
compensation_factor = 0.6 # 从0.7降至0.6
|
||
|
||
# 对于特别大的误差进行更保守的单次校准
|
||
if abs_angle_deg >= 170 and angle_error > 30:
|
||
# 大误差直接一步校准，使用更保守的补偿系数
|
||
adjusted_compensation = compensation_factor * 0.85 # 在已有补偿基础上再降低15%
|
||
adjust_angle = adjust_angle * adjusted_compensation
|
||
|
||
if observe:
|
||
info(f"误差较大({angle_error:.2f}度)，使用更积极的补偿系数{adjusted_compensation:.2f}进行一次性校准: {adjust_angle:.2f}度", "角度")
|
||
|
||
# 使用turn_degree函数进行微调
|
||
turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False)
|
||
|
||
# 等待稳定
|
||
time.sleep(1.0)
|
||
|
||
# 再次检查剩余误差(仅用于显示)
|
||
if observe:
|
||
current_yaw = ctrl.odo_msg.rpy[2]
|
||
current_angle_turned = current_yaw - start_yaw
|
||
# 角度归一化处理
|
||
while current_angle_turned > math.pi:
|
||
current_angle_turned -= 2 * math.pi
|
||
while current_angle_turned < -math.pi:
|
||
current_angle_turned += 2 * math.pi
|
||
|
||
current_angle_deg = math.degrees(current_angle_turned)
|
||
remaining_error = target_angle_deg - current_angle_deg
|
||
info(f"校准后的角度: {current_angle_deg:.2f}度, 剩余误差: {remaining_error:.2f}度", "角度")
|
||
else:
|
||
adjust_angle *= compensation_factor
|
||
|
||
if observe:
|
||
info(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度", "角度")
|
||
|
||
# 使用turn_degree函数进行微调
|
||
turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False)
|
||
|
||
# 等待稳定(仅用于显示)
|
||
if observe:
|
||
time.sleep(0.8)
|
||
current_yaw = ctrl.odo_msg.rpy[2]
|
||
current_angle_turned = current_yaw - start_yaw
|
||
# 角度归一化处理
|
||
while current_angle_turned > math.pi:
|
||
current_angle_turned -= 2 * math.pi
|
||
while current_angle_turned < -math.pi:
|
||
current_angle_turned += 2 * math.pi
|
||
|
||
current_angle_deg = math.degrees(current_angle_turned)
|
||
remaining_error = target_angle_deg - current_angle_deg
|
||
info(f"校准后的角度: {current_angle_deg:.2f}度, 剩余误差: {remaining_error:.2f}度", "角度")
|
||
|
||
if observe:
|
||
info(f"微调结果: {'成功' if turn_success else '失败'}", "成功" if turn_success else "失败")
|
||
|
||
# 移动完成后再检查一次QR码扫描结果
|
||
if scan_qrcode:
|
||
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
|
||
success(f"旋转完成后最终扫描到QR码: {qr_data}", "扫描")
|
||
|
||
# 停止异步扫描
|
||
ctrl.image_processor.stop_async_scan()
|
||
|
||
# 将QR码结果添加到res字典中
|
||
res['qr_result'] = qr_result
|
||
|
||
if observe:
|
||
success("圆弧转弯完成", "完成")
|
||
|
||
# 统一返回结果格式
|
||
return True, res
|
||
|
||
def follow_left_side_track(ctrl, msg, min_distance=600, max_distance=650, speed=0.3, max_time=30, observe=False):
|
||
"""
|
||
控制机器狗向左侧移动并靠近左侧的黄色轨迹线，只进行侧向移动，不进行前进
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
target_distance: 目标与左侧线的像素距离，默认为540像素（考虑机器狗视角、高度的合适距离）
|
||
speed: 侧向移动的最大速度(米/秒)，默认为0.3米/秒
|
||
max_time: 最大执行时间(秒)，默认为30秒
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
|
||
返回:
|
||
bool: 是否成功达到目标位置
|
||
"""
|
||
section("开始左侧轨迹线跟随", "左侧跟踪")
|
||
|
||
# 设置移动命令基本参数
|
||
msg.mode = 11 # Locomotion模式
|
||
msg.gait_id = 26 # 自变频步态
|
||
msg.duration = 0 # wait next cmd
|
||
msg.step_height = [0.06, 0.06] # 抬腿高度
|
||
|
||
# 记录起始时间
|
||
start_time = time.time()
|
||
|
||
# 记录初始检测
|
||
image = ctrl.image_processor.get_current_image()
|
||
track_info, _ = detect_left_side_track(image, observe=observe, save_log=True)
|
||
|
||
if track_info is None:
|
||
error("无法检测到左侧轨迹线，无法开始跟踪", "失败")
|
||
return False
|
||
|
||
initial_distance = track_info["distance_to_left"]
|
||
if observe:
|
||
info(f"初始距离: {initial_distance:.1f}px")
|
||
|
||
# 最大允许误差(像素)
|
||
# 简单比例控制参数
|
||
kp = 0.0005
|
||
|
||
# 开始跟踪循环
|
||
while time.time() - start_time < max_time:
|
||
# 检测左侧轨迹线
|
||
image = ctrl.image_processor.get_current_image()
|
||
track_info, _ = detect_left_side_track(image, observe=observe, save_log=True)
|
||
|
||
if track_info is None:
|
||
warning("未检测到左侧轨迹线，继续使用上一次控制", "警告")
|
||
# 继续执行最后一次的控制命令
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(0.1)
|
||
continue
|
||
|
||
# 获取当前距离
|
||
current_distance = track_info["distance_to_left"]
|
||
|
||
# 计算误差 (正值表示需要向左移动，负值表示需要向右移动)
|
||
error_distance = current_distance - (min_distance + max_distance) / 2 # 新增：计算误差
|
||
if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次
|
||
debug(f"当前距离: {current_distance:.1f}px, 误差: {error:.1f}px", "跟踪")
|
||
|
||
# 检查是否已达到目标
|
||
if min_distance <= current_distance <= max_distance:
|
||
success(f"已达到目标距离，当前距离: {current_distance:.1f}px")
|
||
break
|
||
|
||
# 计算侧向速度 (简单比例控制)
|
||
lateral_velocity = kp * error_distance # 修正：确保 error 已定义
|
||
|
||
# 限制侧向速度
|
||
lateral_velocity = max(-speed, min(speed, lateral_velocity))
|
||
|
||
# 设置速度命令
|
||
msg.vel_des = [0, lateral_velocity, 0] # [前进速度, 侧向速度, 角速度]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 短暂延时
|
||
time.sleep(0.1)
|
||
|
||
# 停止移动
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(0.2)
|
||
ctrl.base_msg.stop()
|
||
|
||
# 最终检查
|
||
image = ctrl.image_processor.get_current_image()
|
||
track_info, _ = detect_left_side_track(image, observe=observe, save_log=True)
|
||
|
||
if track_info is not None:
|
||
final_distance = track_info["distance_to_left"]
|
||
final_success = min_distance <= final_distance <= max_distance
|
||
|
||
if observe:
|
||
if final_success:
|
||
success(f"成功达到目标位置，最终距离: {final_distance:.1f}px", "成功")
|
||
else:
|
||
warning(f"未能精确达到目标位置，最终距离: {final_distance:.1f}px", "警告")
|
||
|
||
return final_success
|
||
else:
|
||
if observe:
|
||
warning("无法检测最终位置的左侧轨迹线", "警告")
|
||
return False
|
||
|
||
def go_straight_until_hori_line(ctrl, msg, target_distance=0.5, max_distance=5.0,
|
||
check_interval=0.5, speed=0.3, detect_func_version=1,
|
||
mode=11, gait_id=26, step_height=[0.06, 0.06],
|
||
observe=False):
|
||
"""
|
||
控制机器人直线前进，直到检测到前方的横向黄线并到达指定距离
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
target_distance: 目标与横向线的距离(米)，默认为0.5米
|
||
max_distance: 最大前进距离(米)，防止无限前进，默认为5.0米
|
||
check_interval: 检查横向线的时间间隔(秒)，默认为0.5秒
|
||
speed: 移动速度(米/秒)，默认为0.3米/秒
|
||
detect_func_version: 使用的横向线检测函数版本，默认为1
|
||
mode: 机器人运动模式，默认为11(Locomotion模式)
|
||
gait_id: 步态ID，默认为26(自变频步态)
|
||
step_height: 抬腿高度，默认为[0.06, 0.06]
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
|
||
返回:
|
||
bool: 是否成功找到横向线并到达目标距离
|
||
"""
|
||
section("开始直线前进寻找横向线", "寻线")
|
||
|
||
# 设置移动命令基本参数
|
||
msg.mode = mode
|
||
msg.gait_id = gait_id
|
||
msg.step_height = step_height
|
||
msg.vel_des = [speed, 0, 0] # 前进速度
|
||
msg.duration = 0 # wait next cmd
|
||
msg.life_count += 1
|
||
|
||
# 记录起始位置
|
||
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)
|
||
|
||
# 发送命令开始移动
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 初始化变量
|
||
total_distance = 0
|
||
line_detected = False
|
||
last_check_time = time.time()
|
||
start_time = time.time()
|
||
|
||
# 设置相机高度
|
||
camera_height = 0.355 # 单位: 米
|
||
|
||
while total_distance < max_distance:
|
||
# 计算已移动距离
|
||
current_position = ctrl.odo_msg.xyz
|
||
dx = current_position[0] - start_position[0]
|
||
dy = current_position[1] - start_position[1]
|
||
total_distance = math.sqrt(dx*dx + dy*dy)
|
||
|
||
# 每隔指定时间检查一次
|
||
current_time = time.time()
|
||
if current_time - last_check_time >= check_interval:
|
||
last_check_time = current_time
|
||
|
||
# 获取当前图像
|
||
image = ctrl.image_processor.get_current_image()
|
||
|
||
# 根据指定版本检测横向线
|
||
if detect_func_version == 1:
|
||
edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True)
|
||
elif detect_func_version == 2:
|
||
edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=observe, save_log=True)
|
||
elif detect_func_version == 3:
|
||
edge_point, edge_info = detect_horizontal_track_edge_v3(image, observe=observe, save_log=True)
|
||
elif detect_func_version == 4:
|
||
edge_point, edge_info = detect_furthest_horizontal_intersection(image, observe=observe, save_log=True)
|
||
else:
|
||
error("未指定检测版本，请检查参数", "失败")
|
||
|
||
if edge_point is not None and edge_info is not None:
|
||
line_detected = True
|
||
|
||
# 计算当前距离
|
||
current_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe)
|
||
|
||
if current_distance is None:
|
||
warning("无法计算到横向线的距离，继续前进", "警告")
|
||
continue
|
||
|
||
if observe:
|
||
info(f"检测到横向线，当前距离: {current_distance:.3f}米, 目标距离: {target_distance:.3f}米", "距离")
|
||
|
||
# 判断是否达到目标距离
|
||
if current_distance <= target_distance + 0.1: # additional_distance 额外距离
|
||
success(f"已达到目标距离，当前距离: {current_distance:.3f}米", "完成")
|
||
break
|
||
|
||
# 如果距离比目标距离小，需要后退
|
||
if current_distance < target_distance - 0.1:
|
||
warning(f"过度接近横向线，当前距离: {current_distance:.3f}米，需要后退", "警告")
|
||
# 平滑停止当前移动
|
||
ctrl.base_msg.stop()
|
||
time.sleep(0.5)
|
||
|
||
# 计算需要后退的距离
|
||
back_distance = target_distance - current_distance
|
||
|
||
# 设置后退命令
|
||
msg.vel_des = [-speed/2, 0, 0] # 降低后退速度
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 后退指定时间
|
||
back_time = back_distance / (speed/2)
|
||
time.sleep(back_time)
|
||
|
||
# 停止后退
|
||
ctrl.base_msg.stop()
|
||
|
||
if observe:
|
||
info(f"后退完成，预计后退距离: {back_distance:.3f}米", "后退")
|
||
|
||
# 重新检查距离
|
||
time.sleep(0.5) # 等待稳定
|
||
image = ctrl.image_processor.get_current_image()
|
||
if detect_func_version == 1:
|
||
edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True)
|
||
elif detect_func_version == 2:
|
||
edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=observe, save_log=True)
|
||
elif detect_func_version == 3:
|
||
edge_point, edge_info = detect_horizontal_track_edge_v3(image, observe=observe, 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=observe)
|
||
if current_distance is not None:
|
||
if observe:
|
||
info(f"后退后的距离: {current_distance:.3f}米", "距离")
|
||
|
||
break
|
||
else:
|
||
if observe and time.time() % 2 < 0.05: # 每2秒左右打印一次
|
||
info(f"未检测到横向线，继续前进，已移动: {total_distance:.3f}米", "搜索")
|
||
|
||
# 定期显示移动距离
|
||
if observe and time.time() % 1 < 0.05: # 每1秒左右打印一次
|
||
debug(f"已移动: {total_distance:.3f}米, 最大距离: {max_distance:.3f}米", "移动")
|
||
|
||
# 短暂延时以降低CPU使用率
|
||
time.sleep(0.05)
|
||
|
||
# 停止移动
|
||
ctrl.base_msg.stop()
|
||
|
||
# 移动完成后再次检查
|
||
if line_detected:
|
||
# 等待稳定
|
||
time.sleep(0.5)
|
||
|
||
# 最终检查
|
||
image = ctrl.image_processor.get_current_image()
|
||
if detect_func_version == 1:
|
||
edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True)
|
||
elif detect_func_version == 2:
|
||
edge_point, edge_info = detect_horizontal_track_edge_v2(image, observe=observe, save_log=True)
|
||
elif detect_func_version == 3:
|
||
edge_point, edge_info = detect_horizontal_track_edge_v3(image, observe=observe, save_log=True)
|
||
elif detect_func_version == 4:
|
||
edge_point, edge_info = detect_furthest_horizontal_intersection(image, observe=observe, save_log=True)
|
||
else:
|
||
error("未指定检测版本，请检查参数", "失败")
|
||
|
||
if edge_point is not None and edge_info is not None:
|
||
final_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe)
|
||
|
||
if final_distance is not None:
|
||
if observe:
|
||
success(f"最终距离: {final_distance:.3f}米, 目标距离: {target_distance:.3f}米", "结果")
|
||
|
||
# 判断是否达到目标
|
||
is_success = abs(final_distance - target_distance) < 0.2 # 误差在20cm内算成功
|
||
|
||
if observe:
|
||
if is_success:
|
||
success(f"成功达到目标距离，误差: {abs(final_distance - target_distance):.2f}米", "成功")
|
||
else:
|
||
warning(f"未能精确达到目标距离，误差: {abs(final_distance - target_distance):.2f}米", "警告")
|
||
|
||
return is_success
|
||
|
||
if not line_detected:
|
||
if observe:
|
||
error(f"在最大距离({max_distance}米)内未检测到横向线", "失败")
|
||
return False
|
||
|
||
# 如果没有进行最终检查，但检测到过横向线，默认返回成功
|
||
return True
|
||
|