bf

src/rm_utils/README.md

@@ -0,0 +1,121 @@
+# rm_utils 
+
+## 1. 介绍
+该模块提供通用的工具类以及函数，包括：
+* 扩展卡尔曼滤波器 ExtendedKalmanFilter
+* PnP解算器 PnPSolver
+* 弹道补偿器 TrajectoryCompensator
+* FYT日志库
+* URL路径解析器 URLResolver
+* 心跳发布者 HeartBeatPublisher
+
+## 2. 使用方法
+
+### 2.0 扩展卡尔曼滤波
+
+见armor_solver/src/armor_solver_node.cpp
+
+### 2.1 PnP解算
+
+示例：
+```cpp
+#include "rm_utils/math/pnp_solver.hpp"
+
+// 1. 创建PnP解算器
+// 构造函数参数(cv::Mat, cv::Mat, cv::PnPMethod)：相机内参矩阵、畸变系数、解算方法（默认是IPPE）
+auto pnp_solver = PnPSolver(camera_matrix, distortion_coefficients);
+// 2. 为PnPSolver设置一个物体坐标系
+std::vector<cv::Point3f> object_points = {
+    cv::Point3f(-1.0f, 1.0f, 0.0f),
+    cv::Point3f(1.0f, 1.0f, 0.0f),
+    cv::Point3f(1.0f, -1.0f, 0.0f),
+    cv::Point3f(-1.0f, -1.0f, 0.0f)
+};
+pnp_solver.setObjectPoints(object_points, "object_frame");
+// 3. PnP解算
+cv::Mat rvec, tvec;
+pnp_solver.solvePnP(image_points, rvec, tvec, "object_frame");
+
+// 对于打符来说，因为以前的PnP是直接返回一个表示位姿的Eigen::VectorXd，所以这里也提供了一个函数，用于将rvec和tvec转换为VectorXd
+// 4. 获取位姿
+Eigen::VectorXd pose = pnp_solver.getPose(rvec, tvec);
+```
+
+### 2.2 弹道补偿
+
+示例：
+```cpp
+#include "rm_utils/math/trajectory_compensator.hpp"
+
+auto compensator = CompensatorFactory::createCompensator("ideal");
+double pitch = 0;
+Eigen::Vector3d p = Eigen::Vector3d(1.0, 0, 0);
+double temp_pitch = pitch;
+if (trajectory_compensator_->compensate(p, temp_pitch)) {
+    pitch = temp_pitch;
+}
+```
+
+### 2.3 Eigen和cv::Mat的相互转换
+
+示例：
+```cpp
+#include "rm_utils/math/utils.hpp"
+
+cv::Mat mat = cv::Mat::eye(3, 3, CV_32FC1);
+Eigen::Matrix3f eigen_mat = utils::cvToEigen(mat);
+cv::Mat mat2 = utils::eigenToCv(eigen_mat);
+```
+
+### 2.4 FYT日志库
+
+示例：
+```cpp
+#include "rm_utils/logger/log.hpp"
+
+// 1. 初始化
+// 参数：日志名称、日志文件路径、日志级别
+FYT_REGISTER_LOGGER("test_logger", "/tmp/test_logger.log", INFO);
+
+// 2. 使用
+FYT_INFO("test_logger", "This is a test log");
+int a = 1;
+FYT_WARN("test_logger", "a = {}", a);
+```
+
+### 2.5 URL Resolver
+
+能够用很方便的方式（类camera_info_url）访问文件和或ros2包的install目录
+
+示例:
+```cpp
+#include "rm_utils/url_resolver.hpp"
+
+namespace fs = std::filesystem;
+fs::path model_path =
+  utils::URLResolver::getResolvedPath("package://armor_detector/model/lenet.onnx");
+
+if (fs::exists(model_path)) {
+    std::cout<<model_path.string()<<std::endl;
+}
+
+fs::path file_path = utils::URLResolver::getResolvedPath("file://home/zcf/123.txt");
+```
+
+### 2.6 HearBeatPublisher
+
+定时发布心跳数据
+
+示例：
+```cpp
+#include "rm_utils/heartbeat.hpp"
+
+class TestNode : public rclcp::Node {
+public:
+  TestNode(const rclcpp::NodeOptions& options) : Node("test", opitions) {
+    heartbeat_ = HeartBeatPublisher::create(this); // 传入Node*
+  }
+private:
+  HeartBeatPublisher::SharedPtr heartbeat_;
+};
+```