266 lines
9.2 KiB
Python
266 lines
9.2 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.move_base_hori_line import arc_turn_around_hori_line
|
||
from base_move.turn_degree import turn_degree_v2
|
||
from base_move.go_straight import go_straight
|
||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||
|
||
# 创建日志记录器
|
||
logger = get_logger("弧形运动测试")
|
||
|
||
def test_arc_turn_90_degrees(ctrl, msg, observe=True):
|
||
"""测试90度弧形转弯"""
|
||
section('测试：90度弧形转弯', "弧形转弯")
|
||
info('开始测试90度弧形转弯...', "测试")
|
||
|
||
# 记录初始位置和角度
|
||
initial_pos = ctrl.odo_msg.xyz
|
||
initial_angle = ctrl.odo_msg.rpy[2]
|
||
info(f"初始位置: x={initial_pos[0]:.3f}, y={initial_pos[1]:.3f}", "位置")
|
||
info(f"初始角度: {initial_angle:.2f}度", "角度")
|
||
|
||
# 执行90度弧形转弯
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=90, # 90度左转
|
||
radius=0.5, # 0.5米转弯半径
|
||
observe=observe,
|
||
pass_align=True,
|
||
no_end_reset=True,
|
||
)
|
||
|
||
# 记录最终位置和角度
|
||
final_pos = ctrl.odo_msg.xyz
|
||
final_angle = ctrl.odo_msg.rpy[2]
|
||
distance_moved = ((final_pos[0] - initial_pos[0])**2 + (final_pos[1] - initial_pos[1])**2)**0.5
|
||
angle_diff = final_angle - initial_angle
|
||
|
||
info(f"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}", "位置")
|
||
info(f"最终角度: {final_angle:.2f}度", "角度")
|
||
info(f"移动距离: {distance_moved:.3f}米", "距离")
|
||
info(f"角度变化: {angle_diff:.2f}度", "角度")
|
||
|
||
if turn_success:
|
||
success("90度弧形转弯测试成功", "成功")
|
||
if res:
|
||
info(f"转弯半径: {res.get('radius', 'N/A')}", "参数")
|
||
else:
|
||
error("90度弧形转弯测试失败", "失败")
|
||
|
||
def test_arc_turn_180_degrees(ctrl, msg, observe=True):
|
||
"""测试180度弧形转弯"""
|
||
section('测试：180度弧形转弯', "弧形转弯")
|
||
info('开始测试180度弧形转弯...', "测试")
|
||
|
||
# 记录初始位置和角度
|
||
initial_pos = ctrl.odo_msg.xyz
|
||
initial_angle = ctrl.odo_msg.rpy[2]
|
||
info(f"初始位置: x={initial_pos[0]:.3f}, y={initial_pos[1]:.3f}", "位置")
|
||
info(f"初始角度: {initial_angle:.2f}度", "角度")
|
||
|
||
# 执行180度弧形转弯
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=180, # 180度转弯
|
||
radius=0.6, # 0.6米转弯半径
|
||
observe=observe,
|
||
pass_align=True,
|
||
no_end_reset=True,
|
||
)
|
||
|
||
# 记录最终位置和角度
|
||
final_pos = ctrl.odo_msg.xyz
|
||
final_angle = ctrl.odo_msg.rpy[2]
|
||
distance_moved = ((final_pos[0] - initial_pos[0])**2 + (final_pos[1] - initial_pos[1])**2)**0.5
|
||
angle_diff = final_angle - initial_angle
|
||
|
||
info(f"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}", "位置")
|
||
info(f"最终角度: {final_angle:.2f}度", "角度")
|
||
info(f"移动距离: {distance_moved:.3f}米", "距离")
|
||
info(f"角度变化: {angle_diff:.2f}度", "角度")
|
||
|
||
if turn_success:
|
||
success("180度弧形转弯测试成功", "成功")
|
||
if res:
|
||
info(f"转弯半径: {res.get('radius', 'N/A')}", "参数")
|
||
else:
|
||
error("180度弧形转弯测试失败", "失败")
|
||
|
||
def test_arc_turn_right_90_degrees(ctrl, msg, observe=True):
|
||
"""测试右转90度弧形转弯"""
|
||
section('测试：右转90度弧形转弯', "弧形转弯")
|
||
info('开始测试右转90度弧形转弯...', "测试")
|
||
|
||
# 记录初始位置和角度
|
||
initial_pos = ctrl.odo_msg.xyz
|
||
initial_angle = ctrl.odo_msg.rpy[2]
|
||
info(f"初始位置: x={initial_pos[0]:.3f}, y={initial_pos[1]:.3f}", "位置")
|
||
info(f"初始角度: {initial_angle:.2f}度", "角度")
|
||
|
||
# 执行右转90度弧形转弯
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=-90, # 负值表示右转
|
||
radius=0.5, # 0.5米转弯半径
|
||
observe=observe,
|
||
pass_align=True,
|
||
no_end_reset=True,
|
||
)
|
||
|
||
# 记录最终位置和角度
|
||
final_pos = ctrl.odo_msg.xyz
|
||
final_angle = ctrl.odo_msg.rpy[2]
|
||
distance_moved = ((final_pos[0] - initial_pos[0])**2 + (final_pos[1] - initial_pos[1])**2)**0.5
|
||
angle_diff = final_angle - initial_angle
|
||
|
||
info(f"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}", "位置")
|
||
info(f"最终角度: {final_angle:.2f}度", "角度")
|
||
info(f"移动距离: {distance_moved:.3f}米", "距离")
|
||
info(f"角度变化: {angle_diff:.2f}度", "角度")
|
||
|
||
if turn_success:
|
||
success("右转90度弧形转弯测试成功", "成功")
|
||
if res:
|
||
info(f"转弯半径: {res.get('radius', 'N/A')}", "参数")
|
||
else:
|
||
error("右转90度弧形转弯测试失败", "失败")
|
||
|
||
def test_s_curve_movement(ctrl, msg, observe=True):
|
||
"""测试S形弯道运动"""
|
||
section('测试：S形弯道运动', "复杂运动")
|
||
info('开始测试S形弯道运动...', "测试")
|
||
|
||
# 记录起始位置
|
||
start_pos = ctrl.odo_msg.xyz
|
||
info(f"起始位置: x={start_pos[0]:.3f}, y={start_pos[1]:.3f}", "位置")
|
||
|
||
# 第一个弧形转弯（左转）
|
||
info("执行第一个弧形转弯（左转90度）", "运动")
|
||
turn_success1, res1 = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=90,
|
||
radius=0.5,
|
||
observe=observe,
|
||
pass_align=True,
|
||
no_end_reset=True,
|
||
)
|
||
|
||
time.sleep(1)
|
||
|
||
# 直线段
|
||
info("执行直线段", "运动")
|
||
go_straight(ctrl, msg, distance=1.0, speed=0.5, observe=observe)
|
||
|
||
time.sleep(1)
|
||
|
||
# 第二个弧形转弯（右转）
|
||
info("执行第二个弧形转弯（右转90度）", "运动")
|
||
turn_success2, res2 = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=-90,
|
||
radius=0.5,
|
||
observe=observe,
|
||
pass_align=True,
|
||
no_end_reset=True,
|
||
)
|
||
|
||
# 记录结束位置
|
||
end_pos = ctrl.odo_msg.xyz
|
||
total_distance = ((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"总移动距离: {total_distance:.3f}米", "距离")
|
||
|
||
if turn_success1 and turn_success2:
|
||
success("S形弯道运动测试成功", "成功")
|
||
else:
|
||
error("S形弯道运动测试失败", "失败")
|
||
|
||
def test_different_radius_turns(ctrl, msg, observe=True):
|
||
"""测试不同半径的弧形转弯"""
|
||
section('测试：不同半径的弧形转弯', "弧形转弯")
|
||
|
||
radii = [0.3, 0.5, 0.8, 1.0] # 不同的转弯半径
|
||
|
||
for radius in radii:
|
||
info(f'开始测试半径为{radius}米的弧形转弯...', "测试")
|
||
|
||
# 记录初始位置
|
||
initial_pos = ctrl.odo_msg.xyz
|
||
|
||
# 执行弧形转弯
|
||
turn_success, res = arc_turn_around_hori_line(
|
||
ctrl=ctrl,
|
||
msg=msg,
|
||
angle_deg=90,
|
||
radius=radius,
|
||
observe=observe,
|
||
pass_align=True,
|
||
no_end_reset=True,
|
||
)
|
||
|
||
# 记录最终位置
|
||
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"半径{radius}米转弯完成，移动距离: {distance_moved:.3f}米", "结果")
|
||
|
||
if turn_success:
|
||
success(f"半径{radius}米弧形转弯测试成功", "成功")
|
||
else:
|
||
error(f"半径{radius}米弧形转弯测试失败", "失败")
|
||
|
||
# 每次测试之间暂停
|
||
time.sleep(2)
|
||
|
||
# 回到原始方向
|
||
turn_degree_v2(ctrl, msg, degree=0, absolute=True, precision=True)
|
||
time.sleep(1)
|
||
|
||
def run_arc_movement_tests(ctrl, msg):
|
||
"""运行所有弧形运动测试"""
|
||
section('弧形运动测试套件', "开始")
|
||
|
||
tests = [
|
||
("90度弧形转弯", test_arc_turn_90_degrees),
|
||
("180度弧形转弯", test_arc_turn_180_degrees),
|
||
("右转90度弧形转弯", test_arc_turn_right_90_degrees),
|
||
("S形弯道运动", test_s_curve_movement),
|
||
("不同半径弧形转弯", test_different_radius_turns),
|
||
]
|
||
|
||
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(3)
|
||
|
||
success("所有弧形运动测试完成", "完成")
|
||
|
||
if __name__ == "__main__":
|
||
# 这里可以添加独立运行的代码
|
||
print("弧形运动测试脚本")
|
||
print("使用方法：")
|
||
print("from single_test.test_arc_movements import run_arc_movement_tests")
|
||
print("run_arc_movement_tests(ctrl, msg)") |