#!/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)")