268 lines
10 KiB
Python
268 lines
10 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, auto_detect_dual_track_lines
|
||
|
||
def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False,
|
||
mode=11, gait_id=26, step_height=[0.06, 0.06],
|
||
stone_path_mode=None):
|
||
"""
|
||
控制机器狗仅使用Y轴移动调整到双轨道线的中间位置
|
||
|
||
参数:
|
||
ctrl: Robot_Ctrl 对象，包含里程计信息
|
||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||
max_time: 最大调整时间(秒)，默认为15秒
|
||
max_deviation: 允许的最大偏差(像素)，当偏差小于此值时认为已居中，默认为10像素
|
||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||
mode: 控制模式，默认为11
|
||
gait_id: 步态ID，默认为26
|
||
step_height: 抬腿高度，默认为[0.06, 0.06]
|
||
stone_path_mode: 是否使用石板路模式，None表示自动检测，True或False表示强制使用或不使用
|
||
|
||
返回:
|
||
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()
|
||
|
||
# 记录起始位置
|
||
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)
|
||
|
||
# PID控制参数 - 仅使用比例控制以避免过冲
|
||
kp = 0.003 # 比例系数 - 较小的值以获得平滑的移动
|
||
|
||
# 帧间滤波参数
|
||
filter_size = 5 # 滤波队列大小
|
||
deviation_queue = [] # 偏差值队列
|
||
|
||
# 统计变量
|
||
detection_success_count = 0
|
||
detection_total_count = 0
|
||
|
||
# 稳定计数器 - 连续几次在中心位置才认为稳定
|
||
stable_count = 0
|
||
required_stable_count = 3
|
||
|
||
# 开始调整循环
|
||
while time.time() - start_time < max_time:
|
||
# 获取当前图像
|
||
image = ctrl.image_processor.get_current_image()
|
||
|
||
# 检测双轨道线
|
||
detection_total_count += 1
|
||
|
||
if stone_path_mode is None:
|
||
# 自动检测模式
|
||
center_info, left_info, right_info = auto_detect_dual_track_lines(image, observe=observe, save_log=True)
|
||
else:
|
||
# 指定模式
|
||
center_info, left_info, right_info = detect_dual_track_lines(image, observe=observe, save_log=True, stone_path_mode=stone_path_mode)
|
||
|
||
if center_info is not None:
|
||
detection_success_count += 1
|
||
|
||
# 获取当前偏差
|
||
current_deviation = center_info["deviation"]
|
||
|
||
# 添加到队列
|
||
deviation_queue.append(current_deviation)
|
||
if len(deviation_queue) > filter_size:
|
||
deviation_queue.pop(0)
|
||
|
||
# 计算滤波后的偏差值 (去除最大和最小值后的平均)
|
||
if len(deviation_queue) >= 3:
|
||
filtered_deviations = sorted(deviation_queue)[1:-1] if len(deviation_queue) > 2 else deviation_queue
|
||
filtered_deviation = sum(filtered_deviations) / len(filtered_deviations)
|
||
else:
|
||
filtered_deviation = current_deviation
|
||
|
||
if observe:
|
||
debug(f"原始偏差: {current_deviation:.1f}px, 滤波后: {filtered_deviation:.1f}px", "偏差")
|
||
|
||
# 判断是否已经居中
|
||
if abs(filtered_deviation) <= max_deviation:
|
||
stable_count += 1
|
||
if observe:
|
||
info(f"已接近中心，稳定计数: {stable_count}/{required_stable_count}", "居中")
|
||
|
||
if stable_count >= required_stable_count:
|
||
# 已经稳定在中心位置
|
||
if observe:
|
||
success(f"成功居中，最终偏差: {filtered_deviation:.1f}px", "完成")
|
||
break
|
||
else:
|
||
# 不在中心，重置稳定计数
|
||
stable_count = 0
|
||
|
||
# 计算横向移动速度 (只使用y轴移动)
|
||
# 注意：偏差为正表示需要向左移动（y轴正方向），偏差为负表示需要向右移动（y轴负方向）
|
||
lateral_velocity = kp * filtered_deviation
|
||
|
||
# 限制横向移动速度
|
||
max_lateral_velocity = 0.3 # 最大横向速度 (米/秒)
|
||
lateral_velocity = max(-max_lateral_velocity, min(max_lateral_velocity, lateral_velocity))
|
||
|
||
if observe:
|
||
debug(f"横向移动速度: {lateral_velocity:.3f}m/s", "控制")
|
||
|
||
# 设置速度命令 - 只使用y轴移动，不前进和转向
|
||
msg.vel_des = [0, lateral_velocity, 0] # [前进速度, 侧向速度, 角速度]
|
||
|
||
# 发送命令
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
else:
|
||
warning("未检测到双轨道线", "警告")
|
||
|
||
# 如果已经有了一些有效的检测，暂时停止移动
|
||
if len(deviation_queue) > 0:
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
if observe:
|
||
warning("暂停移动，等待有效检测", "暂停")
|
||
else:
|
||
# 如果一开始就没有检测到，可以尝试小范围搜索
|
||
if detection_total_count < 10:
|
||
if detection_total_count % 2 == 0:
|
||
# 向右搜索
|
||
msg.vel_des = [0, -0.1, 0]
|
||
else:
|
||
# 向左搜索
|
||
msg.vel_des = [0, 0.1, 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],
|
||
stone_path_mode=None):
|
||
"""
|
||
先居中到双轨道线中间，然后沿轨道线行走指定距离
|
||
|
||
参数:
|
||
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]
|
||
stone_path_mode: 是否使用石板路模式，None表示自动检测
|
||
|
||
返回:
|
||
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,
|
||
stone_path_mode=stone_path_mode
|
||
)
|
||
|
||
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 |