285 lines
12 KiB
Python
285 lines
12 KiB
Python
import math
|
||
import time
|
||
import sys
|
||
import os
|
||
|
||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
from utils.detect_dual_track_lines import detect_dual_track_lines
|
||
|
||
def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=False,
|
||
mode=11, gait_id=26, step_height=[0.06, 0.06],
|
||
estimated_track_distance=1.0,
|
||
detect_height=0.4,
|
||
detection_interval=0.2):
|
||
"""
|
||
控制机器狗仅使用Y轴移动调整到双轨道线的中间位置
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
max_time: 最大调整时间(秒)，默认为15秒
|
||
max_deviation: 允许的最大偏差(像素)，当偏差小于此值时认为已居中，默认为8像素
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
mode: 控制模式，默认为11
|
||
gait_id: 步态ID，默认为26
|
||
step_height: 抬腿高度，默认为[0.06, 0.06]
|
||
estimated_track_distance: 估计的机器人到轨道的距离(米)，用于计算实际偏差，默认为1.0米
|
||
detect_height: 检测高度，默认为0.4
|
||
detection_interval: 检测间隔时间(秒)，默认为0.2秒
|
||
|
||
返回:
|
||
bool: 是否成功调整到中心位置
|
||
"""
|
||
section("开始双轨道居中", "轨道居中")
|
||
|
||
# 设置移动命令基本参数
|
||
msg.mode = mode
|
||
msg.gait_id = gait_id
|
||
msg.duration = 0 # wait next cmd
|
||
msg.step_height = step_height
|
||
|
||
# 记录起始时间
|
||
start_time = time.time()
|
||
last_detection_time = 0 # 记录上次检测时间
|
||
|
||
# 记录起始位置
|
||
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)
|
||
|
||
# 根据RGB相机参数，计算像素偏差到实际距离的转换
|
||
# 相机水平视场角FOV = 1.46608弧度（约84度）
|
||
# 图像宽度 = 1920像素
|
||
camera_hfov = 1.46608 # 水平视场角（弧度）
|
||
image_width = 1920 # 图像宽度（像素）
|
||
|
||
# 计算像素到角度的转换比例
|
||
pixels_per_radian = image_width / camera_hfov
|
||
|
||
# 统计变量
|
||
detection_success_count = 0
|
||
detection_total_count = 0
|
||
|
||
# 稳定计数器 - 连续几次在中心位置才认为稳定
|
||
stable_count = 0
|
||
required_stable_count = 2
|
||
|
||
# 变量记录最后有效的偏差，用于处理检测失败的情况
|
||
last_valid_deviation = 0
|
||
|
||
# 开始调整循环
|
||
while time.time() - start_time < max_time:
|
||
# 获取当前图像
|
||
image = ctrl.image_processor.get_current_image()
|
||
|
||
# 检测双轨道线
|
||
detection_total_count += 1
|
||
|
||
center_info, left_info, right_info = detect_dual_track_lines(image, observe=observe, save_log=True)
|
||
|
||
if center_info is not None:
|
||
detection_success_count += 1
|
||
|
||
# 获取当前偏差
|
||
current_deviation = center_info["deviation"]
|
||
last_valid_deviation = current_deviation # 记录有效偏差
|
||
|
||
# 转换像素偏差到实际距离偏差
|
||
# 1. 像素偏差转换为弧度
|
||
deviation_rad = current_deviation / pixels_per_radian
|
||
# 2. 根据估计距离计算实际偏差（单位：米）
|
||
actual_deviation = math.tan(deviation_rad) * estimated_track_distance
|
||
|
||
if observe:
|
||
debug(f"偏差: {current_deviation:.1f}px", "偏差")
|
||
debug(f"实际距离偏差: {actual_deviation:.3f}m (距离估计: {estimated_track_distance:.1f}m)", "实际偏差")
|
||
|
||
# 判断是否已经居中
|
||
if abs(current_deviation) <= max_deviation:
|
||
stable_count += 1
|
||
if observe:
|
||
info(f"已接近中心，稳定计数: {stable_count}/{required_stable_count}", "居中")
|
||
info(f"距离偏差: {actual_deviation:.3f}m", "居中")
|
||
|
||
# 即使在稳定过程中，也要保持小幅调整以保持居中
|
||
if stable_count < required_stable_count:
|
||
# 直接根据实际偏差移动
|
||
msg.vel_des = [0, actual_deviation, 0]
|
||
ctrl.Send_cmd(msg)
|
||
|
||
if stable_count >= required_stable_count:
|
||
# 已经稳定在中心位置
|
||
if observe:
|
||
success(f"成功居中，最终偏差: {current_deviation:.1f}px ({actual_deviation:.3f}m)", "完成")
|
||
break
|
||
else:
|
||
# 不在中心，重置稳定计数
|
||
stable_count = 0
|
||
|
||
# 直接根据计算出的实际偏差距离进行移动
|
||
# 为了避免移动过猛，可以限制最大移动距离
|
||
max_move_distance = 0.35 # 最大移动距离（米）
|
||
move_distance = actual_deviation
|
||
if abs(move_distance) > max_move_distance:
|
||
move_distance = max_move_distance * (1 if move_distance > 0 else -1)
|
||
|
||
if observe:
|
||
debug(f"横向移动距离: {move_distance:.3f}m", "控制")
|
||
|
||
# 设置速度命令 - 只使用y轴移动，不前进和转向
|
||
msg.vel_des = [0, move_distance, 0] # [前进速度, 侧向速度, 角速度]
|
||
|
||
# 发送命令
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
else:
|
||
warning("未检测到双轨道线", "警告")
|
||
|
||
# 如果已经有了一些有效的检测，使用最后一次有效的偏差继续移动
|
||
if detection_success_count > 0:
|
||
# 使用最后一次有效偏差值计算移动距离
|
||
deviation_rad = last_valid_deviation / pixels_per_radian
|
||
move_distance = math.tan(deviation_rad) * estimated_track_distance * 0.5 # 缩小系数，避免过度移动
|
||
|
||
# 限制最大移动距离
|
||
max_move_distance = 0.15
|
||
move_distance = max(-max_move_distance, min(max_move_distance, move_distance))
|
||
|
||
msg.vel_des = [0, move_distance, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
if observe:
|
||
warning(f"使用最后偏差值继续移动: {move_distance:.3f}m", "暂停")
|
||
else:
|
||
# 如果一开始就没有检测到，可以尝试小范围搜索
|
||
if detection_total_count < 10:
|
||
if detection_total_count % 2 == 0:
|
||
# 向右搜索
|
||
msg.vel_des = [0, -0.08, 0]
|
||
else:
|
||
# 向左搜索
|
||
msg.vel_des = [0, 0.08, 0]
|
||
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
if observe:
|
||
debug("搜索轨道线...", "搜索")
|
||
else:
|
||
# 超过一定次数仍未检测到，停止搜索
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
if observe:
|
||
error("无法检测到轨道线，放弃调整", "失败")
|
||
break
|
||
|
||
# 短暂延时
|
||
time.sleep(0.05)
|
||
|
||
# 停止移动
|
||
ctrl.base_msg.stop()
|
||
|
||
# 计算最终位置与起始位置的变化
|
||
final_position = ctrl.odo_msg.xyz
|
||
dx = final_position[0] - start_position[0]
|
||
dy = final_position[1] - start_position[1]
|
||
|
||
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"横向移动距离: {abs(dy):.3f}米", "统计")
|
||
|
||
# 显示检测成功率
|
||
if detection_total_count > 0:
|
||
detection_rate = (detection_success_count / detection_total_count) * 100
|
||
info(f"轨迹检测成功率: {detection_rate:.1f}% ({detection_success_count}/{detection_total_count})", "统计")
|
||
|
||
# 判断是否成功
|
||
success = False
|
||
if time.time() - start_time >= max_time:
|
||
if observe:
|
||
warning("超过最大调整时间", "超时")
|
||
else:
|
||
# 如果因为已稳定在中心而退出循环，则认为成功
|
||
if stable_count >= required_stable_count:
|
||
success = True
|
||
|
||
return success
|
||
def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_time=15, observe=False,
|
||
mode=11, gait_id=26, step_height=[0.06, 0.06], estimated_track_distance=1.0,
|
||
detection_interval=0.2):
|
||
"""
|
||
先居中到双轨道线中间，然后沿轨道线行走指定距离
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
distance: 目标前进距离(米)
|
||
speed: 前进速度(米/秒)，默认为0.5米/秒
|
||
max_centering_time: 最大居中调整时间(秒)，默认为15秒
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
mode: 控制模式，默认为11
|
||
gait_id: 步态ID，默认为26
|
||
step_height: 抬腿高度，默认为[0.06, 0.06]
|
||
estimated_track_distance: 估计的机器人到轨道的距离(米)，用于计算实际偏差，默认为1.0米
|
||
detection_interval: 检测间隔时间(秒)，默认为0.2秒
|
||
|
||
返回:
|
||
bool: 是否成功完成居中和轨道跟随
|
||
"""
|
||
section("开始双轨道居中和跟随", "轨道任务")
|
||
|
||
# 第一步：居中到轨道中间
|
||
if observe:
|
||
info("步骤1: 调整到轨道中间", "居中")
|
||
|
||
centering_success = center_on_dual_tracks(
|
||
ctrl, msg,
|
||
max_time=max_centering_time,
|
||
observe=observe,
|
||
mode=mode,
|
||
gait_id=gait_id,
|
||
step_height=step_height,
|
||
estimated_track_distance=estimated_track_distance,
|
||
detection_interval=detection_interval
|
||
)
|
||
|
||
if not centering_success:
|
||
if observe:
|
||
error("居中调整失败，无法继续跟随轨道", "失败")
|
||
return False
|
||
|
||
# 第二步：沿轨道跟随指定距离
|
||
if observe:
|
||
info(f"步骤2: 沿轨道前进 {distance:.2f}米", "跟随")
|
||
|
||
# 导入轨道跟随函数
|
||
from base_move.follow_dual_tracks import follow_dual_tracks
|
||
|
||
following_success = follow_dual_tracks(
|
||
ctrl, msg,
|
||
speed=speed,
|
||
target_distance=distance,
|
||
observe=observe,
|
||
mode=mode,
|
||
gait_id=gait_id,
|
||
step_height=step_height
|
||
)
|
||
|
||
if observe:
|
||
if following_success:
|
||
success("成功完成轨道居中和跟随任务", "完成")
|
||
else:
|
||
warning("轨道跟随未完全成功", "警告")
|
||
|
||
return following_success
|