435 lines
17 KiB
Python
435 lines
17 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""
|
||
复杂运动测试脚本
|
||
测试连续弯道、复杂轨迹、组合运动等高级运动功能
|
||
"""
|
||
|
||
import time
|
||
import sys
|
||
import os
|
||
import math
|
||
|
||
# 添加父目录到路径，以便能够导入utils
|
||
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 base_move.turn_degree import turn_degree_v2
|
||
from base_move.go_straight import go_straight
|
||
from base_move.go_to_xy import go_to_xy_v2
|
||
from base_move.center_on_dual_tracks import center_on_dual_tracks
|
||
|
||
# 创建日志记录器
|
||
logger = get_logger("复杂运动测试")
|
||
|
||
def test_continuous_curves(ctrl, msg, observe=True):
|
||
"""测试连续弯道运动（模拟任务2的三重弯道）"""
|
||
section('测试：连续弯道运动', "复杂运动")
|
||
info('开始测试连续弯道运动...', "测试")
|
||
|
||
try:
|
||
# 记录起始位置
|
||
start_pos = ctrl.odo_msg.xyz
|
||
info(f"起始位置: x={start_pos[0]:.3f}, y={start_pos[1]:.3f}", "位置")
|
||
|
||
# 模拟任务2的连续弯道参数
|
||
# 简化版本的弯道参数
|
||
directions = [
|
||
# [vel_x, vel_y, vel_z, rpy_x, rpy_y, rpy_z, duration]
|
||
(0.4, 0, 0, 0, 0, 0, 1.2), # 直线走一段
|
||
(0.45, 0, 0.77, 0, 0, 0, 3.0), # 第一个圆弧（简化时间）
|
||
(0.3, 0, 0, 0, 0, 0, 0.4), # 微调路径
|
||
(0.4, 0, -0.73, 0, 0, 0, 3.0), # 第二个圆弧
|
||
(0.3, 0, 0, 0, 0, 0, 0.5), # 微调路径
|
||
(0.4, 0, 0.73, 0, 0, 0, 3.0), # 第三个圆弧
|
||
(0.3, 0, 0, 0, 0, 0, 0.5), # 微调路径
|
||
]
|
||
|
||
segment_names = [
|
||
"直线段", "左弯1", "直线调整1", "右弯", "直线调整2", "左弯2", "直线调整3"
|
||
]
|
||
|
||
for i, (vel_x, vel_y, vel_z, rpy_x, rpy_y, rpy_z, duration) in enumerate(directions):
|
||
info(f"执行第{i+1}段: {segment_names[i]}", "运动")
|
||
|
||
# 记录段开始位置
|
||
segment_start = ctrl.odo_msg.xyz
|
||
|
||
# 设置运动参数
|
||
msg.mode = 11 # Locomotion模式
|
||
msg.gait_id = 26 # 自变频步态
|
||
msg.vel_des = [vel_x, vel_y, vel_z]
|
||
msg.rpy_des = [rpy_x, rpy_y, rpy_z]
|
||
msg.duration = 0 # 持续运动
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
|
||
# 执行运动
|
||
start_time = time.time()
|
||
while time.time() - start_time < duration:
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(0.1)
|
||
|
||
# 停止运动
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.rpy_des = [0, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 记录段结束位置
|
||
segment_end = ctrl.odo_msg.xyz
|
||
segment_distance = ((segment_end[0] - segment_start[0])**2 +
|
||
(segment_end[1] - segment_start[1])**2)**0.5
|
||
|
||
info(f"第{i+1}段完成，移动距离: {segment_distance:.3f}米", "结果")
|
||
|
||
# 段间暂停
|
||
time.sleep(0.5)
|
||
|
||
# 记录总结果
|
||
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}米", "距离")
|
||
|
||
success("连续弯道运动测试完成", "成功")
|
||
|
||
except Exception as e:
|
||
error(f"连续弯道运动测试失败: {str(e)}", "失败")
|
||
finally:
|
||
# 确保停止运动
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.rpy_des = [0, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
def test_figure_eight_movement(ctrl, msg, observe=True):
|
||
"""测试8字形运动"""
|
||
section('测试：8字形运动', "复杂运动")
|
||
info('开始测试8字形运动...', "测试")
|
||
|
||
try:
|
||
# 记录起始位置
|
||
start_pos = ctrl.odo_msg.xyz
|
||
info(f"起始位置: x={start_pos[0]:.3f}, y={start_pos[1]:.3f}", "位置")
|
||
|
||
# 8字形运动参数
|
||
radius = 1.0 # 圆弧半径
|
||
speed = 0.3 # 运动速度
|
||
|
||
# 第一个圆（顺时针）
|
||
info("执行第一个圆（顺时针）", "运动")
|
||
msg.mode = 11
|
||
msg.gait_id = 26
|
||
msg.vel_des = [speed, 0, -0.3] # 右转
|
||
msg.duration = 0
|
||
msg.step_height = [0.06, 0.06]
|
||
msg.life_count += 1
|
||
|
||
# 执行第一个圆
|
||
start_time = time.time()
|
||
circle_duration = 2 * math.pi * radius / speed # 计算完成一个圆所需时间
|
||
while time.time() - start_time < circle_duration:
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(0.1)
|
||
|
||
# 暂停并记录中间位置
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(1)
|
||
|
||
mid_pos = ctrl.odo_msg.xyz
|
||
info(f"第一个圆完成，中间位置: x={mid_pos[0]:.3f}, y={mid_pos[1]:.3f}", "位置")
|
||
|
||
# 第二个圆（逆时针）
|
||
info("执行第二个圆（逆时针）", "运动")
|
||
msg.vel_des = [speed, 0, 0.3] # 左转
|
||
msg.life_count += 1
|
||
|
||
# 执行第二个圆
|
||
start_time = time.time()
|
||
while time.time() - start_time < circle_duration:
|
||
ctrl.Send_cmd(msg)
|
||
time.sleep(0.1)
|
||
|
||
# 停止运动
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 记录结束位置
|
||
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}米", "距离")
|
||
|
||
success("8字形运动测试完成", "成功")
|
||
|
||
except Exception as e:
|
||
error(f"8字形运动测试失败: {str(e)}", "失败")
|
||
finally:
|
||
# 确保停止运动
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
def test_spiral_movement(ctrl, msg, observe=True):
|
||
"""测试螺旋运动"""
|
||
section('测试：螺旋运动', "复杂运动")
|
||
info('开始测试螺旋运动...', "测试")
|
||
|
||
try:
|
||
# 记录起始位置
|
||
start_pos = ctrl.odo_msg.xyz
|
||
info(f"起始位置: x={start_pos[0]:.3f}, y={start_pos[1]:.3f}", "位置")
|
||
|
||
# 螺旋运动参数
|
||
initial_speed = 0.2
|
||
max_speed = 0.5
|
||
angular_velocity = 0.5
|
||
duration = 10 # 总持续时间
|
||
|
||
info(f"开始螺旋运动，持续{duration}秒", "运动")
|
||
|
||
msg.mode = 11
|
||
msg.gait_id = 26
|
||
msg.duration = 0
|
||
msg.step_height = [0.06, 0.06]
|
||
|
||
start_time = time.time()
|
||
while time.time() - start_time < duration:
|
||
# 计算当前时间比例
|
||
time_ratio = (time.time() - start_time) / duration
|
||
|
||
# 线性增加前进速度
|
||
current_speed = initial_speed + (max_speed - initial_speed) * time_ratio
|
||
|
||
# 设置运动参数
|
||
msg.vel_des = [current_speed, 0, angular_velocity]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
if observe and int((time.time() - start_time) * 2) % 2 == 0:
|
||
current_pos = ctrl.odo_msg.xyz
|
||
distance_from_start = ((current_pos[0] - start_pos[0])**2 +
|
||
(current_pos[1] - start_pos[1])**2)**0.5
|
||
info(f"螺旋运动进行中，当前速度: {current_speed:.2f}m/s, 距起点: {distance_from_start:.3f}m", "状态")
|
||
|
||
time.sleep(0.1)
|
||
|
||
# 停止运动
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
# 记录结束位置
|
||
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}米", "距离")
|
||
|
||
success("螺旋运动测试完成", "成功")
|
||
|
||
except Exception as e:
|
||
error(f"螺旋运动测试失败: {str(e)}", "失败")
|
||
finally:
|
||
# 确保停止运动
|
||
msg.vel_des = [0, 0, 0]
|
||
msg.life_count += 1
|
||
ctrl.Send_cmd(msg)
|
||
|
||
def test_precision_positioning(ctrl, msg, observe=True):
|
||
"""测试精确定位"""
|
||
section('测试：精确定位', "复杂运动")
|
||
info('开始测试精确定位...', "测试")
|
||
|
||
try:
|
||
# 记录起始位置
|
||
start_pos = ctrl.odo_msg.xyz
|
||
info(f"起始位置: x={start_pos[0]:.3f}, y={start_pos[1]:.3f}", "位置")
|
||
|
||
# 定义目标位置序列
|
||
target_positions = [
|
||
(start_pos[0] + 1.0, start_pos[1] + 0.5), # 右前方
|
||
(start_pos[0] + 0.5, start_pos[1] + 1.0), # 前左方
|
||
(start_pos[0] - 0.5, start_pos[1] + 0.5), # 左前方
|
||
(start_pos[0], start_pos[1]), # 回到起点
|
||
]
|
||
|
||
for i, (target_x, target_y) in enumerate(target_positions, 1):
|
||
info(f"移动到目标位置{i}: x={target_x:.3f}, y={target_y:.3f}", "目标")
|
||
|
||
# 记录移动前位置
|
||
before_pos = ctrl.odo_msg.xyz
|
||
|
||
# 使用精确定位函数
|
||
go_to_xy_v2(ctrl, msg, target_x, target_y, speed=0.3, observe=observe)
|
||
|
||
# 记录移动后位置
|
||
after_pos = ctrl.odo_msg.xyz
|
||
|
||
# 计算定位误差
|
||
error_x = abs(after_pos[0] - target_x)
|
||
error_y = abs(after_pos[1] - target_y)
|
||
total_error = math.sqrt(error_x**2 + error_y**2)
|
||
|
||
info(f"实际位置: x={after_pos[0]:.3f}, y={after_pos[1]:.3f}", "位置")
|
||
info(f"定位误差: x={error_x:.3f}m, y={error_y:.3f}m, 总误差={total_error:.3f}m", "误差")
|
||
|
||
if total_error < 0.1: # 10cm以内认为精确
|
||
success(f"目标位置{i}到达精确", "成功")
|
||
else:
|
||
warning(f"目标位置{i}定位误差较大", "警告")
|
||
|
||
# 暂停
|
||
time.sleep(1)
|
||
|
||
# 最终位置检查
|
||
final_pos = ctrl.odo_msg.xyz
|
||
return_error = ((final_pos[0] - start_pos[0])**2 + (final_pos[1] - start_pos[1])**2)**0.5
|
||
info(f"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}", "位置")
|
||
info(f"回到起点误差: {return_error:.3f}米", "误差")
|
||
|
||
success("精确定位测试完成", "成功")
|
||
|
||
except Exception as e:
|
||
error(f"精确定位测试失败: {str(e)}", "失败")
|
||
|
||
def test_dual_track_following(ctrl, msg, observe=True):
|
||
"""测试双轨道跟随"""
|
||
section('测试：双轨道跟随', "复杂运动")
|
||
info('开始测试双轨道跟随...', "测试")
|
||
|
||
try:
|
||
# 记录起始位置
|
||
start_pos = ctrl.odo_msg.xyz
|
||
info(f"起始位置: x={start_pos[0]:.3f}, y={start_pos[1]:.3f}", "位置")
|
||
|
||
# 测试双轨道居中
|
||
info("执行双轨道居中校准", "校准")
|
||
center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=observe, detect_height=0.3)
|
||
|
||
# 记录校准后位置
|
||
centered_pos = ctrl.odo_msg.xyz
|
||
lateral_adjustment = abs(centered_pos[1] - start_pos[1])
|
||
info(f"校准后位置: x={centered_pos[0]:.3f}, y={centered_pos[1]:.3f}", "位置")
|
||
info(f"横向调整距离: {lateral_adjustment:.3f}米", "调整")
|
||
|
||
# 沿轨道直线移动
|
||
info("沿双轨道直线移动", "移动")
|
||
go_straight(ctrl, msg, distance=2.0, speed=0.5, observe=observe)
|
||
|
||
# 记录移动后位置
|
||
moved_pos = ctrl.odo_msg.xyz
|
||
distance_moved = ((moved_pos[0] - centered_pos[0])**2 + (moved_pos[1] - centered_pos[1])**2)**0.5
|
||
info(f"移动后位置: x={moved_pos[0]:.3f}, y={moved_pos[1]:.3f}", "位置")
|
||
info(f"移动距离: {distance_moved:.3f}米", "距离")
|
||
|
||
# 再次校准检查
|
||
info("再次执行双轨道居中校准", "校准")
|
||
center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=observe, detect_height=0.3)
|
||
|
||
# 记录最终位置
|
||
final_pos = ctrl.odo_msg.xyz
|
||
final_adjustment = abs(final_pos[1] - moved_pos[1])
|
||
info(f"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}", "位置")
|
||
info(f"最终横向调整: {final_adjustment:.3f}米", "调整")
|
||
|
||
success("双轨道跟随测试完成", "成功")
|
||
|
||
except Exception as e:
|
||
error(f"双轨道跟随测试失败: {str(e)}", "失败")
|
||
|
||
def test_combined_movements(ctrl, msg, observe=True):
|
||
"""测试组合运动"""
|
||
section('测试：组合运动', "复杂运动")
|
||
info('开始测试组合运动...', "测试")
|
||
|
||
try:
|
||
# 记录起始位置
|
||
start_pos = ctrl.odo_msg.xyz
|
||
info(f"起始位置: x={start_pos[0]:.3f}, y={start_pos[1]:.3f}", "位置")
|
||
|
||
# 组合运动序列
|
||
movements = [
|
||
("直线前进", lambda: go_straight(ctrl, msg, distance=1.0, speed=0.5, observe=observe)),
|
||
("右转90度", lambda: turn_degree_v2(ctrl, msg, degree=90, absolute=False, precision=True)),
|
||
("直线前进", lambda: go_straight(ctrl, msg, distance=1.0, speed=0.5, observe=observe)),
|
||
("左转180度", lambda: turn_degree_v2(ctrl, msg, degree=-180, absolute=False, precision=True)),
|
||
("直线前进", lambda: go_straight(ctrl, msg, distance=1.0, speed=0.5, observe=observe)),
|
||
("右转90度", lambda: turn_degree_v2(ctrl, msg, degree=90, absolute=False, precision=True)),
|
||
("返回起点", lambda: go_straight(ctrl, msg, distance=1.0, speed=0.5, observe=observe)),
|
||
]
|
||
|
||
for i, (movement_name, movement_func) in enumerate(movements, 1):
|
||
info(f"执行第{i}个动作: {movement_name}", "动作")
|
||
|
||
# 记录动作前位置
|
||
before_pos = ctrl.odo_msg.xyz
|
||
before_angle = ctrl.odo_msg.rpy[2]
|
||
|
||
# 执行动作
|
||
movement_func()
|
||
|
||
# 记录动作后位置
|
||
after_pos = ctrl.odo_msg.xyz
|
||
after_angle = ctrl.odo_msg.rpy[2]
|
||
|
||
# 计算变化
|
||
distance_change = ((after_pos[0] - before_pos[0])**2 + (after_pos[1] - before_pos[1])**2)**0.5
|
||
angle_change = after_angle - before_angle
|
||
|
||
info(f"动作完成，位移: {distance_change:.3f}m, 角度变化: {angle_change:.2f}°", "结果")
|
||
|
||
# 动作间暂停
|
||
time.sleep(0.5)
|
||
|
||
# 检查最终位置
|
||
final_pos = ctrl.odo_msg.xyz
|
||
final_angle = ctrl.odo_msg.rpy[2]
|
||
return_error = ((final_pos[0] - start_pos[0])**2 + (final_pos[1] - start_pos[1])**2)**0.5
|
||
|
||
info(f"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}, 角度: {final_angle:.2f}°", "位置")
|
||
info(f"回到起点误差: {return_error:.3f}米", "误差")
|
||
|
||
if return_error < 0.2: # 20cm以内认为成功
|
||
success("组合运动测试成功完成", "成功")
|
||
else:
|
||
warning("组合运动测试完成，但回到起点误差较大", "警告")
|
||
|
||
except Exception as e:
|
||
error(f"组合运动测试失败: {str(e)}", "失败")
|
||
|
||
def run_complex_movement_tests(ctrl, msg):
|
||
"""运行所有复杂运动测试"""
|
||
section('复杂运动测试套件', "开始")
|
||
|
||
tests = [
|
||
("连续弯道运动", test_continuous_curves),
|
||
("8字形运动", test_figure_eight_movement),
|
||
("螺旋运动", test_spiral_movement),
|
||
("精确定位", test_precision_positioning),
|
||
("双轨道跟随", test_dual_track_following),
|
||
("组合运动", test_combined_movements),
|
||
]
|
||
|
||
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_complex_movements import run_complex_movement_tests")
|
||
print("run_complex_movement_tests(ctrl, msg)") |