184 lines
6.8 KiB
Python
184 lines
6.8 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""
|
||
基础运动测试脚本
|
||
测试原地旋转、直线移动、侧向移动等基本运动函数
|
||
"""
|
||
|
||
import time
|
||
import sys
|
||
import os
|
||
|
||
# 添加父目录到路径，以便能够导入utils
|
||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||
|
||
from base_move.turn_degree import turn_degree_v2
|
||
from base_move.go_straight import go_straight, go_lateral
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
|
||
# 创建日志记录器
|
||
logger = get_logger("基础运动测试")
|
||
|
||
def test_turn_90_degrees(ctrl, msg, observe=True):
|
||
"""测试原地旋转90度"""
|
||
section('测试：原地旋转90度', "旋转")
|
||
info('开始测试原地旋转90度...', "测试")
|
||
|
||
# 记录初始角度
|
||
initial_angle = ctrl.odo_msg.rpy[2]
|
||
info(f"初始角度: {initial_angle:.2f}度", "角度")
|
||
|
||
# 执行90度旋转
|
||
turn_degree_v2(ctrl, msg, degree=90, absolute=False, precision=True)
|
||
|
||
# 记录最终角度
|
||
final_angle = ctrl.odo_msg.rpy[2]
|
||
angle_diff = final_angle - initial_angle
|
||
info(f"最终角度: {final_angle:.2f}度", "角度")
|
||
info(f"角度变化: {angle_diff:.2f}度", "角度")
|
||
|
||
success("原地旋转90度测试完成", "完成")
|
||
|
||
def test_turn_180_degrees(ctrl, msg, observe=True):
|
||
"""测试原地旋转180度"""
|
||
section('测试：原地旋转180度', "旋转")
|
||
info('开始测试原地旋转180度...', "测试")
|
||
|
||
# 记录初始角度
|
||
initial_angle = ctrl.odo_msg.rpy[2]
|
||
info(f"初始角度: {initial_angle:.2f}度", "角度")
|
||
|
||
# 执行180度旋转
|
||
turn_degree_v2(ctrl, msg, degree=180, absolute=False, precision=True)
|
||
|
||
# 记录最终角度
|
||
final_angle = ctrl.odo_msg.rpy[2]
|
||
angle_diff = final_angle - initial_angle
|
||
info(f"最终角度: {final_angle:.2f}度", "角度")
|
||
info(f"角度变化: {angle_diff:.2f}度", "角度")
|
||
|
||
success("原地旋转180度测试完成", "完成")
|
||
|
||
def test_go_straight_1m(ctrl, msg, observe=True):
|
||
"""测试直线前进1米"""
|
||
section('测试：直线前进1米', "移动")
|
||
info('开始测试直线前进1米...', "测试")
|
||
|
||
# 记录初始位置
|
||
initial_pos = ctrl.odo_msg.xyz
|
||
info(f"初始位置: x={initial_pos[0]:.3f}, y={initial_pos[1]:.3f}", "位置")
|
||
|
||
# 执行1米直线移动
|
||
go_straight(ctrl, msg, distance=1.0, speed=0.5, observe=observe)
|
||
|
||
# 记录最终位置
|
||
final_pos = ctrl.odo_msg.xyz
|
||
distance_moved = ((final_pos[0] - initial_pos[0])**2 + (final_pos[1] - initial_pos[1])**2)**0.5
|
||
info(f"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}", "位置")
|
||
info(f"实际移动距离: {distance_moved:.3f}米", "距离")
|
||
|
||
success("直线前进1米测试完成", "完成")
|
||
|
||
def test_go_straight_backward_1m(ctrl, msg, observe=True):
|
||
"""测试直线后退1米"""
|
||
section('测试：直线后退1米', "移动")
|
||
info('开始测试直线后退1米...', "测试")
|
||
|
||
# 记录初始位置
|
||
initial_pos = ctrl.odo_msg.xyz
|
||
info(f"初始位置: x={initial_pos[0]:.3f}, y={initial_pos[1]:.3f}", "位置")
|
||
|
||
# 执行1米直线后退
|
||
go_straight(ctrl, msg, distance=-1.0, speed=0.5, observe=observe)
|
||
|
||
# 记录最终位置
|
||
final_pos = ctrl.odo_msg.xyz
|
||
distance_moved = ((final_pos[0] - initial_pos[0])**2 + (final_pos[1] - initial_pos[1])**2)**0.5
|
||
info(f"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}", "位置")
|
||
info(f"实际移动距离: {distance_moved:.3f}米", "距离")
|
||
|
||
success("直线后退1米测试完成", "完成")
|
||
|
||
def test_lateral_movement(ctrl, msg, observe=True):
|
||
"""测试侧向移动"""
|
||
section('测试：侧向移动', "移动")
|
||
|
||
# 测试右侧移动
|
||
info('开始测试右侧移动0.5米...', "测试")
|
||
initial_pos = ctrl.odo_msg.xyz
|
||
go_lateral(ctrl, msg, distance=0.5, speed=0.3, observe=observe)
|
||
final_pos = ctrl.odo_msg.xyz
|
||
distance_moved = ((final_pos[0] - initial_pos[0])**2 + (final_pos[1] - initial_pos[1])**2)**0.5
|
||
info(f"右侧移动距离: {distance_moved:.3f}米", "距离")
|
||
|
||
time.sleep(1)
|
||
|
||
# 测试左侧移动
|
||
info('开始测试左侧移动0.5米...', "测试")
|
||
initial_pos = ctrl.odo_msg.xyz
|
||
go_lateral(ctrl, msg, distance=-0.5, speed=0.3, observe=observe)
|
||
final_pos = ctrl.odo_msg.xyz
|
||
distance_moved = ((final_pos[0] - initial_pos[0])**2 + (final_pos[1] - initial_pos[1])**2)**0.5
|
||
info(f"左侧移动距离: {distance_moved:.3f}米", "距离")
|
||
|
||
success("侧向移动测试完成", "完成")
|
||
|
||
def test_square_movement(ctrl, msg, observe=True):
|
||
"""测试正方形移动路径"""
|
||
section('测试：正方形移动路径', "移动")
|
||
info('开始测试正方形移动路径...', "测试")
|
||
|
||
# 记录起始位置
|
||
start_pos = ctrl.odo_msg.xyz
|
||
info(f"起始位置: x={start_pos[0]:.3f}, y={start_pos[1]:.3f}", "位置")
|
||
|
||
# 正方形的四条边
|
||
for i in range(4):
|
||
info(f"正方形第{i+1}条边", "移动")
|
||
# 前进1米
|
||
go_straight(ctrl, msg, distance=1.0, speed=0.5, observe=observe)
|
||
time.sleep(0.5)
|
||
# 右转90度
|
||
turn_degree_v2(ctrl, msg, degree=90, absolute=False, precision=True)
|
||
time.sleep(0.5)
|
||
|
||
# 记录结束位置
|
||
end_pos = ctrl.odo_msg.xyz
|
||
distance_from_start = ((end_pos[0] - start_pos[0])**2 + (end_pos[1] - start_pos[1])**2)**0.5
|
||
info(f"结束位置: x={end_pos[0]:.3f}, y={end_pos[1]:.3f}", "位置")
|
||
info(f"与起始位置的距离: {distance_from_start:.3f}米", "距离")
|
||
|
||
success("正方形移动路径测试完成", "完成")
|
||
|
||
def run_basic_movement_tests(ctrl, msg):
|
||
"""运行所有基础运动测试"""
|
||
section('基础运动测试套件', "开始")
|
||
|
||
tests = [
|
||
("原地旋转90度", test_turn_90_degrees),
|
||
("原地旋转180度", test_turn_180_degrees),
|
||
("直线前进1米", test_go_straight_1m),
|
||
("直线后退1米", test_go_straight_backward_1m),
|
||
("侧向移动", test_lateral_movement),
|
||
("正方形移动路径", test_square_movement),
|
||
]
|
||
|
||
for test_name, test_func in tests:
|
||
try:
|
||
info(f"开始执行测试: {test_name}", "测试")
|
||
test_func(ctrl, msg)
|
||
success(f"测试 {test_name} 成功完成", "成功")
|
||
except Exception as e:
|
||
error(f"测试 {test_name} 失败: {str(e)}", "失败")
|
||
|
||
# 每个测试之间暂停
|
||
time.sleep(2)
|
||
|
||
success("所有基础运动测试完成", "完成")
|
||
|
||
if __name__ == "__main__":
|
||
# 这里可以添加独立运行的代码
|
||
print("基础运动测试脚本")
|
||
print("使用方法：")
|
||
print("from single_test.test_basic_movements import run_basic_movement_tests")
|
||
print("run_basic_movement_tests(ctrl, msg)") |