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()
|