101 lines
3.5 KiB
Python
101 lines
3.5 KiB
Python
import sys
|
||
import os
|
||
import time
|
||
import argparse
|
||
|
||
sys.path.append(os.path.dirname(os.path.abspath(__file__)))
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
|
||
def main():
|
||
"""
|
||
测试双轨道线居中功能
|
||
"""
|
||
# 解析命令行参数
|
||
parser = argparse.ArgumentParser(description='测试双轨道线居中功能')
|
||
parser.add_argument('--distance', type=float, default=0.0,
|
||
help='居中后要前进的距离(米)，默认为0，即仅执行居中操作')
|
||
parser.add_argument('--speed', type=float, default=0.5,
|
||
help='前进速度(米/秒)，默认为0.5')
|
||
parser.add_argument('--max_time', type=float, default=15.0,
|
||
help='最大居中时间(秒)，默认为15秒')
|
||
parser.add_argument('--stone_path', action='store_true',
|
||
help='强制使用石板路模式')
|
||
parser.add_argument('--normal_path', action='store_true',
|
||
help='强制使用普通路径模式')
|
||
args = parser.parse_args()
|
||
|
||
# 初始化日志
|
||
LogHelper.init('dual_track_centering')
|
||
section("双轨道线居中测试", "开始")
|
||
|
||
try:
|
||
# 导入机器人控制模块
|
||
from robot_controller import RobotCtrl
|
||
|
||
# 初始化机器人控制器
|
||
ctrl = RobotCtrl(observe=True)
|
||
msg = ctrl.initialize()
|
||
|
||
if msg is None:
|
||
error("初始化机器人控制器失败", "失败")
|
||
return
|
||
|
||
info("机器人控制器初始化成功", "初始化")
|
||
|
||
# 等待机器人准备就绪
|
||
time.sleep(1)
|
||
|
||
# 确定使用哪种模式
|
||
stone_path_mode = None # 默认自动检测
|
||
if args.stone_path:
|
||
stone_path_mode = True
|
||
info("强制使用石板路模式", "模式")
|
||
elif args.normal_path:
|
||
stone_path_mode = False
|
||
info("强制使用普通路径模式", "模式")
|
||
else:
|
||
info("使用自动检测模式", "模式")
|
||
|
||
# 导入双轨道线居中模块
|
||
from base_move.center_on_dual_tracks import center_on_dual_tracks, center_and_follow_dual_tracks
|
||
|
||
# 执行居中操作
|
||
if args.distance > 0:
|
||
# 居中后沿轨道前进
|
||
info(f"执行居中并前进 {args.distance} 米，速度 {args.speed} m/s", "任务")
|
||
success = center_and_follow_dual_tracks(
|
||
ctrl, msg,
|
||
distance=args.distance,
|
||
speed=args.speed,
|
||
max_centering_time=args.max_time,
|
||
observe=True,
|
||
stone_path_mode=stone_path_mode
|
||
)
|
||
else:
|
||
# 仅执行居中
|
||
info("仅执行居中操作", "任务")
|
||
success = center_on_dual_tracks(
|
||
ctrl, msg,
|
||
max_time=args.max_time,
|
||
observe=True,
|
||
stone_path_mode=stone_path_mode
|
||
)
|
||
|
||
# 显示结果
|
||
if success:
|
||
success("测试成功完成", "结果")
|
||
else:
|
||
warning("测试未完全成功", "结果")
|
||
|
||
# 清理资源
|
||
ctrl.cleanup()
|
||
|
||
except KeyboardInterrupt:
|
||
warning("用户中断测试", "中断")
|
||
except Exception as e:
|
||
error(f"测试过程中发生异常: {str(e)}", "异常")
|
||
|
||
section("双轨道线居中测试", "结束")
|
||
|
||
if __name__ == "__main__":
|
||
main() |