188 lines
5.9 KiB
Python
188 lines
5.9 KiB
Python
#!/usr/bin/env python3
|
||
"""
|
||
低头匍匐步态集成测试脚本
|
||
用于验证所有模块是否正确导入和配置
|
||
"""
|
||
|
||
import sys
|
||
import os
|
||
|
||
# 添加路径
|
||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||
|
||
def test_imports():
|
||
"""测试所有模块导入"""
|
||
print("测试模块导入...")
|
||
|
||
try:
|
||
# 测试基本模块导入
|
||
from task_2.crawl_gait import (
|
||
low_crawl_gait,
|
||
transition_to_crawl_position,
|
||
recover_from_crawl_position,
|
||
run_low_crawl_demo
|
||
)
|
||
print("✓ crawl_gait模块导入成功")
|
||
|
||
from task_2.task_2 import (
|
||
run_task_2_with_crawl_demo,
|
||
test_crawl_gait_only,
|
||
test_crawl_transition_only
|
||
)
|
||
print("✓ task_2模块导入成功")
|
||
|
||
from task_2.file_send_lcmt import file_send_lcmt
|
||
from task_2.robot_control_cmd_lcmt import robot_control_cmd_lcmt
|
||
print("✓ LCM消息模块导入成功")
|
||
|
||
return True
|
||
|
||
except ImportError as e:
|
||
print(f"✗ 导入失败: {e}")
|
||
return False
|
||
|
||
def test_config_files():
|
||
"""测试配置文件存在性"""
|
||
print("\n测试配置文件...")
|
||
|
||
files_to_check = [
|
||
"./task_2/Gait_Def_crawl.toml",
|
||
"./task_2/Gait_Params_crawl.toml",
|
||
]
|
||
|
||
all_exist = True
|
||
for file_path in files_to_check:
|
||
if os.path.exists(file_path):
|
||
print(f"✓ {file_path} 存在")
|
||
else:
|
||
print(f"✗ {file_path} 不存在")
|
||
all_exist = False
|
||
|
||
return all_exist
|
||
|
||
def test_toml_syntax():
|
||
"""测试TOML文件语法"""
|
||
print("\n测试TOML文件语法...")
|
||
|
||
try:
|
||
import toml
|
||
|
||
# 测试步态定义文件
|
||
with open("./task_2/Gait_Def_crawl.toml", 'r') as f:
|
||
gait_def = toml.load(f)
|
||
print(f"✓ Gait_Def_crawl.toml 语法正确，包含 {len(gait_def['section'])} 个步态段")
|
||
|
||
# 测试步态参数文件
|
||
with open("./task_2/Gait_Params_crawl.toml", 'r') as f:
|
||
gait_params = toml.load(f)
|
||
print(f"✓ Gait_Params_crawl.toml 语法正确，包含 {len(gait_params['step'])} 个步态参数")
|
||
|
||
return True
|
||
|
||
except ImportError:
|
||
print("! toml模块未安装，跳过语法检查")
|
||
return True
|
||
except Exception as e:
|
||
print(f"✗ TOML文件语法错误: {e}")
|
||
return False
|
||
|
||
def validate_gait_parameters():
|
||
"""验证步态参数的合理性"""
|
||
print("\n验证步态参数...")
|
||
|
||
try:
|
||
import toml
|
||
|
||
with open("./task_2/Gait_Params_crawl.toml", 'r') as f:
|
||
gait_params = toml.load(f)
|
||
|
||
# 检查关键参数
|
||
first_step = gait_params['step'][0]
|
||
|
||
# 检查body_pos_des
|
||
body_pos = first_step['body_pos_des']
|
||
if len(body_pos) == 6:
|
||
print(f"✓ body_pos_des格式正确: roll={body_pos[0]}, pitch={body_pos[1]}, yaw={body_pos[2]}, x={body_pos[3]}, y={body_pos[4]}, z={body_pos[5]}")
|
||
else:
|
||
print(f"✗ body_pos_des格式错误，应为6个元素，实际{len(body_pos)}个")
|
||
return False
|
||
|
||
# 检查body_vel_des
|
||
body_vel = first_step['body_vel_des']
|
||
if len(body_vel) == 3:
|
||
print(f"✓ body_vel_des格式正确: vx={body_vel[0]}, vy={body_vel[1]}, vyaw={body_vel[2]}")
|
||
else:
|
||
print(f"✗ body_vel_des格式错误，应为3个元素，实际{len(body_vel)}个")
|
||
return False
|
||
|
||
# 检查landing_pos_des
|
||
landing_pos = first_step['landing_pos_des']
|
||
if len(landing_pos) == 12:
|
||
print(f"✓ landing_pos_des格式正确: 12个落足位置参数")
|
||
else:
|
||
print(f"✗ landing_pos_des格式错误，应为12个元素，实际{len(landing_pos)}个")
|
||
return False
|
||
|
||
# 检查step_height
|
||
step_height = first_step['step_height']
|
||
if len(step_height) == 4:
|
||
print(f"✓ step_height格式正确: FR={step_height[0]}, FL={step_height[1]}, RR={step_height[2]}, RL={step_height[3]}")
|
||
else:
|
||
print(f"✗ step_height格式错误，应为4个元素，实际{len(step_height)}个")
|
||
return False
|
||
|
||
return True
|
||
|
||
except ImportError:
|
||
print("! toml模块未安装，跳过参数验证")
|
||
return True
|
||
except Exception as e:
|
||
print(f"✗ 参数验证失败: {e}")
|
||
return False
|
||
|
||
def main():
|
||
"""主测试函数"""
|
||
print("=" * 50)
|
||
print("低头匍匐步态集成测试")
|
||
print("=" * 50)
|
||
|
||
tests = [
|
||
("模块导入", test_imports),
|
||
("配置文件检查", test_config_files),
|
||
("TOML语法检查", test_toml_syntax),
|
||
("参数验证", validate_gait_parameters),
|
||
]
|
||
|
||
results = []
|
||
for test_name, test_func in tests:
|
||
print(f"\n[{test_name}]")
|
||
result = test_func()
|
||
results.append((test_name, result))
|
||
|
||
print("\n" + "=" * 50)
|
||
print("测试结果汇总:")
|
||
print("=" * 50)
|
||
|
||
all_passed = True
|
||
for test_name, result in results:
|
||
status = "✓ 通过" if result else "✗ 失败"
|
||
print(f"{test_name:15} {status}")
|
||
if not result:
|
||
all_passed = False
|
||
|
||
print("\n" + "=" * 50)
|
||
if all_passed:
|
||
print("🎉 所有测试通过！低头匍匐步态已准备就绪。")
|
||
print("\n使用方法:")
|
||
print("1. 完整演示: run_low_crawl_demo(ctrl, msg, distance=2.0)")
|
||
print("2. 与任务2集成: run_task_2_with_crawl_demo(ctrl, msg)")
|
||
print("3. 仅测试步态: test_crawl_gait_only(ctrl, msg)")
|
||
print("4. 仅测试姿态: test_crawl_transition_only(ctrl, msg)")
|
||
else:
|
||
print("❌ 部分测试失败，请检查配置。")
|
||
print("=" * 50)
|
||
|
||
if __name__ == "__main__":
|
||
main()
|
||
|