装甲板姿态解算与滤波预测系统（ROS2）

项目简介

本项目基于ROS2实现装甲板三维姿态的解算、滤波、预测与可视化，适用于机器人装甲板识别与跟踪场景。系统包含数据发布、PnP解算、EKF滤波与预测、误差可视化等完整数据流，支持一键launch启动。

主要功能

• publisher：读取CSV数据并发布为ROS2 topic
• pose_solver：订阅原始数据，进行PnP姿态解算，发布真实姿态
• filter：对姿态数据进行EKF滤波与速度预测，发布估计与预测结果
• plot_compare.py：实时三线对比可视化（True/EKF/Predicted）
• plot_error.py：误差曲线可视化
• 一键启动：通过armor.launch.py自动启动所有核心节点

目录结构

armor_ws/
├── src/
│   ├── armor_pose_estimator/
│   │   ├── armor_pose_estimator/
│   │   │   ├── publisher.py
│   │   │   ├── pose_solver.py
│   │   │   ├── filter.py
│   │   ├── launch/armor.launch.py
│   ├── armor_plot/
│   │   ├── plot_compare.py
│   │   ├── plot_error.py

依赖环境

• ROS2 Foxy/Humble等
• Python 3.8+
• filterpy
• opencv-python
• matplotlib

安装依赖：

pip install filterpy opencv-python matplotlib

启动方法

1. 编译工作空间：

colcon build --symlink-install
source install/setup.bash

2. 启动所有节点：

ros2 launch armor_pose_estimator armor.launch.py

3. 可视化对比：

python3 src/armor_plot/plot_compare.py

4. 可视化误差：

python3 src/armor_plot/plot_error.py

说明

• 所有topic均已在launch文件中自动注册
• 支持自定义数据源与参数调整
• 可根据实际需求扩展节点与功能