From 40fafa1dc410be07164b851d01d327f08c0dc8a0 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 1 Sep 2025 14:37:02 +0800 Subject: [PATCH 01/93] refactor(utils): rename transform.hpp to coordinate.hpp --- src/tongji/solver/solver.cpp | 16 ++++++++-------- src/util/{transform.hpp => coordinate.hpp} | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) rename src/util/{transform.hpp => coordinate.hpp} (96%) diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 48665c5..49496e5 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -16,9 +16,9 @@ #include "interfaces/time_stamped.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" +#include "util/coordinate.hpp" #include "util/index.hpp" #include "util/math.hpp" -#include "util/transform.hpp" namespace world_exe::tongji::solver { class ArmorInCameraImpl final : public interfaces::IArmorInCamera, public interfaces::ITimeStamped { @@ -66,7 +66,7 @@ class Solver::Impl { result->SetTimeStamp(armors_in_image->GetTimeStamped().GetTimeStamp()); for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { - const auto armor_id = static_cast(1 << i); + const auto armor_id = static_cast(1 << i); const auto& image_armors = armors_in_image->GetArmors(armor_id); for (const auto& armor_image : image_armors) { @@ -103,10 +103,10 @@ class Solver::Impl { Eigen::Quaterniond orientation_in_camera(R_armor2camera); world_exe::data::ArmorCameraSpacing armor_in_camera; - auto orientation_ros = util::transform::opencv2ros_orientation(orientation_in_camera); + auto orientation_ros = util::coordinate::opencv2ros_orientation(orientation_in_camera); orientation_ros.normalize(); armor_in_camera.id = armor_in_image.id; - armor_in_camera.position = util::transform::opencv2ros_position(xyz_in_camera); + armor_in_camera.position = util::coordinate::opencv2ros_position(xyz_in_camera); armor_in_camera.orientation = orientation_ros; if (armor_in_camera.position.norm() > MaxArmorDistance) { @@ -120,13 +120,13 @@ class Solver::Impl { const world_exe::data::ArmorCameraSpacing& armor_in_camera) const { Eigen::Vector3d armor_in_camera_ocv = - util::transform::ros2opencv_position(armor_in_camera.position); + util::coordinate::ros2opencv_position(armor_in_camera.position); Eigen::Vector3d armor_xyz_in_gimbal = R_camera2gimbal_ * armor_in_camera_ocv + t_camera2gimbal_; Eigen::Vector3d armor_xyz_in_world = R_gimbal2world_ * armor_xyz_in_gimbal; // why no t? Eigen::Quaterniond armor_orientation_in_camera_ocv = - util::transform::ros2opencv_orientation(armor_in_camera.orientation); + util::coordinate::ros2opencv_orientation(armor_in_camera.orientation); Eigen::Matrix3d R_armor2camera_ocv = armor_orientation_in_camera_ocv.toRotationMatrix(); Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera_ocv; Eigen::Matrix3d R_armor2world_initial = R_gimbal2world_ * R_armor2gimbal; @@ -216,8 +216,8 @@ class Solver::Impl { const Eigen::Vector3d& armor_xyz_in_world, const double& yaw, const double& inclined) const { - auto image_points = ReprojectArmor( - armor_xyz_in_world, yaw, armor_in_image.isLargeArmor, armor_in_image.id); + auto image_points = + ReprojectArmor(armor_xyz_in_world, yaw, armor_in_image.isLargeArmor, armor_in_image.id); auto error = 0.0; for (int i = 0; i < 4; i++) error += cv::norm(armor_in_image.image_points[i] - image_points[i]); diff --git a/src/util/transform.hpp b/src/util/coordinate.hpp similarity index 96% rename from src/util/transform.hpp rename to src/util/coordinate.hpp index 21c0dce..a0c6e96 100644 --- a/src/util/transform.hpp +++ b/src/util/coordinate.hpp @@ -2,7 +2,7 @@ #include -namespace world_exe::util::transform { +namespace world_exe::util::coordinate { static inline Eigen::Vector3d opencv2ros_position(const Eigen::Vector3d& position) { return Eigen::Vector3d(position.z(), -position.x(), -position.y()); From 83751c5657fb5221d9a21f88c984a965481151ac Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 2 Sep 2025 19:02:04 +0800 Subject: [PATCH 02/93] feat(classifier): add classifier - Return armor_id from input armor_pattern --- src/tongji/identifier/classifier.cpp | 160 +++++++++++++++++++++++++++ src/tongji/identifier/classifier.hpp | 16 +++ src/tongji/solver/solver.cpp | 1 - 3 files changed, 176 insertions(+), 1 deletion(-) create mode 100644 src/tongji/identifier/classifier.cpp create mode 100644 src/tongji/identifier/classifier.hpp diff --git a/src/tongji/identifier/classifier.cpp b/src/tongji/identifier/classifier.cpp new file mode 100644 index 0000000..f538a0b --- /dev/null +++ b/src/tongji/identifier/classifier.cpp @@ -0,0 +1,160 @@ +#include "classifier.hpp" +#include "enum/armor_id.hpp" + +#include +#include +#include +#include +#include + +namespace world_exe::tongji::identifier { +class Classifier::Impl final { +public: + explicit Impl(const std::string& model_path, int model_image_width, int model_image_height) + : model_image_width_(model_image_width) + , model_image_height_(model_image_height) { + + net_ = cv::dnn::readNetFromONNX(model_path); + auto ovmodel = core_.read_model(model_path); + compiled_model_ = core_.compile_model( + ovmodel, "AUTO", ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY)); + } + + enumeration::ArmorIdFlag classify(const cv::Mat& armor_pattern) { + if (armor_pattern.empty()) { + return enumeration::ArmorIdFlag::Unknow; + } + + cv::Mat gray; + cv::cvtColor(armor_pattern, gray, cv::COLOR_BGR2GRAY); + + auto input = cv::Mat(model_image_height_, model_image_width_, CV_8UC1, cv::Scalar(0)); + auto x_scale = static_cast(model_image_width_) / gray.cols; + auto y_scale = static_cast(model_image_height_) / gray.rows; + auto scale = std::min(x_scale, y_scale); + auto h = static_cast(gray.rows * scale); + auto w = static_cast(gray.cols * scale); + + if (h == 0 || w == 0) { + return enumeration::ArmorIdFlag::Unknow; + } + auto roi = cv::Rect(0, 0, w, h); + cv::resize(gray, input(roi), { w, h }); + + auto blob = cv::dnn::blobFromImage(input, 1.0 / 255.0, cv::Size(), cv::Scalar()); + + net_.setInput(blob); + cv::Mat outputs = net_.forward(); + + // softmax + float max = *std::max_element(outputs.begin(), outputs.end()); + cv::exp(outputs - max, outputs); + float sum = cv::sum(outputs)[0]; + outputs /= sum; + + double confidence; + cv::Point label_point; + cv::minMaxLoc(outputs.reshape(1, 1), nullptr, &confidence, nullptr, &label_point); + int label_id = label_point.x; + + enumeration::ArmorIdFlag armor_id; + + if (label_id == 3) { + armor_id = enumeration::ArmorIdFlag::InfantryIII; + } else if (label_id == 4) { + armor_id = enumeration::ArmorIdFlag::InfantryIV; + } else if (label_id == 5) { + armor_id = enumeration::ArmorIdFlag::InfantryV; + } else if (label_id == 6) { + armor_id = enumeration::ArmorIdFlag::Outpost; + } else if (label_id == 1) { + armor_id = enumeration::ArmorIdFlag::Hero; + } else if (label_id == 2) { + armor_id = enumeration::ArmorIdFlag::Engineer; + } else if (label_id == 0) { + armor_id = enumeration::ArmorIdFlag::Sentry; + } else if (label_id == 7) { + armor_id = enumeration::ArmorIdFlag::Base; + }; + + return armor_id; + } + + enumeration::ArmorIdFlag ovclassify(const cv::Mat& armor_pattern) { + + if (armor_pattern.empty()) { + return enumeration::ArmorIdFlag::Unknow; + } + + cv::Mat gray; + cv::cvtColor(armor_pattern, gray, cv::COLOR_BGR2GRAY); + + // Resize image + auto input = cv::Mat(model_image_height_, model_image_width_, CV_8UC1, cv::Scalar(0)); + auto x_scale = static_cast(model_image_width_) / gray.cols; + auto y_scale = static_cast(model_image_height_) / gray.rows; + auto scale = std::min(x_scale, y_scale); + auto h = static_cast(gray.rows * scale); + auto w = static_cast(gray.cols * scale); + + if (h == 0 || w == 0) { + return enumeration::ArmorIdFlag::Unknow; + } + + auto roi = cv::Rect(0, 0, w, h); + cv::resize(gray, input(roi), { w, h }); + // Normalize the input image to [0, 1] range + input.convertTo(input, CV_32F, 1.0 / 255.0); + + ov::Tensor input_tensor(ov::element::f32, { 1, 1, 32, 32 }, input.data); + + ov::InferRequest infer_request = compiled_model_.create_infer_request(); + infer_request.set_input_tensor(input_tensor); + infer_request.infer(); + + auto output_tensor = infer_request.get_output_tensor(); + auto output_shape = output_tensor.get_shape(); + cv::Mat outputs(1, 9, CV_32F, output_tensor.data()); + + // Softmax + float max = *std::max_element(outputs.begin(), outputs.end()); + cv::exp(outputs - max, outputs); + float sum = cv::sum(outputs)[0]; + outputs /= sum; + + double confidence; + cv::Point label_point; + cv::minMaxLoc(outputs.reshape(1, 1), nullptr, &confidence, nullptr, &label_point); + int label_id = label_point.x; + enumeration::ArmorIdFlag armor_id; + + if (label_id == 3) { + armor_id = enumeration::ArmorIdFlag::InfantryIII; + } else if (label_id == 4) { + armor_id = enumeration::ArmorIdFlag::InfantryIV; + } else if (label_id == 5) { + armor_id = enumeration::ArmorIdFlag::InfantryV; + } else if (label_id == 6) { + armor_id = enumeration::ArmorIdFlag::Outpost; + } else if (label_id == 1) { + armor_id = enumeration::ArmorIdFlag::Hero; + } else if (label_id == 2) { + armor_id = enumeration::ArmorIdFlag::Engineer; + } else if (label_id == 0) { + armor_id = enumeration::ArmorIdFlag::Sentry; + } else if (label_id == 7) { + armor_id = enumeration::ArmorIdFlag::Base; + }; + + return armor_id; + }; + +private: + cv::dnn::Net net_; + ov::Core core_; + ov::CompiledModel compiled_model_; + + const int model_image_height_ = 640; + const int model_image_width_ = 640; +}; +} \ No newline at end of file diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp new file mode 100644 index 0000000..6889475 --- /dev/null +++ b/src/tongji/identifier/classifier.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include +namespace world_exe::tongji::identifier { + +class Classifier final { +public: + Classifier(); + ~Classifier(); + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} \ No newline at end of file diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 49496e5..658841e 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -12,7 +12,6 @@ #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" #include "interfaces/armor_in_camera.hpp" -#include "interfaces/armor_in_gimbal_control.hpp" #include "interfaces/time_stamped.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" From 477a35fd443bfc7894cc51baddc1ee92e79b1cab Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 2 Sep 2025 19:42:37 +0800 Subject: [PATCH 03/93] fix(classifier): resolve issues in classifier class implementation --- src/tongji/identifier/classifier.cpp | 12 +++++++++++- src/tongji/identifier/classifier.hpp | 9 ++++++++- src/tongji/identifier/identifier.cpp | 0 src/tongji/identifier/identifier.hpp | 0 4 files changed, 19 insertions(+), 2 deletions(-) create mode 100644 src/tongji/identifier/identifier.cpp create mode 100644 src/tongji/identifier/identifier.hpp diff --git a/src/tongji/identifier/classifier.cpp b/src/tongji/identifier/classifier.cpp index f538a0b..c7e8139 100644 --- a/src/tongji/identifier/classifier.cpp +++ b/src/tongji/identifier/classifier.cpp @@ -81,7 +81,6 @@ class Classifier::Impl final { } enumeration::ArmorIdFlag ovclassify(const cv::Mat& armor_pattern) { - if (armor_pattern.empty()) { return enumeration::ArmorIdFlag::Unknow; } @@ -157,4 +156,15 @@ class Classifier::Impl final { const int model_image_height_ = 640; const int model_image_width_ = 640; }; + +Classifier::Classifier(const std::string& model_path, int model_image_width, int model_image_height) + : pimpl_(std::make_unique(model_path, model_image_width, model_image_height)) { } +Classifier::~Classifier() = default; + +enumeration::ArmorIdFlag Classifier::classify(const cv::Mat& armor_pattern) { + return pimpl_->classify(armor_pattern); +} +enumeration::ArmorIdFlag Classifier::ovclassify(const cv::Mat& armor_pattern){ + return pimpl_->ovclassify(armor_pattern); +} } \ No newline at end of file diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp index 6889475..86080bc 100644 --- a/src/tongji/identifier/classifier.hpp +++ b/src/tongji/identifier/classifier.hpp @@ -1,13 +1,20 @@ #pragma once #include +#include + +#include "enum/armor_id.hpp" namespace world_exe::tongji::identifier { class Classifier final { public: - Classifier(); + Classifier(const std::string& model_path, int model_image_width, int model_image_height); ~Classifier(); + enumeration::ArmorIdFlag classify(const cv::Mat& armor_pattern); + + enumeration::ArmorIdFlag ovclassify(const cv::Mat& armor_pattern); + private: class Impl; std::unique_ptr pimpl_; diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/tongji/identifier/identifier.hpp b/src/tongji/identifier/identifier.hpp new file mode 100644 index 0000000..e69de29 From cd95986a98ea9d1dfb9e1dd1d24bd105760eb3b4 Mon Sep 17 00:00:00 2001 From: fan Date: Tue, 2 Sep 2025 22:55:08 +0800 Subject: [PATCH 04/93] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/util/scanline.hpp | 4 - src/v1/identifier/identifier.cpp | 152 +++++++++++++++++---- src/v1/predictor/car/car_predictor_ekf.hpp | 137 ++++++++++++------- src/v1/predictor/predictor_manager.cpp | 6 +- 4 files changed, 220 insertions(+), 79 deletions(-) diff --git a/src/util/scanline.hpp b/src/util/scanline.hpp index c09fefa..32185c3 100644 --- a/src/util/scanline.hpp +++ b/src/util/scanline.hpp @@ -11,7 +11,6 @@ class ScanLine { if (contour.empty()) return {}; std::vector points; - points.reserve(contour.size()); for (const auto& point : contour) points.emplace_back(point); @@ -163,11 +162,9 @@ class ScanLine { } static inline std::vector collect_poly_edges(const std::vector& points) { - int delta = ((1 << 0) >> 1); const auto points_size = points.size(); cv::Point2l pt0 = points[points_size - 1], pt1; pt0.x = (pt0.x) << XY_SHIFT; - pt0.y = pt0.y + delta; std::vector edges; edges.reserve(edges.size() + points_size); @@ -175,7 +172,6 @@ class ScanLine { cv::Point2l t0, t1; pt1 = points[i]; pt1.x = (pt1.x) << XY_SHIFT; - pt1.y = pt1.y + delta; t0.x = pt0.x; t1.x = pt1.x; diff --git a/src/v1/identifier/identifier.cpp b/src/v1/identifier/identifier.cpp index 9fb1bb6..be64c60 100644 --- a/src/v1/identifier/identifier.cpp +++ b/src/v1/identifier/identifier.cpp @@ -1,3 +1,12 @@ +/** + * @file identifier.cpp + * @brief 装甲板识别器实现文件 + * + * 该识别器使用深大开源模型进行识别获取到装甲板位置，但是模型是典型的四点模型， + * 我们只能把模型输出结果当做roi再去做精细的识别处理，得到装甲板的灯条的四个角点， + * 然后通过传统视觉方法进行灯条匹配，最后基于颜色信息进行敌我识别。 + */ + #include "identifier.hpp" #include "identifier_armor.hpp" #include "util/scanline.hpp" @@ -10,14 +19,23 @@ #include namespace world_exe::v1::identifier { + class Identifier::Impl { public: + /** + * @brief 构造函数，初始化 OpenVINO 模型和相关参数 + * @param model_path ONNX 模型文件路径 + * @param device 推理设备（如 "CPU", "GPU" 等） + * @param image_width 输入图像宽度（默认1440） + * @param image_height 输入图像高度（默认1080） + */ explicit Impl(const std::string& model_path, const std::string& device, const int& image_width = 1440, const int& image_height = 1080) : image_width_(image_width) , image_height_(image_height) , width_ratio_(static_cast(image_width_) / model_image_width_) , height_ratio_(static_cast(image_height_) / model_image_height_) { + // 此处主要是对模型推理的初始化 ov::Core core_; const auto adevice = core_.get_available_devices(); auto model_ = core_.read_model(model_path); @@ -25,6 +43,7 @@ class Identifier::Impl { std::unique_ptr pre_post_processor_ = std::make_unique(model_); ov::Shape input_shape_ { 1, model_image_height_, model_image_width_, 3 }; + pre_post_processor_->input() .tensor() .set_element_type(ov::element::u8) @@ -36,7 +55,6 @@ class Identifier::Impl { .convert_element_type(ov::element::f32) .convert_color(ov::preprocess::ColorFormat::RGB) .scale({ 255., 255., 255. }); - pre_post_processor_->input().model().set_layout("NCHW"); pre_post_processor_->output().tensor().set_element_type(ov::element::f32); model_ = pre_post_processor_->build(); @@ -44,26 +62,54 @@ class Identifier::Impl { compiled_model_ = core_.compile_model(model_, device); } + /** + * @brief 设置目标颜色（敌我识别） + * @param target_color false 为蓝色，true 为红色 + */ inline void SetTargetColor(bool target_color) { target_color_ = target_color; } + /** + * @brief 主识别接口，对输入图像进行装甲板识别 + * @param input_image 输入的 BGR 格式图像(默认就是) + */ std::tuple, enumeration::CarIDFlag> Identify( const cv::Mat& input_image) { + // 首先使用深度学习模型进行装甲板检测得到roi区域 const auto armor_infos = model_infer(input_image); + // 然后进行灯条匹配验证 return matchPlate(input_image, armor_infos); } + /** + * @brief 设置装甲板匹配时的放大倍数，这个放大倍数是对于模型输出的roi， + * 我们要以中心点为原点放大一定的比例来匹配灯条，这样主要是为了避免灯条再模型直接输出的roi内不完整 + * @param ratio 放大倍数，用于扩大装甲板区域进行灯条检测 + */ inline void set_match_magnification_ratio(const double& ratio) { match_magnification_ratio_ = ratio; } private: struct ArmorInfo { - cv::Rect rect_; - enumeration::ArmorIdFlag id_; + cv::Rect rect_; ///< 装甲板在图像中的边界框 + enumeration::ArmorIdFlag id_; ///< 装甲板类型（英雄、步兵等） }; + /** + * @brief 使用深度学习模型进行装甲板检测和分类 + * @param img 输入图像 + * @return 检测到的装甲板信息列表 + * + * 处理流程： + * 1. 图像预处理（缩放到模型输入尺寸） + * 2. 模型推理 + * 3. 后处理（解析输出、过滤、NMS） + * 4. 返回符合要求的装甲板信息 + */ std::vector model_infer(const cv::Mat& img) { + // 注意模型的输入为640x640，所以此处要resize，然后后面处理时要再变回去 cv::resize(img, resized_img_, cv::Size(model_image_width_, model_image_height_)); + const auto input_tensor = ov::Tensor { compiled_model_.input().get_element_type(), compiled_model_.input().get_shape(), resized_img_.data }; ov::InferRequest infer_request = compiled_model_.create_infer_request(); @@ -81,22 +127,30 @@ class Identifier::Impl { std::vector class_scores; std::vector confidences; std::vector tmp_objects_; + + // 以下就是模型推理获取结果的具体流程 for (int i = 0; i < output_buffer.rows; i++) { + // 获取置信度并应用 sigmoid 激活 float confidence = output_buffer.at(i, 8); confidence = static_cast(sigmoid(confidence)); - if (confidence < conf_threshold_) continue; + if (confidence < conf_threshold_) continue; // 过滤低置信度检测 + // 解析颜色分类结果（蓝、红、紫、无色） const auto color_scores = output_buffer.row(i).colRange(9, 13); // color + // 解析车辆类型分类结果（哨兵、英雄、工程等） const auto classes_scores = output_buffer.row(i).colRange(13, 22); // num cv::Point class_id, color_id; double score_color, score_num; cv::minMaxLoc(classes_scores, nullptr, &score_num, nullptr, &class_id); cv::minMaxLoc(color_scores, nullptr, &score_color, nullptr, &color_id); + // 颜色过滤：只保留目标颜色的装甲板 + // color_id.x: 0=蓝色, 1=红色, 2+=其他颜色 if (color_id.x >= 2 || (color_id.x == 1 && !target_color_) || (color_id.x == 0 && target_color_)) continue; + // 根据类别ID映射到装甲板类型枚举，这里尽量去吧高可能性的放前面，然后这个模型其实还支持识别其他的装甲板，但基本并不需要，如果有需求，可以去深大开源readme里找 ArmorInfo obj; if (class_id.x == 3) { obj.id_ = enumeration::ArmorIdFlag::InfantryIII; @@ -114,15 +168,18 @@ class Identifier::Impl { obj.id_ = enumeration::ArmorIdFlag::Sentry; } else if (class_id.x == 7) { obj.id_ = enumeration::ArmorIdFlag::Base; - } else continue; + } else continue; // 未知类型，跳过 tmp_objects_.emplace_back(obj); + // 解析装甲板的四个角点坐标 std::array points { cv::Point2f { output_buffer.at(i, 0), output_buffer.at(i, 1) }, cv::Point2f { output_buffer.at(i, 6), output_buffer.at(i, 7) }, cv::Point2f { output_buffer.at(i, 4), output_buffer.at(i, 5) }, cv::Point2f { output_buffer.at(i, 2), output_buffer.at(i, 3) } }; + + // 计算包围盒：找到四个点的最小和最大 x,y 坐标 float min_x = points[0].x; float max_x = points[0].x; float min_y = points[0].y; @@ -134,13 +191,17 @@ class Identifier::Impl { if (points[i].y > max_y) max_y = points[i].y; } + // 将坐标从模型尺寸映射回原图像尺寸 boxes.emplace_back(min_x * width_ratio_, min_y * height_ratio_, (max_x - min_x) * width_ratio_, (max_y - min_y) * height_ratio_); confidences.emplace_back(score_num); } + // 应用非最大抑制（NMS）去除重复检测 std::vector indices; cv::dnn::NMSBoxes(boxes, confidences, conf_threshold_, nms_threshold_, indices); + + // 构建最终的检测结果 std::vector objects_; for (const std::size_t valid_index : indices) if (valid_index <= boxes.size()) { @@ -158,8 +219,8 @@ class Identifier::Impl { } struct LightBar { - cv::Point2f top_, bottom_; - float angle_; + cv::Point2f top_, bottom_; ///< 灯条的上下端点 + float angle_; ///< 灯条的角度（这个实际并没有用，可以扔掉） LightBar(const cv::Point2f& top, const cv::Point2f& bottom, float angle) : top_(top) @@ -169,12 +230,19 @@ class Identifier::Impl { std::tuple, enumeration::CarIDFlag> matchPlate( const cv::Mat& img, const std::vector& armor_infos) { + if (armor_infos.empty()) return {}; + + // 图像预处理：转换为灰度图并二值化，为后续灯条检测做准备,由于我们前面通过模型拿到了灯条的roi区域，在区域内基本不会误识别，所以后面我们通过传统方式来匹配时各种阈值都可以给松一些，注意，这里的阈值可能要根据实际情况做出调整 cv::cvtColor(img, gray_img_, cv::COLOR_BGR2GRAY); cv::threshold(gray_img_, gray_img_, 30, 255, cv::THRESH_BINARY); + // 初始化结果容器 uint32_t all_car_id = static_cast(enumeration::ArmorIdFlag::None); std::vector armor_plates; + + // 对每个模型检测的装甲板进行验证 for (const auto& armor : armor_infos) { + // 这里是为了把模型直接输出的roi加个放大的因数放大，然后再进行匹配 const auto offset = cv::Point { std::clamp(static_cast(armor.rect_.x - armor.rect_.width / 2. * (match_magnification_ratio_ - 1.)), @@ -183,18 +251,20 @@ class Identifier::Impl { - armor.rect_.height / 2. * (match_magnification_ratio_ - 1.)), 0, image_width_) }; - cv::Size rect_size { std::clamp(static_cast( armor.rect_.width * match_magnification_ratio_), 0, image_width_ - offset.x), std::clamp(static_cast(armor.rect_.height * match_magnification_ratio_), 0, image_height_ - offset.y) }; - const auto armor_roi = gray_img_(cv::Rect { offset, rect_size }); + // 在 ROI 中寻找轮廓（潜在的灯条） std::vector> contours; cv::findContours(armor_roi, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + + // 至少需要两个灯条才能构成装甲板 if (contours.size() >= 2) { + // 按面积降序排序轮廓，优先处理较大的轮廓 std::sort(contours.begin(), contours.end(), [](const std::vector& a, const std::vector& b) { return cv::contourArea(a, false) > cv::contourArea(b, false); @@ -202,55 +272,72 @@ class Identifier::Impl { double first_area { 0. }; std::vector lightbars_; + + // 遍历轮廓，寻找合适的灯条 for (const auto& contour : contours) { auto b_rect = cv::boundingRect(contour); + // 计算轮廓在原图中的位置 cv::Point roi_point { std::clamp(offset.x + b_rect.x, 0, image_width_), std::clamp(offset.y + b_rect.y, 0, image_height_) }; cv::Size roi_size { std::clamp(b_rect.width, 0, image_width_ - roi_point.x), std::clamp(b_rect.height, 0, image_height_ - roi_point.y) }; const auto light_bar_roi = img(cv::Rect { roi_point, roi_size }); + // 颜色过滤：基于 BGR 通道的差值判断灯条颜色 const auto channels = cv::mean(light_bar_roi); - const auto b_r_difference = channels[0] - channels[2]; + const auto b_r_difference = channels[0] - channels[2]; // B通道 - R通道 + // 如果目标是红色且B-R>0（偏蓝），或目标是蓝色且B-R<0（偏红），则跳过 if ((target_color_ && b_r_difference > 0) || (!target_color_ && b_r_difference < 0)) continue; + // 使用扫描线算法获取轮廓内的所有点，然后这个扫描线时把opencv中的源码掏出来精简得到的，然后里面有一些奇怪的操作，我个人也没看太明白 const auto points = util::ScanLine::get_points(armor_roi, contour); if (points.empty()) continue; + + // 使用 PCA 分析获取灯条的主轴端点 const auto [high_point, low_point] = perform_pca(points); + // 处理第一个有效的灯条 if (lightbars_.empty()) { first_area = cv::contourArea(contour); lightbars_.emplace_back(high_point + offset, low_point + offset, 0.); } else { + // 对于后续灯条，检查面积比例是否合理（避免尺寸差异过大的灯条配对，这里的目的是为了避免识别到特别侧着的装甲板，因为这个神经网络啊，实在太敏感了，即便是很偏的情况依然能够识别到，但是太偏的话，我们pca拿到的角点是不准的，所以我们不要） const auto area_ratio = first_area / cv::contourArea(contour); if (area_ratio < 10. && area_ratio > 1. / 10.) lightbars_.emplace_back(high_point + offset, low_point + offset, 0.); } + // 找到两个灯条就停止搜索 if (lightbars_.size() == 2) break; } const auto light_bar_size_ = lightbars_.size(); + // 如果找到了两个灯条，构建装甲板 if (light_bar_size_ == 2) { const auto& first = lightbars_[0]; const auto& second = lightbars_[1]; + + // 根据 x 坐标排序灯条，确保左灯条在前 if ((first.top_.x + first.bottom_.x) / 2. < (second.bottom_.x + second.top_.x) / 2.) { + // 构建装甲板的四个角点：左上、右上、右下、左下 armor_plates.emplace_back(data::ArmorImageSpacing { armor.id_, { first.top_, second.top_, second.bottom_, first.bottom_ } }); } else { armor_plates.emplace_back(data::ArmorImageSpacing { armor.id_, { second.top_, first.top_, first.bottom_, second.bottom_ } }); } + // 将装甲板类型添加到检测到的车辆类型标志位中 all_car_id |= static_cast(armor.id_); } } } + // 返回验证后的装甲板信息和检测到的车辆类型 return { std::make_shared(armor_plates), static_cast(all_car_id) }; } @@ -258,17 +345,23 @@ class Identifier::Impl { static inline std::tuple perform_pca( const std::vector& points) { const int points_num { static_cast(points.size()) }; + + // 将点坐标转换为 PCA 算法需要的矩阵格式 cv::Mat data(points_num, 2, CV_32F); for (int j = 0; j < points_num; ++j) { data.at(j, 0) = static_cast(points[j].x); data.at(j, 1) = static_cast(points[j].y); } + // 执行 PCA 分析 cv::PCA pca(data, cv::Mat(), cv::PCA::DATA_AS_ROW); + + // 获取第一主成分（主轴方向） cv::Vec2f principal_axis( pca.eigenvectors.at(0, 0), pca.eigenvectors.at(0, 1)); cv::Point2f center(pca.mean.at(0, 0), pca.mean.at(0, 1)); + // 将所有点投影到主轴上，找到最小和最大投影值 float min_proj = std::numeric_limits::max(); float max_proj = std::numeric_limits::lowest(); @@ -279,31 +372,37 @@ class Identifier::Impl { if (proj > max_proj) max_proj = proj; } + // 重构主轴的两个端点 const cv::Point reconstructed_min_point_local = center + cv::Point2f { principal_axis * min_proj }; const cv::Point reconstructed_max_point_local = center + cv::Point2f { principal_axis * max_proj }; + // 确保返回的第一个点是上端点（y坐标较小） if (reconstructed_min_point_local.y < reconstructed_max_point_local.y) return { reconstructed_min_point_local, reconstructed_max_point_local }; else return { reconstructed_max_point_local, reconstructed_min_point_local }; } - static constexpr int model_image_height_ = 640; - static constexpr int model_image_width_ = 640; - int image_height_ = 1080; - int image_width_ = 1440; - double width_ratio_ = static_cast(image_width_) / model_image_width_; - double height_ratio_ = static_cast(image_height_) / model_image_height_; - static constexpr double conf_threshold_ = 0.65; - static constexpr double nms_threshold_ = 0.45; - - double match_magnification_ratio_ = 1.5; - - cv::Mat resized_img_; - cv::Mat gray_img_; - bool target_color_ { false }; - ov::CompiledModel compiled_model_; + static constexpr int model_image_height_ = 640; ///< 模型输入图像高度 + static constexpr int model_image_width_ = 640; ///< 模型输入图像宽度 + + int image_height_ = 1080; ///< 实际输入图像高度 + int image_width_ = 1440; ///< 实际输入图像宽度 + double width_ratio_ = static_cast(image_width_) / model_image_width_; ///< 宽度缩放比例 + double height_ratio_ = + static_cast(image_height_) / model_image_height_; ///< 高度缩放比例 + + static constexpr double conf_threshold_ = 0.65; ///< 置信度阈值 + static constexpr double nms_threshold_ = 0.45; ///< NMS 阈值 + + double match_magnification_ratio_ = 1.5; ///< 装甲板匹配时的放大倍数 + + cv::Mat resized_img_; ///< 缩放后的图像缓存 + cv::Mat gray_img_; ///< 灰度图像缓存 + + bool target_color_ { false }; ///< 目标颜色：false=蓝色，true=红色 + ov::CompiledModel compiled_model_; ///< 编译后的 OpenVINO 模型 }; Identifier::Identifier(const std::string& model_path, const std::string& device, @@ -322,4 +421,5 @@ void Identifier::set_match_magnification_ratio(const double& ratio) { } Identifier::~Identifier() = default; -} \ No newline at end of file + +} // namespace world_exe::v1::identifier \ No newline at end of file diff --git a/src/v1/predictor/car/car_predictor_ekf.hpp b/src/v1/predictor/car/car_predictor_ekf.hpp index 9fbfd74..029551c 100644 --- a/src/v1/predictor/car/car_predictor_ekf.hpp +++ b/src/v1/predictor/car/car_predictor_ekf.hpp @@ -8,37 +8,63 @@ namespace world_exe::v1::predictor { +/** + * @class CarPredictEkf + * @brief 车辆预测EKF核心类 + * + * 状态向量维度: 11维 [x, vx, y, vy, z, z1, z2, r1, r2, θ, ω] + * 观测向量维度: 4维 [θ, yaw, pitch, distance] + * + * 这个是整个预测的核心，无论是逻辑的维护还是预测的模型都放在这里了，然后其他的主要是为了接口实现 + */ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { public: + /** + * @brief 构造函数，初始化状态向量和协方差矩阵 + * 状态: [x=2.0, vx=0, y=0, vy=0, z=-0.3, z1=-0.3, z2=-0.3, r1=0.2, r2=0.2, θ=π, ω=0] + */ CarPredictEkf() { X_k << 2.0, 0.0, 0.0, 0.0, -0.3, -0.3, -0.3, 0.2, 0.2, std::numbers::pi, 0.0; P_k.diagonal() << 1., 1., 1., 1., 1., 0., 0., 1., 1., 1., 1.; } + /** + * @brief 设置第二装甲板标志 + * 这里这个函数需要在同时识别到两个装甲板时调用，在update传入第一块装甲板的信息后，update传入第二块装甲板的信息前，这样做的目的是因为这个模型内维护有最近装甲板的角度，但又需要在识别到两块装甲板时能充分利用两块装甲板来更新状态，所以说需要一个标志位来动态地改变模型的一些参数，主要是 + + */ inline void set_second_armor() { second_armor_flag = true; } + /** + * @brief 获取预测的装甲板位置 + * @param id 装甲板类型 + * @param sec 预测时间（秒） + * @return 四个装甲板的预测位置和姿态 + */ inline std::vector get_predict_output_armor( const enumeration::ArmorIdFlag& id, const double& sec) const { std::vector target_armors; const auto model_output = X_k; - const auto vx = model_output(1); - const auto vy = model_output(3); + const auto vx = model_output(1); // x方向速度 + const auto vy = model_output(3); // y方向速度 + // 预测车辆中心位置 const double car_x = model_output(0) + vx * sec; const double car_y = model_output(2) + vy * sec; - const double car_z = model_output(4); - const double z1 = model_output(5); - const double z2 = model_output(6); - const double r1 = model_output(7); - const double r2 = model_output(8); - double model_yaw = model_output(9) + model_output(10) * sec; + const double car_z = model_output(4); // 车辆高度 + const double z1 = model_output(5); // 装甲板1高度 + const double z2 = model_output(6); // 装甲板2高度 + const double r1 = model_output(7); // 装甲板1到中心距离 + const double r2 = model_output(8); // 装甲板2到中心距离 + double model_yaw = model_output(9) + model_output(10) * sec; // 预测yaw角 - // TODO: 把 pitch 与 z 关联起来 + // TODO: 把 pitch 与 z 关联起来，这里可以通过z和z1、z2把pitch算出来 const double pitch1 = 15. / 180. * std::numbers::pi; const double pitch2 = 15. / 180. * std::numbers::pi; + // 根据side_flag_生成四个装甲板的位置 if (side_flag_) { const auto armor1 = data::ArmorGimbalControlSpacing { id, { car_x - r1 * std::cos(model_yaw), car_y - r1 * std::sin(model_yaw), z1 }, @@ -93,20 +119,24 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { private: friend class world_exe::util::Ekf<11, 4, CarPredictEkf>; - // 0, 1, 2, 3, 4, 5, 6, 7, 8, 9,10 - // x分别代表[ x, v_x, y, v_y, z, z_1, z_2, r1, r2, θ, ω ] - // z分别代表[ θ, yaw, pitch, distance ] + /** + * @brief 观测预处理函数 + * 状态: x分别代表[ x, v_x, y, v_y, z, z_1, z_2, r1, r2, θ, ω ] + * 观测: z分别代表[ θ, yaw, pitch, distance ] + */ inline ZVec process_z(const ZVec& z_k) { ZVec processed_z { z_k }; if (z_k(0) < 0.) processed_z(0) += std::numbers::pi * 2.; + // 检测装甲板切换，switch_angle_difference_根据实际情况可能需要调整 double offset { processed_z(0) - last_theta_ }; if (std::abs(offset) >= switch_angle_difference_) { - side_flag_ = !side_flag_; + side_flag_ = !side_flag_; // 切换装甲板面 } else offset = 0.; last_theta_ = processed_z(0); + // 根据当前装甲板面更新高度 if (side_flag_) X_k(5) = z_k(3) * std::cos(z_k(1)) * -std::sin(z_k(2)); else X_k(6) = z_k(3) * std::cos(z_k(1)) * -std::sin(z_k(2)); @@ -116,9 +146,11 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { return processed_z; }; + /** @brief 状态向量归一化 */ inline XVec normalize_x(const XVec& x_k) { normalized_x = x_k; auto camera_yaw = x_k(9); + // 角度归一化到 [0, 2π) while (camera_yaw < 0.) camera_yaw += 2 * std::numbers::pi; while (camera_yaw >= 2 * std::numbers::pi) @@ -128,37 +160,41 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { return normalized_x; }; + /** @brief 状态转移函数 f(x,u,w,dt) */ inline XVec f(const XVec& X_k, const UVec&, const WVec&, const double& dt) { - const double x = X_k(0) + X_k(1) * dt; - const double y = X_k(2) + X_k(3) * dt; - const double z = (X_k(5) + X_k(6)) / 2.; - const double theta = X_k(9) + X_k(10) * dt; + const double x = X_k(0) + X_k(1) * dt; // 位置更新 + const double y = X_k(2) + X_k(3) * dt; // 位置更新 + const double z = (X_k(5) + X_k(6)) / 2.; // 平均高度 + const double theta = X_k(9) + X_k(10) * dt; // 角度更新 f_ << x, X_k(1), y, X_k(3), z, X_k(5), X_k(6), X_k(7), X_k(8), theta, X_k(10); return f_; }; + /** @brief 观测函数 h(x,v) */ inline ZVec h(const XVec& x_k_n, const VVec&) { double r, z; + // 根据当前装甲板面选择参数 if (side_flag_) { - r = x_k_n(7); - z = x_k_n(5); + r = x_k_n(7); // r1 + z = x_k_n(5); // z1 } else { - r = x_k_n(8); - z = x_k_n(6); + r = x_k_n(8); // r2 + z = x_k_n(6); // z2 } const double theta = x_k_n(9); - const double armor_x = x_k_n(0) + r * std::cos(theta); - const double armor_y = x_k_n(2) + r * std::sin(theta); - const double yaw = std::atan(armor_y / armor_x); - const double pitch = -std::atan(z / armor_x); - const double distance = std::sqrt(armor_x * armor_x + armor_y * armor_y + z * z); + const double armor_x = x_k_n(0) + r * std::cos(theta); // 装甲板x坐标 + const double armor_y = x_k_n(2) + r * std::sin(theta); // 装甲板y坐标 + const double yaw = std::atan(armor_y / armor_x); // yaw角 + const double pitch = -std::atan(z / armor_x); // pitch角 + const double distance = std::sqrt(armor_x * armor_x + armor_y * armor_y + z * z); // 距离 h_ << x_k_n(9), yaw, pitch, distance; return h_; }; + /** @brief 状态转移雅可比矩阵 A */ inline AMat A(const XVec&, const XVec&, const XVec&, const double& dt) { // clang-format off A_ << 1., dt, 0., 0., 0., 0., 0., 0., 0., 0., 0., @@ -176,8 +212,10 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { return A_; }; + /** @brief 过程噪声雅可比矩阵 W */ inline WMat W(const XVec&, const XVec&, const XVec&) const { return W_; }; + /** @brief 观测雅可比矩阵 H */ inline HMat H(const XVec& x_k_n, const VVec&) { const double theta = x_k_n(9); @@ -260,8 +298,10 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { return H_; }; + /** @brief 观测噪声雅可比矩阵 V */ inline VMat V(const XVec&, const VVec&) const { return V_; }; + /** @brief 过程噪声协方差矩阵 Q */ inline QMat Q(const double& dt) { double x; x = 100; @@ -303,6 +343,7 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { const double yaw_center_difference_ = std::abs(std::sin(z(0))) + 1.; if (second_armor_flag) { second_armor_flag = false; + // 同时识别到两块装甲板时，第二装甲板时忽略θ观测，因为第一个输入装甲板是比较近的且较准 R_.diagonal() << 9999999999999., yaw_center_difference_ * r_theta_yaw_, yaw_center_difference_ * r_theta_pitch_, yaw_center_difference_ * r_theta_distance_; } else { @@ -313,25 +354,27 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { return R_; }; - XVec f_ {}; - ZVec h_ {}; - AMat A_ {}; - WMat W_ { WMat::Identity() }; - HMat H_ {}; - VMat V_ { VMat::Identity() }; - QMat Q_ {}; - RMat R_ {}; - XVec normalized_x {}; - - bool debug_ { true }; - double last_theta_ { std::numbers::pi }; - static constexpr double switch_angle_difference_ = std::numbers::pi * 70. / 180.; - static constexpr double r_theta_theta_ = 0.1; - static constexpr double r_theta_yaw_ = 1e-3; - static constexpr double r_theta_pitch_ = 1e-3; - static constexpr double r_theta_distance_ = 1.; - - bool side_flag_ { true }; - bool second_armor_flag { false }; + XVec f_ {}; ///< 状态转移函数输出缓存 + ZVec h_ {}; ///< 观测函数输出缓存 + AMat A_ {}; ///< 状态转移雅可比矩阵 + WMat W_ { WMat::Identity() }; ///< 过程噪声雅可比矩阵 + HMat H_ {}; ///< 观测雅可比矩阵 + VMat V_ { VMat::Identity() }; ///< 观测噪声雅可比矩阵 + QMat Q_ {}; ///< 过程噪声协方差矩阵 + RMat R_ {}; ///< 观测噪声协方差矩阵 + XVec normalized_x {}; ///< 归一化状态向量缓存 + + bool debug_ { true }; ///< 调试标志 + double last_theta_ { std::numbers::pi }; ///< 上次观测角度 + static constexpr double switch_angle_difference_ = + std::numbers::pi * 70. / 180.; ///< 装甲板切换角度阈值 + static constexpr double r_theta_theta_ = 0.1; ///< θ观测噪声 + static constexpr double r_theta_yaw_ = 1e-3; ///< yaw观测噪声 + static constexpr double r_theta_pitch_ = 1e-3; ///< pitch观测噪声 + static constexpr double r_theta_distance_ = 1.; ///< 距离观测噪声 + + bool side_flag_ { true }; ///< 装甲板面标志位 + bool second_armor_flag { false }; ///< 第二装甲板标志位 }; -} \ No newline at end of file + +} // namespace world_exe::v1::predictor \ No newline at end of file diff --git a/src/v1/predictor/predictor_manager.cpp b/src/v1/predictor/predictor_manager.cpp index 37f0c61..a685f94 100644 --- a/src/v1/predictor/predictor_manager.cpp +++ b/src/v1/predictor/predictor_manager.cpp @@ -11,9 +11,9 @@ namespace world_exe::v1::predictor { class PredictorManager::Impl { public: - inline void Update(std::shared_ptr data) { + inline void Update(const std::shared_ptr& data) { const auto time_stamp = data->GetTimeStamped().GetTimeStamp(); - const auto dt = (time_stamp - last_update_time_stamp_.GetTimeStamp()) / 1.e9; + const auto dt = (time_stamp - last_update_time_stamp_.GetTimeStamp()); const auto transform = data->GetTransform(); const auto rotation_transform = Eigen::Quaterniond { transform.linear() }; @@ -46,6 +46,7 @@ class PredictorManager::Impl { -std::atan(tmp_armor0.position.z() / tmp_armor0.position.x()), tmp_armor0.position.norm(); predictors_[i].Update(input, {}, dt); + // 同时识别到一辆车的两块装甲板时要调这个函数 predictors_[i].set_second_armor(); input << util::math::get_yaw_from_quaternion(tmp_armor1.orientation), std::atan(tmp_armor1.position.y() / tmp_armor1.position.x()), @@ -58,6 +59,7 @@ class PredictorManager::Impl { -std::atan(tmp_armor1.position.z() / tmp_armor1.position.x()), tmp_armor1.position.norm(); predictors_[i].Update(input, {}, dt); + // 同时识别到一辆车的两块装甲板时要调这个函数 predictors_[i].set_second_armor(); input << util::math::get_yaw_from_quaternion(tmp_armor0.orientation), std::atan(tmp_armor0.position.y() / tmp_armor0.position.x()), From 879682b8abfd9eaa68af01cfc726832450a6a651 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 3 Sep 2025 02:57:23 +0800 Subject: [PATCH 05/93] feat(logging): add logger module --- CMakeLists.txt | 12 ++++++++++++ src/util/logger.cpp | 32 ++++++++++++++++++++++++++++++++ 2 files changed, 44 insertions(+) create mode 100644 src/util/logger.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1716dd6..f3e5b7e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,6 +53,18 @@ include_directories(${CERES_INCLUDE_DIRS}) target_link_libraries(${PROJECT_NAME} ${CERES_LIBRARIES}) ### ceres End === +### libspdlog-dev === +find_package(spdlog REQUIRED) +target_include_directories(${fmt_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${fmt_LIBRARIES}) +### libspdlog-dev End === + +### libfmt-dev === +find_package(fmt REQUIRED) +target_include_directories(${fmt_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${fmt_LIBRARIES}) +### libfmt-dev End === + # 安装库文件 target_include_directories(${PROJECT_NAME} PUBLIC $ diff --git a/src/util/logger.cpp b/src/util/logger.cpp new file mode 100644 index 0000000..8b12095 --- /dev/null +++ b/src/util/logger.cpp @@ -0,0 +1,32 @@ +#pragma once + +#include +#include +#include +#include + +#include + +namespace world_exe::util::logger { +std::shared_ptr logger_ = nullptr; + +void set_logger() { + auto file_name = fmt::format("logs/{:%Y-%m-%d_%H-%M-%S}.log", std::chrono::system_clock::now()); + auto file_sink = std::make_shared(file_name, true); + file_sink->set_level(spdlog::level::debug); + + auto console_sink = std::make_shared(); + console_sink->set_level(spdlog::level::debug); + + logger_ = + std::make_shared("", spdlog::sinks_init_list { file_sink, console_sink }); + logger_->set_level(spdlog::level::debug); + logger_->flush_on(spdlog::level::info); +} + +std::shared_ptr logger() { + if (!logger_) set_logger(); + return logger_; +} + +} // namespace logger \ No newline at end of file From 0e7e50559b21e2bf6eac431d0ebacb8ea956ff85 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Sep 2025 00:30:04 +0800 Subject: [PATCH 06/93] fix(build): correct CMakeLists.txt include directory configuration --- CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f3e5b7e..1780a17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,13 +55,11 @@ target_link_libraries(${PROJECT_NAME} ${CERES_LIBRARIES}) ### libspdlog-dev === find_package(spdlog REQUIRED) -target_include_directories(${fmt_INCLUDE_DIRS}) target_link_libraries(${PROJECT_NAME} ${fmt_LIBRARIES}) ### libspdlog-dev End === ### libfmt-dev === find_package(fmt REQUIRED) -target_include_directories(${fmt_INCLUDE_DIRS}) target_link_libraries(${PROJECT_NAME} ${fmt_LIBRARIES}) ### libfmt-dev End === From 78f16f9b5104444207232585d065437be43a21ee Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Sep 2025 00:32:50 +0800 Subject: [PATCH 07/93] feat(logging): add logger module --- src/util/logger.cpp | 6 +++--- src/util/logger.hpp | 8 ++++++++ 2 files changed, 11 insertions(+), 3 deletions(-) create mode 100644 src/util/logger.hpp diff --git a/src/util/logger.cpp b/src/util/logger.cpp index 8b12095..43e45e2 100644 --- a/src/util/logger.cpp +++ b/src/util/logger.cpp @@ -1,12 +1,12 @@ -#pragma once +#include "logger.hpp" + +#include #include #include #include #include -#include - namespace world_exe::util::logger { std::shared_ptr logger_ = nullptr; diff --git a/src/util/logger.hpp b/src/util/logger.hpp new file mode 100644 index 0000000..801db97 --- /dev/null +++ b/src/util/logger.hpp @@ -0,0 +1,8 @@ +#pragma once + +#include + +namespace world_exe::util::logger { +std::shared_ptr logger(); + +} // namespace logger From 7e477e68635645bcf16a41b20e90721382f30fd2 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Sep 2025 00:35:49 +0800 Subject: [PATCH 08/93] feat(debug): add stringifier utility for ID string conversion --- src/util/stringifier.hpp | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 src/util/stringifier.hpp diff --git a/src/util/stringifier.hpp b/src/util/stringifier.hpp new file mode 100644 index 0000000..0953582 --- /dev/null +++ b/src/util/stringifier.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include + +#include "enum/armor_id.hpp" + +namespace world_exe::util::stringifier { +static inline std::string ToString(const world_exe::enumeration::ArmorIdFlag& id) { + + switch (id) { + case enumeration::ArmorIdFlag::Hero: + return "Hero"; + case enumeration::ArmorIdFlag::Engineer: + return "Engineer"; + case enumeration::ArmorIdFlag::InfantryIII: + return "InfantryIII"; + case enumeration::ArmorIdFlag::InfantryIV: + return "InfantryIV"; + case enumeration::ArmorIdFlag::InfantryV: + return "InfantryV"; + case enumeration::ArmorIdFlag::Sentry: + return "Sentry"; + case enumeration::ArmorIdFlag::Base: + return "Base"; + case enumeration::ArmorIdFlag::Outpost: + return "Outpost"; + case enumeration::ArmorIdFlag::Unknow: + return "Unknow"; + case enumeration::ArmorIdFlag::None: + return "None"; + default: + return "UnknownID"; + } +} +} \ No newline at end of file From 1586caf13035921a649b4ac1c1bfa9f41028dba9 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Sep 2025 04:24:10 +0800 Subject: [PATCH 09/93] feat(identifier): add identifier module for armor identification --- src/tongji/identifier/classifier.cpp | 170 ---------- src/tongji/identifier/classifier.hpp | 148 ++++++++- src/tongji/identifier/identifier.cpp | 345 +++++++++++++++++++++ src/tongji/identifier/identifier.hpp | 55 ++++ src/tongji/identifier/identifier_armor.hpp | 36 +++ 5 files changed, 575 insertions(+), 179 deletions(-) delete mode 100644 src/tongji/identifier/classifier.cpp create mode 100644 src/tongji/identifier/identifier_armor.hpp diff --git a/src/tongji/identifier/classifier.cpp b/src/tongji/identifier/classifier.cpp deleted file mode 100644 index c7e8139..0000000 --- a/src/tongji/identifier/classifier.cpp +++ /dev/null @@ -1,170 +0,0 @@ -#include "classifier.hpp" -#include "enum/armor_id.hpp" - -#include -#include -#include -#include -#include - -namespace world_exe::tongji::identifier { -class Classifier::Impl final { -public: - explicit Impl(const std::string& model_path, int model_image_width, int model_image_height) - : model_image_width_(model_image_width) - , model_image_height_(model_image_height) { - - net_ = cv::dnn::readNetFromONNX(model_path); - auto ovmodel = core_.read_model(model_path); - compiled_model_ = core_.compile_model( - ovmodel, "AUTO", ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY)); - } - - enumeration::ArmorIdFlag classify(const cv::Mat& armor_pattern) { - if (armor_pattern.empty()) { - return enumeration::ArmorIdFlag::Unknow; - } - - cv::Mat gray; - cv::cvtColor(armor_pattern, gray, cv::COLOR_BGR2GRAY); - - auto input = cv::Mat(model_image_height_, model_image_width_, CV_8UC1, cv::Scalar(0)); - auto x_scale = static_cast(model_image_width_) / gray.cols; - auto y_scale = static_cast(model_image_height_) / gray.rows; - auto scale = std::min(x_scale, y_scale); - auto h = static_cast(gray.rows * scale); - auto w = static_cast(gray.cols * scale); - - if (h == 0 || w == 0) { - return enumeration::ArmorIdFlag::Unknow; - } - auto roi = cv::Rect(0, 0, w, h); - cv::resize(gray, input(roi), { w, h }); - - auto blob = cv::dnn::blobFromImage(input, 1.0 / 255.0, cv::Size(), cv::Scalar()); - - net_.setInput(blob); - cv::Mat outputs = net_.forward(); - - // softmax - float max = *std::max_element(outputs.begin(), outputs.end()); - cv::exp(outputs - max, outputs); - float sum = cv::sum(outputs)[0]; - outputs /= sum; - - double confidence; - cv::Point label_point; - cv::minMaxLoc(outputs.reshape(1, 1), nullptr, &confidence, nullptr, &label_point); - int label_id = label_point.x; - - enumeration::ArmorIdFlag armor_id; - - if (label_id == 3) { - armor_id = enumeration::ArmorIdFlag::InfantryIII; - } else if (label_id == 4) { - armor_id = enumeration::ArmorIdFlag::InfantryIV; - } else if (label_id == 5) { - armor_id = enumeration::ArmorIdFlag::InfantryV; - } else if (label_id == 6) { - armor_id = enumeration::ArmorIdFlag::Outpost; - } else if (label_id == 1) { - armor_id = enumeration::ArmorIdFlag::Hero; - } else if (label_id == 2) { - armor_id = enumeration::ArmorIdFlag::Engineer; - } else if (label_id == 0) { - armor_id = enumeration::ArmorIdFlag::Sentry; - } else if (label_id == 7) { - armor_id = enumeration::ArmorIdFlag::Base; - }; - - return armor_id; - } - - enumeration::ArmorIdFlag ovclassify(const cv::Mat& armor_pattern) { - if (armor_pattern.empty()) { - return enumeration::ArmorIdFlag::Unknow; - } - - cv::Mat gray; - cv::cvtColor(armor_pattern, gray, cv::COLOR_BGR2GRAY); - - // Resize image - auto input = cv::Mat(model_image_height_, model_image_width_, CV_8UC1, cv::Scalar(0)); - auto x_scale = static_cast(model_image_width_) / gray.cols; - auto y_scale = static_cast(model_image_height_) / gray.rows; - auto scale = std::min(x_scale, y_scale); - auto h = static_cast(gray.rows * scale); - auto w = static_cast(gray.cols * scale); - - if (h == 0 || w == 0) { - return enumeration::ArmorIdFlag::Unknow; - } - - auto roi = cv::Rect(0, 0, w, h); - cv::resize(gray, input(roi), { w, h }); - // Normalize the input image to [0, 1] range - input.convertTo(input, CV_32F, 1.0 / 255.0); - - ov::Tensor input_tensor(ov::element::f32, { 1, 1, 32, 32 }, input.data); - - ov::InferRequest infer_request = compiled_model_.create_infer_request(); - infer_request.set_input_tensor(input_tensor); - infer_request.infer(); - - auto output_tensor = infer_request.get_output_tensor(); - auto output_shape = output_tensor.get_shape(); - cv::Mat outputs(1, 9, CV_32F, output_tensor.data()); - - // Softmax - float max = *std::max_element(outputs.begin(), outputs.end()); - cv::exp(outputs - max, outputs); - float sum = cv::sum(outputs)[0]; - outputs /= sum; - - double confidence; - cv::Point label_point; - cv::minMaxLoc(outputs.reshape(1, 1), nullptr, &confidence, nullptr, &label_point); - int label_id = label_point.x; - enumeration::ArmorIdFlag armor_id; - - if (label_id == 3) { - armor_id = enumeration::ArmorIdFlag::InfantryIII; - } else if (label_id == 4) { - armor_id = enumeration::ArmorIdFlag::InfantryIV; - } else if (label_id == 5) { - armor_id = enumeration::ArmorIdFlag::InfantryV; - } else if (label_id == 6) { - armor_id = enumeration::ArmorIdFlag::Outpost; - } else if (label_id == 1) { - armor_id = enumeration::ArmorIdFlag::Hero; - } else if (label_id == 2) { - armor_id = enumeration::ArmorIdFlag::Engineer; - } else if (label_id == 0) { - armor_id = enumeration::ArmorIdFlag::Sentry; - } else if (label_id == 7) { - armor_id = enumeration::ArmorIdFlag::Base; - }; - - return armor_id; - }; - -private: - cv::dnn::Net net_; - ov::Core core_; - ov::CompiledModel compiled_model_; - - const int model_image_height_ = 640; - const int model_image_width_ = 640; -}; - -Classifier::Classifier(const std::string& model_path, int model_image_width, int model_image_height) - : pimpl_(std::make_unique(model_path, model_image_width, model_image_height)) { } -Classifier::~Classifier() = default; - -enumeration::ArmorIdFlag Classifier::classify(const cv::Mat& armor_pattern) { - return pimpl_->classify(armor_pattern); -} -enumeration::ArmorIdFlag Classifier::ovclassify(const cv::Mat& armor_pattern){ - return pimpl_->ovclassify(armor_pattern); -} -} \ No newline at end of file diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp index 86080bc..47924f3 100644 --- a/src/tongji/identifier/classifier.hpp +++ b/src/tongji/identifier/classifier.hpp @@ -1,23 +1,153 @@ #pragma once -#include #include +#include +#include +#include +#include #include "enum/armor_id.hpp" -namespace world_exe::tongji::identifier { +namespace world_exe::tongji::identifier { class Classifier final { public: - Classifier(const std::string& model_path, int model_image_width, int model_image_height); - ~Classifier(); + explicit Classifier( + const std::string& model_path, int model_image_width, int model_image_height) + : model_image_width_(model_image_width) + , model_image_height_(model_image_height) { + net_ = cv::dnn::readNetFromONNX(model_path); + auto ovmodel = core_.read_model(model_path); + compiled_model_ = core_.compile_model( + ovmodel, "AUTO", ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY)); + } + + void Classify(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag& armor_id, + double& armor_confidence) { + if (armor_pattern.empty()) { + armor_id = enumeration::ArmorIdFlag::Unknow; + } + + cv::Mat gray; + cv::cvtColor(armor_pattern, gray, cv::COLOR_BGR2GRAY); + + auto input = cv::Mat(model_image_height_, model_image_width_, CV_8UC1, cv::Scalar(0)); + auto x_scale = static_cast(model_image_width_) / gray.cols; + auto y_scale = static_cast(model_image_height_) / gray.rows; + auto scale = std::min(x_scale, y_scale); + auto h = static_cast(gray.rows * scale); + auto w = static_cast(gray.cols * scale); + + if (h == 0 || w == 0) { + armor_id = enumeration::ArmorIdFlag::Unknow; + } + auto roi = cv::Rect(0, 0, w, h); + cv::resize(gray, input(roi), { w, h }); + + auto blob = cv::dnn::blobFromImage(input, 1.0 / 255.0, cv::Size(), cv::Scalar()); + + net_.setInput(blob); + cv::Mat outputs = net_.forward(); + + // softmax + float max = *std::max_element(outputs.begin(), outputs.end()); + cv::exp(outputs - max, outputs); + float sum = cv::sum(outputs)[0]; + outputs /= sum; + + cv::Point label_point; + cv::minMaxLoc(outputs.reshape(1, 1), nullptr, &armor_confidence, nullptr, &label_point); + int label_id = label_point.x; + + if (label_id == 3) { + armor_id = enumeration::ArmorIdFlag::InfantryIII; + } else if (label_id == 4) { + armor_id = enumeration::ArmorIdFlag::InfantryIV; + } else if (label_id == 5) { + armor_id = enumeration::ArmorIdFlag::InfantryV; + } else if (label_id == 6) { + armor_id = enumeration::ArmorIdFlag::Outpost; + } else if (label_id == 1) { + armor_id = enumeration::ArmorIdFlag::Hero; + } else if (label_id == 2) { + armor_id = enumeration::ArmorIdFlag::Engineer; + } else if (label_id == 0) { + armor_id = enumeration::ArmorIdFlag::Sentry; + } else if (label_id == 7) { + armor_id = enumeration::ArmorIdFlag::Base; + }; + } - enumeration::ArmorIdFlag classify(const cv::Mat& armor_pattern); + void OvClassify(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag& armor_id, + double& armor_confidence) { + if (armor_pattern.empty()) { + armor_id = enumeration::ArmorIdFlag::Unknow; + } - enumeration::ArmorIdFlag ovclassify(const cv::Mat& armor_pattern); + cv::Mat gray; + cv::cvtColor(armor_pattern, gray, cv::COLOR_BGR2GRAY); + + // Resize image + auto input = cv::Mat(model_image_height_, model_image_width_, CV_8UC1, cv::Scalar(0)); + auto x_scale = static_cast(model_image_width_) / gray.cols; + auto y_scale = static_cast(model_image_height_) / gray.rows; + auto scale = std::min(x_scale, y_scale); + auto h = static_cast(gray.rows * scale); + auto w = static_cast(gray.cols * scale); + + if (h == 0 || w == 0) { + armor_id = enumeration::ArmorIdFlag::Unknow; + } + + auto roi = cv::Rect(0, 0, w, h); + cv::resize(gray, input(roi), { w, h }); + // Normalize the input image to [0, 1] range + input.convertTo(input, CV_32F, 1.0 / 255.0); + + ov::Tensor input_tensor(ov::element::f32, { 1, 1, 32, 32 }, input.data); + + ov::InferRequest infer_request = compiled_model_.create_infer_request(); + infer_request.set_input_tensor(input_tensor); + infer_request.infer(); + + auto output_tensor = infer_request.get_output_tensor(); + auto output_shape = output_tensor.get_shape(); + cv::Mat outputs(1, 9, CV_32F, output_tensor.data()); + + // Softmax + float max = *std::max_element(outputs.begin(), outputs.end()); + cv::exp(outputs - max, outputs); + float sum = cv::sum(outputs)[0]; + outputs /= sum; + + cv::Point label_point; + cv::minMaxLoc(outputs.reshape(1, 1), nullptr, &armor_confidence, nullptr, &label_point); + int label_id = label_point.x; + + if (label_id == 3) { + armor_id = enumeration::ArmorIdFlag::InfantryIII; + } else if (label_id == 4) { + armor_id = enumeration::ArmorIdFlag::InfantryIV; + } else if (label_id == 5) { + armor_id = enumeration::ArmorIdFlag::InfantryV; + } else if (label_id == 6) { + armor_id = enumeration::ArmorIdFlag::Outpost; + } else if (label_id == 1) { + armor_id = enumeration::ArmorIdFlag::Hero; + } else if (label_id == 2) { + armor_id = enumeration::ArmorIdFlag::Engineer; + } else if (label_id == 0) { + armor_id = enumeration::ArmorIdFlag::Sentry; + } else if (label_id == 7) { + armor_id = enumeration::ArmorIdFlag::Base; + }; + }; private: - class Impl; - std::unique_ptr pimpl_; -}; + cv::dnn::Net net_; + ov::Core core_; + ov::CompiledModel compiled_model_; + const int model_image_height_ = 640; + const int model_image_width_ = 640; +}; } \ No newline at end of file diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index e69de29..2705d6f 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -0,0 +1,345 @@ +#include "identifier.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" +#include "identifier_armor.hpp" +#include "interfaces/armor_in_image.hpp" +#include "tongji/identifier/classifier.hpp" +#include "util/logger.hpp" +#include "util/stringifier.hpp" + +namespace world_exe::tongji::identifier { + +class Identifier::Impl { +public: + explicit Impl(const std::string& model_path, const int& model_image_width, + const int& model_image_height, const double& threshold, const double& max_angle_error, + const double& min_lightbar_ratio, const double& max_lightbar_ratio, + const double& min_lightbar_length, const double& max_armor_ratio, + const double& min_armor_ratio, const double& max_side_ratio, const double& min_confidence, + const double& max_rectangular_error, const std::string& save_path, const bool& debug = true) + : classifier_(model_path, model_image_width, model_image_height) + , threshold_(threshold) + , max_angle_error_(max_angle_error / 57.3) // degree to rad + , min_lightbar_ratio_(min_lightbar_ratio) + , max_lightbar_ratio_(max_lightbar_ratio) + , min_lightbar_length_(min_lightbar_length) + , max_armor_ratio_(max_armor_ratio) + , min_armor_ratio_(min_armor_ratio) + , max_side_ratio_(max_side_ratio) + , min_confidence_(min_confidence) + , max_rectangular_error_(max_rectangular_error / 57.3) // degree to rad + , save_path_(std::move(save_path)) + , debug_(debug) { + + if (!std::filesystem::exists(save_path_)) std::filesystem::create_directories(save_path_); + } + + void SetTargetColor(bool target_color) { target_color_ = target_color; } + + const std::tuple, enumeration::CarIDFlag> + Identify(const cv::Mat& bgr_img) { + // 彩色图转灰度图 + cv::Mat gray_img; + cv::cvtColor(bgr_img, gray_img, cv::COLOR_BGR2GRAY); + + // 进行二值化 + cv::Mat binary_img; + cv::threshold(gray_img, binary_img, threshold_, 255, cv::THRESH_BINARY); + cv::imshow("binary_img", binary_img); + + // 获取轮廓点 + std::vector> contours; + cv::findContours(binary_img, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + + // 获取灯条 + std::size_t lightbar_id = 0; + std::list lightbars; + + for (const auto& contour : contours) { + auto rotated_rect = cv::minAreaRect(contour); + auto lightbar = Lightbar(rotated_rect, lightbar_id); + + if (!CheckGeometry(lightbar)) continue; + + lightbar.color = GetColor(bgr_img, contour); + lightbars.emplace_back(lightbar); + lightbar_id += 1; + } + + // 将灯条从左到右排序 + lightbars.sort( + [](const Lightbar& a, const Lightbar& b) { return a.center.x < b.center.x; }); + + struct ArmorCandidate { + Lightbar left, right; + cv::Mat pattern; + enumeration::ArmorIdFlag id = enumeration::ArmorIdFlag::Unknow; + double confidence; + bool duplicated = false; + + explicit ArmorCandidate(const Lightbar& left, const Lightbar& right) + : left(left) + , right(right) { } + }; + + // 获取装甲板 + std::list armor_candidates; + for (auto left = lightbars.begin(); left != lightbars.end(); left++) { + for (auto right = std::next(left); right != lightbars.end(); right++) { + + if (left->color != right->color) continue; + + auto armor_candidate = ArmorCandidate(*left, *right); + + auto left2right = right->center - left->center; + auto width = cv::norm(left2right); + auto max_lightbar_length = std::max(left->length, right->length); + auto min_lightbar_length = std::min(left->length, right->length); + + auto armor_ratio = + width / max_lightbar_length; // 两灯条的中点连线与较长灯条的长度之比 + auto armor_side_ratio = + max_lightbar_length / min_lightbar_length; // 长灯条与短灯条的长度之比 + + auto roll = std::atan2(left2right.y, left2right.x); + auto left_rectangular_error = std::abs(left->angle - roll - M_PI / 2); + auto right_rectangular_error = std::abs(right->angle - roll - CV_PI / 2); + auto armor_rectangular_error = std::max(left_rectangular_error, + right_rectangular_error); // 灯条和中点连线所成夹角与π/2的差值 + + if (!CheckGeometry(armor_ratio, armor_side_ratio, armor_rectangular_error)) + continue; + + armor_candidate.pattern = GetPattern(bgr_img, *left, *right); + + classifier_.Classify( + armor_candidate.pattern, armor_candidate.id, armor_candidate.confidence); + + if (!CheckName( + armor_candidate.id, armor_candidate.confidence, armor_candidate.pattern)) + continue; + + auto is_large = IsLargeArmor(armor_ratio, armor_candidate.id); + if (!CheckType(armor_candidate.id, !is_large, armor_candidate.pattern)) continue; + + auto armor_center = + (left->center + right->center) / 2.; // 不是对角线交点，不能作为实际中心！ + auto armor_center_norm = GetCenterNorm(bgr_img, armor_center); + armor_candidates.emplace_back(armor_candidate); + } + } + + // 检查装甲板是否存在共用灯条的情况 + for (auto armor1 = armor_candidates.begin(); armor1 != armor_candidates.end(); armor1++) { + for (auto armor2 = std::next(armor1); armor2 != armor_candidates.end(); armor2++) { + if (armor1->left.id != armor2->left.id && armor1->left.id != armor2->right.id + && armor1->right.id != armor2->left.id + && armor1->right.id != armor2->right.id) { + continue; + } + + // 装甲板重叠, 保留roi小的 + if (armor1->left.id == armor2->left.id || armor1->right.id == armor2->right.id) { + auto area1 = armor1->pattern.cols * armor1->pattern.rows; + auto area2 = armor2->pattern.cols * armor2->pattern.rows; + if (area1 < area2) armor2->duplicated = true; + else armor1->duplicated = true; + } + + // 装甲板相连，保留置信度大的 + if (armor1->left.id == armor2->right.id || armor1->right.id == armor2->left.id) { + if (armor1->confidence < armor2->confidence) armor1->duplicated = true; + else armor2->duplicated = true; + } + } + } + + armor_candidates.remove_if([&](const ArmorCandidate& a) { return a.duplicated; }); + armor_candidates.remove_if( + [&](const ArmorCandidate& a) { return a.left.color != target_color_; }); + + std::vector armor_plates; + uint32_t all_car_id = static_cast(enumeration::ArmorIdFlag::None); + for (const auto& armor : armor_candidates) { + armor_plates.emplace_back(data::ArmorImageSpacing { armor.id, + { + armor.left.top, + armor.right.top, + armor.right.bottom, + armor.left.bottom, + } }); + + all_car_id |= static_cast(armor.id); + } + + return { std::make_shared(armor_plates), + static_cast(all_car_id) }; + } + +private: + void Save(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag armor_id) const { + auto file_name = fmt::format("{:%Y-%m-%d_%H-%M-%S}", std::chrono::system_clock::now()); + auto img_path = fmt::format( + "{}/{}_{}.jpg", save_path_, util::stringifier::ToString(armor_id), file_name); + cv::imwrite(img_path, armor_pattern); + } + + Color GetColor(const cv::Mat& bgr_img, const std::vector& contour) const { + int red_sum = 0, blue_sum = 0; + + for (const auto& point : contour) { + red_sum += bgr_img.at(point)[2]; + blue_sum += bgr_img.at(point)[0]; + } + + return blue_sum > red_sum ? Color::blue : Color::red; + } + + cv::Mat GetPattern(const cv::Mat& bgr_img, const Lightbar& left_lightbar, + const Lightbar& right_lightbar) const { + // 延长灯条获得装甲板角点 + // 1.125 = 0.5 * armor_height / lightbar_length = 0.5 * 126mm / 56mm + auto tl = left_lightbar.center - left_lightbar.top2bottom * 1.125; + auto bl = left_lightbar.center + left_lightbar.top2bottom * 1.125; + auto tr = right_lightbar.center - right_lightbar.top2bottom * 1.125; + auto br = right_lightbar.center + right_lightbar.top2bottom * 1.125; + + auto roi_left = std::max(std::min(tl.x, bl.x), 0); + auto roi_top = std::max(std::min(tl.y, tr.y), 0); + auto roi_right = std::min(std::max(tr.x, br.x), bgr_img.cols); + auto roi_bottom = std::min(std::max(bl.y, br.y), bgr_img.rows); + auto roi_tl = cv::Point(roi_left, roi_top); + auto roi_br = cv::Point(roi_right, roi_bottom); + auto roi = cv::Rect(roi_tl, roi_br); + + return bgr_img(roi); + } + + cv::Point2f GetCenterNorm(const cv::Mat& bgr_img, const cv::Point2f& center) const { + auto h = bgr_img.rows; + auto w = bgr_img.cols; + return { center.x / w, center.y / h }; + } + + bool IsLargeArmor(double armor_ratio, enumeration::ArmorIdFlag armor_id) const { + /// 优先根据当前armor.ratio判断 + /// TODO: 26赛季是否还需要根据比例判断大小装甲？能否根据图案直接判断？ + + if (armor_ratio > 3.0) { + // util::logger::logger()->debug("[Identifier] get armor type by ratio: LARGE {} with " + // "ratio :{:.2f}", + // util::stringifier::ToString(armor_id), armor_ratio); + return true; + } + + if (armor_ratio < 2.5) { + // util::logger::logger()->debug("[Identifier] get armor type by ratio: NORMAL {} with " + // "ratio: {:.2f}", + // util::stringifier::ToString(armor_id), armor_ratio); + return false; + } + + // util::logger::logger()->debug( + // "[Detector] get armor type by name: {}", util::stringifier::ToString(armor_id)); + + // 英雄、基地只能是大装甲板 + if (armor_id == enumeration::ArmorIdFlag::Hero + || armor_id == enumeration::ArmorIdFlag::Base) { + return true; + } + + // 其他所有（工程、哨兵、前哨站、步兵）都是小装甲板 + /// TODO: 基地顶装甲是小装甲板 + return false; + } + + bool CheckGeometry(const Lightbar& lightbar) const { + auto angle_ok = lightbar.angle_error < max_angle_error_; + auto ratio_ok = + lightbar.ratio > min_lightbar_ratio_ && lightbar.ratio < max_lightbar_ratio_; + auto length_ok = lightbar.length > min_lightbar_length_; + return angle_ok && ratio_ok && length_ok; + } + + bool CheckGeometry(const double& armor_ratio, const double& armor_side_ratio, + const double& armor_rectangular_error) const { + + auto ratio_ok = armor_ratio > min_armor_ratio_ && armor_ratio < max_armor_ratio_; + auto side_ratio_ok = armor_side_ratio < max_side_ratio_; + auto rectangular_error_ok = armor_rectangular_error < max_rectangular_error_; + + return ratio_ok && side_ratio_ok && rectangular_error_ok; + } + + bool CheckName(const enumeration::ArmorIdFlag& armor_id, const double& armor_confidence, + const cv::Mat& armor_pattern) const { + auto name_ok = (armor_id != enumeration::ArmorIdFlag::Unknow); + auto confidence_ok = armor_confidence > min_confidence_; + + // 保存不确定的图案，用于分类器的迭代 + if (name_ok && !confidence_ok) Save(armor_pattern, armor_id); + + // 出现 5号 则显示 debug 信息。但不过滤。 + if (armor_id == enumeration::ArmorIdFlag::InfantryV) + world_exe::util::logger::logger()->debug("See pattern 5 : InfantryV "); + + return name_ok && confidence_ok; + } + + bool CheckType(const enumeration::ArmorIdFlag& armor_id, bool is_normal_armor, + const cv::Mat& armor_pattern) const { + auto name_ok = is_normal_armor ? (armor_id != enumeration::ArmorIdFlag::Hero + && armor_id != enumeration::ArmorIdFlag::Base) + : (armor_id == enumeration::ArmorIdFlag::Hero + || armor_id == enumeration::ArmorIdFlag::Base); + + // 保存异常的图案，用于分类器的迭代 + if (!name_ok) { + util::logger::logger()->debug( + "see strange armor: {}", util::stringifier::ToString(armor_id)); + Save(armor_pattern, armor_id); + } + return name_ok; + } + +private: + Classifier classifier_; + + double threshold_; + double max_angle_error_; + double min_lightbar_ratio_, max_lightbar_ratio_; + double min_lightbar_length_; + double min_armor_ratio_, max_armor_ratio_; + double max_side_ratio_; + double min_confidence_; + double max_rectangular_error_; + + bool target_color_ = true; // true: red, false: blue + + bool debug_; + std::string save_path_; +}; + +const std::tuple, enumeration::CarIDFlag> +Identifier::identify(const cv::Mat& input_image) { + return pimpl_->Identify(input_image); +} + +void Identifier::SetTargetColor(Color target_color) { return pimpl_->SetTargetColor(target_color); } +} \ No newline at end of file diff --git a/src/tongji/identifier/identifier.hpp b/src/tongji/identifier/identifier.hpp index e69de29..01e73a4 100644 --- a/src/tongji/identifier/identifier.hpp +++ b/src/tongji/identifier/identifier.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include "interfaces/identifier.hpp" + +namespace world_exe::tongji::identifier { + +enum Color { red, blue, extinguish, purple }; + +struct Lightbar { + std::size_t id; + Color color; + cv::Point2f center, top, bottom, top2bottom; + std::vector points; + double angle, angle_error, length, width, ratio; + cv::RotatedRect rotated_rect; + + Lightbar(const cv::RotatedRect& rotated_rect, std::size_t id) + : id(id) + , rotated_rect(rotated_rect) { + std::vector corners(4); + rotated_rect.points(&corners[0]); + std::sort(corners.begin(), corners.end(), + [](const cv::Point2f& a, const cv::Point2f& b) { return a.y < b.y; }); + + this->center = rotated_rect.center; + this->top = (corners[0] + corners[1]) / 2.; + this->bottom = (corners[2] + corners[3]) / 2.; + this->top2bottom = bottom - top; + + this->points.emplace_back(top); + this->points.emplace_back(bottom); + + this->width = cv::norm(corners[0] - corners[1]); + this->angle = std::atan2(top2bottom.y, top2bottom.x); + this->angle_error = std::abs(angle - M_PI / 2); + this->length = cv::norm(top2bottom); + this->ratio = length / width; + } +}; +class Identifier final : public interfaces::IIdentifier { +public: + Identifier(const std::string& model_path, int model_image_width, int model_image_height); + ~Identifier(); + + const std::tuple, enumeration::CarIDFlag> + identify(const cv::Mat& input_image) override; + + void SetTargetColor(Color target_color); + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} \ No newline at end of file diff --git a/src/tongji/identifier/identifier_armor.hpp b/src/tongji/identifier/identifier_armor.hpp new file mode 100644 index 0000000..ec94241 --- /dev/null +++ b/src/tongji/identifier/identifier_armor.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include "enum/armor_id.hpp" +#include "interfaces/armor_in_image.hpp" +#include "interfaces/time_stamped.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::identifier { +class IdentifierArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { +public: + explicit IdentifierArmor(const std::vector& armors) { + SetArmors(armors); + } + + const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } + + const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + + void SetArmors(const std::vector& armors) { + std::array, 8> temp_armors; + for (const auto& armor : armors) { + temp_armors[util::enumeration::GetIndex(armor.id)].emplace_back(armor); + } + armors_ = std::move(temp_armors); + } + + const std::vector& GetArmors( + const enumeration::ArmorIdFlag& armor_id) const override { + return armors_[util::enumeration::GetIndex(armor_id)]; + } + +private: + std::time_t time_stamp_ { 0 }; + std::array, 8> armors_; +}; +} \ No newline at end of file From 541f531b9ebe26544cb84ea8d0a5937e1cf9c679 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Sep 2025 04:26:25 +0800 Subject: [PATCH 10/93] fix(utils): correct incomplete namespace in stringifier module --- src/util/stringifier.hpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/util/stringifier.hpp b/src/util/stringifier.hpp index 0953582..0fe4974 100644 --- a/src/util/stringifier.hpp +++ b/src/util/stringifier.hpp @@ -8,25 +8,25 @@ namespace world_exe::util::stringifier { static inline std::string ToString(const world_exe::enumeration::ArmorIdFlag& id) { switch (id) { - case enumeration::ArmorIdFlag::Hero: + case world_exe::enumeration::ArmorIdFlag::Hero: return "Hero"; - case enumeration::ArmorIdFlag::Engineer: + case world_exe::enumeration::ArmorIdFlag::Engineer: return "Engineer"; - case enumeration::ArmorIdFlag::InfantryIII: + case world_exe::enumeration::ArmorIdFlag::InfantryIII: return "InfantryIII"; - case enumeration::ArmorIdFlag::InfantryIV: + case world_exe::enumeration::ArmorIdFlag::InfantryIV: return "InfantryIV"; - case enumeration::ArmorIdFlag::InfantryV: + case world_exe::enumeration::ArmorIdFlag::InfantryV: return "InfantryV"; - case enumeration::ArmorIdFlag::Sentry: + case world_exe::enumeration::ArmorIdFlag::Sentry: return "Sentry"; - case enumeration::ArmorIdFlag::Base: + case world_exe::enumeration::ArmorIdFlag::Base: return "Base"; - case enumeration::ArmorIdFlag::Outpost: + case world_exe::enumeration::ArmorIdFlag::Outpost: return "Outpost"; - case enumeration::ArmorIdFlag::Unknow: + case world_exe::enumeration::ArmorIdFlag::Unknow: return "Unknow"; - case enumeration::ArmorIdFlag::None: + case world_exe::enumeration::ArmorIdFlag::None: return "None"; default: return "UnknownID"; From 4304b8c758c3f697785a0f3a87ab1e4095db3857 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 4 Sep 2025 08:25:49 +0800 Subject: [PATCH 11/93] refactor(solver): rewrite interface implementation and add index-to-id function --- src/tongji/solver/solver.cpp | 78 ++++++++---------------------- src/tongji/solver/solver.hpp | 1 - src/tongji/solver/solver_armor.hpp | 39 +++++++++++++++ src/util/index.hpp | 9 ++++ 4 files changed, 67 insertions(+), 60 deletions(-) create mode 100644 src/tongji/solver/solver_armor.hpp diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 658841e..2213d5a 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -1,55 +1,19 @@ #include "solver.hpp" -#include -#include +#include #include #include -#include -#include #include "data/armor_camera_spacing.hpp" #include "data/armor_gimbal_control_spacing.hpp" -#include "data/armor_image_spaceing.hpp" -#include "enum/armor_id.hpp" -#include "interfaces/armor_in_camera.hpp" -#include "interfaces/time_stamped.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" +#include "solver_armor.hpp" #include "util/coordinate.hpp" #include "util/index.hpp" #include "util/math.hpp" namespace world_exe::tongji::solver { -class ArmorInCameraImpl final : public interfaces::IArmorInCamera, public interfaces::ITimeStamped { -public: - void AddArmor(const data::ArmorCameraSpacing& armor) { - int index = util::enumeration::GetIndex(armor.id); - armors_.at(index).push_back(armor); - } - - void SetTimeStamp(const std::time_t& timestamp) { timestamp_ = timestamp; } - - const std::vector& GetArmors( - const enumeration::ArmorIdFlag& armor_id) const override { - static const std::vector empty_vector; - try { - int index = util::enumeration::GetIndex(armor_id); - return armors_.at(index); - } catch (const std::exception& e) { - return empty_vector; - } - } - - const std::time_t& GetTimeStamp() const override { return timestamp_; } - - const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - -private: - std::vector> armors_ { static_cast( - enumeration::ArmorIdFlag::Count) }; - std::time_t timestamp_ = 0; -}; - class Solver::Impl { public: Impl(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, @@ -59,24 +23,20 @@ class Solver::Impl { , t_camera2gimbal_(t_camera2gimbal) { } std::shared_ptr SolvePnp( - std::shared_ptr armors_in_image) const { - - auto result = std::make_shared(); - result->SetTimeStamp(armors_in_image->GetTimeStamped().GetTimeStamp()); - - for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { - const auto armor_id = static_cast(1 << i); - const auto& image_armors = armors_in_image->GetArmors(armor_id); - - for (const auto& armor_image : image_armors) { - const auto armor_in_camera_optional = Solve(armor_image); - if (armor_in_camera_optional.has_value()) { - result->AddArmor(armor_in_camera_optional.value()); + std::shared_ptr armors_in_image) { + std::vector armor_plates; + + for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); i++) { + const auto& armor_id = util::enumeration::GetArmorIdFlag(i); + const auto& armors = armors_in_image->GetArmors(armor_id); + for (const auto& armor : armors) { + auto solved_armor = Solve(armor); + if (solved_armor.has_value()) { + armor_plates.emplace_back(solved_armor.value()); } } } - - return result; + return std::make_shared(armor_plates); } std::optional Solve( @@ -167,6 +127,8 @@ class Solver::Impl { return result; } + const std::time_t& GetTimeStamp() const { return time_stamp_; } + private: std::vector ReprojectArmor(const Eigen::Vector3d& xyz_in_world, const double& yaw, const bool& is_large, const enumeration::ArmorIdFlag& id) const { @@ -332,21 +294,19 @@ class Solver::Impl { Eigen::Matrix3d R_gimbal2world_; Eigen::Vector3d t_camera2gimbal_; inline constexpr static const double MaxArmorDistance = 15.0; + + std::time_t time_stamp_ { 0 }; }; Solver::Solver(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, Eigen::Vector3d t_camera2gimbal) - : pimpl_(std::make_unique(R_camera2gimbal, R_gimbal2world, t_camera2gimbal)) - , last_processed_time_(0) { } + : pimpl_(std::make_unique(R_camera2gimbal, R_gimbal2world, t_camera2gimbal)) { } Solver::~Solver() = default; -const std::time_t& Solver::GetTimeStamp() const { return last_processed_time_; } - -class ArmorInCameraImpl; +const std::time_t& Solver::GetTimeStamp() const { return pimpl_->GetTimeStamp(); } std::shared_ptr Solver::SolvePnp( std::shared_ptr armors_in_image) { - last_processed_time_ = armors_in_image->GetTimeStamped().GetTimeStamp(); return pimpl_->SolvePnp(armors_in_image); } diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index b3a1177..5f6c2f4 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -19,7 +19,6 @@ class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStam private: class Impl; std::unique_ptr pimpl_; - std::time_t last_processed_time_; }; } \ No newline at end of file diff --git a/src/tongji/solver/solver_armor.hpp b/src/tongji/solver/solver_armor.hpp new file mode 100644 index 0000000..1e8145b --- /dev/null +++ b/src/tongji/solver/solver_armor.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include + +#include "interfaces/armor_in_camera.hpp" +#include "interfaces/time_stamped.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::solver { +class SolverArmor final : public interfaces::IArmorInCamera, public interfaces::ITimeStamped { +public: + explicit SolverArmor(const std::vector& armors) { + SetArmors(armors); + time_stamp_ = 0; + } + + const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } + + const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + + void SetArmors(const std::vector& armors) { + std::array, 8> temp_armors; + for (const auto& armor : armors) { + temp_armors[util::enumeration::GetIndex(armor.id)].emplace_back(armor); + } + armors_ = std::move(temp_armors); + } + + const std::vector& GetArmors( + const enumeration::ArmorIdFlag& armor_id) const override { + return armors_[util::enumeration::GetIndex(armor_id)]; + } + +private: + std::time_t time_stamp_ { 0 }; + std::array, 8> armors_; +}; +} \ No newline at end of file diff --git a/src/util/index.hpp b/src/util/index.hpp index 483cb93..350fe26 100644 --- a/src/util/index.hpp +++ b/src/util/index.hpp @@ -22,4 +22,13 @@ static inline int GetIndex(const world_exe::enumeration::ArmorIdFlag& flag) { throw std::runtime_error("Invalid ArmorIdFlag value: " + std::to_string(value)); } } + +static const world_exe::enumeration::ArmorIdFlag GetArmorIdFlag(int index) { + if (index >= 0 && index < static_cast(world_exe::enumeration::ArmorIdFlag::Count)) { + uint32_t value = 1U << index; + return static_cast(value); + } else { + throw std::out_of_range("Invalid index for ArmorIdFlag conversion."); + } +} } \ No newline at end of file From 43109dcf873681c16529b9b35f2f4a820c7e9c82 Mon Sep 17 00:00:00 2001 From: fan Date: Fri, 5 Sep 2025 01:20:35 +0800 Subject: [PATCH 12/93] add comment --- src/util/scanline.hpp | 88 +++++++++++++++------- src/v1/predictor/car/car_predictor_ekf.hpp | 10 +-- src/v1/predictor/predictor_manager.cpp | 2 + 3 files changed, 67 insertions(+), 33 deletions(-) diff --git a/src/util/scanline.hpp b/src/util/scanline.hpp index 32185c3..2cd5fa3 100644 --- a/src/util/scanline.hpp +++ b/src/util/scanline.hpp @@ -6,6 +6,17 @@ namespace world_exe::util { class ScanLine { public: + /** + * @brief 获取多边形轮廓内部的所有像素点 + * + * 该函数使用扫描线算法填充多边形，返回轮廓内部的所有像素点坐标。 + * + * @param image 输入图像，用于确定图像边界 + * @param contour 多边形轮廓点的集合 + * @return std::vector 返回多边形内部所有像素点的坐标集合 + * + * @note 如果轮廓为空，则返回空向量 + */ static inline std::vector get_points( const cv::Mat& image, const std::vector& contour) { if (contour.empty()) return {}; @@ -15,27 +26,43 @@ class ScanLine { for (const auto& point : contour) points.emplace_back(point); + // 收集多边形的所有边 auto edges = collect_poly_edges(points); + // 使用边表填充算法获取内部点 return fill_edge_collection(image, edges); } private: struct PolyEdge { PolyEdge() - : y0(0) - , y1(0) - , x(0) - , dx(0) - , next(nullptr) { } - - int y0, y1; - int64 x, dx; - PolyEdge* next; + : y0(0) // 边的起始Y坐标 + , y1(0) // 边的结束Y坐标 + , x(0) // 当前X坐标（定点数） + , dx(0) // X方向的增量（定点数） + , next(nullptr) { } // 指向下一条边的指针 + + int y0, y1; // 边的Y坐标范围 + int64 x, dx; // X坐标和X增量（使用定点数提高精度） + PolyEdge* next; // 链表指针 }; + /** + * @brief 定点数相关常量定义 + * + * XY_SHIFT: 定点数位移量，用于提高坐标计算精度 + * XY_ONE: 定点数的1.0表示 + * DRAWING_STORAGE_BLOCK: 绘制存储块大小 + */ enum { XY_SHIFT = 16, XY_ONE = 1 << XY_SHIFT, DRAWING_STORAGE_BLOCK = (1 << 12) - 256 }; + /** + * @details 算法流程： + * 1. 计算边界框并进行边界检查 + * 2. 按Y坐标对边进行排序 + * 3. 逐行扫描，维护活动边表 + * 4. 在每条扫描线上，计算交点并填充像素 + */ static inline std::vector fill_edge_collection( const cv::Mat& img, std::vector& edges) { PolyEdge tmp; @@ -47,11 +74,11 @@ class ScanLine { if (total < 2) return {}; + // 计算边界框 for (i = 0; i < total; i++) { PolyEdge& e1 = edges[i]; assert(e1.y0 < e1.y1); - // Determine x-coordinate of the end of the edge. - // (This is not necessary x-coordinate of any vertex in the array.) + // 计算边末端的X坐标（不一定是顶点的X坐标） int64 x1 = e1.x + (e1.y1 - e1.y0) * e1.dx; y_min = std::min(y_min, e1.y0); y_max = std::max(y_max, e1.y1); @@ -61,45 +88,48 @@ class ScanLine { x_max = std::max(x_max, x1); } + // 边界检查：如果多边形完全在图像外部，直接返回 if (y_max < 0 || y_min >= size.height || x_max < 0 || x_min >= ((int64)size.width << XY_SHIFT)) return {}; + // 按Y坐标排序边，Y相同时按X坐标排序，X也相同时按斜率排序 std::sort(edges.begin(), edges.end(), [](const PolyEdge& e1, const PolyEdge& e2) { return e1.y0 - e2.y0 ? e1.y0 < e2.y0 : e1.x - e2.x ? e1.x < e2.x : e1.dx < e2.dx; }); tmp.y0 = std::numeric_limits::max(); - edges.push_back(tmp); // after this point we do not add - // any elements to edges, thus we can use pointers + edges.push_back(tmp); // 添加哨兵边，此后不再向edges添加元素，可以安全使用指针 i = 0; tmp.next = nullptr; e = &edges[i]; y_max = MIN(y_max, size.height); std::vector points; + // 逐行扫描 for (y = e->y0; y < y_max; y++) { PolyEdge *last, *prelast, *keep_prelast; int sort_flag = 0; - int draw = 0; - int clipline = y < 0; + int draw = 0; // 绘制标志，奇偶规则 + int clipline = y < 0; // 当前行是否在图像外部 + // 维护活动边表 prelast = &tmp; last = tmp.next; while (last || e->y0 == y) { if (last && last->y1 == y) { - // exclude edge if y reaches its lower point + // 如果边到达下端点，从活动边表中移除 prelast->next = last->next; last = last->next; continue; } keep_prelast = prelast; if (last && (e->y0 > y || last->x < e->x)) { - // go to the next edge in active list + // 移动到活动边表中的下一条边 prelast = last; last = last->next; } else if (i < total) { - // insert new edge into active list if y reaches its upper point + // 如果到达边的上端点，将新边插入活动边表 prelast->next = e; e->next = last; prelast = e; @@ -108,7 +138,7 @@ class ScanLine { if (draw) { if (!clipline) { - // convert x's from fixed-point to image coordinates + // 将定点数X坐标转换为图像坐标 int x1, x2; if (keep_prelast->x > prelast->x) { @@ -119,7 +149,7 @@ class ScanLine { x2 = (int)(prelast->x >> XY_SHIFT); } - // clip and draw the line + // 裁剪并绘制线段 if (x1 < size.width && x2 >= 0) { if (x1 < 0) x1 = 0; if (x2 >= size.width) x2 = size.width - 1; @@ -127,13 +157,14 @@ class ScanLine { points.emplace_back(i, y); } } + // 更新边的X坐标 keep_prelast->x += keep_prelast->dx; prelast->x += prelast->dx; } - draw ^= 1; + draw ^= 1; // 切换绘制状态（奇偶规则） } - // sort edges (using bubble sort) + // 对活动边表中的边按X坐标排序（使用冒泡排序） keep_prelast = 0; do { @@ -143,7 +174,7 @@ class ScanLine { while (last != keep_prelast && last->next != 0) { PolyEdge* te = last->next; - // swap edges + // 交换边的位置 if (last->x > te->x) { prelast->next = te; last->next = te->next; @@ -164,23 +195,27 @@ class ScanLine { static inline std::vector collect_poly_edges(const std::vector& points) { const auto points_size = points.size(); cv::Point2l pt0 = points[points_size - 1], pt1; - pt0.x = (pt0.x) << XY_SHIFT; + pt0.x = (pt0.x) << XY_SHIFT; // 转换为定点数 std::vector edges; edges.reserve(edges.size() + points_size); + + // 遍历所有相邻顶点对，构建边 for (std::size_t i = 0; i < points_size; i++, pt0 = pt1) { cv::Point2l t0, t1; pt1 = points[i]; - pt1.x = (pt1.x) << XY_SHIFT; + pt1.x = (pt1.x) << XY_SHIFT; // 转换为定点数 t0.x = pt0.x; t1.x = pt1.x; t0.y = pt0.y << XY_SHIFT; t1.y = pt1.y << XY_SHIFT; + // 跳过水平边 if (pt0.y == pt1.y) continue; PolyEdge edge; + // 确保边的方向统一：y0 < y1 if (pt0.y < pt1.y) { edge.y0 = static_cast(pt0.y); edge.y1 = static_cast(pt1.y); @@ -190,10 +225,11 @@ class ScanLine { edge.y1 = static_cast(pt0.y); edge.x = pt1.x; } + // 计算X方向的增量（斜率的倒数） edge.dx = (pt1.x - pt0.x) / (pt1.y - pt0.y); edges.emplace_back(edge); } return edges; } }; -} +} // namespace world_exe::util diff --git a/src/v1/predictor/car/car_predictor_ekf.hpp b/src/v1/predictor/car/car_predictor_ekf.hpp index 029551c..bf82f85 100644 --- a/src/v1/predictor/car/car_predictor_ekf.hpp +++ b/src/v1/predictor/car/car_predictor_ekf.hpp @@ -12,17 +12,13 @@ namespace world_exe::v1::predictor { * @class CarPredictEkf * @brief 车辆预测EKF核心类 * - * 状态向量维度: 11维 [x, vx, y, vy, z, z1, z2, r1, r2, θ, ω] + * 状态向量维度: 11维 [x, vx, y, vy, z, z1, z2, r1, r2, θ, ω]，其中 θ为最近装甲板与车辆中心连线与选定系的x轴的夹角 * 观测向量维度: 4维 [θ, yaw, pitch, distance] * * 这个是整个预测的核心，无论是逻辑的维护还是预测的模型都放在这里了，然后其他的主要是为了接口实现 */ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { public: - /** - * @brief 构造函数，初始化状态向量和协方差矩阵 - * 状态: [x=2.0, vx=0, y=0, vy=0, z=-0.3, z1=-0.3, z2=-0.3, r1=0.2, r2=0.2, θ=π, ω=0] - */ CarPredictEkf() { X_k << 2.0, 0.0, 0.0, 0.0, -0.3, -0.3, -0.3, 0.2, 0.2, std::numbers::pi, 0.0; @@ -37,7 +33,7 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { inline void set_second_armor() { second_armor_flag = true; } /** - * @brief 获取预测的装甲板位置 + * @brief 获取预测的装甲板位置，这里的预测尚未加入速度擦差分得加速度 * @param id 装甲板类型 * @param sec 预测时间（秒） * @return 四个装甲板的预测位置和姿态 @@ -60,7 +56,7 @@ class CarPredictEkf : public world_exe::util::Ekf<11, 4, CarPredictEkf> { const double r2 = model_output(8); // 装甲板2到中心距离 double model_yaw = model_output(9) + model_output(10) * sec; // 预测yaw角 - // TODO: 把 pitch 与 z 关联起来，这里可以通过z和z1、z2把pitch算出来 + // 这里也可以通过z和z1、z2把pitch算出来 const double pitch1 = 15. / 180. * std::numbers::pi; const double pitch2 = 15. / 180. * std::numbers::pi; diff --git a/src/v1/predictor/predictor_manager.cpp b/src/v1/predictor/predictor_manager.cpp index a685f94..c2a004b 100644 --- a/src/v1/predictor/predictor_manager.cpp +++ b/src/v1/predictor/predictor_manager.cpp @@ -23,6 +23,7 @@ class PredictorManager::Impl { static_cast(0b00000001 << i)); if (armors.empty()) continue; CarPredictEkf::ZVec input; + // 此处只对识别到一块或两块装甲板时做处理，因为对同一辆车不可能有更多，即便有此处也不应处理，当作异常 if (armors.size() == 1) { const auto tmp_armor = data::ArmorGimbalControlSpacing { armors[0].id, transform * armors[0].position, rotation_transform * armors[0].orientation }; @@ -32,6 +33,7 @@ class PredictorManager::Impl { tmp_armor.position.norm(); predictors_[i].Update(input, {}, dt); } else if (armors.size() == 2) { + // 当同时识别到两块装甲板时，优先更新近的那块，再更新远的 const auto armor0_yaw = util::math::get_yaw_from_quaternion(armors[0].orientation); const auto armor1_yaw = util::math::get_yaw_from_quaternion(armors[1].orientation); From 8b24d9808cca1f88bec4c33b8527587a7377bb05 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 5 Sep 2025 05:14:38 +0800 Subject: [PATCH 13/93] fix(identifier):enhance identifier debug output and change target color type --- src/tongji/identifier/classifier.hpp | 2 +- src/tongji/identifier/identifier.cpp | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp index 47924f3..175c534 100644 --- a/src/tongji/identifier/classifier.hpp +++ b/src/tongji/identifier/classifier.hpp @@ -20,7 +20,7 @@ class Classifier final { compiled_model_ = core_.compile_model( ovmodel, "AUTO", ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY)); } - +// void Classify(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag& armor_id, double& armor_confidence) { if (armor_pattern.empty()) { diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index 2705d6f..b79b572 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -45,12 +45,13 @@ class Identifier::Impl { , min_confidence_(min_confidence) , max_rectangular_error_(max_rectangular_error / 57.3) // degree to rad , save_path_(std::move(save_path)) - , debug_(debug) { + , debug_(debug) + , target_color_(blue) { if (!std::filesystem::exists(save_path_)) std::filesystem::create_directories(save_path_); } - void SetTargetColor(bool target_color) { target_color_ = target_color; } + void SetTargetColor(Color target_color) { target_color_ = target_color; } const std::tuple, enumeration::CarIDFlag> Identify(const cv::Mat& bgr_img) { @@ -296,7 +297,7 @@ class Identifier::Impl { if (name_ok && !confidence_ok) Save(armor_pattern, armor_id); // 出现 5号 则显示 debug 信息。但不过滤。 - if (armor_id == enumeration::ArmorIdFlag::InfantryV) + if (armor_id == enumeration::ArmorIdFlag::InfantryV && debug_) world_exe::util::logger::logger()->debug("See pattern 5 : InfantryV "); return name_ok && confidence_ok; @@ -310,7 +311,7 @@ class Identifier::Impl { || armor_id == enumeration::ArmorIdFlag::Base); // 保存异常的图案，用于分类器的迭代 - if (!name_ok) { + if (!name_ok && debug_) { util::logger::logger()->debug( "see strange armor: {}", util::stringifier::ToString(armor_id)); Save(armor_pattern, armor_id); @@ -330,7 +331,7 @@ class Identifier::Impl { double min_confidence_; double max_rectangular_error_; - bool target_color_ = true; // true: red, false: blue + Color target_color_; bool debug_; std::string save_path_; From 2bc4e9d45c94d54f3632187c34804721f8db0a1a Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 6 Sep 2025 05:08:28 +0800 Subject: [PATCH 14/93] fix(identifier):enhance identifier debug output --- src/tongji/identifier/identifier.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index b79b572..e7457fb 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -294,7 +294,7 @@ class Identifier::Impl { auto confidence_ok = armor_confidence > min_confidence_; // 保存不确定的图案，用于分类器的迭代 - if (name_ok && !confidence_ok) Save(armor_pattern, armor_id); + if (name_ok && !confidence_ok && debug_) Save(armor_pattern, armor_id); // 出现 5号 则显示 debug 信息。但不过滤。 if (armor_id == enumeration::ArmorIdFlag::InfantryV && debug_) From 99a4bc7a53cc575a3f6880e02697090a239b2c70 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 6 Sep 2025 06:00:21 +0800 Subject: [PATCH 15/93] fix(identifier):add internal recording flag --- src/tongji/identifier/identifier.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index e7457fb..0b2c382 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -32,7 +32,8 @@ class Identifier::Impl { const double& min_lightbar_ratio, const double& max_lightbar_ratio, const double& min_lightbar_length, const double& max_armor_ratio, const double& min_armor_ratio, const double& max_side_ratio, const double& min_confidence, - const double& max_rectangular_error, const std::string& save_path, const bool& debug = true) + const double& max_rectangular_error, const std::string& save_path, const bool& debug = true, + const bool& record = true) : classifier_(model_path, model_image_width, model_image_height) , threshold_(threshold) , max_angle_error_(max_angle_error / 57.3) // degree to rad @@ -46,6 +47,7 @@ class Identifier::Impl { , max_rectangular_error_(max_rectangular_error / 57.3) // degree to rad , save_path_(std::move(save_path)) , debug_(debug) + , record_(record) , target_color_(blue) { if (!std::filesystem::exists(save_path_)) std::filesystem::create_directories(save_path_); @@ -294,7 +296,7 @@ class Identifier::Impl { auto confidence_ok = armor_confidence > min_confidence_; // 保存不确定的图案，用于分类器的迭代 - if (name_ok && !confidence_ok && debug_) Save(armor_pattern, armor_id); + if (name_ok && !confidence_ok && record_) Save(armor_pattern, armor_id); // 出现 5号 则显示 debug 信息。但不过滤。 if (armor_id == enumeration::ArmorIdFlag::InfantryV && debug_) @@ -334,6 +336,7 @@ class Identifier::Impl { Color target_color_; bool debug_; + bool record_; std::string save_path_; }; From 53fd55affaf5e548239ea6218a3470193b69b9b8 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Sun, 7 Sep 2025 09:49:17 +0800 Subject: [PATCH 16/93] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E6=8E=A5?= =?UTF-8?q?=E5=8F=A3=E7=9A=84=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/interfaces/armor_in_camera.hpp | 9 ++++++++ .../interfaces/armor_in_gimbal_control.hpp | 9 ++++++++ include/interfaces/armor_in_image.hpp | 17 ++++++++++++++ include/interfaces/car_state.hpp | 14 +++++++++-- include/interfaces/drawable.hpp | 4 ++++ include/interfaces/fire_controller.hpp | 18 ++++++++++++++- include/interfaces/identifier.hpp | 11 ++++++++- include/interfaces/pnp_solver.hpp | 10 ++++++++ include/interfaces/predictor.hpp | 17 +++++++++++++- .../interfaces/predictor_update_package.hpp | 20 ++++++++++++++-- include/interfaces/sync_block.hpp | 5 ++++ include/interfaces/sync_load.hpp | 5 ++++ include/interfaces/target_predictor.hpp | 23 ++++++++++++++++--- include/interfaces/time_stamped.hpp | 3 +++ src/v1/auto_aim_system_v1.cpp | 4 ++-- src/v1/predictor/predictor_manager.hpp | 9 ++++---- 16 files changed, 162 insertions(+), 16 deletions(-) diff --git a/include/interfaces/armor_in_camera.hpp b/include/interfaces/armor_in_camera.hpp index 24bf577..52ea074 100644 --- a/include/interfaces/armor_in_camera.hpp +++ b/include/interfaces/armor_in_camera.hpp @@ -6,10 +6,19 @@ #include namespace world_exe::interfaces { + +/** + * @brief + 某一确定时刻 + 相机坐标系下 + 装甲板集合 + */ class IArmorInCamera { public: + /// 获取时间戳，标志其内容装甲板的准确时间点 COMBINE_TIME_STAMPED; + /// 获取某个车辆ID的装甲板集合 virtual const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const = 0; }; diff --git a/include/interfaces/armor_in_gimbal_control.hpp b/include/interfaces/armor_in_gimbal_control.hpp index dccdbd6..cedad6e 100644 --- a/include/interfaces/armor_in_gimbal_control.hpp +++ b/include/interfaces/armor_in_gimbal_control.hpp @@ -6,11 +6,20 @@ #include namespace world_exe::interfaces { + +/** + * @brief + 某一确定时刻 + 云台控制指令使用的坐标系（世界坐标系）下 + 装甲板集合 + */ class IArmorInGimbalControl { public: + /// 获取时间戳，标志其内容装甲板的准确时间点 COMBINE_TIME_STAMPED; + /// 获取某个车辆ID的装甲板集合 virtual const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const = 0; }; diff --git a/include/interfaces/armor_in_image.hpp b/include/interfaces/armor_in_image.hpp index b68b285..e07fdc5 100644 --- a/include/interfaces/armor_in_image.hpp +++ b/include/interfaces/armor_in_image.hpp @@ -7,11 +7,28 @@ #include namespace world_exe::interfaces { + +/** + * @brief + 某一确定时刻 + 相机的画面坐标系下 + 装甲板集合 + * @warning + ArmorImageSpacing 为二维装甲板 + 其实际坐标系可以看作以相机光轴为x，光心为原点的 x-forward y-up 实体坐标系 + */ class IArmorInImage { public: + /// 获取时间戳，标志其内容装甲板的准确时间点 COMBINE_TIME_STAMPED; + /** + * @brief 获取某个车辆ID的装甲板集合 + * + * @param armor_id 要查找的车辆ID + * @return const std::vector& + */ virtual const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const = 0; }; diff --git a/include/interfaces/car_state.hpp b/include/interfaces/car_state.hpp index f2b6bae..6c2863a 100644 --- a/include/interfaces/car_state.hpp +++ b/include/interfaces/car_state.hpp @@ -4,10 +4,20 @@ #include namespace world_exe::interfaces { + +/** + * @brief + ** 确定自瞄系统中某个ID代表的车辆是否可以进行云台锁定、开火 + * @todo 命名鬼才，可能要改下命名 + */ class ICarState { public: - virtual const ICarState& Update(const enumeration::CarIDFlag& car_detected) = 0; - virtual const enumeration::CarIDFlag& GetAllowdToFires() const = 0; + /** + * @brief + 获取可以开火的车辆ID + 示例： 某辆车在镜头中出现的时间够长，确定已经被自瞄系统追踪上 + */ + virtual const enumeration::CarIDFlag& GetAllowdToFires() const = 0; }; } \ No newline at end of file diff --git a/include/interfaces/drawable.hpp b/include/interfaces/drawable.hpp index a3087a0..9e50cac 100644 --- a/include/interfaces/drawable.hpp +++ b/include/interfaces/drawable.hpp @@ -4,6 +4,10 @@ #include namespace world_exe::interfaces { + +/** + * @brief 可以进行可视化调试，提供绘制方法 + */ class IDrawable { public: virtual void Draw(cv::InputArray, cv::OutputArray) = 0; diff --git a/include/interfaces/fire_controller.hpp b/include/interfaces/fire_controller.hpp index 77a9710..881fdbd 100644 --- a/include/interfaces/fire_controller.hpp +++ b/include/interfaces/fire_controller.hpp @@ -5,9 +5,25 @@ #include namespace world_exe::interfaces { + +/** + * @brief 火控系统，获取云台控制方向 + */ class IFireControl { public: + /** + * @brief 计算云台和发射系统控制量 + * + * @param time_duration 额外时间差 典型值：当前时刻到当前帧传感器传入内容的时间差 + * @return const data::FireControl + */ virtual const data::FireControl CalculateTarget(const std::time_t& time_duration) const = 0; - virtual const enumeration::CarIDFlag GetAttackCarId() const = 0; + + /** + * @brief 获取当前火控系统锁定的车辆ID + * + * @return const enumeration::CarIDFlag : 火控系统锁定的车辆ID + */ + virtual const enumeration::CarIDFlag GetAttackCarId() const = 0; }; } \ No newline at end of file diff --git a/include/interfaces/identifier.hpp b/include/interfaces/identifier.hpp index 510f56e..db441da 100644 --- a/include/interfaces/identifier.hpp +++ b/include/interfaces/identifier.hpp @@ -6,9 +6,18 @@ #include namespace world_exe::interfaces { + +/** + * @brief 识别器 + */ class IIdentifier { public: - /// return : all armors , all armors id + /** + * @brief 识别传入画面中所有的装甲板及其ID + * + * @param input_image 相机捕捉到的画面 + * @return const std::tuple, enumeration::CarIDFlag> + */ virtual const std::tuple, enumeration::CarIDFlag> identify( const cv::Mat& input_image) = 0; }; diff --git a/include/interfaces/pnp_solver.hpp b/include/interfaces/pnp_solver.hpp index 3c908d2..a340df8 100644 --- a/include/interfaces/pnp_solver.hpp +++ b/include/interfaces/pnp_solver.hpp @@ -3,8 +3,18 @@ #include "interfaces/armor_in_camera.hpp" #include "interfaces/armor_in_image.hpp" namespace world_exe::interfaces { + +/** + * @brief Pnp求解器 + 指一切可以通过2维装甲板求解3维装甲板的算法器 + */ class IPnpSolver { public: + /** + * @brief 求解装甲板的pnp问题 + * @param std::shared_ptr 二维装甲板 + * @return std::shared_ptr 三维装甲板 + */ virtual std::shared_ptr SolvePnp( std::shared_ptr) = 0; }; diff --git a/include/interfaces/predictor.hpp b/include/interfaces/predictor.hpp index f4e34fe..b257638 100644 --- a/include/interfaces/predictor.hpp +++ b/include/interfaces/predictor.hpp @@ -5,9 +5,24 @@ #include namespace world_exe::interfaces { +/** + * @brief 预测器，通常通过某一个时刻的状态向量x 及其状态转移矩阵: x' = Ax + Bu 获取未来状态向量x' + * 继承自这个接口的类一般是Record, 创建后内容不会改变 + */ class IPredictor { public: - virtual const enumeration::ArmorIdFlag& GetId() const = 0; + /** + * @brief 获取当前预测器可以进行预测的所有车辆ID + * + * @return const enumeration::ArmorIdFlag& + */ + virtual const enumeration::ArmorIdFlag& GetId() const = 0; + /** + * @brief 求解出未来某个时间的装甲板集合 + * + * @param time_stamp 未来的某个时间点 + * @return const IArmorInGimbalControl& + */ virtual const IArmorInGimbalControl& Predictor(const std::time_t& time_stamp) const = 0; }; } \ No newline at end of file diff --git a/include/interfaces/predictor_update_package.hpp b/include/interfaces/predictor_update_package.hpp index 5828dfb..026e941 100644 --- a/include/interfaces/predictor_update_package.hpp +++ b/include/interfaces/predictor_update_package.hpp @@ -6,13 +6,29 @@ #include namespace world_exe::interfaces { + +/** + * @brief 给出滤波器需要的所有数据 + * @todo 应该使用结构体，但是先写着无所谓 + */ class IPreDictorUpdatePackage { public: + /** + * @brief 传感器数据获取时的时间戳 + */ COMBINE_TIME_STAMPED; + /** + * @brief 求解好的装甲板三维信息 + * + * @return std::shared_ptr + */ virtual std::shared_ptr GetArmors() const = 0; - /// - /// Affine form image to gimbal_control + /** + * @brief 相机坐标系到世界坐标系的仿射变换 + * + * @return Eigen::Affine3d + */ virtual Eigen::Affine3d GetTransform() const = 0; }; } \ No newline at end of file diff --git a/include/interfaces/sync_block.hpp b/include/interfaces/sync_block.hpp index e44c39c..98e996c 100644 --- a/include/interfaces/sync_block.hpp +++ b/include/interfaces/sync_block.hpp @@ -3,6 +3,11 @@ #include namespace world_exe::interfaces { +/** + * @brief 不必管他 + * + * @tparam T + */ template class ISyncBlock { public: diff --git a/include/interfaces/sync_load.hpp b/include/interfaces/sync_load.hpp index 37b2991..7a6fd35 100644 --- a/include/interfaces/sync_load.hpp +++ b/include/interfaces/sync_load.hpp @@ -6,6 +6,11 @@ namespace world_exe::interfaces { +/** + * @brief 不必管他 + * + * @tparam T + */ template class ISyncLoad { public: diff --git a/include/interfaces/target_predictor.hpp b/include/interfaces/target_predictor.hpp index 01324b3..4340364 100644 --- a/include/interfaces/target_predictor.hpp +++ b/include/interfaces/target_predictor.hpp @@ -3,16 +3,33 @@ #include "armor_in_gimbal_control.hpp" #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" -#include "predictor_update_package.hpp" #include #include namespace world_exe::interfaces { + +/** + * @brief 类似IPredictor但是其内容不保证稳定 + * + */ class ITargetPredictor { public: + /** + * @brief + * + * @param id + * @param time_stamp + * @return std::shared_ptr + */ virtual std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) = 0; - virtual void Update(std::shared_ptr data) = 0; + const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) = 0; + + /** + * @brief 按照传入的id生成IPredictor + * + * @param id + * @return const IPredictor& + */ virtual const IPredictor& GetPredictor(const enumeration::ArmorIdFlag& id) const = 0; }; } \ No newline at end of file diff --git a/include/interfaces/time_stamped.hpp b/include/interfaces/time_stamped.hpp index acc377e..20d5463 100644 --- a/include/interfaces/time_stamped.hpp +++ b/include/interfaces/time_stamped.hpp @@ -5,6 +5,9 @@ #define COMBINE_TIME_STAMPED virtual const ITimeStamped& GetTimeStamped() const = 0 namespace world_exe::interfaces { +/** + * @brief 时间戳，通常是SteadyClock + */ class ITimeStamped { public: virtual const std::time_t& GetTimeStamp() const = 0; diff --git a/src/v1/auto_aim_system_v1.cpp b/src/v1/auto_aim_system_v1.cpp index d149a96..443bb4e 100644 --- a/src/v1/auto_aim_system_v1.cpp +++ b/src/v1/auto_aim_system_v1.cpp @@ -93,7 +93,7 @@ template class SystemV1ImplTemp : public world_exe::v1::SystemV1::System ParamsForSystemV1::car_id_identify_event, // [this](const auto& data) { FLOW_IN(ParamsForSystemV1::car_id_identify_event, enumeration::CarIDFlag) - car_state_->Update(data); + state_machine->Update(data); core::EventBus::Publish( // ParamsForSystemV1::car_tracing_event, car_state_->GetAllowdToFires()); FLOW_OUT(ParamsForSystemV1::car_id_identify_event, enumeration::CarIDFlag) @@ -136,7 +136,7 @@ template class SystemV1ImplTemp : public world_exe::v1::SystemV1::System FLOW_IN( ParamsForSystemV1::tracker_update_event, interfaces::IPreDictorUpdatePackage) fire_control->SetTimeStamp(data->GetTimeStamped().GetTimeStamp()); - tracker_->Update(data); + predictor->Update(data); FLOW_OUT( ParamsForSystemV1::tracker_update_event, interfaces::IPreDictorUpdatePackage) }); diff --git a/src/v1/predictor/predictor_manager.hpp b/src/v1/predictor/predictor_manager.hpp index bf64970..b99d80f 100644 --- a/src/v1/predictor/predictor_manager.hpp +++ b/src/v1/predictor/predictor_manager.hpp @@ -1,19 +1,20 @@ #pragma once +#include "interfaces/predictor_update_package.hpp" #include "interfaces/target_predictor.hpp" namespace world_exe::v1::predictor { -class PredictorManager : public world_exe::interfaces::ITargetPredictor { +class PredictorManager final : public world_exe::interfaces::ITargetPredictor { public: PredictorManager(); ~PredictorManager(); - void Update(std::shared_ptr data) override; + void Update(std::shared_ptr data); virtual std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) override; + const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp); - const interfaces::IPredictor& GetPredictor(const enumeration::ArmorIdFlag& id) const override; + const interfaces::IPredictor& GetPredictor(const enumeration::ArmorIdFlag& id) const; private: class Impl; From 570d10024d869e90410c0cf18e2cec1c4ed93fda Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Fri, 12 Sep 2025 08:38:39 +0800 Subject: [PATCH 17/93] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E9=94=99=E8=AF=AF?= =?UTF-8?q?=E7=9A=84=E5=87=BD=E6=95=B0=E7=AD=BE=E5=90=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit target_predictor --- include/interfaces/predictor.hpp | 3 ++- include/interfaces/target_predictor.hpp | 2 +- src/v1/auto_aim_system_v1.cpp | 4 ++-- src/v1/fire_controller/fire_controller.cpp | 4 ++-- src/v1/predictor/car/car_predictor.cpp | 13 ++++++------- src/v1/predictor/car/car_predictor.hpp | 2 +- src/v1/predictor/predictor_manager.cpp | 12 ++++++------ src/v1/predictor/predictor_manager.hpp | 2 +- 8 files changed, 21 insertions(+), 21 deletions(-) diff --git a/include/interfaces/predictor.hpp b/include/interfaces/predictor.hpp index b257638..68c4fb7 100644 --- a/include/interfaces/predictor.hpp +++ b/include/interfaces/predictor.hpp @@ -23,6 +23,7 @@ class IPredictor { * @param time_stamp 未来的某个时间点 * @return const IArmorInGimbalControl& */ - virtual const IArmorInGimbalControl& Predictor(const std::time_t& time_stamp) const = 0; + virtual std::shared_ptr Predictor( + const std::time_t& time_stamp) const = 0; }; } \ No newline at end of file diff --git a/include/interfaces/target_predictor.hpp b/include/interfaces/target_predictor.hpp index 4340364..70492cb 100644 --- a/include/interfaces/target_predictor.hpp +++ b/include/interfaces/target_predictor.hpp @@ -30,6 +30,6 @@ class ITargetPredictor { * @param id * @return const IPredictor& */ - virtual const IPredictor& GetPredictor(const enumeration::ArmorIdFlag& id) const = 0; + virtual std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const = 0; }; } \ No newline at end of file diff --git a/src/v1/auto_aim_system_v1.cpp b/src/v1/auto_aim_system_v1.cpp index 443bb4e..7e93773 100644 --- a/src/v1/auto_aim_system_v1.cpp +++ b/src/v1/auto_aim_system_v1.cpp @@ -1,8 +1,8 @@ #include "./auto_aim_system_v1.hpp" +#include "core/event_bus.hpp" #include "data/fire_control.hpp" #include "data/sync_data.hpp" #include "enum/armor_id.hpp" -#include "event_bus.hpp" #include "fire_controller/fire_controller.hpp" #include "identifier/identifier.hpp" #include "interfaces/armor_in_camera.hpp" @@ -157,7 +157,7 @@ template class SystemV1ImplTemp : public world_exe::v1::SystemV1::System [this](const auto& data) { FLOW_IN(ParamsForSystemV1::get_lastest_predictor_event, enumeration::CarIDFlag) if (data != enumeration::CarIDFlag::None) - core::EventBus::Publish( + core::EventBus::Publish>( ParamsForSystemV1::get_lastest_predictor_event, tracker_->GetPredictor(data)); FLOW_OUT(ParamsForSystemV1::get_lastest_predictor_event, enumeration::CarIDFlag) diff --git a/src/v1/fire_controller/fire_controller.cpp b/src/v1/fire_controller/fire_controller.cpp index d508a17..54a16b4 100644 --- a/src/v1/fire_controller/fire_controller.cpp +++ b/src/v1/fire_controller/fire_controller.cpp @@ -48,7 +48,7 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { const world_exe::data::FireControl CalculateTarget(const std::time_t& time_duration) { time_t fly_time = 0; const auto& pre1 = predictor_->Predictor(fly_time + time_duration + control_delay_); - const auto& pre2 = pre1.GetArmors(predictor_->GetId()); + const auto& pre2 = pre1->GetArmors(predictor_->GetId()); double min_angular_dis = 1e9; int index = -1, index_ = 0; for (const auto vec : pre2) { @@ -67,7 +67,7 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { for (int i = 5; i-- > 0;) { const auto& armors_in_gimbal_control = predictor_->Predictor(fly_time + time_duration + control_delay_); - const auto& armors = armors_in_gimbal_control.GetArmors(predictor_->GetId()); + const auto& armors = armors_in_gimbal_control->GetArmors(predictor_->GetId()); const auto& [fly_time, dir] = trajectory_solver::gravity_only(armors[index].position, velocity_begin_, gravity_); } diff --git a/src/v1/predictor/car/car_predictor.cpp b/src/v1/predictor/car/car_predictor.cpp index 7d503c1..cc67c1f 100644 --- a/src/v1/predictor/car/car_predictor.cpp +++ b/src/v1/predictor/car/car_predictor.cpp @@ -17,11 +17,12 @@ class CarPredictor::Impl { const enumeration::ArmorIdFlag& GetId() const { return id_; } - const interfaces::IArmorInGimbalControl& Predictor(const std::time_t& time_stamp) { - target_armors_.SetWithSingleId(ekf_.get_predict_output_armor(id_, - (time_stamp - create_time_stamp_.GetTimeStamp()) / 1.e9), + std::shared_ptr Predictor(const std::time_t& time_stamp) { + auto ptr = std::make_shared(); + ptr->SetWithSingleId(ekf_.get_predict_output_armor( + id_, (time_stamp - create_time_stamp_.GetTimeStamp()) / 1.e9), time_stamp); - return target_armors_; + return ptr; } inline void SetId(const enumeration::CarIDFlag& id) { id_ = id; } @@ -36,8 +37,6 @@ class CarPredictor::Impl { enumeration::CarIDFlag id_; CarPredictEkf ekf_; PredictTimeStamp create_time_stamp_; - - PredictArmorInGimbalControl target_armors_; }; CarPredictor::CarPredictor() @@ -52,7 +51,7 @@ CarPredictor::~CarPredictor() = default; const enumeration::ArmorIdFlag& CarPredictor::GetId() const { return pimpl_->GetId(); } -const interfaces::IArmorInGimbalControl& CarPredictor::Predictor( +std::shared_ptr CarPredictor::Predictor( const std::time_t& time_stamp) const { return pimpl_->Predictor(time_stamp); } diff --git a/src/v1/predictor/car/car_predictor.hpp b/src/v1/predictor/car/car_predictor.hpp index 22889fa..acebd0b 100644 --- a/src/v1/predictor/car/car_predictor.hpp +++ b/src/v1/predictor/car/car_predictor.hpp @@ -17,7 +17,7 @@ class CarPredictor : public interfaces::IPredictor { void SetTimeStamp(const interfaces::ITimeStamped& time_stamp); const enumeration::ArmorIdFlag& GetId() const override; - const interfaces::IArmorInGimbalControl& Predictor( + std::shared_ptr Predictor( const std::time_t& time_stamp) const override; private: diff --git a/src/v1/predictor/predictor_manager.cpp b/src/v1/predictor/predictor_manager.cpp index c2a004b..e964dd2 100644 --- a/src/v1/predictor/predictor_manager.cpp +++ b/src/v1/predictor/predictor_manager.cpp @@ -90,10 +90,11 @@ class PredictorManager::Impl { return std::make_shared(armors, last_update_time_stamp_); }; - const interfaces::IPredictor& GetPredictor(const enumeration::ArmorIdFlag& id) { - predictor.SetId(id); - predictor.SetEkf(predictors_[util::enumeration::GetIndex(id)]); - predictor.SetTimeStamp(last_update_time_stamp_); + std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) { + auto predictor = std::make_shared(); + predictor->SetId(id); + predictor->SetEkf(predictors_[util::enumeration::GetIndex(id)]); + predictor->SetTimeStamp(last_update_time_stamp_); return predictor; } @@ -103,7 +104,6 @@ class PredictorManager::Impl { Eigen::Affine3d transform_from_camera_to_gimbal_; PredictArmorInGimbalControl predictted_armors_; - CarPredictor predictor; }; PredictorManager::PredictorManager() @@ -120,7 +120,7 @@ void PredictorManager::Update(std::shared_ptrUpdate(data); }; -const interfaces::IPredictor& PredictorManager::GetPredictor( +std::shared_ptr PredictorManager::GetPredictor( const enumeration::ArmorIdFlag& id) const { return pimpl_->GetPredictor(id); }; diff --git a/src/v1/predictor/predictor_manager.hpp b/src/v1/predictor/predictor_manager.hpp index b99d80f..77b53f6 100644 --- a/src/v1/predictor/predictor_manager.hpp +++ b/src/v1/predictor/predictor_manager.hpp @@ -14,7 +14,7 @@ class PredictorManager final : public world_exe::interfaces::ITargetPredictor { virtual std::shared_ptr Predict( const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp); - const interfaces::IPredictor& GetPredictor(const enumeration::ArmorIdFlag& id) const; + std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const; private: class Impl; From 7fe53dff2f4302b7f51ea22f8f03e69a2bf1e90f Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Sep 2025 01:33:04 +0800 Subject: [PATCH 18/93] feat: add state machine --- .../state_machine/car_state_manager.hpp | 55 +++++++++++++++++++ src/tongji/state_machine/state_machine.cpp | 40 ++++++++++++++ src/tongji/state_machine/state_machine.hpp | 24 ++++++++ 3 files changed, 119 insertions(+) create mode 100644 src/tongji/state_machine/car_state_manager.hpp create mode 100644 src/tongji/state_machine/state_machine.cpp create mode 100644 src/tongji/state_machine/state_machine.hpp diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp new file mode 100644 index 0000000..da80ff9 --- /dev/null +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include + +namespace world_exe::tongji::car_state { + +class CarStateManager { +public: + CarStateManager(int switch_threshold = 5) + : switch_threshold_(switch_threshold) { } + + void Update(bool detected, std::chrono::steady_clock::time_point now) { + if (detected) { + count_ = std::min(count_ + 1, switch_threshold_); + last_seen_ = now; + } else { + count_ = std::max(count_ - 1, 0); + } + is_locked_ = (count_ >= switch_threshold_); + } + + bool IsLocked() const { return is_locked_; } + bool IsConverged() const { return is_converged_; } + bool IsDiverged() const { return is_diverged_; } + + void SetPriority(int p); + void SetThreshold(const int& value) { switch_threshold_ = value; } + void SetDiverged(bool diverged) { + is_diverged_ = diverged; + if (diverged) is_converged_ = false; + } + void SetPriority(const int& p) { priority_ = p; } + int GetPriority() const { return priority_; } + + std::chrono::steady_clock::time_point LastSeen() const { return last_seen_; } + + void IncrementUpdateCount() { + update_count_++; + if (update_count_ > 3 && !is_diverged_) { + is_converged_ = true; + } + } + +private: + int count_ = 0; + int switch_threshold_; + int update_count_ = 0; + + bool is_locked_ = false; + bool is_converged_ = false; + bool is_diverged_ = false; + int priority_ = 100; // 默认最低优先级 + std::chrono::steady_clock::time_point last_seen_; +}; +} \ No newline at end of file diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp new file mode 100644 index 0000000..918adb3 --- /dev/null +++ b/src/tongji/state_machine/state_machine.cpp @@ -0,0 +1,40 @@ +#include "state_machine.hpp" + +#include "enum/car_id.hpp" + +namespace world_exe::tongji::state_machine { + +StateMachine::StateMachine(int switch_threshold) + : switch_threshold_(switch_threshold) { + for (auto& manager : car_states_) { + manager = car_state::CarStateManager(switch_threshold_); + } +} + +const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return tracing_state_; } + +const interfaces::ICarState& StateMachine::Update( + const enumeration::CarIDFlag& car_detected, std::chrono::steady_clock::time_point now) { + tracing_state_ = enumeration::CarIDFlag::None; + + for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { + auto id = static_cast(1 << i); + bool detected = (static_cast(car_detected) >> i) & 0x01; + + car_states_[i].Update(detected, now); + + if (detected) { + car_states_[i].IncrementUpdateCount(); + } + + if (car_states_[i].IsLocked() && car_states_[i].IsConverged() + && !car_states_[i].IsDiverged()) { + tracing_state_ = + static_cast(static_cast(tracing_state_) | (1 << i)); + } + } + + return *this; +} + +} diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp new file mode 100644 index 0000000..f6b55c9 --- /dev/null +++ b/src/tongji/state_machine/state_machine.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include + +#include "car_state_manager.hpp" +#include "enum/car_id.hpp" +#include "interfaces/car_state.hpp" + +namespace world_exe::tongji::state_machine { +class StateMachine final : public interfaces::ICarState { +public: + StateMachine(int switch_threshold = 5); + + const enumeration::CarIDFlag& GetAllowdToFires() const override; + + const interfaces::ICarState& Update( + const enumeration::CarIDFlag& car_detected, std::chrono::steady_clock::time_point now); + +private: + enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; + std::array car_states_; + int switch_threshold_; +}; +} \ No newline at end of file From 244d7c57b789deec2626e59b9c73e84be572eb9b Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Sep 2025 21:02:48 +0800 Subject: [PATCH 19/93] feat: Add predictor module --- src/tongji/identifier/classifier.hpp | 2 +- src/tongji/predictor/car_predictor.hpp | 420 ++++++++++++++++++ src/tongji/predictor/car_predictor_boss.cpp | 56 +++ src/tongji/predictor/car_predictor_boss.hpp | 23 + .../predictor/car_predictor_manager.cpp | 134 ++++++ .../predictor/car_predictor_manager.hpp | 26 ++ .../predictor/in_gimbal_control_armor.hpp | 59 +++ src/tongji/predictor/time_stamp.hpp | 24 + .../state_machine/car_state_manager.hpp | 15 +- src/tongji/state_machine/state_machine.cpp | 2 +- src/tongji/state_machine/state_machine.hpp | 4 +- src/util/extended_kalman_filter.hpp | 115 +++++ src/util/math.hpp | 26 ++ 13 files changed, 898 insertions(+), 8 deletions(-) create mode 100644 src/tongji/predictor/car_predictor.hpp create mode 100644 src/tongji/predictor/car_predictor_boss.cpp create mode 100644 src/tongji/predictor/car_predictor_boss.hpp create mode 100644 src/tongji/predictor/car_predictor_manager.cpp create mode 100644 src/tongji/predictor/car_predictor_manager.hpp create mode 100644 src/tongji/predictor/in_gimbal_control_armor.hpp create mode 100644 src/tongji/predictor/time_stamp.hpp create mode 100644 src/util/extended_kalman_filter.hpp diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp index 175c534..b83bbac 100644 --- a/src/tongji/identifier/classifier.hpp +++ b/src/tongji/identifier/classifier.hpp @@ -20,7 +20,7 @@ class Classifier final { compiled_model_ = core_.compile_model( ovmodel, "AUTO", ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY)); } -// + void Classify(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag& armor_id, double& armor_confidence) { if (armor_pattern.empty()) { diff --git a/src/tongji/predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor.hpp new file mode 100644 index 0000000..4ce3fbe --- /dev/null +++ b/src/tongji/predictor/car_predictor.hpp @@ -0,0 +1,420 @@ +#pragma once + +#include +#include + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "interfaces/predictor_update_package.hpp" +#include "tongji/predictor/time_stamp.hpp" +#include "util/extended_kalman_filter.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { + +struct PredictorStatus { + bool jumped = false; + bool switched = false; + bool converged = false; + bool diverged = false; + bool lost = false; + bool reidentified = false; + int switch_count = 0; + int update_count = 0; + int lost_count = 0; +}; + +class CarPredictor { +public: + CarPredictor() = default; + explicit CarPredictor(const enumeration::ArmorIdFlag armor_id, + const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, + std::time_t time_stamp, double radius, int armor_num, Eigen::VectorXd P0_dig) + : armor_id_(armor_id) + , last_id(0) + , radius_(radius) + , armor_num_(armor_num) + , last_predict_time_stamp_(time_stamp) { + const Eigen::VectorXd& xyz = armor_xyz_in_world; + const Eigen::VectorXd& ypr = armor_ypr_in_world; + + // 旋转中心的坐标 + auto center_x = xyz[0] + radius * std::cos(ypr[0]); + auto center_y = xyz[1] + radius * std::sin(ypr[0]); + auto center_z = xyz[2]; + + // x vx y vy z vz a w r l h + // a: angle + // w: angular velocity + // l: r2 - r1 + // h: z2 - z1 + Eigen::VectorXd x0 { { center_x, 0, center_y, 0, center_z, 0, ypr[0], 0, radius, 0, + 0 } }; // 初始化预测量 + P0_dig_ = P0_dig; + Eigen::MatrixXd P0 = P0_dig.asDiagonal(); + + // 防止夹角求和出现异常值 + x_add_ = [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) -> Eigen::VectorXd { + Eigen::VectorXd c = a + b; + c[6] = util::math::clamp_pm_pi(c[6]); + return c; + }; + + ekf_ = util::ExtendedKalmanFilter(x0, P0, x_add_); // 初始化滤波器（预测量、预测量协方差） + } + + void SetEkf(const util::ExtendedKalmanFilter& ekf) { ekf_ = ekf; } + + std::vector GetPredictedArmors( + enumeration::ArmorIdFlag armor_id, const TimeStamp time_stamp) { + PredictTo(time_stamp.GetTimeStamp()); + UpdateStatus(); + + // 装甲板匹配 + int id; + auto min_angle_error = 1e10; + const std::vector& xyza_list = armor_xyza_list(); + + std::vector> xyza_i_list; + for (int i = 0; i < armor_num_; i++) { + xyza_i_list.push_back({ xyza_list[i], i }); + } + + std::sort(xyza_i_list.begin(), xyza_i_list.end(), + [](const std::pair& a, const std::pair& b) { + Eigen::Vector3d ypd1 = util::math::xyz2ypd(a.first.head(3)); + Eigen::Vector3d ypd2 = util::math::xyz2ypd(b.first.head(3)); + return ypd1[2] < ypd2[2]; + }); + + std::vector armors; + for (int i = 0; i < armor_num_; i++) { + xyza_i_list.push_back({ xyza_list[i], i }); + const auto pos = xyza_i_list[i].first.head<3>(); + const auto yaw = util::math::clamp_pm_pi(ekf_.x[6]); + const double pitch = 15. / 180. * std::numbers::pi; + armors.push_back(data::ArmorGimbalControlSpacing { + armor_id_, pos, util::math::euler_to_quaternion(yaw, pitch, 0.) }); + } + return armors; + } + + void Update(std::shared_ptr data) { + const auto& transform = data->GetTransform(); + const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); + const auto armors = data->GetArmors()->GetArmors(armor_id_); + + if (armors.empty()) return; + for (auto armor : armors) { + const auto& armor_in_world_position = transform * armor.position; + const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; + this->update(armor_in_world_position, + util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), + util::math::xyz2ypd(armor_in_world_position)); + } + } + + const PredictorStatus& GetStatus() const { return status_; } + + void PredictTo(const TimeStamp& now) { + if (last_predict_time_stamp_.GetTimeStamp() == 0) { + last_predict_time_stamp_ = now; + return; + } + const double& dt = now.SecondsSince(last_predict_time_stamp_); + last_predict_time_stamp_ = now; + + if (dt <= 0.0 || dt > 1.0) { + // util::logger::logger()->warn( + // "[CarPredictor] Abnormal dt: {:.3f}, skipping prediction", dt); + return; + } + + predict(dt); + } + + void Reset(const Eigen::Vector3d& xyz, const Eigen::Vector3d& ypr, std::time_t time_stamp) { + last_predict_time_stamp_ = TimeStamp::FromRaw(time_stamp); + status_ = PredictorStatus {}; // 清空状态计数器 + + // 重新构造滤波器状态 + auto center_x = xyz[0] + radius_ * std::cos(ypr[0]); + auto center_y = xyz[1] + radius_ * std::sin(ypr[0]); + auto center_z = xyz[2]; + + Eigen::VectorXd x0 { { center_x, 0, center_y, 0, center_z, 0, ypr[0], 0, radius_, 0, 0 } }; + Eigen::MatrixXd P0 = P0_dig_.asDiagonal(); + + ekf_ = util::ExtendedKalmanFilter(x0, P0_dig_, x_add_); + } + + void TickStatus() { + static int last_update_count = -1; + if (status_.update_count == last_update_count) { + status_.lost_count++; + } else { + status_.lost_count = 0; + } + last_update_count = status_.update_count; + status_.lost = (status_.lost_count > 30 || IsDiverged()); + status_.reidentified = (!status_.lost && IsConverged() && status_.update_count > 3); + } + + bool IsLocked() const { return status_.converged && !status_.diverged; } + bool IsLost() const { return status_.lost; } + bool IsReidentified() const { return status_.reidentified; } + +private: + bool IsConverged() const { return status_.converged; } + bool IsDiverged() const { return status_.diverged; } + + int MatchArmor(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { + + const auto& xyza_list = armor_xyza_list(); + std::vector> xyza_i_list; + + for (int i = 0; i < armor_num_; ++i) { + xyza_i_list.emplace_back(xyza_list[i], i); + } + + std::sort(xyza_i_list.begin(), xyza_i_list.end(), [](const auto& a, const auto& b) { + auto ypd1 = util::math::xyz2ypd(a.first.head(3)); + auto ypd2 = util::math::xyz2ypd(b.first.head(3)); + return ypd1[2] < ypd2[2]; + }); + + int best_id = 0; + double min_error = 1e10; + + for (int i = 0; i < std::min(3, armor_num_); ++i) { + const auto& xyza = xyza_i_list[i].first; + auto ypd = util::math::xyz2ypd(xyza.head(3)); + double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_world[0] - xyza[3])) + + std::abs(util::math::clamp_pm_pi(armor_ypd_in_world[0] - ypd[0])); + + if (error < min_error) { + min_error = error; + best_id = xyza_i_list[i].second; + } + } + + return best_id; + } + + auto F(double dt) const -> Eigen::MatrixXd { + // 状态转移矩阵 + // clang-format off + return Eigen::MatrixXd { + {1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1} + }; + // clang-format on + } + + auto Q(double dt) const -> Eigen::MatrixXd { + // Piecewise White Noise Model + // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb + double v1, v2; + if (armor_id_ == enumeration::CarIDFlag::Outpost) { + v1 = 10; // 前哨站加速度方差 + v2 = 0.1; // 前哨站角加速度方差 + } else { + v1 = 100; // 加速度方差 + v2 = 400; // 角加速度方差 + } + auto a = dt * dt * dt * dt / 4; + auto b = dt * dt * dt / 2; + auto c = dt * dt; + // 预测过程噪声偏差的方差 + // clang-format off + return Eigen::MatrixXd { + {a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + {b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + { 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0}, + { 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0}, + { 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0}, + { 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0}, + { 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0}, + { 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0}, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, + { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} + }; + // clang-format on + } + + void predict(const double& dt) { + // 防止夹角求和出现异常值 + auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + Eigen::VectorXd x_prior = this->F(dt) * x; + x_prior[6] = util::math::clamp_pm_pi(x_prior[6]); + return x_prior; + }; + + // 前哨站转速特判 + if (IsConverged() && this->armor_id_ == enumeration::CarIDFlag::Outpost + && std::abs(this->ekf_.x[7]) > 2) + this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; + + ekf_.predict(this->F(dt), this->Q(dt), f); + } + + void UpdateStatus() { + // 发散判断 + double r = ekf_.x[8]; + double l = ekf_.x[9]; + status_.diverged = !(r > 0.05 && r < 0.5 && r + l > 0.05 && r + l < 0.5); + + // 收敛判断 + if (armor_id_ == enumeration::CarIDFlag::Outpost) { + status_.converged = (status_.update_count > 10 && !status_.diverged); + } else { + status_.converged = (status_.update_count > 3 && !status_.diverged); + } + } + + void update(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { + // 装甲板匹配 + int id = MatchArmor(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); + + if (id != 0) status_.jumped = true; + status_.switched = (id != last_id); + if (status_.switched) status_.switch_count++; + + last_id = id; + update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); + status_.update_count++; + UpdateStatus(); + } + + void update_ypda(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, + int id) { + // 观测jacobi + Eigen::MatrixXd H = h_jacobian(ekf_.x, id); + // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; + auto center_yaw = std::atan2(armor_xyz_in_world[1], armor_xyz_in_world[0]); + auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world[0] - center_yaw); + Eigen::VectorXd R_dig { { 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, + log(std::abs(armor_ypd_in_world[2]) + 1) / 200 + 9e-2 } }; + + // 测量过程噪声偏差的方差 + Eigen::MatrixXd R = R_dig.asDiagonal(); + + // 定义非线性转换函数h: x -> z + auto h = [&](const Eigen::VectorXd& x) -> Eigen::Vector4d { + Eigen::VectorXd xyz = h_armor_xyz(x, id); + Eigen::VectorXd ypd = util::math::xyz2ypd(xyz); + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); + return { ypd[0], ypd[1], ypd[2], angle }; + }; + + // 防止夹角求差出现异常值 + auto z_subtract = [](const Eigen::VectorXd& a, + const Eigen::VectorXd& b) -> Eigen::VectorXd { + Eigen::VectorXd c = a - b; + c[0] = util::math::clamp_pm_pi(c[0]); + c[1] = util::math::clamp_pm_pi(c[1]); + c[3] = util::math::clamp_pm_pi(c[3]); + return c; + }; + + const Eigen::VectorXd& ypd = armor_ypd_in_world; + const Eigen::VectorXd& ypr = armor_ypr_in_world; + Eigen::VectorXd z { { ypd[0], ypd[1], ypd[2], ypr[0] } }; // 获得观测量 + + ekf_.update(z, H, R, h, z_subtract); + } + + Eigen::VectorXd ekf_x() const { return ekf_.x; } + + const util::ExtendedKalmanFilter& ekf() const { return ekf_; } + + std::vector armor_xyza_list() const { + std::vector _armor_xyza_list; + + for (int i = 0; i < armor_num_; i++) { + auto angle = util::math::clamp_pm_pi(ekf_.x[6] + i * 2 * CV_PI / armor_num_); + Eigen::Vector3d xyz = h_armor_xyz(ekf_.x, i); + _armor_xyza_list.push_back({ xyz[0], xyz[1], xyz[2], angle }); + } + return _armor_xyza_list; + } + + // 计算出装甲板中心的坐标（考虑长短轴） + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto armor_x = x[0] - r * std::cos(angle); + auto armor_y = x[2] - r * std::sin(angle); + auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; + + return { armor_x, armor_y, armor_z }; + } + + Eigen::MatrixXd h_jacobian(const Eigen::VectorXd& x, int id) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto dx_da = r * std::sin(angle); + auto dy_da = -r * std::cos(angle); + + auto dx_dr = -std::cos(angle); + auto dy_dr = -std::sin(angle); + auto dx_dl = (use_l_h) ? -std::cos(angle) : 0.0; + auto dy_dl = (use_l_h) ? -std::sin(angle) : 0.0; + + auto dz_dh = (use_l_h) ? 1.0 : 0.0; + + // clang-format off + Eigen::MatrixXd H_armor_xyza{ + {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, + {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, + {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, + {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} + }; + // clang-format on + + Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); + Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); + // clang-format off + Eigen::MatrixXd H_armor_ypda{ + {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, + {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, + {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, + { 0, 0, 0, 1} + }; + // clang-format on + + return H_armor_ypda * H_armor_xyza; + } + + int armor_num_; + double radius_; + PredictorStatus status_; + + enumeration::ArmorIdFlag armor_id_; + int last_id = -1; // debug only + + std::function x_add_; + Eigen::VectorXd P0_dig_; + util::ExtendedKalmanFilter ekf_; + TimeStamp last_predict_time_stamp_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor_boss.cpp b/src/tongji/predictor/car_predictor_boss.cpp new file mode 100644 index 0000000..af82658 --- /dev/null +++ b/src/tongji/predictor/car_predictor_boss.cpp @@ -0,0 +1,56 @@ +#include "car_predictor_boss.hpp" + +#include +#include +#include + +#include "enum/car_id.hpp" +#include "enum/enum_tools.hpp" +#include "tongji/predictor/car_predictor_manager.hpp" + +namespace world_exe::tongji::predictor { +class CarPredictorBoss::Impl { +public: + Impl(std::shared_ptr manager) + : manager_(manager) { + if (!manager_) { + throw std::invalid_argument("CarPredictorManager must not be null"); + } + } + + std ::shared_ptr Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { + for (int i = 0; i < 8; ++i) { + auto car_id = static_cast( + static_cast(enumeration::CarIDFlag::Hero) << i); + + if (enumeration::IsFlagContains(id, car_id)) { + manager_->SetPredictorBySingleID(car_id, time_stamp); + } + } + return manager_->Predictor(time_stamp); + } + + std ::shared_ptr GetPredictor( + const enumeration ::ArmorIdFlag& id) const { + return manager_; + } + +private: + std::shared_ptr manager_; +}; + +std ::shared_ptr CarPredictorBoss::Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { + return pimpl_->Predict(id, time_stamp); +} + +std ::shared_ptr CarPredictorBoss::GetPredictor( + const enumeration ::ArmorIdFlag& id) const { + return pimpl_->GetPredictor(id); +} + +CarPredictorBoss::CarPredictorBoss(std::shared_ptr manager) + : pimpl_(std::make_unique(manager)) { } + +} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor_boss.hpp b/src/tongji/predictor/car_predictor_boss.hpp new file mode 100644 index 0000000..35b7198 --- /dev/null +++ b/src/tongji/predictor/car_predictor_boss.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include + +#include "interfaces/target_predictor.hpp" +#include "tongji/predictor/car_predictor_manager.hpp" + +namespace world_exe::tongji::predictor { +class CarPredictorBoss final : public interfaces::ITargetPredictor { +public: + CarPredictorBoss(std::shared_ptr manager); + + std ::shared_ptr GetPredictor( + const enumeration ::ArmorIdFlag& id) const override; + + std ::shared_ptr Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; + +private: + class Impl; + std::unique_ptr pimpl_; +}; +} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor_manager.cpp new file mode 100644 index 0000000..1572cde --- /dev/null +++ b/src/tongji/predictor/car_predictor_manager.cpp @@ -0,0 +1,134 @@ +#include "car_predictor_manager.hpp" + +#include +#include +#include +#include + +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "tongji/predictor/car_predictor.hpp" +#include "tongji/predictor/in_gimbal_control_armor.hpp" +#include "tongji/predictor/time_stamp.hpp" + +namespace world_exe::tongji::predictor { +class CarPredictorManager::Impl { +public: + Impl() = default; + + const enumeration ::CarIDFlag& GetId() { + if (ids_dirty_) { + ids_ = enumeration::CarIDFlag::None; + for (const auto& [id, _] : predictors_) { + ids_ = static_cast( + static_cast(ids_) | static_cast(id)); + } + ids_dirty_ = false; + } + return ids_; + } + + auto Predictor(const std ::time_t& time_stamp) { + InGimbalControlArmor target_armors; + for (auto& predictor : predictors_) { + target_armors.Add( + predictor.first, predictor.second.GetPredictedArmors(predictor.first, time_stamp)); + } + return std::make_shared(target_armors); + } + + void Tick(const TimeStamp& now) { + std::vector to_remove; + + for (auto& [id, predictor] : predictors_) { + predictor.PredictTo(now); + predictor.TickStatus(); + + if (predictor.IsLost()) { + to_remove.push_back(id); + } + } + // logger()->info("Removing lost predictor: {}", static_cast(id)); + for (auto id : to_remove) { + predictors_.erase(id); + } + } + + void SetPredictorBySingleID(const enumeration::CarIDFlag& id, + const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, + std::time_t init_time_stamp) { + + auto it = predictors_.find(id); + if (it != predictors_.end()) { + it->second.Reset(armor_xyz_in_world, armor_ypr_in_world, init_time_stamp); + } else { + bool is_balance = (id == enumeration::CarIDFlag::InfantryIII + | id == enumeration::CarIDFlag::InfantryIV + | id == enumeration::CarIDFlag::InfantryV); + + Eigen::VectorXd P0_dig(11); + double radius; + int armor_num; + + if (is_balance) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 2; + } else if (id == enumeration::CarIDFlag::Outpost) { + P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; + radius = 0.2765; + armor_num = 3; + } else if (id == enumeration::CarIDFlag::Base) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; + radius = 0.3205; + armor_num = 3; + } else { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 4; + } + + predictors_.emplace(id, + CarPredictor { id, armor_xyz_in_world, armor_ypr_in_world, init_time_stamp, radius, + armor_num, P0_dig }); + ids_dirty_ = true; + } + } + + void RemovePredictorBySingleID(const enumeration::CarIDFlag& id) { + predictors_.erase(id); + ids_dirty_ = true; + } + + bool HasPredictor(const enumeration::CarIDFlag& id) const { + return predictors_.find(id) != predictors_.end(); + } + +private: + enumeration::CarIDFlag ids_ { enumeration::CarIDFlag::None }; + std::unordered_map predictors_; + mutable bool ids_dirty_ = true; +}; + +const enumeration ::ArmorIdFlag& CarPredictorManager::GetId() const { return pimpl_->GetId(); } + +std::shared_ptr CarPredictorManager::Predictor( + const std ::time_t& time_stamp) const { + return pimpl_->Predictor(time_stamp); +} + +void CarPredictorManager::SetPredictorBySingleID(const enumeration::CarIDFlag& id, + Eigen::Vector3d armor_xyz_in_world, Eigen::Vector3d armor_ypr_in_world, time_t time_stamp) { + return pimpl_->SetPredictorBySingleID(id, armor_xyz_in_world, armor_ypr_in_world, time_stamp); +} + +void CarPredictorManager::SetPredictorBySingleID( + const enumeration::CarIDFlag& id, time_t time_stamp) { + return pimpl_->SetPredictorBySingleID( + id, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), time_stamp); +} + +CarPredictorManager::CarPredictorManager() + : pimpl_(std::make_unique()) { } + +} diff --git a/src/tongji/predictor/car_predictor_manager.hpp b/src/tongji/predictor/car_predictor_manager.hpp new file mode 100644 index 0000000..149a223 --- /dev/null +++ b/src/tongji/predictor/car_predictor_manager.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include + +#include "enum/car_id.hpp" +#include "interfaces/predictor.hpp" + +namespace world_exe::tongji::predictor { +class CarPredictorManager final : public interfaces::IPredictor { +public: + CarPredictorManager(); + const enumeration ::ArmorIdFlag& GetId() const override; + + std::shared_ptr Predictor( + const std ::time_t& time_stamp) const override; + + void SetPredictorBySingleID(const enumeration::CarIDFlag& id, + Eigen::Vector3d armor_xyz_in_world, Eigen::Vector3d armor_ypr_in_world, time_t time_stamp); + + void SetPredictorBySingleID(const enumeration::CarIDFlag& id, time_t time_stamp); + +private: + class Impl; + std::unique_ptr pimpl_; +}; +} \ No newline at end of file diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp new file mode 100644 index 0000000..9f9fdae --- /dev/null +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/car_id.hpp" +#include "interfaces/armor_in_gimbal_control.hpp" +#include "tongji/predictor/time_stamp.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::predictor { + +class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { +public: + InGimbalControlArmor() = default; + InGimbalControlArmor(const std::array, 8>& armors, + TimeStamp time_stamp) + : armors_(armors) + , time_stamp_(time_stamp) { } + + const std ::vector& GetArmors( + const enumeration ::ArmorIdFlag& armor_id) const override { + auto armor_id_index = util::enumeration::GetIndex(armor_id); + return armors_.at(armor_id_index); + } + + void Set(const std::array, 8>& armors, + const predictor::TimeStamp& predict_time_stamp) { + armors_ = armors; + time_stamp_ = predict_time_stamp; + } + + void Add( + enumeration::CarIDFlag car_id, const std::vector& armors) { + auto& target_vector = armors_.at(util::enumeration::GetIndex(car_id)); + target_vector.insert(target_vector.end(), armors.begin(), armors.end()); + } + + void Clear() { + for (auto& vec : armors_) { + vec.clear(); + } + } + + void SetArmorsByCarId( + const std::vector& armors, TimeStamp time_stamp) { + if (armors.empty()) return; + armors_[util::enumeration::GetIndex(armors.at(0).id)] = armors; + } + + const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } + + void SetTimeStamp(const TimeStamp time_stamp) { time_stamp_ = time_stamp; } + +private: + TimeStamp time_stamp_; + std::array, 8> armors_; +}; +} \ No newline at end of file diff --git a/src/tongji/predictor/time_stamp.hpp b/src/tongji/predictor/time_stamp.hpp new file mode 100644 index 0000000..ebcbc26 --- /dev/null +++ b/src/tongji/predictor/time_stamp.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include "interfaces/time_stamped.hpp" + +namespace world_exe::tongji::predictor { +class TimeStamp : public interfaces::ITimeStamped { +public: + TimeStamp() = default; + TimeStamp(const std::time_t& time_stamp) + : time_stamp_(time_stamp) { } + + static TimeStamp FromRaw(std::time_t raw) { return TimeStamp(raw); } + + inline void SetTimeStamp(const time_t& time_stamp) { time_stamp_ = time_stamp; } + + double SecondsSince(const TimeStamp& other) const { + return std::difftime(time_stamp_, other.time_stamp_); + } + const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + +private: + std::time_t time_stamp_; +}; +} \ No newline at end of file diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index da80ff9..8dca5eb 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -1,6 +1,8 @@ #pragma once -#include +#include + +#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::car_state { @@ -9,9 +11,10 @@ class CarStateManager { CarStateManager(int switch_threshold = 5) : switch_threshold_(switch_threshold) { } - void Update(bool detected, std::chrono::steady_clock::time_point now) { + void Update(bool detected, std::time_t now_raw) { if (detected) { count_ = std::min(count_ + 1, switch_threshold_); + auto now = predictor::TimeStamp::FromRaw(now_raw); last_seen_ = now; } else { count_ = std::max(count_ - 1, 0); @@ -19,6 +22,10 @@ class CarStateManager { is_locked_ = (count_ >= switch_threshold_); } + bool IsLost(const predictor::TimeStamp& now, double timeout_sec) const { + return now.SecondsSince(last_seen_) > timeout_sec; + } + bool IsLocked() const { return is_locked_; } bool IsConverged() const { return is_converged_; } bool IsDiverged() const { return is_diverged_; } @@ -32,7 +39,7 @@ class CarStateManager { void SetPriority(const int& p) { priority_ = p; } int GetPriority() const { return priority_; } - std::chrono::steady_clock::time_point LastSeen() const { return last_seen_; } + predictor::TimeStamp LastSeen() const { return last_seen_; } void IncrementUpdateCount() { update_count_++; @@ -50,6 +57,6 @@ class CarStateManager { bool is_converged_ = false; bool is_diverged_ = false; int priority_ = 100; // 默认最低优先级 - std::chrono::steady_clock::time_point last_seen_; + predictor::TimeStamp last_seen_; }; } \ No newline at end of file diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 918adb3..442148d 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -14,7 +14,7 @@ StateMachine::StateMachine(int switch_threshold) const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return tracing_state_; } const interfaces::ICarState& StateMachine::Update( - const enumeration::CarIDFlag& car_detected, std::chrono::steady_clock::time_point now) { + const enumeration::CarIDFlag& car_detected, std::time_t now) { tracing_state_ = enumeration::CarIDFlag::None; for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index f6b55c9..8fb9872 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include #include "car_state_manager.hpp" #include "enum/car_id.hpp" @@ -14,7 +14,7 @@ class StateMachine final : public interfaces::ICarState { const enumeration::CarIDFlag& GetAllowdToFires() const override; const interfaces::ICarState& Update( - const enumeration::CarIDFlag& car_detected, std::chrono::steady_clock::time_point now); + const enumeration::CarIDFlag& car_detected, std::time_t now); private: enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; diff --git a/src/util/extended_kalman_filter.hpp b/src/util/extended_kalman_filter.hpp new file mode 100644 index 0000000..9129372 --- /dev/null +++ b/src/util/extended_kalman_filter.hpp @@ -0,0 +1,115 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace world_exe::util { +class ExtendedKalmanFilter { +public: + Eigen::VectorXd x; + Eigen::MatrixXd P; + + ExtendedKalmanFilter() = default; + + ExtendedKalmanFilter( + const Eigen::VectorXd& x0, const Eigen::MatrixXd& P0, + std::function x_add = + [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; }) { + data["residual_yaw"] = 0.0; + data["residual_pitch"] = 0.0; + data["residual_distance"] = 0.0; + data["residual_angle"] = 0.0; + data["nis"] = 0.0; + data["nees"] = 0.0; + data["nis_fail"] = 0.0; + data["nees_fail"] = 0.0; + data["recent_nis_failures"] = 0.0; + } + + Eigen::VectorXd predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q) { + return predict(F, Q, [&](const Eigen::VectorXd& x) { return F * x; }); + } + + Eigen::VectorXd predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q, + std::function f) { + P = F * P * F.transpose() + Q; + x = f(x); + return x; + } + + Eigen::VectorXd update( + const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, + std::function z_subtract = + [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { + return update(z, H, R, [&](const Eigen::VectorXd& x) { return H * x; }, z_subtract); + } + + Eigen::VectorXd update( + const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, + std::function h, + std::function z_subtract = + [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { + Eigen::VectorXd x_prior = x; + Eigen::MatrixXd K = P * H.transpose() * (H * P * H.transpose() + R).inverse(); + + // Stable Compution of the Posterior Covariance + // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb + P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose(); + + x = x_add(x, K * z_subtract(z, h(x))); + + /// 卡方检验 + Eigen::VectorXd residual = z_subtract(z, h(x)); + // 新增检验 + Eigen::MatrixXd S = H * P * H.transpose() + R; + double nis = residual.transpose() * S.inverse() * residual; + double nees = (x - x_prior).transpose() * P.inverse() * (x - x_prior); + + // 卡方检验阈值（自由度=4，取置信水平95%） + constexpr double nis_threshold = 0.711; + constexpr double nees_threshold = 0.711; + + if (nis > nis_threshold) nis_count_++, data["nis_fail"] = 1; + if (nees > nees_threshold) nees_count_++, data["nees_fail"] = 1; + total_count_++; + last_nis = nis; + + recent_nis_failures.push_back(nis > nis_threshold ? 1 : 0); + + if (recent_nis_failures.size() > window_size) { + recent_nis_failures.pop_front(); + } + + int recent_failures = + std::accumulate(recent_nis_failures.begin(), recent_nis_failures.end(), 0); + double recent_rate = static_cast(recent_failures) / recent_nis_failures.size(); + + data["residual_yaw"] = residual[0]; + data["residual_pitch"] = residual[1]; + data["residual_distance"] = residual[2]; + data["residual_angle"] = residual[3]; + data["nis"] = nis; + data["nees"] = nees; + data["recent_nis_failures"] = recent_rate; + + return x; + } + + std::map data; // 卡方检验数据 + std::deque recent_nis_failures { 0 }; + size_t window_size = 100; + double last_nis; + +private: + Eigen::MatrixXd I; + std::function x_add; + + int nees_count_ = 0; + int nis_count_ = 0; + int total_count_ = 0; +}; + +} // namespace tools diff --git a/src/util/math.hpp b/src/util/math.hpp index a3ea6b0..6d91a7e 100644 --- a/src/util/math.hpp +++ b/src/util/math.hpp @@ -198,6 +198,32 @@ static inline Eigen::Vector3d xyz2ypd(const Eigen::Vector3d& xyz) { return { yaw, pitch, distance }; } +static Eigen::Matrix xyz2ypd_jacobian(const Eigen::Vector3d& xyz) { + auto x = xyz[0], y = xyz[1], z = xyz[2]; + + auto dyaw_dx = -y / (x * x + y * y); + auto dyaw_dy = x / (x * x + y * y); + auto dyaw_dz = 0.0; + + auto dpitch_dx = -(x * z) / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 1.5)); + auto dpitch_dy = -(y * z) / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 1.5)); + auto dpitch_dz = 1 / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 0.5)); + + auto ddistance_dx = x / std::pow((x * x + y * y + z * z), 0.5); + auto ddistance_dy = y / std::pow((x * x + y * y + z * z), 0.5); + auto ddistance_dz = z / std::pow((x * x + y * y + z * z), 0.5); + + // clang-format off + Eigen::Matrix J{ + {dyaw_dx, dyaw_dy, dyaw_dz}, + {dpitch_dx, dpitch_dy, dpitch_dz}, + {ddistance_dx, ddistance_dy, ddistance_dz} + }; + // clang-format on + + return J; +} + template T square(T const& a) { return a * a; }; } // namespace rmcs_auto_aim::util::math \ No newline at end of file From af9017c8648adb9902c5d7b4bff4d93a6cd175a6 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Sep 2025 23:21:17 +0800 Subject: [PATCH 20/93] feat(env): Add libfmt-dev and libspdlog-dev dependencies --- env/ubuntu22.04.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/env/ubuntu22.04.sh b/env/ubuntu22.04.sh index 7a28bea..e5c97b5 100644 --- a/env/ubuntu22.04.sh +++ b/env/ubuntu22.04.sh @@ -9,7 +9,7 @@ sudo gpg --output /etc/apt/trusted.gpg.d/intel.gpg --dearmor GPG-PUB-KEY-INTEL-S rm GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB echo "deb https://apt.repos.intel.com/openvino ubuntu22 main" | sudo tee /etc/apt/sources.list.d/intel-openvino.list sudo apt update -sudo apt install -y libtbb-dev libeigen3-dev libopencv-dev openvino gcc-13 g++-13 libceres-dev libdwarf-dev libbackward-cpp-dev binutils-dev libdw-dev +sudo apt install -y libtbb-dev libeigen3-dev libopencv-dev openvino gcc-13 g++-13 libceres-dev libdwarf-dev libbackward-cpp-dev binutils-dev libdw-dev libfmt-dev libspdlog-dev sudo ln -s /usr/include/libdwarf/libdwarf.h /usr/include/libdwarf.h From d0241830a1c1eff8b0a6f98e1ac2ed470958168b Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 18 Sep 2025 02:17:26 +0800 Subject: [PATCH 21/93] refactor(interface): Rename the file of armors and change the armor interface implements to be read-only and immutable. --- src/tongji/identifier/identified_armor.hpp | 30 ++++++++++++++++++++ src/tongji/solver/solved_armor.hpp | 33 ++++++++++++++++++++++ 2 files changed, 63 insertions(+) create mode 100644 src/tongji/identifier/identified_armor.hpp create mode 100644 src/tongji/solver/solved_armor.hpp diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp new file mode 100644 index 0000000..b6ac9ff --- /dev/null +++ b/src/tongji/identifier/identified_armor.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include "enum/armor_id.hpp" +#include "interfaces/armor_in_image.hpp" +#include "interfaces/time_stamped.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::identifier { +class IdentifiedArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { +public: + explicit IdentifiedArmor(const std::vector& armors) { + for (const auto& armor : armors) { + armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); + } + } + + const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } + + const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + + const std::vector& GetArmors( + const enumeration::ArmorIdFlag& armor_id) const override { + return armors_[util::enumeration::GetIndex(armor_id)]; + } + +private: + std::time_t time_stamp_ { 0 }; + std::array, 8> armors_; +}; +} \ No newline at end of file diff --git a/src/tongji/solver/solved_armor.hpp b/src/tongji/solver/solved_armor.hpp new file mode 100644 index 0000000..43a79eb --- /dev/null +++ b/src/tongji/solver/solved_armor.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include +#include + +#include "interfaces/armor_in_camera.hpp" +#include "interfaces/time_stamped.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::solver { +class SolvedArmor final : public interfaces::IArmorInCamera, public interfaces::ITimeStamped { +public: + explicit SolvedArmor(const std::vector& armors) { + for (const auto& armor : armors) { + armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); + } + time_stamp_ = 0; + } + + const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } + + const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + + const std::vector& GetArmors( + const enumeration::ArmorIdFlag& armor_id) const override { + return armors_[util::enumeration::GetIndex(armor_id)]; + } + +private: + std::time_t time_stamp_ { 0 }; + std::array, 8> armors_; +}; +} \ No newline at end of file From deef68016ccf6508bc4eb8b9643b8429af263313 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 18 Sep 2025 02:21:43 +0800 Subject: [PATCH 22/93] WIP: partially implement ITargetPredictor interface Note: The implement of ITargetPredictor interface is not fully implemented yet. --- src/tongji/identifier/identifier.cpp | 4 +- src/tongji/identifier/identifier_armor.hpp | 36 -- src/tongji/predictor/car_predictor.hpp | 420 ------------------ src/tongji/predictor/car_predictor_boss.cpp | 56 --- .../predictor/car_predictor_manager.cpp | 134 ------ .../predictor/car_predictor_manager.hpp | 26 -- .../predictor/in_gimbal_control_armor.hpp | 43 +- src/tongji/predictor/target.hpp | 331 ++++++++++++++ src/tongji/predictor/target_predictor.cpp | 97 ++++ ...redictor_boss.hpp => target_predictor.hpp} | 10 +- .../predictor/target_snapshot_predictor.cpp | 88 ++++ .../predictor/target_snapshot_predictor.hpp | 22 + src/tongji/predictor/time_stamp.hpp | 5 +- src/tongji/solver/solver.cpp | 4 +- src/tongji/solver/solver_armor.hpp | 39 -- .../state_machine/car_state_manager.hpp | 4 +- src/util/extended_kalman_filter.hpp | 6 +- 17 files changed, 562 insertions(+), 763 deletions(-) delete mode 100644 src/tongji/identifier/identifier_armor.hpp delete mode 100644 src/tongji/predictor/car_predictor.hpp delete mode 100644 src/tongji/predictor/car_predictor_boss.cpp delete mode 100644 src/tongji/predictor/car_predictor_manager.cpp delete mode 100644 src/tongji/predictor/car_predictor_manager.hpp create mode 100644 src/tongji/predictor/target.hpp create mode 100644 src/tongji/predictor/target_predictor.cpp rename src/tongji/predictor/{car_predictor_boss.hpp => target_predictor.hpp} (70%) create mode 100644 src/tongji/predictor/target_snapshot_predictor.cpp create mode 100644 src/tongji/predictor/target_snapshot_predictor.hpp delete mode 100644 src/tongji/solver/solver_armor.hpp diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index 0b2c382..c314f74 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -17,7 +17,7 @@ #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" -#include "identifier_armor.hpp" +#include "identified_armor.hpp" #include "interfaces/armor_in_image.hpp" #include "tongji/identifier/classifier.hpp" #include "util/logger.hpp" @@ -191,7 +191,7 @@ class Identifier::Impl { all_car_id |= static_cast(armor.id); } - return { std::make_shared(armor_plates), + return { std::make_shared(armor_plates), static_cast(all_car_id) }; } diff --git a/src/tongji/identifier/identifier_armor.hpp b/src/tongji/identifier/identifier_armor.hpp deleted file mode 100644 index ec94241..0000000 --- a/src/tongji/identifier/identifier_armor.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#include "enum/armor_id.hpp" -#include "interfaces/armor_in_image.hpp" -#include "interfaces/time_stamped.hpp" -#include "util/index.hpp" - -namespace world_exe::tongji::identifier { -class IdentifierArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { -public: - explicit IdentifierArmor(const std::vector& armors) { - SetArmors(armors); - } - - const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; - - void SetArmors(const std::vector& armors) { - std::array, 8> temp_armors; - for (const auto& armor : armors) { - temp_armors[util::enumeration::GetIndex(armor.id)].emplace_back(armor); - } - armors_ = std::move(temp_armors); - } - - const std::vector& GetArmors( - const enumeration::ArmorIdFlag& armor_id) const override { - return armors_[util::enumeration::GetIndex(armor_id)]; - } - -private: - std::time_t time_stamp_ { 0 }; - std::array, 8> armors_; -}; -} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor.hpp deleted file mode 100644 index 4ce3fbe..0000000 --- a/src/tongji/predictor/car_predictor.hpp +++ /dev/null @@ -1,420 +0,0 @@ -#pragma once - -#include -#include - -#include "data/armor_gimbal_control_spacing.hpp" -#include "enum/armor_id.hpp" -#include "enum/car_id.hpp" -#include "interfaces/predictor_update_package.hpp" -#include "tongji/predictor/time_stamp.hpp" -#include "util/extended_kalman_filter.hpp" -#include "util/math.hpp" - -namespace world_exe::tongji::predictor { - -struct PredictorStatus { - bool jumped = false; - bool switched = false; - bool converged = false; - bool diverged = false; - bool lost = false; - bool reidentified = false; - int switch_count = 0; - int update_count = 0; - int lost_count = 0; -}; - -class CarPredictor { -public: - CarPredictor() = default; - explicit CarPredictor(const enumeration::ArmorIdFlag armor_id, - const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - std::time_t time_stamp, double radius, int armor_num, Eigen::VectorXd P0_dig) - : armor_id_(armor_id) - , last_id(0) - , radius_(radius) - , armor_num_(armor_num) - , last_predict_time_stamp_(time_stamp) { - const Eigen::VectorXd& xyz = armor_xyz_in_world; - const Eigen::VectorXd& ypr = armor_ypr_in_world; - - // 旋转中心的坐标 - auto center_x = xyz[0] + radius * std::cos(ypr[0]); - auto center_y = xyz[1] + radius * std::sin(ypr[0]); - auto center_z = xyz[2]; - - // x vx y vy z vz a w r l h - // a: angle - // w: angular velocity - // l: r2 - r1 - // h: z2 - z1 - Eigen::VectorXd x0 { { center_x, 0, center_y, 0, center_z, 0, ypr[0], 0, radius, 0, - 0 } }; // 初始化预测量 - P0_dig_ = P0_dig; - Eigen::MatrixXd P0 = P0_dig.asDiagonal(); - - // 防止夹角求和出现异常值 - x_add_ = [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) -> Eigen::VectorXd { - Eigen::VectorXd c = a + b; - c[6] = util::math::clamp_pm_pi(c[6]); - return c; - }; - - ekf_ = util::ExtendedKalmanFilter(x0, P0, x_add_); // 初始化滤波器（预测量、预测量协方差） - } - - void SetEkf(const util::ExtendedKalmanFilter& ekf) { ekf_ = ekf; } - - std::vector GetPredictedArmors( - enumeration::ArmorIdFlag armor_id, const TimeStamp time_stamp) { - PredictTo(time_stamp.GetTimeStamp()); - UpdateStatus(); - - // 装甲板匹配 - int id; - auto min_angle_error = 1e10; - const std::vector& xyza_list = armor_xyza_list(); - - std::vector> xyza_i_list; - for (int i = 0; i < armor_num_; i++) { - xyza_i_list.push_back({ xyza_list[i], i }); - } - - std::sort(xyza_i_list.begin(), xyza_i_list.end(), - [](const std::pair& a, const std::pair& b) { - Eigen::Vector3d ypd1 = util::math::xyz2ypd(a.first.head(3)); - Eigen::Vector3d ypd2 = util::math::xyz2ypd(b.first.head(3)); - return ypd1[2] < ypd2[2]; - }); - - std::vector armors; - for (int i = 0; i < armor_num_; i++) { - xyza_i_list.push_back({ xyza_list[i], i }); - const auto pos = xyza_i_list[i].first.head<3>(); - const auto yaw = util::math::clamp_pm_pi(ekf_.x[6]); - const double pitch = 15. / 180. * std::numbers::pi; - armors.push_back(data::ArmorGimbalControlSpacing { - armor_id_, pos, util::math::euler_to_quaternion(yaw, pitch, 0.) }); - } - return armors; - } - - void Update(std::shared_ptr data) { - const auto& transform = data->GetTransform(); - const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); - const auto armors = data->GetArmors()->GetArmors(armor_id_); - - if (armors.empty()) return; - for (auto armor : armors) { - const auto& armor_in_world_position = transform * armor.position; - const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; - this->update(armor_in_world_position, - util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), - util::math::xyz2ypd(armor_in_world_position)); - } - } - - const PredictorStatus& GetStatus() const { return status_; } - - void PredictTo(const TimeStamp& now) { - if (last_predict_time_stamp_.GetTimeStamp() == 0) { - last_predict_time_stamp_ = now; - return; - } - const double& dt = now.SecondsSince(last_predict_time_stamp_); - last_predict_time_stamp_ = now; - - if (dt <= 0.0 || dt > 1.0) { - // util::logger::logger()->warn( - // "[CarPredictor] Abnormal dt: {:.3f}, skipping prediction", dt); - return; - } - - predict(dt); - } - - void Reset(const Eigen::Vector3d& xyz, const Eigen::Vector3d& ypr, std::time_t time_stamp) { - last_predict_time_stamp_ = TimeStamp::FromRaw(time_stamp); - status_ = PredictorStatus {}; // 清空状态计数器 - - // 重新构造滤波器状态 - auto center_x = xyz[0] + radius_ * std::cos(ypr[0]); - auto center_y = xyz[1] + radius_ * std::sin(ypr[0]); - auto center_z = xyz[2]; - - Eigen::VectorXd x0 { { center_x, 0, center_y, 0, center_z, 0, ypr[0], 0, radius_, 0, 0 } }; - Eigen::MatrixXd P0 = P0_dig_.asDiagonal(); - - ekf_ = util::ExtendedKalmanFilter(x0, P0_dig_, x_add_); - } - - void TickStatus() { - static int last_update_count = -1; - if (status_.update_count == last_update_count) { - status_.lost_count++; - } else { - status_.lost_count = 0; - } - last_update_count = status_.update_count; - status_.lost = (status_.lost_count > 30 || IsDiverged()); - status_.reidentified = (!status_.lost && IsConverged() && status_.update_count > 3); - } - - bool IsLocked() const { return status_.converged && !status_.diverged; } - bool IsLost() const { return status_.lost; } - bool IsReidentified() const { return status_.reidentified; } - -private: - bool IsConverged() const { return status_.converged; } - bool IsDiverged() const { return status_.diverged; } - - int MatchArmor(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { - - const auto& xyza_list = armor_xyza_list(); - std::vector> xyza_i_list; - - for (int i = 0; i < armor_num_; ++i) { - xyza_i_list.emplace_back(xyza_list[i], i); - } - - std::sort(xyza_i_list.begin(), xyza_i_list.end(), [](const auto& a, const auto& b) { - auto ypd1 = util::math::xyz2ypd(a.first.head(3)); - auto ypd2 = util::math::xyz2ypd(b.first.head(3)); - return ypd1[2] < ypd2[2]; - }); - - int best_id = 0; - double min_error = 1e10; - - for (int i = 0; i < std::min(3, armor_num_); ++i) { - const auto& xyza = xyza_i_list[i].first; - auto ypd = util::math::xyz2ypd(xyza.head(3)); - double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_world[0] - xyza[3])) - + std::abs(util::math::clamp_pm_pi(armor_ypd_in_world[0] - ypd[0])); - - if (error < min_error) { - min_error = error; - best_id = xyza_i_list[i].second; - } - } - - return best_id; - } - - auto F(double dt) const -> Eigen::MatrixXd { - // 状态转移矩阵 - // clang-format off - return Eigen::MatrixXd { - {1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0}, - {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1} - }; - // clang-format on - } - - auto Q(double dt) const -> Eigen::MatrixXd { - // Piecewise White Noise Model - // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb - double v1, v2; - if (armor_id_ == enumeration::CarIDFlag::Outpost) { - v1 = 10; // 前哨站加速度方差 - v2 = 0.1; // 前哨站角加速度方差 - } else { - v1 = 100; // 加速度方差 - v2 = 400; // 角加速度方差 - } - auto a = dt * dt * dt * dt / 4; - auto b = dt * dt * dt / 2; - auto c = dt * dt; - // 预测过程噪声偏差的方差 - // clang-format off - return Eigen::MatrixXd { - {a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - {b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - { 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0}, - { 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0}, - { 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0}, - { 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0}, - { 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0}, - { 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0}, - { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, - { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} - }; - // clang-format on - } - - void predict(const double& dt) { - // 防止夹角求和出现异常值 - auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { - Eigen::VectorXd x_prior = this->F(dt) * x; - x_prior[6] = util::math::clamp_pm_pi(x_prior[6]); - return x_prior; - }; - - // 前哨站转速特判 - if (IsConverged() && this->armor_id_ == enumeration::CarIDFlag::Outpost - && std::abs(this->ekf_.x[7]) > 2) - this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; - - ekf_.predict(this->F(dt), this->Q(dt), f); - } - - void UpdateStatus() { - // 发散判断 - double r = ekf_.x[8]; - double l = ekf_.x[9]; - status_.diverged = !(r > 0.05 && r < 0.5 && r + l > 0.05 && r + l < 0.5); - - // 收敛判断 - if (armor_id_ == enumeration::CarIDFlag::Outpost) { - status_.converged = (status_.update_count > 10 && !status_.diverged); - } else { - status_.converged = (status_.update_count > 3 && !status_.diverged); - } - } - - void update(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { - // 装甲板匹配 - int id = MatchArmor(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); - - if (id != 0) status_.jumped = true; - status_.switched = (id != last_id); - if (status_.switched) status_.switch_count++; - - last_id = id; - update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); - status_.update_count++; - UpdateStatus(); - } - - void update_ypda(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, - int id) { - // 观测jacobi - Eigen::MatrixXd H = h_jacobian(ekf_.x, id); - // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; - auto center_yaw = std::atan2(armor_xyz_in_world[1], armor_xyz_in_world[0]); - auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world[0] - center_yaw); - Eigen::VectorXd R_dig { { 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, - log(std::abs(armor_ypd_in_world[2]) + 1) / 200 + 9e-2 } }; - - // 测量过程噪声偏差的方差 - Eigen::MatrixXd R = R_dig.asDiagonal(); - - // 定义非线性转换函数h: x -> z - auto h = [&](const Eigen::VectorXd& x) -> Eigen::Vector4d { - Eigen::VectorXd xyz = h_armor_xyz(x, id); - Eigen::VectorXd ypd = util::math::xyz2ypd(xyz); - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); - return { ypd[0], ypd[1], ypd[2], angle }; - }; - - // 防止夹角求差出现异常值 - auto z_subtract = [](const Eigen::VectorXd& a, - const Eigen::VectorXd& b) -> Eigen::VectorXd { - Eigen::VectorXd c = a - b; - c[0] = util::math::clamp_pm_pi(c[0]); - c[1] = util::math::clamp_pm_pi(c[1]); - c[3] = util::math::clamp_pm_pi(c[3]); - return c; - }; - - const Eigen::VectorXd& ypd = armor_ypd_in_world; - const Eigen::VectorXd& ypr = armor_ypr_in_world; - Eigen::VectorXd z { { ypd[0], ypd[1], ypd[2], ypr[0] } }; // 获得观测量 - - ekf_.update(z, H, R, h, z_subtract); - } - - Eigen::VectorXd ekf_x() const { return ekf_.x; } - - const util::ExtendedKalmanFilter& ekf() const { return ekf_; } - - std::vector armor_xyza_list() const { - std::vector _armor_xyza_list; - - for (int i = 0; i < armor_num_; i++) { - auto angle = util::math::clamp_pm_pi(ekf_.x[6] + i * 2 * CV_PI / armor_num_); - Eigen::Vector3d xyz = h_armor_xyz(ekf_.x, i); - _armor_xyza_list.push_back({ xyz[0], xyz[1], xyz[2], angle }); - } - return _armor_xyza_list; - } - - // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); - auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); - - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto armor_x = x[0] - r * std::cos(angle); - auto armor_y = x[2] - r * std::sin(angle); - auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; - - return { armor_x, armor_y, armor_z }; - } - - Eigen::MatrixXd h_jacobian(const Eigen::VectorXd& x, int id) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); - auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); - - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto dx_da = r * std::sin(angle); - auto dy_da = -r * std::cos(angle); - - auto dx_dr = -std::cos(angle); - auto dy_dr = -std::sin(angle); - auto dx_dl = (use_l_h) ? -std::cos(angle) : 0.0; - auto dy_dl = (use_l_h) ? -std::sin(angle) : 0.0; - - auto dz_dh = (use_l_h) ? 1.0 : 0.0; - - // clang-format off - Eigen::MatrixXd H_armor_xyza{ - {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, - {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, - {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, - {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} - }; - // clang-format on - - Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); - Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); - // clang-format off - Eigen::MatrixXd H_armor_ypda{ - {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, - {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, - {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, - { 0, 0, 0, 1} - }; - // clang-format on - - return H_armor_ypda * H_armor_xyza; - } - - int armor_num_; - double radius_; - PredictorStatus status_; - - enumeration::ArmorIdFlag armor_id_; - int last_id = -1; // debug only - - std::function x_add_; - Eigen::VectorXd P0_dig_; - util::ExtendedKalmanFilter ekf_; - TimeStamp last_predict_time_stamp_; -}; - -} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor_boss.cpp b/src/tongji/predictor/car_predictor_boss.cpp deleted file mode 100644 index af82658..0000000 --- a/src/tongji/predictor/car_predictor_boss.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include "car_predictor_boss.hpp" - -#include -#include -#include - -#include "enum/car_id.hpp" -#include "enum/enum_tools.hpp" -#include "tongji/predictor/car_predictor_manager.hpp" - -namespace world_exe::tongji::predictor { -class CarPredictorBoss::Impl { -public: - Impl(std::shared_ptr manager) - : manager_(manager) { - if (!manager_) { - throw std::invalid_argument("CarPredictorManager must not be null"); - } - } - - std ::shared_ptr Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { - for (int i = 0; i < 8; ++i) { - auto car_id = static_cast( - static_cast(enumeration::CarIDFlag::Hero) << i); - - if (enumeration::IsFlagContains(id, car_id)) { - manager_->SetPredictorBySingleID(car_id, time_stamp); - } - } - return manager_->Predictor(time_stamp); - } - - std ::shared_ptr GetPredictor( - const enumeration ::ArmorIdFlag& id) const { - return manager_; - } - -private: - std::shared_ptr manager_; -}; - -std ::shared_ptr CarPredictorBoss::Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { - return pimpl_->Predict(id, time_stamp); -} - -std ::shared_ptr CarPredictorBoss::GetPredictor( - const enumeration ::ArmorIdFlag& id) const { - return pimpl_->GetPredictor(id); -} - -CarPredictorBoss::CarPredictorBoss(std::shared_ptr manager) - : pimpl_(std::make_unique(manager)) { } - -} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor_manager.cpp deleted file mode 100644 index 1572cde..0000000 --- a/src/tongji/predictor/car_predictor_manager.cpp +++ /dev/null @@ -1,134 +0,0 @@ -#include "car_predictor_manager.hpp" - -#include -#include -#include -#include - -#include "enum/armor_id.hpp" -#include "enum/car_id.hpp" -#include "tongji/predictor/car_predictor.hpp" -#include "tongji/predictor/in_gimbal_control_armor.hpp" -#include "tongji/predictor/time_stamp.hpp" - -namespace world_exe::tongji::predictor { -class CarPredictorManager::Impl { -public: - Impl() = default; - - const enumeration ::CarIDFlag& GetId() { - if (ids_dirty_) { - ids_ = enumeration::CarIDFlag::None; - for (const auto& [id, _] : predictors_) { - ids_ = static_cast( - static_cast(ids_) | static_cast(id)); - } - ids_dirty_ = false; - } - return ids_; - } - - auto Predictor(const std ::time_t& time_stamp) { - InGimbalControlArmor target_armors; - for (auto& predictor : predictors_) { - target_armors.Add( - predictor.first, predictor.second.GetPredictedArmors(predictor.first, time_stamp)); - } - return std::make_shared(target_armors); - } - - void Tick(const TimeStamp& now) { - std::vector to_remove; - - for (auto& [id, predictor] : predictors_) { - predictor.PredictTo(now); - predictor.TickStatus(); - - if (predictor.IsLost()) { - to_remove.push_back(id); - } - } - // logger()->info("Removing lost predictor: {}", static_cast(id)); - for (auto id : to_remove) { - predictors_.erase(id); - } - } - - void SetPredictorBySingleID(const enumeration::CarIDFlag& id, - const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - std::time_t init_time_stamp) { - - auto it = predictors_.find(id); - if (it != predictors_.end()) { - it->second.Reset(armor_xyz_in_world, armor_ypr_in_world, init_time_stamp); - } else { - bool is_balance = (id == enumeration::CarIDFlag::InfantryIII - | id == enumeration::CarIDFlag::InfantryIV - | id == enumeration::CarIDFlag::InfantryV); - - Eigen::VectorXd P0_dig(11); - double radius; - int armor_num; - - if (is_balance) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 2; - } else if (id == enumeration::CarIDFlag::Outpost) { - P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; - radius = 0.2765; - armor_num = 3; - } else if (id == enumeration::CarIDFlag::Base) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; - radius = 0.3205; - armor_num = 3; - } else { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 4; - } - - predictors_.emplace(id, - CarPredictor { id, armor_xyz_in_world, armor_ypr_in_world, init_time_stamp, radius, - armor_num, P0_dig }); - ids_dirty_ = true; - } - } - - void RemovePredictorBySingleID(const enumeration::CarIDFlag& id) { - predictors_.erase(id); - ids_dirty_ = true; - } - - bool HasPredictor(const enumeration::CarIDFlag& id) const { - return predictors_.find(id) != predictors_.end(); - } - -private: - enumeration::CarIDFlag ids_ { enumeration::CarIDFlag::None }; - std::unordered_map predictors_; - mutable bool ids_dirty_ = true; -}; - -const enumeration ::ArmorIdFlag& CarPredictorManager::GetId() const { return pimpl_->GetId(); } - -std::shared_ptr CarPredictorManager::Predictor( - const std ::time_t& time_stamp) const { - return pimpl_->Predictor(time_stamp); -} - -void CarPredictorManager::SetPredictorBySingleID(const enumeration::CarIDFlag& id, - Eigen::Vector3d armor_xyz_in_world, Eigen::Vector3d armor_ypr_in_world, time_t time_stamp) { - return pimpl_->SetPredictorBySingleID(id, armor_xyz_in_world, armor_ypr_in_world, time_stamp); -} - -void CarPredictorManager::SetPredictorBySingleID( - const enumeration::CarIDFlag& id, time_t time_stamp) { - return pimpl_->SetPredictorBySingleID( - id, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), time_stamp); -} - -CarPredictorManager::CarPredictorManager() - : pimpl_(std::make_unique()) { } - -} diff --git a/src/tongji/predictor/car_predictor_manager.hpp b/src/tongji/predictor/car_predictor_manager.hpp deleted file mode 100644 index 149a223..0000000 --- a/src/tongji/predictor/car_predictor_manager.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#include - -#include "enum/car_id.hpp" -#include "interfaces/predictor.hpp" - -namespace world_exe::tongji::predictor { -class CarPredictorManager final : public interfaces::IPredictor { -public: - CarPredictorManager(); - const enumeration ::ArmorIdFlag& GetId() const override; - - std::shared_ptr Predictor( - const std ::time_t& time_stamp) const override; - - void SetPredictorBySingleID(const enumeration::CarIDFlag& id, - Eigen::Vector3d armor_xyz_in_world, Eigen::Vector3d armor_ypr_in_world, time_t time_stamp); - - void SetPredictorBySingleID(const enumeration::CarIDFlag& id, time_t time_stamp); - -private: - class Impl; - std::unique_ptr pimpl_; -}; -} \ No newline at end of file diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index 9f9fdae..a8f48c9 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -1,59 +1,36 @@ #pragma once +#include #include #include "data/armor_gimbal_control_spacing.hpp" -#include "enum/car_id.hpp" +#include "enum/armor_id.hpp" #include "interfaces/armor_in_gimbal_control.hpp" #include "tongji/predictor/time_stamp.hpp" -#include "util/index.hpp" namespace world_exe::tongji::predictor { class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { public: - InGimbalControlArmor() = default; - InGimbalControlArmor(const std::array, 8>& armors, + InGimbalControlArmor( + std::unordered_map> + armors, TimeStamp time_stamp) : armors_(armors) , time_stamp_(time_stamp) { } const std ::vector& GetArmors( const enumeration ::ArmorIdFlag& armor_id) const override { - auto armor_id_index = util::enumeration::GetIndex(armor_id); - return armors_.at(armor_id_index); - } - - void Set(const std::array, 8>& armors, - const predictor::TimeStamp& predict_time_stamp) { - armors_ = armors; - time_stamp_ = predict_time_stamp; - } - - void Add( - enumeration::CarIDFlag car_id, const std::vector& armors) { - auto& target_vector = armors_.at(util::enumeration::GetIndex(car_id)); - target_vector.insert(target_vector.end(), armors.begin(), armors.end()); - } - - void Clear() { - for (auto& vec : armors_) { - vec.clear(); - } - } - - void SetArmorsByCarId( - const std::vector& armors, TimeStamp time_stamp) { - if (armors.empty()) return; - armors_[util::enumeration::GetIndex(armors.at(0).id)] = armors; + static const std::vector empty; + auto it = armors_.find(armor_id); + return it != armors_.end() ? it->second : empty; } const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } - void SetTimeStamp(const TimeStamp time_stamp) { time_stamp_ = time_stamp; } - private: TimeStamp time_stamp_; - std::array, 8> armors_; + std::unordered_map> + armors_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/target.hpp b/src/tongji/predictor/target.hpp new file mode 100644 index 0000000..48d88dc --- /dev/null +++ b/src/tongji/predictor/target.hpp @@ -0,0 +1,331 @@ +#pragma once + +#include +#include + +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "interfaces/predictor_update_package.hpp" +#include "tongji/predictor/time_stamp.hpp" +#include "util/extended_kalman_filter.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { + +struct TargetStatus { + bool jumped = false; + bool switched = false; + bool converged = false; + bool diverged = false; + bool lost = false; + bool reidentified = false; + int last_id = -1; + int switch_count = 0; + int update_count = 0; + int lost_count = 0; +}; + +class Target { +public: + TargetStatus status_; + + Target(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, + const enumeration::ArmorIdFlag& id, const std::time_t& t, const double& radius, + const int& armor_num, const Eigen::VectorXd& P0_dig) + : time_stamp_(t) + , car_id_(id) + , armor_num_(armor_num) { + + auto r = radius; + + // 旋转中心的坐标 + auto center_x = armor_xyz_in_world[0] + r * std::cos(armor_ypr_in_world[0]); + auto center_y = armor_xyz_in_world[1] + r * std::sin(armor_ypr_in_world[0]); + auto center_z = armor_xyz_in_world[2]; + + // x vx y vy z vz a w r l h + // a: angle + // w: angular velocity + // l: r2 - r1 + // h: z2 - z1 + Eigen::VectorXd x0 { { center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, r, + 0, 0 } }; // 初始化预测量 + Eigen::MatrixXd P0 = P0_dig.asDiagonal(); + + // 防止夹角求和出现异常值 + auto x_add = [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) -> Eigen::VectorXd { + Eigen::VectorXd c = a + b; + c[6] = util::math::clamp_pm_pi(c[6]); + return c; + }; + + ekf_ = util::ExtendedKalmanFilter(x0, P0, x_add); // 初始化滤波器（预测量、预测量协方差） + } + + Eigen::VectorXd GetEkf_x() const { return ekf_.x; } + + void Predict(double dt) { + // 防止夹角求和出现异常值 + auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + Eigen::VectorXd x_prior = this->F(dt) * x; + x_prior[6] = util::math::clamp_pm_pi(x_prior[6]); + return x_prior; + }; + + // 前哨站转速特判 + if (this->Convergened() && this->car_id_ == enumeration::CarIDFlag::Outpost + && std::abs(this->ekf_.x[7]) > 2) + this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; + + ekf_.Predict(this->F(dt), this->Q(dt), f); + } + + std::time_t GetTimeStamp() const { return time_stamp_.GetTimeStamp(); } + int GetArmorNum() const { return armor_num_; } + enumeration::CarIDFlag GetId() const { return car_id_; } + + void Update(std::shared_ptr data) { + const auto& transform = data->GetTransform(); + const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); + const auto armors = data->GetArmors()->GetArmors(car_id_); + + if (armors.empty()) return; + for (auto armor : armors) { + const auto& armor_in_world_position = transform * armor.position; + const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; + this->Update(armor_in_world_position, + util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), + util::math::xyz2ypd(armor_in_world_position)); + } + } + + bool Convergened() { + if (this->car_id_ != enumeration::CarIDFlag::Outpost && status_.update_count > 3 + && !this->Diverged()) { + status_.converged = true; + } + + // 前哨站特殊判断 + if (this->car_id_ == enumeration::CarIDFlag::Outpost && status_.update_count > 10 + && !this->Diverged()) { + status_.converged = true; + } + + return status_.converged; + } + + bool Diverged() const { + auto r_ok = ekf_.x[8] > 0.05 && ekf_.x[8] < 0.5; + auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; + + if (r_ok && l_ok) return false; + + // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); + return true; + } + +private: + void Update(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { + // 装甲板匹配 + int id = MatchArmor(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); + if (id != 0) status_.jumped = true; + status_.switched = (id != status_.last_id); + if (status_.switched) status_.switch_count++; + + status_.last_id = id; + Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); + status_.update_count++; + } + + void Update_ypda(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, + int id) { + // 观测jacobi + Eigen::MatrixXd H = h_jacobian(ekf_.x, id); + // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; + auto center_yaw = std::atan2(armor_xyz_in_world[1], armor_xyz_in_world[0]); + auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world[0] - center_yaw); + Eigen::VectorXd R_dig { { 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, + log(std::abs(armor_ypd_in_world[2]) + 1) / 200 + 9e-2 } }; + + // 测量过程噪声偏差的方差 + Eigen::MatrixXd R = R_dig.asDiagonal(); + + // 定义非线性转换函数h: x -> z + auto h = [&](const Eigen::VectorXd& x) -> Eigen::Vector4d { + Eigen::VectorXd xyz = h_armor_xyz(x, id); + Eigen::VectorXd ypd = util::math::xyz2ypd(xyz); + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); + return { ypd[0], ypd[1], ypd[2], angle }; + }; + + // 防止夹角求差出现异常值 + auto z_subtract = [](const Eigen::VectorXd& a, + const Eigen::VectorXd& b) -> Eigen::VectorXd { + Eigen::VectorXd c = a - b; + c[0] = util::math::clamp_pm_pi(c[0]); + c[1] = util::math::clamp_pm_pi(c[1]); + c[3] = util::math::clamp_pm_pi(c[3]); + return c; + }; + + const Eigen::VectorXd& ypd = armor_ypd_in_world; + const Eigen::VectorXd& ypr = armor_ypr_in_world; + Eigen::VectorXd z { { ypd[0], ypd[1], ypd[2], ypr[0] } }; // 获得观测量 + + ekf_.update(z, H, R, h, z_subtract); + } + + Eigen::MatrixXd h_jacobian(const Eigen::VectorXd& x, int id) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto dx_da = r * std::sin(angle); + auto dy_da = -r * std::cos(angle); + + auto dx_dr = -std::cos(angle); + auto dy_dr = -std::sin(angle); + auto dx_dl = (use_l_h) ? -std::cos(angle) : 0.0; + auto dy_dl = (use_l_h) ? -std::sin(angle) : 0.0; + + auto dz_dh = (use_l_h) ? 1.0 : 0.0; + + // clang-format off + Eigen::MatrixXd H_armor_xyza{ + {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, + {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, + {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, + {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} + }; + // clang-format on + + Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); + Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); + // clang-format off + Eigen::MatrixXd H_armor_ypda{ + {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, + {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, + {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, + { 0, 0, 0, 1} + }; + // clang-format on + + return H_armor_ypda * H_armor_xyza; + } + + int MatchArmor(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { + + const auto& xyza_list = armor_xyza_list(); + std::vector> xyza_i_list; + + for (int i = 0; i < armor_num_; ++i) { + xyza_i_list.emplace_back(xyza_list[i], i); + } + + std::sort(xyza_i_list.begin(), xyza_i_list.end(), [](const auto& a, const auto& b) { + auto ypd1 = util::math::xyz2ypd(a.first.head(3)); + auto ypd2 = util::math::xyz2ypd(b.first.head(3)); + return ypd1[2] < ypd2[2]; + }); + + int best_id = 0; + double min_error = 1e10; + + for (int i = 0; i < std::min(3, armor_num_); ++i) { + const auto& xyza = xyza_i_list[i].first; + auto ypd = util::math::xyz2ypd(xyza.head(3)); + double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_world[0] - xyza[3])) + + std::abs(util::math::clamp_pm_pi(armor_ypd_in_world[0] - ypd[0])); + + if (error < min_error) { + min_error = error; + best_id = xyza_i_list[i].second; + } + } + + return best_id; + } + + std::vector armor_xyza_list() const { + std::vector _armor_xyza_list; + + for (int i = 0; i < armor_num_; i++) { + auto angle = util::math::clamp_pm_pi(ekf_.x[6] + i * 2 * CV_PI / armor_num_); + Eigen::Vector3d xyz = h_armor_xyz(ekf_.x, i); + _armor_xyza_list.push_back({ xyz[0], xyz[1], xyz[2], angle }); + } + return _armor_xyza_list; + } + + // 计算出装甲板中心的坐标（考虑长短轴） + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto armor_x = x[0] - r * std::cos(angle); + auto armor_y = x[2] - r * std::sin(angle); + auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; + + return { armor_x, armor_y, armor_z }; + } + + auto F(double dt) -> Eigen::MatrixXd { + // 状态转移矩阵 + // clang-format off + return (Eigen::MatrixXd(11, 11) << + 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1).finished(); +} + +auto Q(double dt)->Eigen::MatrixXd{ + // Piecewise White Noise Model + // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb + double v1, v2; + if (car_id_ == enumeration::CarIDFlag::Outpost) { + v1 = 10; // 前哨站加速度方差 + v2 = 0.1; // 前哨站角加速度方差 + } else { + v1 = 100; // 加速度方差 + v2 = 400; // 角加速度方差 + } + auto a = dt * dt * dt * dt / 4; + auto b = dt * dt * dt / 2; + auto c = dt * dt; + + // 预测过程噪声偏差的方差 + // clang-format off + return (Eigen::MatrixXd(11, 11) << + a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0, + 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(); + // clang-format on + } + + TimeStamp time_stamp_; + util::ExtendedKalmanFilter ekf_; + enumeration::CarIDFlag car_id_; + int armor_num_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.cpp b/src/tongji/predictor/target_predictor.cpp new file mode 100644 index 0000000..ee576ca --- /dev/null +++ b/src/tongji/predictor/target_predictor.cpp @@ -0,0 +1,97 @@ +#include "target_predictor.hpp" + +#include +#include +#include + +#include "enum/car_id.hpp" +#include "enum/enum_tools.hpp" +#include "target.hpp" +#include "tongji/predictor/target_snapshot_predictor.hpp" + +namespace world_exe::tongji::predictor { + +std::vector ConstructTargets(const enumeration::ArmorIdFlag& id, std::time_t time_stamp) { + std::vector targets; + + for (int i = 0; i < 8; ++i) { + auto car_id = static_cast(1 << i); + if (!enumeration::IsFlagContains(id, car_id)) continue; + + bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII + || car_id == enumeration::CarIDFlag::InfantryIV + || car_id == enumeration::CarIDFlag::InfantryV); + + Eigen::VectorXd P0_dig(11); + double radius; + int armor_num; + + if (is_balance) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 2; + } else if (car_id == enumeration::CarIDFlag::Outpost) { + P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; + radius = 0.2765; + armor_num = 3; + } else if (car_id == enumeration::CarIDFlag::Base) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; + radius = 0.3205; + armor_num = 3; + } else { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 4; + } + + Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); + Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); + + targets.emplace_back(Target( + armor_xyz_in_world, armor_ypr_in_world, car_id, time_stamp, radius, armor_num, P0_dig)); + return targets; + } + + return targets; +} + +class TargetPredictor::Impl { +public: + Impl(const std::vector& targets) + : selected_targets_(targets) { } + + std::shared_ptr Predict( + const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { + + } + + std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { + auto targets = ConstructTargets(id, last_update_time_stamp_); + return std::make_shared(targets); + } + + void Update(const std::shared_ptr& data) { + for (auto target : selected_targets_) { + target.Update(data); + } + } + +private: + std::vector selected_targets_; + + std::time_t last_update_time_stamp_ { 0 }; +}; + +std ::shared_ptr TargetPredictor::Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { + return pimpl_->Predict(id, time_stamp); +} + +std ::shared_ptr TargetPredictor::GetPredictor( + const enumeration ::ArmorIdFlag& id) const { + return pimpl_->GetPredictor(id); +} + +TargetPredictor::TargetPredictor(const enumeration::ArmorIdFlag& id, std::time_t t) + : pimpl_(std::make_unique(ConstructTargets(id, t))) { } +} \ No newline at end of file diff --git a/src/tongji/predictor/car_predictor_boss.hpp b/src/tongji/predictor/target_predictor.hpp similarity index 70% rename from src/tongji/predictor/car_predictor_boss.hpp rename to src/tongji/predictor/target_predictor.hpp index 35b7198..8958794 100644 --- a/src/tongji/predictor/car_predictor_boss.hpp +++ b/src/tongji/predictor/target_predictor.hpp @@ -3,19 +3,17 @@ #include #include "interfaces/target_predictor.hpp" -#include "tongji/predictor/car_predictor_manager.hpp" namespace world_exe::tongji::predictor { -class CarPredictorBoss final : public interfaces::ITargetPredictor { +class TargetPredictor final : public interfaces::ITargetPredictor { public: - CarPredictorBoss(std::shared_ptr manager); + TargetPredictor(const enumeration::ArmorIdFlag& id, std::time_t t); + std ::shared_ptr Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; std ::shared_ptr GetPredictor( const enumeration ::ArmorIdFlag& id) const override; - std ::shared_ptr Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; - private: class Impl; std::unique_ptr pimpl_; diff --git a/src/tongji/predictor/target_snapshot_predictor.cpp b/src/tongji/predictor/target_snapshot_predictor.cpp new file mode 100644 index 0000000..4bc42f9 --- /dev/null +++ b/src/tongji/predictor/target_snapshot_predictor.cpp @@ -0,0 +1,88 @@ +#include "target_snapshot_predictor.hpp" + +#include +#include +#include + +#include +#include + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/armor_id.hpp" +#include "enum/enum_tools.hpp" +#include "in_gimbal_control_armor.hpp" +#include "target.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { +class MultiTargetSnapshotPredictor::Impl { +public: + Impl(const std::vector& targets) + : targets_(targets) + , last_time_stamp_(0) { + id_ = enumeration::ArmorIdFlag::Unknow; + for (const auto& target : targets_) { + id_ = static_cast( + static_cast(id_) | static_cast(target.GetId())); + } + } + + const enumeration::ArmorIdFlag& GetId() const { return id_; } + + std::shared_ptr Predictor(const std::time_t& time_stamp) { + std::unordered_map> + armor_map; + const double dt = static_cast(time_stamp - last_time_stamp_); + + for (auto& target : targets_) { + Target iteration_target = target; + iteration_target.Predict(dt); + + Eigen::VectorXd iteration_x = iteration_target.GetEkf_x(); + std::vector armors; + + for (int j = 0; j < iteration_target.GetArmorNum(); ++j) { + auto angle = util::math::clamp_pm_pi( + iteration_x[6] + j * 2 * CV_PI / iteration_target.GetArmorNum()); + Eigen::Vector3d xyz = h_armor_xyz(iteration_x, j, iteration_target.GetArmorNum()); + data::ArmorGimbalControlSpacing armor; + armor.id = iteration_target.GetId(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15 / 180. * CV_PI, 0); + } + armor_map[iteration_target.GetId()] = armors; + } + return std::make_shared(armor_map, last_time_stamp_); + } + +private: + std::vector targets_; + enumeration::ArmorIdFlag id_; + std::time_t last_time_stamp_; + + // 计算出装甲板中心的坐标（考虑长短轴） + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id, int armor_num) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num); + auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto armor_x = x[0] - r * std::cos(angle); + auto armor_y = x[2] - r * std::sin(angle); + auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; + + return { armor_x, armor_y, armor_z }; + } +}; + +MultiTargetSnapshotPredictor::MultiTargetSnapshotPredictor(const std::vector& targets) + : pimpl_(std::make_unique(targets)) { } + +const enumeration ::ArmorIdFlag& MultiTargetSnapshotPredictor::GetId() const { + return pimpl_->GetId(); +} + +std ::shared_ptr MultiTargetSnapshotPredictor::Predictor( + const std ::time_t& time_stamp) const { + return pimpl_->Predictor(time_stamp); +} +} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_predictor.hpp b/src/tongji/predictor/target_snapshot_predictor.hpp new file mode 100644 index 0000000..cea2fad --- /dev/null +++ b/src/tongji/predictor/target_snapshot_predictor.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include + +#include "interfaces/predictor.hpp" +#include "tongji/predictor/target.hpp" + +namespace world_exe::tongji::predictor { + +class MultiTargetSnapshotPredictor final : public interfaces::IPredictor { +public: + MultiTargetSnapshotPredictor(const std::vector& targets); + const enumeration ::ArmorIdFlag& GetId() const override; + + std ::shared_ptr Predictor( + const std ::time_t& time_stamp) const override; + +private: + class Impl; + std::unique_ptr pimpl_; +}; +} diff --git a/src/tongji/predictor/time_stamp.hpp b/src/tongji/predictor/time_stamp.hpp index ebcbc26..90deacf 100644 --- a/src/tongji/predictor/time_stamp.hpp +++ b/src/tongji/predictor/time_stamp.hpp @@ -5,13 +5,10 @@ namespace world_exe::tongji::predictor { class TimeStamp : public interfaces::ITimeStamped { public: - TimeStamp() = default; TimeStamp(const std::time_t& time_stamp) : time_stamp_(time_stamp) { } - static TimeStamp FromRaw(std::time_t raw) { return TimeStamp(raw); } - - inline void SetTimeStamp(const time_t& time_stamp) { time_stamp_ = time_stamp; } + void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_ = time_stamp; } double SecondsSince(const TimeStamp& other) const { return std::difftime(time_stamp_, other.time_stamp_); diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 2213d5a..2202025 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -8,7 +8,7 @@ #include "data/armor_gimbal_control_spacing.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" -#include "solver_armor.hpp" +#include "solved_armor.hpp" #include "util/coordinate.hpp" #include "util/index.hpp" #include "util/math.hpp" @@ -36,7 +36,7 @@ class Solver::Impl { } } } - return std::make_shared(armor_plates); + return std::make_shared(armor_plates); } std::optional Solve( diff --git a/src/tongji/solver/solver_armor.hpp b/src/tongji/solver/solver_armor.hpp deleted file mode 100644 index 1e8145b..0000000 --- a/src/tongji/solver/solver_armor.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include -#include - -#include "interfaces/armor_in_camera.hpp" -#include "interfaces/time_stamped.hpp" -#include "util/index.hpp" - -namespace world_exe::tongji::solver { -class SolverArmor final : public interfaces::IArmorInCamera, public interfaces::ITimeStamped { -public: - explicit SolverArmor(const std::vector& armors) { - SetArmors(armors); - time_stamp_ = 0; - } - - const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; - - void SetArmors(const std::vector& armors) { - std::array, 8> temp_armors; - for (const auto& armor : armors) { - temp_armors[util::enumeration::GetIndex(armor.id)].emplace_back(armor); - } - armors_ = std::move(temp_armors); - } - - const std::vector& GetArmors( - const enumeration::ArmorIdFlag& armor_id) const override { - return armors_[util::enumeration::GetIndex(armor_id)]; - } - -private: - std::time_t time_stamp_ { 0 }; - std::array, 8> armors_; -}; -} \ No newline at end of file diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index 8dca5eb..219e457 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -14,7 +14,7 @@ class CarStateManager { void Update(bool detected, std::time_t now_raw) { if (detected) { count_ = std::min(count_ + 1, switch_threshold_); - auto now = predictor::TimeStamp::FromRaw(now_raw); + auto now = predictor::TimeStamp(now_raw); last_seen_ = now; } else { count_ = std::max(count_ - 1, 0); @@ -57,6 +57,6 @@ class CarStateManager { bool is_converged_ = false; bool is_diverged_ = false; int priority_ = 100; // 默认最低优先级 - predictor::TimeStamp last_seen_; + predictor::TimeStamp last_seen_ { 0 }; }; } \ No newline at end of file diff --git a/src/util/extended_kalman_filter.hpp b/src/util/extended_kalman_filter.hpp index 9129372..8b1a2e0 100644 --- a/src/util/extended_kalman_filter.hpp +++ b/src/util/extended_kalman_filter.hpp @@ -30,17 +30,17 @@ class ExtendedKalmanFilter { } Eigen::VectorXd predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q) { - return predict(F, Q, [&](const Eigen::VectorXd& x) { return F * x; }); + return Predict(F, Q, [&](const Eigen::VectorXd& x) { return F * x; }); } - Eigen::VectorXd predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q, + Eigen::VectorXd Predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q, std::function f) { P = F * P * F.transpose() + Q; x = f(x); return x; } - Eigen::VectorXd update( + Eigen::VectorXd Update( const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, std::function z_subtract = [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { From 00fdd5e790ed974641d6672880bb9866448a22ed Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 18 Sep 2025 21:52:23 +0800 Subject: [PATCH 23/93] WIP: partially implement ITargetPredictor interface --- src/tongji/predictor/target.hpp | 1 - src/tongji/predictor/target_predict.cpp | 118 ++++++++++++++++++++++ src/tongji/predictor/target_predict.hpp | 15 +++ src/tongji/predictor/target_predictor.cpp | 29 +----- 4 files changed, 137 insertions(+), 26 deletions(-) create mode 100644 src/tongji/predictor/target_predict.cpp create mode 100644 src/tongji/predictor/target_predict.hpp diff --git a/src/tongji/predictor/target.hpp b/src/tongji/predictor/target.hpp index 48d88dc..3f6ec3c 100644 --- a/src/tongji/predictor/target.hpp +++ b/src/tongji/predictor/target.hpp @@ -119,7 +119,6 @@ class Target { auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; if (r_ok && l_ok) return false; - // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); return true; } diff --git a/src/tongji/predictor/target_predict.cpp b/src/tongji/predictor/target_predict.cpp new file mode 100644 index 0000000..bed8a9e --- /dev/null +++ b/src/tongji/predictor/target_predict.cpp @@ -0,0 +1,118 @@ +#include "target_predict.hpp" + +#include +#include +#include + +#include "enum/car_id.hpp" +#include "enum/enum_tools.hpp" +#include "tongji/predictor/in_gimbal_control_armor.hpp" +#include "tongji/predictor/target.hpp" +#include "tongji/predictor/target_snapshot_predictor.hpp" + +namespace world_exe::tongji::predictor { + +class TargetPredict::Impl { +public: + Impl(std::list targets) { + for (auto target : targets) { + targets_map_.insert({ target.GetId(), target }); + } + } + + std::shared_ptr Predict( + const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { + std::unordered_map> + armor_map; + + const auto& dt = time_stamp - last_time_stamp_; + last_time_stamp_ = time_stamp; + + for (auto& target_value : targets_map_) { + target_value.second.Predict(dt); + auto x = target_value.second.GetEkf_x(); + + for (int j = 0; j < target_value.second.GetArmorNum(); ++j) { + auto angle = util::math::clamp_pm_pi( + x[6] + j * 2 * CV_PI / target_value.second.GetArmorNum()); + Eigen::Vector3d xyz = h_armor_xyz(x, j, target_value.second.GetArmorNum()); + data::ArmorGimbalControlSpacing armor; + armor.id = target_value.second.GetId(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15 / 180. * CV_PI, 0); + } + } + return std::make_shared(armor_map, last_time_stamp_); + } + + std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { + std::vector targets; + for (int i; i < 8; i++) { + const auto& car_id = static_cast(1 << i); + if (enumeration::IsFlagContains(id, car_id)) { + targets.emplace_back(ConstructTargets(car_id, )); // TODO + } + } + return std::make_shared(targets); + } + +private: + std::vector ConstructTargets( + const enumeration::ArmorIdFlag& id, std::time_t& time_stamp) const { + std::vector targets; + for (int i = 0; i < 8; ++i) { + auto car_id = static_cast(1 << i); + if (!enumeration::IsFlagContains(id, car_id)) continue; + + bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII + || car_id == enumeration::CarIDFlag::InfantryIV + || car_id == enumeration::CarIDFlag::InfantryV); + + Eigen::VectorXd P0_dig(11); + double radius; + int armor_num; + + if (is_balance) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 2; + } else if (car_id == enumeration::CarIDFlag::Outpost) { + P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; + radius = 0.2765; + armor_num = 3; + } else if (car_id == enumeration::CarIDFlag::Base) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; + radius = 0.3205; + armor_num = 3; + } else { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 4; + } + // 不知道这个armor_xyz_in_world、armor_ypr_in_world（可以理解为x）可以从哪里传入 + Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); + Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); + targets.emplace_back(Target(armor_xyz_in_world, armor_ypr_in_world, car_id, time_stamp, + radius, armor_num, P0_dig)); + } + return targets; + } + + // 计算出装甲板中心的坐标（考虑长短轴） + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id, int armor_num) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num); + auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto armor_x = x[0] - r * std::cos(angle); + auto armor_y = x[2] - r * std::sin(angle); + auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; + + return { armor_x, armor_y, armor_z }; + } + + std::time_t last_time_stamp_ { 0 }; + std::unordered_map targets_map_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/target_predict.hpp b/src/tongji/predictor/target_predict.hpp new file mode 100644 index 0000000..0acf243 --- /dev/null +++ b/src/tongji/predictor/target_predict.hpp @@ -0,0 +1,15 @@ +#pragma once + +#include + +#include "interfaces/target_predictor.hpp" + +namespace world_exe::tongji::predictor { +class TargetPredict final : public interfaces::ITargetPredictor { +public: + +private: + class Impl; + std::unique_ptr pimpl_; +}; +} \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.cpp b/src/tongji/predictor/target_predictor.cpp index ee576ca..9697080 100644 --- a/src/tongji/predictor/target_predictor.cpp +++ b/src/tongji/predictor/target_predictor.cpp @@ -7,13 +7,10 @@ #include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "target.hpp" -#include "tongji/predictor/target_snapshot_predictor.hpp" namespace world_exe::tongji::predictor { std::vector ConstructTargets(const enumeration::ArmorIdFlag& id, std::time_t time_stamp) { - std::vector targets; - for (int i = 0; i < 8; ++i) { auto car_id = static_cast(1 << i); if (!enumeration::IsFlagContains(id, car_id)) continue; @@ -46,39 +43,21 @@ std::vector ConstructTargets(const enumeration::ArmorIdFlag& id, std::ti Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); - - targets.emplace_back(Target( - armor_xyz_in_world, armor_ypr_in_world, car_id, time_stamp, radius, armor_num, P0_dig)); - return targets; } - - return targets; } class TargetPredictor::Impl { public: - Impl(const std::vector& targets) - : selected_targets_(targets) { } + Impl() { } std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { - - } + const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { } std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { - auto targets = ConstructTargets(id, last_update_time_stamp_); - return std::make_shared(targets); - } - - void Update(const std::shared_ptr& data) { - for (auto target : selected_targets_) { - target.Update(data); - } + } private: - std::vector selected_targets_; - std::time_t last_update_time_stamp_ { 0 }; }; @@ -93,5 +72,5 @@ std ::shared_ptr TargetPredictor::GetPredictor( } TargetPredictor::TargetPredictor(const enumeration::ArmorIdFlag& id, std::time_t t) - : pimpl_(std::make_unique(ConstructTargets(id, t))) { } + : pimpl_() { } } \ No newline at end of file From 9596d7ad9997b3bc5a58eed4db983ad2b20e6bff Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 18 Sep 2025 21:54:30 +0800 Subject: [PATCH 24/93] WIP: partially implement ITargetPredictor interface --- src/tongji/predictor/target_predict.cpp | 118 ----------------- src/tongji/predictor/target_predict.hpp | 15 --- src/tongji/predictor/target_predictor.cpp | 150 ++++++++++++++-------- src/tongji/predictor/target_predictor.hpp | 10 +- 4 files changed, 98 insertions(+), 195 deletions(-) delete mode 100644 src/tongji/predictor/target_predict.cpp delete mode 100644 src/tongji/predictor/target_predict.hpp diff --git a/src/tongji/predictor/target_predict.cpp b/src/tongji/predictor/target_predict.cpp deleted file mode 100644 index bed8a9e..0000000 --- a/src/tongji/predictor/target_predict.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#include "target_predict.hpp" - -#include -#include -#include - -#include "enum/car_id.hpp" -#include "enum/enum_tools.hpp" -#include "tongji/predictor/in_gimbal_control_armor.hpp" -#include "tongji/predictor/target.hpp" -#include "tongji/predictor/target_snapshot_predictor.hpp" - -namespace world_exe::tongji::predictor { - -class TargetPredict::Impl { -public: - Impl(std::list targets) { - for (auto target : targets) { - targets_map_.insert({ target.GetId(), target }); - } - } - - std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { - std::unordered_map> - armor_map; - - const auto& dt = time_stamp - last_time_stamp_; - last_time_stamp_ = time_stamp; - - for (auto& target_value : targets_map_) { - target_value.second.Predict(dt); - auto x = target_value.second.GetEkf_x(); - - for (int j = 0; j < target_value.second.GetArmorNum(); ++j) { - auto angle = util::math::clamp_pm_pi( - x[6] + j * 2 * CV_PI / target_value.second.GetArmorNum()); - Eigen::Vector3d xyz = h_armor_xyz(x, j, target_value.second.GetArmorNum()); - data::ArmorGimbalControlSpacing armor; - armor.id = target_value.second.GetId(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15 / 180. * CV_PI, 0); - } - } - return std::make_shared(armor_map, last_time_stamp_); - } - - std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { - std::vector targets; - for (int i; i < 8; i++) { - const auto& car_id = static_cast(1 << i); - if (enumeration::IsFlagContains(id, car_id)) { - targets.emplace_back(ConstructTargets(car_id, )); // TODO - } - } - return std::make_shared(targets); - } - -private: - std::vector ConstructTargets( - const enumeration::ArmorIdFlag& id, std::time_t& time_stamp) const { - std::vector targets; - for (int i = 0; i < 8; ++i) { - auto car_id = static_cast(1 << i); - if (!enumeration::IsFlagContains(id, car_id)) continue; - - bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII - || car_id == enumeration::CarIDFlag::InfantryIV - || car_id == enumeration::CarIDFlag::InfantryV); - - Eigen::VectorXd P0_dig(11); - double radius; - int armor_num; - - if (is_balance) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 2; - } else if (car_id == enumeration::CarIDFlag::Outpost) { - P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; - radius = 0.2765; - armor_num = 3; - } else if (car_id == enumeration::CarIDFlag::Base) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; - radius = 0.3205; - armor_num = 3; - } else { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 4; - } - // 不知道这个armor_xyz_in_world、armor_ypr_in_world（可以理解为x）可以从哪里传入 - Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); - Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); - targets.emplace_back(Target(armor_xyz_in_world, armor_ypr_in_world, car_id, time_stamp, - radius, armor_num, P0_dig)); - } - return targets; - } - - // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id, int armor_num) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num); - auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); - - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto armor_x = x[0] - r * std::cos(angle); - auto armor_y = x[2] - r * std::sin(angle); - auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; - - return { armor_x, armor_y, armor_z }; - } - - std::time_t last_time_stamp_ { 0 }; - std::unordered_map targets_map_; -}; - -} \ No newline at end of file diff --git a/src/tongji/predictor/target_predict.hpp b/src/tongji/predictor/target_predict.hpp deleted file mode 100644 index 0acf243..0000000 --- a/src/tongji/predictor/target_predict.hpp +++ /dev/null @@ -1,15 +0,0 @@ -#pragma once - -#include - -#include "interfaces/target_predictor.hpp" - -namespace world_exe::tongji::predictor { -class TargetPredict final : public interfaces::ITargetPredictor { -public: - -private: - class Impl; - std::unique_ptr pimpl_; -}; -} \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.cpp b/src/tongji/predictor/target_predictor.cpp index 9697080..7c41bf0 100644 --- a/src/tongji/predictor/target_predictor.cpp +++ b/src/tongji/predictor/target_predictor.cpp @@ -1,76 +1,118 @@ #include "target_predictor.hpp" -#include +#include #include -#include +#include #include "enum/car_id.hpp" #include "enum/enum_tools.hpp" -#include "target.hpp" +#include "tongji/predictor/in_gimbal_control_armor.hpp" +#include "tongji/predictor/target.hpp" +#include "tongji/predictor/target_snapshot_predictor.hpp" namespace world_exe::tongji::predictor { -std::vector ConstructTargets(const enumeration::ArmorIdFlag& id, std::time_t time_stamp) { - for (int i = 0; i < 8; ++i) { - auto car_id = static_cast(1 << i); - if (!enumeration::IsFlagContains(id, car_id)) continue; - - bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII - || car_id == enumeration::CarIDFlag::InfantryIV - || car_id == enumeration::CarIDFlag::InfantryV); - - Eigen::VectorXd P0_dig(11); - double radius; - int armor_num; - - if (is_balance) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 2; - } else if (car_id == enumeration::CarIDFlag::Outpost) { - P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; - radius = 0.2765; - armor_num = 3; - } else if (car_id == enumeration::CarIDFlag::Base) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; - radius = 0.3205; - armor_num = 3; - } else { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 4; +class TargetPredict::Impl { +public: + Impl(std::list targets) { + for (auto target : targets) { + targets_map_.insert({ target.GetId(), target }); } - - Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); - Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); } -} - -class TargetPredictor::Impl { -public: - Impl() { } std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { } + const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { + std::unordered_map> + armor_map; + + const auto& dt = time_stamp - last_time_stamp_; + last_time_stamp_ = time_stamp; + + for (auto& target_value : targets_map_) { + target_value.second.Predict(dt); + auto x = target_value.second.GetEkf_x(); + + for (int j = 0; j < target_value.second.GetArmorNum(); ++j) { + auto angle = util::math::clamp_pm_pi( + x[6] + j * 2 * CV_PI / target_value.second.GetArmorNum()); + Eigen::Vector3d xyz = h_armor_xyz(x, j, target_value.second.GetArmorNum()); + data::ArmorGimbalControlSpacing armor; + armor.id = target_value.second.GetId(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15 / 180. * CV_PI, 0); + } + } + return std::make_shared(armor_map, last_time_stamp_); + } std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { - + std::vector targets; + for (int i; i < 8; i++) { + const auto& car_id = static_cast(1 << i); + if (enumeration::IsFlagContains(id, car_id)) { + targets.emplace_back(ConstructTargets(car_id, )); // TODO + } + } + return std::make_shared(targets); } private: - std::time_t last_update_time_stamp_ { 0 }; -}; + std::vector ConstructTargets( + const enumeration::ArmorIdFlag& id, std::time_t& time_stamp) const { + std::vector targets; + for (int i = 0; i < 8; ++i) { + auto car_id = static_cast(1 << i); + if (!enumeration::IsFlagContains(id, car_id)) continue; + + bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII + || car_id == enumeration::CarIDFlag::InfantryIV + || car_id == enumeration::CarIDFlag::InfantryV); + + Eigen::VectorXd P0_dig(11); + double radius; + int armor_num; + + if (is_balance) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 2; + } else if (car_id == enumeration::CarIDFlag::Outpost) { + P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; + radius = 0.2765; + armor_num = 3; + } else if (car_id == enumeration::CarIDFlag::Base) { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; + radius = 0.3205; + armor_num = 3; + } else { + P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + radius = 0.2; + armor_num = 4; + } + //TODO 不知道这个armor_xyz_in_world、armor_ypr_in_world（可以理解为x）可以从哪里传入 + Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); + Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); + targets.emplace_back(Target(armor_xyz_in_world, armor_ypr_in_world, car_id, time_stamp, + radius, armor_num, P0_dig)); + } + return targets; + } + + // 计算出装甲板中心的坐标（考虑长短轴） + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id, int armor_num) const { + auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num); + auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); -std ::shared_ptr TargetPredictor::Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { - return pimpl_->Predict(id, time_stamp); -} + auto r = (use_l_h) ? x[8] + x[9] : x[8]; + auto armor_x = x[0] - r * std::cos(angle); + auto armor_y = x[2] - r * std::sin(angle); + auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; -std ::shared_ptr TargetPredictor::GetPredictor( - const enumeration ::ArmorIdFlag& id) const { - return pimpl_->GetPredictor(id); -} + return { armor_x, armor_y, armor_z }; + } + + std::time_t last_time_stamp_ { 0 }; + std::unordered_map targets_map_; +}; -TargetPredictor::TargetPredictor(const enumeration::ArmorIdFlag& id, std::time_t t) - : pimpl_() { } } \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.hpp b/src/tongji/predictor/target_predictor.hpp index 8958794..0acf243 100644 --- a/src/tongji/predictor/target_predictor.hpp +++ b/src/tongji/predictor/target_predictor.hpp @@ -5,15 +5,9 @@ #include "interfaces/target_predictor.hpp" namespace world_exe::tongji::predictor { -class TargetPredictor final : public interfaces::ITargetPredictor { +class TargetPredict final : public interfaces::ITargetPredictor { public: - TargetPredictor(const enumeration::ArmorIdFlag& id, std::time_t t); - std ::shared_ptr Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; - - std ::shared_ptr GetPredictor( - const enumeration ::ArmorIdFlag& id) const override; - + private: class Impl; std::unique_ptr pimpl_; From dc596de7bba6069d2ce46a0c81cf0e8e262f9a89 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 20 Sep 2025 15:56:55 +0800 Subject: [PATCH 25/93] refactor(prediction): refactor IPredictor and ITargetPredictor implementations --- .../predictor/in_gimbal_control_armor.hpp | 18 +-- src/tongji/predictor/predictor_record.cpp | 58 ++++++++ ...hot_predictor.hpp => predictor_record.hpp} | 9 +- src/tongji/predictor/target.hpp | 64 ++++++--- src/tongji/predictor/target_predictor.cpp | 131 ++++++------------ src/tongji/predictor/target_predictor.hpp | 14 +- .../predictor/target_snapshot_predictor.cpp | 88 ------------ 7 files changed, 176 insertions(+), 206 deletions(-) create mode 100644 src/tongji/predictor/predictor_record.cpp rename src/tongji/predictor/{target_snapshot_predictor.hpp => predictor_record.hpp} (58%) delete mode 100644 src/tongji/predictor/target_snapshot_predictor.cpp diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index a8f48c9..0d6cef2 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -12,18 +13,16 @@ namespace world_exe::tongji::predictor { class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { public: - InGimbalControlArmor( - std::unordered_map> - armors, - TimeStamp time_stamp) - : armors_(armors) + InGimbalControlArmor(const std::unordered_map>& all_armors, + const std::time_t& time_stamp) + : armors_map_(all_armors) , time_stamp_(time_stamp) { } const std ::vector& GetArmors( const enumeration ::ArmorIdFlag& armor_id) const override { - static const std::vector empty; - auto it = armors_.find(armor_id); - return it != armors_.end() ? it->second : empty; + auto it = armors_map_.find(armor_id); + return it != armors_map_.end() ? it->second : empty; } const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } @@ -31,6 +30,7 @@ class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { private: TimeStamp time_stamp_; std::unordered_map> - armors_; + armors_map_; + static const std::vector empty; }; } \ No newline at end of file diff --git a/src/tongji/predictor/predictor_record.cpp b/src/tongji/predictor/predictor_record.cpp new file mode 100644 index 0000000..b040c8c --- /dev/null +++ b/src/tongji/predictor/predictor_record.cpp @@ -0,0 +1,58 @@ +#include "predictor_record.hpp" + +#include +#include +#include + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/car_id.hpp" +#include "enum/enum_tools.hpp" +#include "in_gimbal_control_armor.hpp" + +namespace world_exe::tongji::predictor { +class PredictorRecord::Impl { +public: + Impl(const enumeration::ArmorIdFlag& id, + const std::unordered_map> snapshots) + : id_(id) + , snapshots_(snapshots) { } + + const enumeration::ArmorIdFlag& GetId() const { return id_; } + + std::shared_ptr Predictor(const std::time_t& time_stamp) { + std::unordered_map> + result; + + for (int i = 0; i < 8; i++) { + auto decomposed_id = static_cast( + static_cast(enumeration::CarIDFlag::Hero) << i); + if (enumeration::IsFlagContains(id_, decomposed_id)) { + auto it = snapshots_.find(decomposed_id); + if (it == snapshots_.end()) continue; + + auto copy = std::make_shared(*(it->second)); + copy->Predict(time_stamp); + result[decomposed_id] = copy->GetArmorGimbalControlSpacings(); + } + } + + return std::make_shared(result, time_stamp); + } + +private: + std::unordered_map> snapshots_; + enumeration::ArmorIdFlag id_; +}; + +PredictorRecord::PredictorRecord(const enumeration::ArmorIdFlag& id, + const std::unordered_map>& snapshots) + : pimpl_(std::make_unique(id, snapshots)) { } +PredictorRecord::~PredictorRecord() = default; + +const enumeration ::ArmorIdFlag& PredictorRecord::GetId() const { return pimpl_->GetId(); } + +std ::shared_ptr PredictorRecord::Predictor( + const std ::time_t& time_stamp) const { + return pimpl_->Predictor(time_stamp); +} +} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_predictor.hpp b/src/tongji/predictor/predictor_record.hpp similarity index 58% rename from src/tongji/predictor/target_snapshot_predictor.hpp rename to src/tongji/predictor/predictor_record.hpp index cea2fad..409c37f 100644 --- a/src/tongji/predictor/target_snapshot_predictor.hpp +++ b/src/tongji/predictor/predictor_record.hpp @@ -1,15 +1,20 @@ #pragma once #include +#include +#include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" #include "tongji/predictor/target.hpp" namespace world_exe::tongji::predictor { -class MultiTargetSnapshotPredictor final : public interfaces::IPredictor { +class PredictorRecord final : public interfaces::IPredictor { public: - MultiTargetSnapshotPredictor(const std::vector& targets); + PredictorRecord(const enumeration::ArmorIdFlag& id, + const std::unordered_map>& snapshots); + ~PredictorRecord(); + const enumeration ::ArmorIdFlag& GetId() const override; std ::shared_ptr Predictor( diff --git a/src/tongji/predictor/target.hpp b/src/tongji/predictor/target.hpp index 3f6ec3c..7861f1b 100644 --- a/src/tongji/predictor/target.hpp +++ b/src/tongji/predictor/target.hpp @@ -2,11 +2,13 @@ #include #include +#include +#include +#include "data/armor_gimbal_control_spacing.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" #include "interfaces/predictor_update_package.hpp" -#include "tongji/predictor/time_stamp.hpp" #include "util/extended_kalman_filter.hpp" #include "util/math.hpp" @@ -32,7 +34,7 @@ class Target { Target(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const enumeration::ArmorIdFlag& id, const std::time_t& t, const double& radius, const int& armor_num, const Eigen::VectorXd& P0_dig) - : time_stamp_(t) + : last_time_stamp_(t) , car_id_(id) , armor_num_(armor_num) { @@ -64,26 +66,16 @@ class Target { Eigen::VectorXd GetEkf_x() const { return ekf_.x; } - void Predict(double dt) { - // 防止夹角求和出现异常值 - auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { - Eigen::VectorXd x_prior = this->F(dt) * x; - x_prior[6] = util::math::clamp_pm_pi(x_prior[6]); - return x_prior; - }; - - // 前哨站转速特判 - if (this->Convergened() && this->car_id_ == enumeration::CarIDFlag::Outpost - && std::abs(this->ekf_.x[7]) > 2) - this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; - - ekf_.Predict(this->F(dt), this->Q(dt), f); - } - - std::time_t GetTimeStamp() const { return time_stamp_.GetTimeStamp(); } + std::time_t GetTimeStamp() const { return last_time_stamp_; } int GetArmorNum() const { return armor_num_; } enumeration::CarIDFlag GetId() const { return car_id_; } + void Predict(const std::time_t& time_stamp) { + const double dt = std::difftime(time_stamp, last_time_stamp_); + Predict(dt); + last_time_stamp_ = time_stamp; + } + void Update(std::shared_ptr data) { const auto& transform = data->GetTransform(); const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); @@ -123,7 +115,38 @@ class Target { return true; } + std::vector GetArmorGimbalControlSpacings() { + std::vector armors; + for (int id = 0; id < armor_num_; id++) { + auto angle = util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / armor_num_); + auto xyz = h_armor_xyz(this->ekf_.x, id); + + data::ArmorGimbalControlSpacing armor; + armor.id = car_id_; + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armors.emplace_back(std::move(armor)); + } + return armors; + } + private: + void Predict(double dt) { + // 防止夹角求和出现异常值 + auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { + Eigen::VectorXd x_prior = this->F(dt) * x; + x_prior[6] = util::math::clamp_pm_pi(x_prior[6]); + return x_prior; + }; + + // 前哨站转速特判 + if (this->Convergened() && this->car_id_ == enumeration::CarIDFlag::Outpost + && std::abs(this->ekf_.x[7]) > 2) + this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; + + ekf_.Predict(this->F(dt), this->Q(dt), f); + } + void Update(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { // 装甲板匹配 @@ -321,10 +344,9 @@ auto Q(double dt)->Eigen::MatrixXd{ // clang-format on } - TimeStamp time_stamp_; + std::time_t last_time_stamp_; util::ExtendedKalmanFilter ekf_; enumeration::CarIDFlag car_id_; int armor_num_; }; - } \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.cpp b/src/tongji/predictor/target_predictor.cpp index 7c41bf0..436b823 100644 --- a/src/tongji/predictor/target_predictor.cpp +++ b/src/tongji/predictor/target_predictor.cpp @@ -1,118 +1,79 @@ #include "target_predictor.hpp" -#include #include #include +#include -#include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "tongji/predictor/in_gimbal_control_armor.hpp" +#include "tongji/predictor/predictor_record.hpp" #include "tongji/predictor/target.hpp" -#include "tongji/predictor/target_snapshot_predictor.hpp" namespace world_exe::tongji::predictor { class TargetPredict::Impl { public: - Impl(std::list targets) { - for (auto target : targets) { - targets_map_.insert({ target.GetId(), target }); - } + Impl(std::unordered_map> targets) { } + void RegisterTarget(enumeration::ArmorIdFlag id, const Target& target) { + targets_[id] = std::make_shared(target); } + void RemoveTarget(enumeration::ArmorIdFlag id) { } + std::shared_ptr Predict( const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { std::unordered_map> - armor_map; - - const auto& dt = time_stamp - last_time_stamp_; - last_time_stamp_ = time_stamp; - - for (auto& target_value : targets_map_) { - target_value.second.Predict(dt); - auto x = target_value.second.GetEkf_x(); - - for (int j = 0; j < target_value.second.GetArmorNum(); ++j) { - auto angle = util::math::clamp_pm_pi( - x[6] + j * 2 * CV_PI / target_value.second.GetArmorNum()); - Eigen::Vector3d xyz = h_armor_xyz(x, j, target_value.second.GetArmorNum()); - data::ArmorGimbalControlSpacing armor; - armor.id = target_value.second.GetId(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15 / 180. * CV_PI, 0); + result; + + for (int i = 0; i < 8; i++) { + auto decomposed_id = static_cast( + static_cast(enumeration::CarIDFlag::Hero) << i); + if (enumeration::IsFlagContains(id, decomposed_id)) { + auto it = targets_.find(decomposed_id); + if (it == targets_.end()) continue; + + it->second->Predict(time_stamp); + result[decomposed_id] = it->second->GetArmorGimbalControlSpacings(); } } - return std::make_shared(armor_map, last_time_stamp_); + + return std::make_shared(result, time_stamp); } std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { - std::vector targets; - for (int i; i < 8; i++) { - const auto& car_id = static_cast(1 << i); - if (enumeration::IsFlagContains(id, car_id)) { - targets.emplace_back(ConstructTargets(car_id, )); // TODO - } - } - return std::make_shared(targets); - } + std::unordered_map> snapshot_map; -private: - std::vector ConstructTargets( - const enumeration::ArmorIdFlag& id, std::time_t& time_stamp) const { - std::vector targets; - for (int i = 0; i < 8; ++i) { - auto car_id = static_cast(1 << i); - if (!enumeration::IsFlagContains(id, car_id)) continue; - - bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII - || car_id == enumeration::CarIDFlag::InfantryIV - || car_id == enumeration::CarIDFlag::InfantryV); - - Eigen::VectorXd P0_dig(11); - double radius; - int armor_num; - - if (is_balance) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 2; - } else if (car_id == enumeration::CarIDFlag::Outpost) { - P0_dig << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; - radius = 0.2765; - armor_num = 3; - } else if (car_id == enumeration::CarIDFlag::Base) { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; - radius = 0.3205; - armor_num = 3; - } else { - P0_dig << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - radius = 0.2; - armor_num = 4; + for (int i = 0; i < 8; i++) { + auto decomposed_id = static_cast( + static_cast(enumeration::CarIDFlag::Hero) << i); + if (enumeration::IsFlagContains(id, decomposed_id)) { + + auto it = targets_.find(decomposed_id); + if (it == targets_.end()) continue; + + snapshot_map[decomposed_id] = std::make_shared(*(it->second)); } - //TODO 不知道这个armor_xyz_in_world、armor_ypr_in_world（可以理解为x）可以从哪里传入 - Eigen::Vector3d armor_xyz_in_world = Eigen::Vector3d::Zero(); - Eigen::Vector3d armor_ypr_in_world = Eigen::Vector3d::Zero(); - targets.emplace_back(Target(armor_xyz_in_world, armor_ypr_in_world, car_id, time_stamp, - radius, armor_num, P0_dig)); } - return targets; + return std::make_shared(id, snapshot_map); } - // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id, int armor_num) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num); - auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); +private: + std::unordered_map> targets_; +}; - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto armor_x = x[0] - r * std::cos(angle); - auto armor_y = x[2] - r * std::sin(angle); - auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; +TargetPredict::TargetPredict( + std::unordered_map> targets) + : pimpl_(std::make_unique(std::move(targets))) { } +TargetPredict::~TargetPredict() = default; - return { armor_x, armor_y, armor_z }; - } +std ::shared_ptr TargetPredict::Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { + return pimpl_->Predict(id, time_stamp); +} - std::time_t last_time_stamp_ { 0 }; - std::unordered_map targets_map_; -}; +std ::shared_ptr TargetPredict::GetPredictor( + const enumeration ::ArmorIdFlag& id) const { + return pimpl_->GetPredictor(id); +} } \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.hpp b/src/tongji/predictor/target_predictor.hpp index 0acf243..d13ced6 100644 --- a/src/tongji/predictor/target_predictor.hpp +++ b/src/tongji/predictor/target_predictor.hpp @@ -2,14 +2,26 @@ #include +#include "enum/armor_id.hpp" #include "interfaces/target_predictor.hpp" +#include "target.hpp" namespace world_exe::tongji::predictor { + class TargetPredict final : public interfaces::ITargetPredictor { public: - + TargetPredict(std::unordered_map> targets); + ~TargetPredict(); + + std ::shared_ptr Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; + + std ::shared_ptr GetPredictor( + const enumeration ::ArmorIdFlag& id) const override; + private: class Impl; std::unique_ptr pimpl_; }; + } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_predictor.cpp b/src/tongji/predictor/target_snapshot_predictor.cpp deleted file mode 100644 index 4bc42f9..0000000 --- a/src/tongji/predictor/target_snapshot_predictor.cpp +++ /dev/null @@ -1,88 +0,0 @@ -#include "target_snapshot_predictor.hpp" - -#include -#include -#include - -#include -#include - -#include "data/armor_gimbal_control_spacing.hpp" -#include "enum/armor_id.hpp" -#include "enum/enum_tools.hpp" -#include "in_gimbal_control_armor.hpp" -#include "target.hpp" -#include "util/math.hpp" - -namespace world_exe::tongji::predictor { -class MultiTargetSnapshotPredictor::Impl { -public: - Impl(const std::vector& targets) - : targets_(targets) - , last_time_stamp_(0) { - id_ = enumeration::ArmorIdFlag::Unknow; - for (const auto& target : targets_) { - id_ = static_cast( - static_cast(id_) | static_cast(target.GetId())); - } - } - - const enumeration::ArmorIdFlag& GetId() const { return id_; } - - std::shared_ptr Predictor(const std::time_t& time_stamp) { - std::unordered_map> - armor_map; - const double dt = static_cast(time_stamp - last_time_stamp_); - - for (auto& target : targets_) { - Target iteration_target = target; - iteration_target.Predict(dt); - - Eigen::VectorXd iteration_x = iteration_target.GetEkf_x(); - std::vector armors; - - for (int j = 0; j < iteration_target.GetArmorNum(); ++j) { - auto angle = util::math::clamp_pm_pi( - iteration_x[6] + j * 2 * CV_PI / iteration_target.GetArmorNum()); - Eigen::Vector3d xyz = h_armor_xyz(iteration_x, j, iteration_target.GetArmorNum()); - data::ArmorGimbalControlSpacing armor; - armor.id = iteration_target.GetId(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15 / 180. * CV_PI, 0); - } - armor_map[iteration_target.GetId()] = armors; - } - return std::make_shared(armor_map, last_time_stamp_); - } - -private: - std::vector targets_; - enumeration::ArmorIdFlag id_; - std::time_t last_time_stamp_; - - // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id, int armor_num) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num); - auto use_l_h = (armor_num == 4) && (id == 1 || id == 3); - - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto armor_x = x[0] - r * std::cos(angle); - auto armor_y = x[2] - r * std::sin(angle); - auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; - - return { armor_x, armor_y, armor_z }; - } -}; - -MultiTargetSnapshotPredictor::MultiTargetSnapshotPredictor(const std::vector& targets) - : pimpl_(std::make_unique(targets)) { } - -const enumeration ::ArmorIdFlag& MultiTargetSnapshotPredictor::GetId() const { - return pimpl_->GetId(); -} - -std ::shared_ptr MultiTargetSnapshotPredictor::Predictor( - const std ::time_t& time_stamp) const { - return pimpl_->Predictor(time_stamp); -} -} \ No newline at end of file From 0dcffbf46dd6f0051d4efc5bbf7eb01a8324bb08 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 22 Sep 2025 10:33:57 +0800 Subject: [PATCH 26/93] refactor(predictor): overhaul predictor implementation --- .../predictor/in_gimbal_control_armor.hpp | 2 +- src/tongji/predictor/live_target.hpp | 149 ++++++++ src/tongji/predictor/live_target_manager.cpp | 104 ++++++ ..._predictor.hpp => live_target_manager.hpp} | 8 +- src/tongji/predictor/predict_model.hpp | 258 +++++++++++++ src/tongji/predictor/predictor_record.cpp | 58 --- src/tongji/predictor/target.hpp | 352 ------------------ src/tongji/predictor/target_predictor.cpp | 79 ---- src/tongji/predictor/target_snapshot.hpp | 36 ++ .../predictor/target_snapshot_manager.cpp | 51 +++ ...record.hpp => target_snapshot_manager.hpp} | 10 +- src/util/extended_kalman_filter.hpp | 46 +-- 12 files changed, 624 insertions(+), 529 deletions(-) create mode 100644 src/tongji/predictor/live_target.hpp create mode 100644 src/tongji/predictor/live_target_manager.cpp rename src/tongji/predictor/{target_predictor.hpp => live_target_manager.hpp} (70%) create mode 100644 src/tongji/predictor/predict_model.hpp delete mode 100644 src/tongji/predictor/predictor_record.cpp delete mode 100644 src/tongji/predictor/target.hpp delete mode 100644 src/tongji/predictor/target_predictor.cpp create mode 100644 src/tongji/predictor/target_snapshot.hpp create mode 100644 src/tongji/predictor/target_snapshot_manager.cpp rename src/tongji/predictor/{predictor_record.hpp => target_snapshot_manager.hpp} (67%) diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index 0d6cef2..117203a 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -16,7 +16,7 @@ class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { InGimbalControlArmor(const std::unordered_map>& all_armors, const std::time_t& time_stamp) - : armors_map_(all_armors) + : armors_map_(std::move(all_armors)) , time_stamp_(time_stamp) { } const std ::vector& GetArmors( diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp new file mode 100644 index 0000000..102c2d6 --- /dev/null +++ b/src/tongji/predictor/live_target.hpp @@ -0,0 +1,149 @@ +#pragma once + +#include +#include +#include + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/car_id.hpp" +#include "predict_model.hpp" +#include "util/extended_kalman_filter.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { + +struct TargetStatus { + bool jumped = false; + bool switched = false; + bool converged = false; + bool diverged = false; + bool lost = false; + bool reidentified = false; + int last_id = -1; + int switch_count = 0; + int update_count = 0; + int lost_count = 0; +}; + +class LiveTarget { +public: + TargetStatus status_; + + LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, + const enumeration::CarIDFlag& car_id, const std::time_t& t) + : last_time_stamp_(t) + , model_(car_id) { + + // x vx y vy z vz a w r l h + // a: angle + // w: angular velocity + // l: r2 - r1 + // h: z2 - z1 + auto center_x = + armor_xyz_in_world[0] + model_.GetRadius() * std::cos(armor_ypr_in_world[0]); + auto center_y = + armor_xyz_in_world[1] + model_.GetRadius() * std::sin(armor_ypr_in_world[0]); + auto center_z = armor_xyz_in_world[2]; + + Eigen::VectorXd x0(11); + x0 << center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, model_.GetRadius(), + 0, 0; + + Eigen::MatrixXd P0 = model_.GetP0Dig().asDiagonal(); + ekf_ = util::ExtendedKalmanFilter( + x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） + } + + Eigen::VectorXd GetEkfX() const { return ekf_.x; } + Eigen::VectorXd GetP0Dig() const { return model_.GetP0Dig(); } + const PredictModel& GetModel() const { return model_; } + std::time_t GetTimeStamp() const { return last_time_stamp_; } + + std::vector GetArmorGimbalControlSpacings() { + std::vector armors; + for (int id = 0; id < model_.GetArmorNum(); id++) { + auto angle = + util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto xyz = model_.h_armor_xyz(this->ekf_.x, id); + + data::ArmorGimbalControlSpacing armor; + armor.id = model_.GetID(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armors.emplace_back(std::move(armor)); + } + return armors; + } + + void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { + + // 装甲板匹配 + int id = + model_.MatchArmor(ekf_.x, armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); + if (id != 0) status_.jumped = true; + status_.switched = (id != status_.last_id); + if (status_.switched) status_.switch_count++; + + status_.last_id = id; + Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id, dt); + status_.update_count++; + } + +private: + bool Converged() { + if (model_.GetID() != enumeration::CarIDFlag::Outpost && status_.update_count > 3 + && !this->Diverged()) { + status_.converged = true; + } + + // 前哨站特殊判断 + if (model_.GetID() == enumeration::CarIDFlag::Outpost && status_.update_count > 10 + && !this->Diverged()) { + status_.converged = true; + } + return status_.converged; + } + + bool Diverged() const { + auto r_ok = ekf_.x[8] > 0.05 && ekf_.x[8] < 0.5; + auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; + + if (r_ok && l_ok) return false; + // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); + return true; + } + + void Update_ypda(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, + const int& id, const double& dt) { + // 观测jacobi + auto H = model_.H(ekf_.x, id); + auto R = model_.R(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); + auto A = model_.A(dt); + auto Q = model_.Q(dt); + auto f = model_.f; + auto h = [this, id](const Eigen::VectorXd& x) { return model_.h(x, id); }; + auto z_subtract = model_.z_subtract; + + const Eigen::VectorXd& ypd = armor_ypd_in_world; + const Eigen::VectorXd& ypr = armor_ypr_in_world; + + // 获得观测量 + Eigen::VectorXd z(4); + z << ypd[0], ypd[1], ypd[2], ypr[0]; + + ekf_.Update(dt, A, Q, f, z, H, R, h, z_subtract); + + // 前哨站转速特判 + if (this->Converged() && model_.GetID() == enumeration::CarIDFlag::Outpost + && std::abs(this->ekf_.x[7]) > 2) + this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; + } + + std::time_t last_time_stamp_; + util::ExtendedKalmanFilter ekf_; + PredictModel model_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp new file mode 100644 index 0000000..fda3cb3 --- /dev/null +++ b/src/tongji/predictor/live_target_manager.cpp @@ -0,0 +1,104 @@ +#include "live_target_manager.hpp" + +#include +#include +#include + +#include "enum/armor_id.hpp" +#include "enum/enum_tools.hpp" +#include "interfaces/predictor_update_package.hpp" +#include "tongji/predictor/in_gimbal_control_armor.hpp" +#include "tongji/predictor/live_target.hpp" +#include "tongji/predictor/target_snapshot_manager.hpp" + +namespace world_exe::tongji::predictor { + +class LiveTargetManager::Impl { +public: + Impl() = default; + + void RegisterTarget(enumeration::ArmorIdFlag id, std::shared_ptr target) { + targets_[id] = std::move(target); + } + + void RemoveTarget(enumeration::ArmorIdFlag id) { targets_.erase(id); } + + std::shared_ptr Predict( + const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { + std::unordered_map> + result; + + for (auto id : ExpandArmorIdFlags(flag)) { + auto it = targets_.find(id); + if (it != targets_.end() && it->second) { + result[id] = it->second->GetArmorGimbalControlSpacings(); + } + } + + return std::make_shared(result, time_stamp); + } + + std::shared_ptr GetPredictor( + const enumeration::ArmorIdFlag& flag) const { + std::unordered_map> snapshot_map; + + for (auto id : ExpandArmorIdFlags(flag)) { + auto it = targets_.find(id); + if (it != targets_.end() && it->second) { + snapshot_map[id] = it->second; + } + } + + return std::make_shared(flag, snapshot_map); + } + + void Update(std::shared_ptr data, const double& dt) { + const auto& transform = data->GetTransform(); + const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); + + for (const auto& [id, target] : targets_) { + const auto armors = data->GetArmors()->GetArmors(id); + + if (armors.empty()) continue; + + for (auto armor : armors) { + const auto& armor_in_world_position = transform * armor.position; + const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; + target->Update(dt, armor_in_world_position, + util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), + util::math::xyz2ypd(armor_in_world_position)); + } + } + } + +private: + inline std::vector ExpandArmorIdFlags( + enumeration::ArmorIdFlag flags) const { + std::vector result; + for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { + auto single = static_cast( + static_cast(enumeration::ArmorIdFlag::Hero) << i); + if ((static_cast(flags) & static_cast(single)) != 0) { + result.push_back(single); + } + } + return result; + } + + std::unordered_map> targets_; +}; + +LiveTargetManager::LiveTargetManager() = default; +LiveTargetManager::~LiveTargetManager() = default; + +std ::shared_ptr LiveTargetManager::Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { + return pimpl_->Predict(id, time_stamp); +} + +std ::shared_ptr LiveTargetManager::GetPredictor( + const enumeration ::ArmorIdFlag& id) const { + return pimpl_->GetPredictor(id); +} + +} \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.hpp b/src/tongji/predictor/live_target_manager.hpp similarity index 70% rename from src/tongji/predictor/target_predictor.hpp rename to src/tongji/predictor/live_target_manager.hpp index d13ced6..f78580f 100644 --- a/src/tongji/predictor/target_predictor.hpp +++ b/src/tongji/predictor/live_target_manager.hpp @@ -4,14 +4,14 @@ #include "enum/armor_id.hpp" #include "interfaces/target_predictor.hpp" -#include "target.hpp" + namespace world_exe::tongji::predictor { -class TargetPredict final : public interfaces::ITargetPredictor { +class LiveTargetManager final : public interfaces::ITargetPredictor { public: - TargetPredict(std::unordered_map> targets); - ~TargetPredict(); + LiveTargetManager(); + ~LiveTargetManager(); std ::shared_ptr Predict( const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/predict_model.hpp new file mode 100644 index 0000000..8fe4b43 --- /dev/null +++ b/src/tongji/predictor/predict_model.hpp @@ -0,0 +1,258 @@ +#pragma once + +#include + +#include +#include + +#include "enum/car_id.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { + +class PredictModel { +public: + PredictModel(const enumeration::CarIDFlag& car_id) + : car_id_(car_id) { + + bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII + || car_id == enumeration::CarIDFlag::InfantryIV + || car_id == enumeration::CarIDFlag::InfantryV); + + P0_dig_.resize(11); + if (is_balance) { + P0_dig_ << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + } else if (car_id == enumeration::CarIDFlag::Outpost) { + P0_dig_ << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; + } else if (car_id == enumeration::CarIDFlag::Base) { + P0_dig_ << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; + } else { + P0_dig_ << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; + } + + if (car_id == enumeration::CarIDFlag::Outpost) { + radius_ = 0.2765; + } else if (car_id == enumeration::CarIDFlag::Base) { + radius_ = 0.3205; + } else { + radius_ = 0.2; + } + + if (is_balance) { + armor_num_ = 2; + } else if (car_id == enumeration::CarIDFlag::Outpost + | car_id == enumeration::CarIDFlag::Base) { + armor_num_ = 3; + } else { + armor_num_ = 4; + } + } + + auto GetP0Dig() const { return P0_dig_; } + auto GetRadius() const { return radius_; } + auto GetID() const { return car_id_; } + int GetArmorNum() const { return armor_num_; } + + // 防止夹角求和出现异常值 + std::function x_add = + [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { + Eigen::VectorXd c = a + b; + c(6) = util::math::clamp_pm_pi(c(6)); + return c; + }; + + // 防止夹角求和出现异常值 + std::function f = + [=, this](const Eigen::VectorXd& x, const double& dt) -> Eigen::VectorXd { + Eigen::VectorXd x_prior = A(dt) * x; + x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); + return x_prior; + }; + + int MatchArmor(const Eigen::VectorXd& x, const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, + const Eigen::Vector3d& armor_ypd_in_world) const { + + const auto& xyza_list = GetArmorXYZAList(x); + std::vector> xyza_i_list; + + for (int i = 0; i < armor_num_; ++i) { + xyza_i_list.emplace_back(xyza_list[i], i); + } + + std::sort(xyza_i_list.begin(), xyza_i_list.end(), [](const auto& a, const auto& b) { + auto ypd1 = util::math::xyz2ypd(a.first.head(3)); + auto ypd2 = util::math::xyz2ypd(b.first.head(3)); + return ypd1(2) < ypd2(2); + }); + + int best_id = 0; + double min_error = 1e10; + + for (int i = 0; i < std::min(3, armor_num_); ++i) { + const auto& xyza = xyza_i_list[i].first; + auto ypd = util::math::xyz2ypd(xyza.head(3)); + double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_world(0) - xyza(3))) + + std::abs(util::math::clamp_pm_pi(armor_ypd_in_world(0) - ypd(0))); + + if (error < min_error) { + min_error = error; + best_id = xyza_i_list[i].second; + } + } + return best_id; + } + + // 计算出装甲板中心的坐标（考虑长短轴） + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { + auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x(8) + x(9) : x(8); + auto armor_x = x(0) - r * std::cos(angle); + auto armor_y = x(2) - r * std::sin(angle); + auto armor_z = (use_l_h) ? x(4) + x(10) : x(4); + + return { armor_x, armor_y, armor_z }; + } + + Eigen::MatrixXd H(const Eigen::VectorXd& x, int id) const { + auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + + auto r = (use_l_h) ? x(8) + x(9) : x(8); + auto dx_da = r * std::sin(angle); + auto dy_da = -r * std::cos(angle); + + auto dx_dr = -std::cos(angle); + auto dy_dr = -std::sin(angle); + auto dx_dl = (use_l_h) ? -std::cos(angle) : 0.0; + auto dy_dl = (use_l_h) ? -std::sin(angle) : 0.0; + + auto dz_dh = (use_l_h) ? 1.0 : 0.0; + + // clang-format off + Eigen::MatrixXd H_armor_xyza{ + {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, + {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, + {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, + {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} + }; + // clang-format on + + Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); + Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); + // clang-format off + Eigen::MatrixXd H_armor_ypda{ + {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, + {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, + {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, + { 0, 0, 0, 1} + }; + // clang-format on + + return H_armor_ypda * H_armor_xyza; + } + + Eigen::MatrixXd R(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, + int id) { + // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; + auto center_yaw = std::atan2(armor_xyz_in_world(1), armor_xyz_in_world(0)); + auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world(0) - center_yaw); + Eigen::VectorXd R_dig(4); + R_dig << 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, + log(std::abs(armor_ypd_in_world(2)) + 1) / 200 + 9e-2; + + // 测量过程噪声偏差的方差 + return R_dig.asDiagonal(); + } + + Eigen::MatrixXd A(double dt) const { + // 状态转移矩阵 + // clang-format off + return (Eigen::MatrixXd(11, 11) << + 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1).finished(); + } + //clang-format on + + Eigen::MatrixXd Q(double dt) const { + // Piecewise White Noise Model + // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb + double v1, v2; + if (car_id_ == enumeration::CarIDFlag::Outpost) { + v1 = 10; // 前哨站加速度方差 + v2 = 0.1; // 前哨站角加速度方差 + } else { + v1 = 100; // 加速度方差 + v2 = 400; // 角加速度方差 + } + auto a = dt * dt * dt * dt / 4; + auto b = dt * dt * dt / 2; + auto c = dt * dt; + + // 预测过程噪声偏差的方差 + // clang-format off + return (Eigen::MatrixXd(11, 11) << + a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0, + 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(); + // clang-format on + } + + std::function h = [this](const Eigen::VectorXd& x, + int id) { + Eigen::Vector3d xyz = this->h_armor_xyz(x, id); + Eigen::Vector3d ypd = util::math::xyz2ypd(xyz); + double angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); + return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; + }; + + // 防止夹角求差出现异常值 + std::function z_subtract = + [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { + Eigen::VectorXd c = a - b; + c(0) = util::math::clamp_pm_pi(c(0)); + c(1) = util::math::clamp_pm_pi(c(1)); + c(3) = util::math::clamp_pm_pi(c(3)); + return c; + }; + + std::vector GetArmorXYZAList(const Eigen::VectorXd& ekf_x) const { + std::vector _armor_xyza_list; + + for (int i = 0; i < armor_num_; i++) { + auto angle = util::math::clamp_pm_pi(ekf_x(6) + i * 2 * CV_PI / armor_num_); + Eigen::Vector3d xyz = h_armor_xyz(ekf_x, i); + _armor_xyza_list.push_back({ xyz(0), xyz(1), xyz(2), angle }); + } + return _armor_xyza_list; + } + +private: + int armor_num_; + enumeration::CarIDFlag car_id_; + + Eigen::VectorXd P0_dig_; + double radius_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/predictor_record.cpp b/src/tongji/predictor/predictor_record.cpp deleted file mode 100644 index b040c8c..0000000 --- a/src/tongji/predictor/predictor_record.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include "predictor_record.hpp" - -#include -#include -#include - -#include "data/armor_gimbal_control_spacing.hpp" -#include "enum/car_id.hpp" -#include "enum/enum_tools.hpp" -#include "in_gimbal_control_armor.hpp" - -namespace world_exe::tongji::predictor { -class PredictorRecord::Impl { -public: - Impl(const enumeration::ArmorIdFlag& id, - const std::unordered_map> snapshots) - : id_(id) - , snapshots_(snapshots) { } - - const enumeration::ArmorIdFlag& GetId() const { return id_; } - - std::shared_ptr Predictor(const std::time_t& time_stamp) { - std::unordered_map> - result; - - for (int i = 0; i < 8; i++) { - auto decomposed_id = static_cast( - static_cast(enumeration::CarIDFlag::Hero) << i); - if (enumeration::IsFlagContains(id_, decomposed_id)) { - auto it = snapshots_.find(decomposed_id); - if (it == snapshots_.end()) continue; - - auto copy = std::make_shared(*(it->second)); - copy->Predict(time_stamp); - result[decomposed_id] = copy->GetArmorGimbalControlSpacings(); - } - } - - return std::make_shared(result, time_stamp); - } - -private: - std::unordered_map> snapshots_; - enumeration::ArmorIdFlag id_; -}; - -PredictorRecord::PredictorRecord(const enumeration::ArmorIdFlag& id, - const std::unordered_map>& snapshots) - : pimpl_(std::make_unique(id, snapshots)) { } -PredictorRecord::~PredictorRecord() = default; - -const enumeration ::ArmorIdFlag& PredictorRecord::GetId() const { return pimpl_->GetId(); } - -std ::shared_ptr PredictorRecord::Predictor( - const std ::time_t& time_stamp) const { - return pimpl_->Predictor(time_stamp); -} -} \ No newline at end of file diff --git a/src/tongji/predictor/target.hpp b/src/tongji/predictor/target.hpp deleted file mode 100644 index 7861f1b..0000000 --- a/src/tongji/predictor/target.hpp +++ /dev/null @@ -1,352 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "data/armor_gimbal_control_spacing.hpp" -#include "enum/armor_id.hpp" -#include "enum/car_id.hpp" -#include "interfaces/predictor_update_package.hpp" -#include "util/extended_kalman_filter.hpp" -#include "util/math.hpp" - -namespace world_exe::tongji::predictor { - -struct TargetStatus { - bool jumped = false; - bool switched = false; - bool converged = false; - bool diverged = false; - bool lost = false; - bool reidentified = false; - int last_id = -1; - int switch_count = 0; - int update_count = 0; - int lost_count = 0; -}; - -class Target { -public: - TargetStatus status_; - - Target(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - const enumeration::ArmorIdFlag& id, const std::time_t& t, const double& radius, - const int& armor_num, const Eigen::VectorXd& P0_dig) - : last_time_stamp_(t) - , car_id_(id) - , armor_num_(armor_num) { - - auto r = radius; - - // 旋转中心的坐标 - auto center_x = armor_xyz_in_world[0] + r * std::cos(armor_ypr_in_world[0]); - auto center_y = armor_xyz_in_world[1] + r * std::sin(armor_ypr_in_world[0]); - auto center_z = armor_xyz_in_world[2]; - - // x vx y vy z vz a w r l h - // a: angle - // w: angular velocity - // l: r2 - r1 - // h: z2 - z1 - Eigen::VectorXd x0 { { center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, r, - 0, 0 } }; // 初始化预测量 - Eigen::MatrixXd P0 = P0_dig.asDiagonal(); - - // 防止夹角求和出现异常值 - auto x_add = [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) -> Eigen::VectorXd { - Eigen::VectorXd c = a + b; - c[6] = util::math::clamp_pm_pi(c[6]); - return c; - }; - - ekf_ = util::ExtendedKalmanFilter(x0, P0, x_add); // 初始化滤波器（预测量、预测量协方差） - } - - Eigen::VectorXd GetEkf_x() const { return ekf_.x; } - - std::time_t GetTimeStamp() const { return last_time_stamp_; } - int GetArmorNum() const { return armor_num_; } - enumeration::CarIDFlag GetId() const { return car_id_; } - - void Predict(const std::time_t& time_stamp) { - const double dt = std::difftime(time_stamp, last_time_stamp_); - Predict(dt); - last_time_stamp_ = time_stamp; - } - - void Update(std::shared_ptr data) { - const auto& transform = data->GetTransform(); - const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); - const auto armors = data->GetArmors()->GetArmors(car_id_); - - if (armors.empty()) return; - for (auto armor : armors) { - const auto& armor_in_world_position = transform * armor.position; - const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; - this->Update(armor_in_world_position, - util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), - util::math::xyz2ypd(armor_in_world_position)); - } - } - - bool Convergened() { - if (this->car_id_ != enumeration::CarIDFlag::Outpost && status_.update_count > 3 - && !this->Diverged()) { - status_.converged = true; - } - - // 前哨站特殊判断 - if (this->car_id_ == enumeration::CarIDFlag::Outpost && status_.update_count > 10 - && !this->Diverged()) { - status_.converged = true; - } - - return status_.converged; - } - - bool Diverged() const { - auto r_ok = ekf_.x[8] > 0.05 && ekf_.x[8] < 0.5; - auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; - - if (r_ok && l_ok) return false; - // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); - return true; - } - - std::vector GetArmorGimbalControlSpacings() { - std::vector armors; - for (int id = 0; id < armor_num_; id++) { - auto angle = util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / armor_num_); - auto xyz = h_armor_xyz(this->ekf_.x, id); - - data::ArmorGimbalControlSpacing armor; - armor.id = car_id_; - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - armors.emplace_back(std::move(armor)); - } - return armors; - } - -private: - void Predict(double dt) { - // 防止夹角求和出现异常值 - auto f = [&](const Eigen::VectorXd& x) -> Eigen::VectorXd { - Eigen::VectorXd x_prior = this->F(dt) * x; - x_prior[6] = util::math::clamp_pm_pi(x_prior[6]); - return x_prior; - }; - - // 前哨站转速特判 - if (this->Convergened() && this->car_id_ == enumeration::CarIDFlag::Outpost - && std::abs(this->ekf_.x[7]) > 2) - this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; - - ekf_.Predict(this->F(dt), this->Q(dt), f); - } - - void Update(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { - // 装甲板匹配 - int id = MatchArmor(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); - if (id != 0) status_.jumped = true; - status_.switched = (id != status_.last_id); - if (status_.switched) status_.switch_count++; - - status_.last_id = id; - Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); - status_.update_count++; - } - - void Update_ypda(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, - int id) { - // 观测jacobi - Eigen::MatrixXd H = h_jacobian(ekf_.x, id); - // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; - auto center_yaw = std::atan2(armor_xyz_in_world[1], armor_xyz_in_world[0]); - auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world[0] - center_yaw); - Eigen::VectorXd R_dig { { 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, - log(std::abs(armor_ypd_in_world[2]) + 1) / 200 + 9e-2 } }; - - // 测量过程噪声偏差的方差 - Eigen::MatrixXd R = R_dig.asDiagonal(); - - // 定义非线性转换函数h: x -> z - auto h = [&](const Eigen::VectorXd& x) -> Eigen::Vector4d { - Eigen::VectorXd xyz = h_armor_xyz(x, id); - Eigen::VectorXd ypd = util::math::xyz2ypd(xyz); - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); - return { ypd[0], ypd[1], ypd[2], angle }; - }; - - // 防止夹角求差出现异常值 - auto z_subtract = [](const Eigen::VectorXd& a, - const Eigen::VectorXd& b) -> Eigen::VectorXd { - Eigen::VectorXd c = a - b; - c[0] = util::math::clamp_pm_pi(c[0]); - c[1] = util::math::clamp_pm_pi(c[1]); - c[3] = util::math::clamp_pm_pi(c[3]); - return c; - }; - - const Eigen::VectorXd& ypd = armor_ypd_in_world; - const Eigen::VectorXd& ypr = armor_ypr_in_world; - Eigen::VectorXd z { { ypd[0], ypd[1], ypd[2], ypr[0] } }; // 获得观测量 - - ekf_.update(z, H, R, h, z_subtract); - } - - Eigen::MatrixXd h_jacobian(const Eigen::VectorXd& x, int id) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); - auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); - - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto dx_da = r * std::sin(angle); - auto dy_da = -r * std::cos(angle); - - auto dx_dr = -std::cos(angle); - auto dy_dr = -std::sin(angle); - auto dx_dl = (use_l_h) ? -std::cos(angle) : 0.0; - auto dy_dl = (use_l_h) ? -std::sin(angle) : 0.0; - - auto dz_dh = (use_l_h) ? 1.0 : 0.0; - - // clang-format off - Eigen::MatrixXd H_armor_xyza{ - {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, - {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, - {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, - {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} - }; - // clang-format on - - Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); - Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); - // clang-format off - Eigen::MatrixXd H_armor_ypda{ - {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, - {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, - {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, - { 0, 0, 0, 1} - }; - // clang-format on - - return H_armor_ypda * H_armor_xyza; - } - - int MatchArmor(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { - - const auto& xyza_list = armor_xyza_list(); - std::vector> xyza_i_list; - - for (int i = 0; i < armor_num_; ++i) { - xyza_i_list.emplace_back(xyza_list[i], i); - } - - std::sort(xyza_i_list.begin(), xyza_i_list.end(), [](const auto& a, const auto& b) { - auto ypd1 = util::math::xyz2ypd(a.first.head(3)); - auto ypd2 = util::math::xyz2ypd(b.first.head(3)); - return ypd1[2] < ypd2[2]; - }); - - int best_id = 0; - double min_error = 1e10; - - for (int i = 0; i < std::min(3, armor_num_); ++i) { - const auto& xyza = xyza_i_list[i].first; - auto ypd = util::math::xyz2ypd(xyza.head(3)); - double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_world[0] - xyza[3])) - + std::abs(util::math::clamp_pm_pi(armor_ypd_in_world[0] - ypd[0])); - - if (error < min_error) { - min_error = error; - best_id = xyza_i_list[i].second; - } - } - - return best_id; - } - - std::vector armor_xyza_list() const { - std::vector _armor_xyza_list; - - for (int i = 0; i < armor_num_; i++) { - auto angle = util::math::clamp_pm_pi(ekf_.x[6] + i * 2 * CV_PI / armor_num_); - Eigen::Vector3d xyz = h_armor_xyz(ekf_.x, i); - _armor_xyza_list.push_back({ xyz[0], xyz[1], xyz[2], angle }); - } - return _armor_xyza_list; - } - - // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { - auto angle = util::math::clamp_pm_pi(x[6] + id * 2 * CV_PI / armor_num_); - auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); - - auto r = (use_l_h) ? x[8] + x[9] : x[8]; - auto armor_x = x[0] - r * std::cos(angle); - auto armor_y = x[2] - r * std::sin(angle); - auto armor_z = (use_l_h) ? x[4] + x[10] : x[4]; - - return { armor_x, armor_y, armor_z }; - } - - auto F(double dt) -> Eigen::MatrixXd { - // 状态转移矩阵 - // clang-format off - return (Eigen::MatrixXd(11, 11) << - 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1).finished(); -} - -auto Q(double dt)->Eigen::MatrixXd{ - // Piecewise White Noise Model - // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb - double v1, v2; - if (car_id_ == enumeration::CarIDFlag::Outpost) { - v1 = 10; // 前哨站加速度方差 - v2 = 0.1; // 前哨站角加速度方差 - } else { - v1 = 100; // 加速度方差 - v2 = 400; // 角加速度方差 - } - auto a = dt * dt * dt * dt / 4; - auto b = dt * dt * dt / 2; - auto c = dt * dt; - - // 预测过程噪声偏差的方差 - // clang-format off - return (Eigen::MatrixXd(11, 11) << - a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0, - 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(); - // clang-format on - } - - std::time_t last_time_stamp_; - util::ExtendedKalmanFilter ekf_; - enumeration::CarIDFlag car_id_; - int armor_num_; -}; -} \ No newline at end of file diff --git a/src/tongji/predictor/target_predictor.cpp b/src/tongji/predictor/target_predictor.cpp deleted file mode 100644 index 436b823..0000000 --- a/src/tongji/predictor/target_predictor.cpp +++ /dev/null @@ -1,79 +0,0 @@ -#include "target_predictor.hpp" - -#include -#include -#include - -#include "enum/enum_tools.hpp" -#include "tongji/predictor/in_gimbal_control_armor.hpp" -#include "tongji/predictor/predictor_record.hpp" -#include "tongji/predictor/target.hpp" - -namespace world_exe::tongji::predictor { - -class TargetPredict::Impl { -public: - Impl(std::unordered_map> targets) { } - void RegisterTarget(enumeration::ArmorIdFlag id, const Target& target) { - targets_[id] = std::make_shared(target); - } - - void RemoveTarget(enumeration::ArmorIdFlag id) { } - - std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { - std::unordered_map> - result; - - for (int i = 0; i < 8; i++) { - auto decomposed_id = static_cast( - static_cast(enumeration::CarIDFlag::Hero) << i); - if (enumeration::IsFlagContains(id, decomposed_id)) { - auto it = targets_.find(decomposed_id); - if (it == targets_.end()) continue; - - it->second->Predict(time_stamp); - result[decomposed_id] = it->second->GetArmorGimbalControlSpacings(); - } - } - - return std::make_shared(result, time_stamp); - } - - std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const { - std::unordered_map> snapshot_map; - - for (int i = 0; i < 8; i++) { - auto decomposed_id = static_cast( - static_cast(enumeration::CarIDFlag::Hero) << i); - if (enumeration::IsFlagContains(id, decomposed_id)) { - - auto it = targets_.find(decomposed_id); - if (it == targets_.end()) continue; - - snapshot_map[decomposed_id] = std::make_shared(*(it->second)); - } - } - return std::make_shared(id, snapshot_map); - } - -private: - std::unordered_map> targets_; -}; - -TargetPredict::TargetPredict( - std::unordered_map> targets) - : pimpl_(std::make_unique(std::move(targets))) { } -TargetPredict::~TargetPredict() = default; - -std ::shared_ptr TargetPredict::Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { - return pimpl_->Predict(id, time_stamp); -} - -std ::shared_ptr TargetPredict::GetPredictor( - const enumeration ::ArmorIdFlag& id) const { - return pimpl_->GetPredictor(id); -} - -} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp new file mode 100644 index 0000000..0dc4a61 --- /dev/null +++ b/src/tongji/predictor/target_snapshot.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include "tongji/predictor/live_target.hpp" +#include "tongji/predictor/predict_model.hpp" +#include "util/extended_kalman_filter.hpp" + +namespace world_exe::tongji::predictor { + +class TargetSnapshot { +public: + TargetSnapshot(const LiveTarget& target) + : model_(target.GetModel()) + , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) { } + + std::vector GetArmorGimbalControlSpacings() const { + std::vector armors; + for (int id = 0; id < model_.GetArmorNum(); id++) { + auto angle = + util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto xyz = model_.h_armor_xyz(this->ekf_.x, id); + + data::ArmorGimbalControlSpacing armor; + armor.id = model_.GetID(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armors.emplace_back(std::move(armor)); + } + return armors; + } + +private: + PredictModel model_; + util::ExtendedKalmanFilter ekf_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager.cpp new file mode 100644 index 0000000..a660c9b --- /dev/null +++ b/src/tongji/predictor/target_snapshot_manager.cpp @@ -0,0 +1,51 @@ +#include "target_snapshot_manager.hpp" + +#include +#include + +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/enum_tools.hpp" +#include "in_gimbal_control_armor.hpp" +#include "tongji/predictor/target_snapshot.hpp" + +namespace world_exe::tongji::predictor { +class TargetSnapshotManager::Impl { +public: + Impl(const enumeration::ArmorIdFlag& id, + const std::unordered_map>& snapshots) + : id_(id) { + for (const auto& [flag, target] : snapshots) { + snapshots_.emplace(flag, TargetSnapshot(*target)); + } + } + + const enumeration::ArmorIdFlag& GetId() const { return id_; } + + std::shared_ptr Predictor( + const std::time_t& time_stamp) const { + std::unordered_map> + result; + + for (const auto& [flag, snapshot] : snapshots_) { + result[flag] = snapshot.GetArmorGimbalControlSpacings(); + } + return std::make_shared(result, time_stamp); + } + +private: + std::unordered_map snapshots_; + enumeration::ArmorIdFlag id_; +}; + +TargetSnapshotManager::TargetSnapshotManager(const enumeration::ArmorIdFlag& id, + const std::unordered_map> snapshots) + : pimpl_(std::make_unique(id, snapshots)) { } +TargetSnapshotManager::~TargetSnapshotManager() = default; + +const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } + +std ::shared_ptr TargetSnapshotManager::Predictor( + const std ::time_t& time_stamp) const { + return pimpl_->Predictor(time_stamp); +} +} \ No newline at end of file diff --git a/src/tongji/predictor/predictor_record.hpp b/src/tongji/predictor/target_snapshot_manager.hpp similarity index 67% rename from src/tongji/predictor/predictor_record.hpp rename to src/tongji/predictor/target_snapshot_manager.hpp index 409c37f..02394fd 100644 --- a/src/tongji/predictor/predictor_record.hpp +++ b/src/tongji/predictor/target_snapshot_manager.hpp @@ -5,15 +5,15 @@ #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" -#include "tongji/predictor/target.hpp" +#include "tongji/predictor/live_target.hpp" namespace world_exe::tongji::predictor { -class PredictorRecord final : public interfaces::IPredictor { +class TargetSnapshotManager final : public interfaces::IPredictor { public: - PredictorRecord(const enumeration::ArmorIdFlag& id, - const std::unordered_map>& snapshots); - ~PredictorRecord(); + TargetSnapshotManager(const enumeration::ArmorIdFlag& id, + const std::unordered_map> snapshots); + ~TargetSnapshotManager(); const enumeration ::ArmorIdFlag& GetId() const override; diff --git a/src/util/extended_kalman_filter.hpp b/src/util/extended_kalman_filter.hpp index 8b1a2e0..73fc86b 100644 --- a/src/util/extended_kalman_filter.hpp +++ b/src/util/extended_kalman_filter.hpp @@ -29,51 +29,38 @@ class ExtendedKalmanFilter { data["recent_nis_failures"] = 0.0; } - Eigen::VectorXd predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q) { - return Predict(F, Q, [&](const Eigen::VectorXd& x) { return F * x; }); - } - - Eigen::VectorXd Predict(const Eigen::MatrixXd& F, const Eigen::MatrixXd& Q, - std::function f) { - P = F * P * F.transpose() + Q; - x = f(x); - return x; - } - Eigen::VectorXd Update( - const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, - std::function z_subtract = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { - return update(z, H, R, [&](const Eigen::VectorXd& x) { return H * x; }, z_subtract); - } - - Eigen::VectorXd update( + const double& dt, const Eigen::MatrixXd& A, const Eigen::MatrixXd& Q, + std::function f, const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, std::function h, std::function z_subtract = [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { - Eigen::VectorXd x_prior = x; - Eigen::MatrixXd K = P * H.transpose() * (H * P * H.transpose() + R).inverse(); + + auto x_n = f(x, dt); + + auto P_n = A * P * A.transpose() + Q; + + auto residual = z_subtract(z, h(x_n)); + + auto S = H * P * H.transpose() + R; + + auto K = P_n * H.transpose() * S.inverse(); + + x = x_add(x, K * residual); // Stable Compution of the Posterior Covariance // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose(); - x = x_add(x, K * z_subtract(z, h(x))); - /// 卡方检验 - Eigen::VectorXd residual = z_subtract(z, h(x)); // 新增检验 - Eigen::MatrixXd S = H * P * H.transpose() + R; - double nis = residual.transpose() * S.inverse() * residual; - double nees = (x - x_prior).transpose() * P.inverse() * (x - x_prior); + double nis = residual.transpose() * S.inverse() * residual; // 卡方检验阈值（自由度=4，取置信水平95%） - constexpr double nis_threshold = 0.711; - constexpr double nees_threshold = 0.711; + constexpr double nis_threshold = 0.711; if (nis > nis_threshold) nis_count_++, data["nis_fail"] = 1; - if (nees > nees_threshold) nees_count_++, data["nees_fail"] = 1; total_count_++; last_nis = nis; @@ -92,7 +79,6 @@ class ExtendedKalmanFilter { data["residual_distance"] = residual[2]; data["residual_angle"] = residual[3]; data["nis"] = nis; - data["nees"] = nees; data["recent_nis_failures"] = recent_rate; return x; From b873347d5b2aa608594e92c870318b17eaab99f1 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 15 Sep 2025 01:33:04 +0800 Subject: [PATCH 27/93] feat: add state machine --- src/tongji/state_machine/car_state_manager.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index 219e457..799183e 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -1,8 +1,12 @@ #pragma once +<<<<<<< HEAD #include #include "tongji/predictor/time_stamp.hpp" +======= +#include +>>>>>>> 04b0cf7 (feat: add state machine) namespace world_exe::tongji::car_state { From e74646462618f49cc4a2321f1e6eb265ac5b6e0e Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 23 Sep 2025 21:19:07 +0800 Subject: [PATCH 28/93] merge: fix conflicts from git merge --- src/tongji/state_machine/car_state_manager.hpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index 799183e..219e457 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -1,12 +1,8 @@ #pragma once -<<<<<<< HEAD #include #include "tongji/predictor/time_stamp.hpp" -======= -#include ->>>>>>> 04b0cf7 (feat: add state machine) namespace world_exe::tongji::car_state { From 2d7eb5231a51141bc9b3cf9306934225fb341ca7 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 25 Sep 2025 22:49:25 +0800 Subject: [PATCH 29/93] fix(targeting): correct armor target acquisition logic --- src/tongji/predictor/live_target.hpp | 89 ++++++++++++++++---- src/tongji/predictor/live_target_manager.cpp | 20 +---- src/util/index.hpp | 15 ++++ 3 files changed, 93 insertions(+), 31 deletions(-) diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index 102c2d6..c9fece5 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -1,8 +1,10 @@ #pragma once #include +#include +#include #include -#include +#include #include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" @@ -20,6 +22,7 @@ struct TargetStatus { bool lost = false; bool reidentified = false; int last_id = -1; + double lock_id_ = -1; int switch_count = 0; int update_count = 0; int lost_count = 0; @@ -59,20 +62,74 @@ class LiveTarget { const PredictModel& GetModel() const { return model_; } std::time_t GetTimeStamp() const { return last_time_stamp_; } - std::vector GetArmorGimbalControlSpacings() { - std::vector armors; - for (int id = 0; id < model_.GetArmorNum(); id++) { - auto angle = - util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); - auto xyz = model_.h_armor_xyz(this->ekf_.x, id); - - data::ArmorGimbalControlSpacing armor; - armor.id = model_.GetID(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - armors.emplace_back(std::move(armor)); + data::ArmorGimbalControlSpacing GetTargetArmorGimbalControlSpacings() { + const auto& ekf_x = GetEkfX(); + const auto& xyza_list = model_.GetArmorXYZAList(ekf_x); + + Eigen::Vector4d xyza; + data::ArmorGimbalControlSpacing armor; + + if (!status_.jumped) { + // 如果装甲板未发生过跳变，则只有当前装甲板的位置已知 + xyza = xyza_list.at(0); + } + + // 整车旋转中心的球坐标yaw + auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); + std::vector delta_angle_list; + for (int i = 0; i < model_.GetArmorNum(); i++) { + auto delta_angle = util::math::clamp_pm_pi(xyza_list[i][3] - center_yaw); + delta_angle_list.emplace_back(delta_angle); + } + + // 不考虑小陀螺 + if (std::abs(ekf_x[8]) <= 2 && model_.GetID() != enumeration::CarIDFlag::Outpost) { + // 选择在可射击范围内的装甲板 + std::vector id_list; + for (int i = 0; i < model_.GetArmorNum(); i++) { + if (std::abs(delta_angle_list[i]) > 60 / 57.3) continue; + id_list.push_back(i); + } + + // 锁定模式：防止在两个都呈45度的装甲板之间来回切换 + if (id_list.size() > 1) { + int id0 = id_list[0], id1 = id_list[1]; + + // 未处于锁定模式时，选择delta_angle绝对值较小的装甲板，进入锁定模式 + if (status_.lock_id_ != id0 && status_.lock_id_ != id1) + status_.lock_id_ = + (std::abs(delta_angle_list[id0]) < std::abs(delta_angle_list[id1])) ? id0 + : id1; + xyza = xyza_list.at(status_.lock_id_); + } + + // 只有一个装甲板在可射击范围内时，退出锁定模式 + status_.lock_id_ = -1; + xyza = xyza_list.at(id_list[0]); + } + + double coming_angle, leaving_angle; + if (model_.GetID() == enumeration::CarIDFlag::Outpost) { + coming_angle = 70 / 57.3; + leaving_angle = 30 / 57.3; + } else { + coming_angle = comming_angle_; + leaving_angle = leaving_angle_; + } + + // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 + for (int i = 0; i < model_.GetArmorNum(); i++) { + if (std::abs(delta_angle_list[i]) > coming_angle) continue; + if (ekf_x[7] > 0 && delta_angle_list[i] < leaving_angle) xyza = xyza_list[i]; + else if (ekf_x[7] < 0 && delta_angle_list[i] > -leaving_angle) xyza = xyza_list[i]; } - return armors; + + armor.id = model_.GetID(); + armor.position = xyza.head<3>(); + const auto& angle = xyza[3]; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + + return armor; } void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, @@ -144,6 +201,8 @@ class LiveTarget { std::time_t last_time_stamp_; util::ExtendedKalmanFilter ekf_; PredictModel model_; -}; + double comming_angle_ = 60 / 57.3; + double leaving_angle_ = 20 / 57.3; +}; } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index fda3cb3..8d662af 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -10,6 +10,7 @@ #include "tongji/predictor/in_gimbal_control_armor.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/target_snapshot_manager.hpp" +#include "util/index.hpp" namespace world_exe::tongji::predictor { @@ -28,10 +29,10 @@ class LiveTargetManager::Impl { std::unordered_map> result; - for (auto id : ExpandArmorIdFlags(flag)) { + for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { auto it = targets_.find(id); if (it != targets_.end() && it->second) { - result[id] = it->second->GetArmorGimbalControlSpacings(); + result[id].emplace_back(it->second->GetTargetArmorGimbalControlSpacings()); } } @@ -42,7 +43,7 @@ class LiveTargetManager::Impl { const enumeration::ArmorIdFlag& flag) const { std::unordered_map> snapshot_map; - for (auto id : ExpandArmorIdFlags(flag)) { + for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { auto it = targets_.find(id); if (it != targets_.end() && it->second) { snapshot_map[id] = it->second; @@ -72,19 +73,6 @@ class LiveTargetManager::Impl { } private: - inline std::vector ExpandArmorIdFlags( - enumeration::ArmorIdFlag flags) const { - std::vector result; - for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { - auto single = static_cast( - static_cast(enumeration::ArmorIdFlag::Hero) << i); - if ((static_cast(flags) & static_cast(single)) != 0) { - result.push_back(single); - } - } - return result; - } - std::unordered_map> targets_; }; diff --git a/src/util/index.hpp b/src/util/index.hpp index 350fe26..7a300a8 100644 --- a/src/util/index.hpp +++ b/src/util/index.hpp @@ -1,5 +1,6 @@ #pragma once +#include #define BACKWARD_HAS_DW 1 // #include "backward.hpp" #include "enum/armor_id.hpp" @@ -31,4 +32,18 @@ static const world_exe::enumeration::ArmorIdFlag GetArmorIdFlag(int index) { throw std::out_of_range("Invalid index for ArmorIdFlag conversion."); } } + +static std::vector<::world_exe::enumeration::ArmorIdFlag> ExpandArmorIdFlags( + ::world_exe::enumeration::ArmorIdFlag flags) { + std::vector<::world_exe::enumeration::ArmorIdFlag> result; + for (int i = 0; i < static_cast(::world_exe::enumeration::ArmorIdFlag::Count); ++i) { + auto single = static_cast<::world_exe::enumeration::ArmorIdFlag>( + static_cast(::world_exe::enumeration::ArmorIdFlag::Hero) << i); + if ((static_cast(flags) & static_cast(single)) != 0) { + result.push_back(single); + } + } + return result; +} + } \ No newline at end of file From 30235d25e1cac95fedf574c4a6eddf89f70125ac Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 26 Sep 2025 10:18:42 +0800 Subject: [PATCH 30/93] refactor(predict, utils): move Extended Kalman Filter from utils to predict module --- .../predictor}/extended_kalman_filter.hpp | 2 +- src/tongji/predictor/live_target.hpp | 9 ++++----- src/tongji/predictor/target_snapshot.hpp | 4 ++-- 3 files changed, 7 insertions(+), 8 deletions(-) rename src/{util => tongji/predictor}/extended_kalman_filter.hpp (98%) diff --git a/src/util/extended_kalman_filter.hpp b/src/tongji/predictor/extended_kalman_filter.hpp similarity index 98% rename from src/util/extended_kalman_filter.hpp rename to src/tongji/predictor/extended_kalman_filter.hpp index 73fc86b..32450db 100644 --- a/src/util/extended_kalman_filter.hpp +++ b/src/tongji/predictor/extended_kalman_filter.hpp @@ -6,7 +6,7 @@ #include #include -namespace world_exe::util { +namespace world_exe::tongji::predictor { class ExtendedKalmanFilter { public: Eigen::VectorXd x; diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index c9fece5..162d38e 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -8,8 +8,8 @@ #include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" +#include "extended_kalman_filter.hpp" #include "predict_model.hpp" -#include "util/extended_kalman_filter.hpp" #include "util/math.hpp" namespace world_exe::tongji::predictor { @@ -53,8 +53,7 @@ class LiveTarget { 0, 0; Eigen::MatrixXd P0 = model_.GetP0Dig().asDiagonal(); - ekf_ = util::ExtendedKalmanFilter( - x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） + ekf_ = ExtendedKalmanFilter(x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） } Eigen::VectorXd GetEkfX() const { return ekf_.x; } @@ -66,7 +65,7 @@ class LiveTarget { const auto& ekf_x = GetEkfX(); const auto& xyza_list = model_.GetArmorXYZAList(ekf_x); - Eigen::Vector4d xyza; + Eigen::Vector4d xyza = xyza_list.at(0); data::ArmorGimbalControlSpacing armor; if (!status_.jumped) { @@ -199,7 +198,7 @@ class LiveTarget { } std::time_t last_time_stamp_; - util::ExtendedKalmanFilter ekf_; + ExtendedKalmanFilter ekf_; PredictModel model_; double comming_angle_ = 60 / 57.3; diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index 0dc4a61..fcffdf0 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -1,8 +1,8 @@ #pragma once +#include "extended_kalman_filter.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/predict_model.hpp" -#include "util/extended_kalman_filter.hpp" namespace world_exe::tongji::predictor { @@ -30,7 +30,7 @@ class TargetSnapshot { private: PredictModel model_; - util::ExtendedKalmanFilter ekf_; + ExtendedKalmanFilter ekf_; }; } \ No newline at end of file From 1538095ed81b5641cf990339495ffa9206547293 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 29 Sep 2025 01:06:12 +0800 Subject: [PATCH 31/93] feat: add trajectory --- src/tongji/fire_controller/trajectory.hpp | 42 +++++++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 src/tongji/fire_controller/trajectory.hpp diff --git a/src/tongji/fire_controller/trajectory.hpp b/src/tongji/fire_controller/trajectory.hpp new file mode 100644 index 0000000..3799c2c --- /dev/null +++ b/src/tongji/fire_controller/trajectory.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include +namespace world_exe::tongji::fire_control { + +struct TrajectorySolver { + bool solvable = true; + double fly_time; + double pitch; // 抬头为正 + + // 不考虑空气阻力 + // v0 子弹初速度大小，单位：m/s + // d 目标水平距离，单位：m + // h 目标竖直高度，单位：m + // g 重力加速度，单位：m/s^2 + //(g·x²)/(2v₀²)·u² - x·u + (g·x²)/(2v₀²) + y = 0(其中u = tan(θ)) + TrajectorySolver(const double& v0, const double& d, const double& h, const double& g) { + auto a = g * d * d / (2 * v0 * v0); + auto b = -d; + auto c = a + h; + auto delta = b * b - 4 * a * c; + + if (delta < 0) { + solvable = false; + return; + } + + solvable = true; + + auto tan_pitch_1 = (-b + std::sqrt(delta)) / (2 * a); + auto tan_pitch_2 = (-b - std::sqrt(delta)) / (2 * a); + auto pitch_1 = std::atan(tan_pitch_1); + auto pitch_2 = std::atan(tan_pitch_2); + auto t_1 = d / (v0 * std::cos(pitch_1)); + auto t_2 = d / (v0 * std::cos(pitch_2)); + + pitch = (t_1 < t_2) ? pitch_1 : pitch_2; + fly_time = (t_1 < t_2) ? t_1 : t_2; + } +}; + +} \ No newline at end of file From 442f3660590b758ca1c8a0bcedd50c0f21d12ff9 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Thu, 2 Oct 2025 00:54:12 +0800 Subject: [PATCH 32/93] =?UTF-8?q?[Add]=20=E6=B7=BB=E5=8A=A0=E7=81=AB?= =?UTF-8?q?=E6=8E=A7=E5=AE=9E=E7=8E=B0=E7=9A=84=E5=A4=B4=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/tongji/fire_controller/firecontrol.hpp | 14 ++++++++++++ .../fire_controller/firecontrol_impl.cpp | 12 ++++++++++ src/tongji/fire_controller/tracker.hpp | 22 +++++++++++++++++++ src/tongji/predictor/live_target_manager.hpp | 1 - 4 files changed, 48 insertions(+), 1 deletion(-) create mode 100644 src/tongji/fire_controller/firecontrol.hpp create mode 100644 src/tongji/fire_controller/firecontrol_impl.cpp create mode 100644 src/tongji/fire_controller/tracker.hpp diff --git a/src/tongji/fire_controller/firecontrol.hpp b/src/tongji/fire_controller/firecontrol.hpp new file mode 100644 index 0000000..cb81d96 --- /dev/null +++ b/src/tongji/fire_controller/firecontrol.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include "interfaces/fire_controller.hpp" +namespace world_exe::tongji::fire_controller { +class FireControl final : interfaces::IFireControl { + + const data::FireControl CalculateTarget(const std::time_t& time_duration) const override; + + const enumeration::CarIDFlag GetAttackCarId() const override; + + struct Impl; + std::unique_ptr pimpl_; +}; +} diff --git a/src/tongji/fire_controller/firecontrol_impl.cpp b/src/tongji/fire_controller/firecontrol_impl.cpp new file mode 100644 index 0000000..43c55f4 --- /dev/null +++ b/src/tongji/fire_controller/firecontrol_impl.cpp @@ -0,0 +1,12 @@ + +#include "./firecontrol.hpp" +#include "tongji/fire_controller/tracker.hpp" + +using namespace world_exe::tongji::fire_controller; +class FireControl::Impl { + // 后续转依赖注入 +public: + explicit Impl(DefaultTracker tracker) { } + + // impl interface here +}; diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp new file mode 100644 index 0000000..f2314be --- /dev/null +++ b/src/tongji/fire_controller/tracker.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include "../predictor/target_snapshot.hpp" +#include "../predictor/target_snapshot_manager.hpp" + +class DefaultTracker { + using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; + using TargetSnapshot = world_exe::tongji::predictor::TargetSnapshot; + +public: + DefaultTracker(const std::shared_ptr& snapshot_manager) + : snapshot_manager_(snapshot_manager) {}; + + ~DefaultTracker() = default; + + auto CalculateTarget() noexcept -> TargetSnapshot { + // tongji shooter::tracker impl here + } + +private: + const std::shared_ptr snapshot_manager_; +}; \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.hpp b/src/tongji/predictor/live_target_manager.hpp index f78580f..33937ab 100644 --- a/src/tongji/predictor/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager.hpp @@ -5,7 +5,6 @@ #include "enum/armor_id.hpp" #include "interfaces/target_predictor.hpp" - namespace world_exe::tongji::predictor { class LiveTargetManager final : public interfaces::ITargetPredictor { From ba1f6fb1d38dc443097d96b3ce74b4b0f7b67e18 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Fri, 3 Oct 2025 03:48:53 +0800 Subject: [PATCH 33/93] =?UTF-8?q?[Add]=20=E6=B7=BB=E5=8A=A0Tracker?= =?UTF-8?q?=E5=AE=9E=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/tongji/fire_controller/tracker.hpp | 46 ++++++++++++++++--- src/tongji/identifier/identified_armor.hpp | 26 +++++++++++ .../predictor/target_snapshot_manager.cpp | 14 ++++++ .../predictor/target_snapshot_manager.hpp | 4 ++ 4 files changed, 83 insertions(+), 7 deletions(-) diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index f2314be..29091bb 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -2,21 +2,53 @@ #include "../predictor/target_snapshot.hpp" #include "../predictor/target_snapshot_manager.hpp" +#include "enum/enum_tools.hpp" +#include "interfaces/car_state.hpp" +#include "tongji/identifier/identified_armor.hpp" +#include +#include -class DefaultTracker { +class DefaultTracker final { using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; using TargetSnapshot = world_exe::tongji::predictor::TargetSnapshot; + using ArmorInImage = world_exe::tongji::identifier::IdentifiedArmor; + using EnemiesState = world_exe::interfaces::ICarState; public: - DefaultTracker(const std::shared_ptr& snapshot_manager) - : snapshot_manager_(snapshot_manager) {}; + DefaultTracker() = default; ~DefaultTracker() = default; - auto CalculateTarget() noexcept -> TargetSnapshot { - // tongji shooter::tracker impl here + auto CalculateTarget( // + ArmorInImage& armors, // + const std::shared_ptr& snapshot_manager_ // + ) noexcept -> std::unique_ptr { + + const auto& sq_armor_list = armors.get_sq_armor(); + + sq_armor_list->sort( + [](const std::tuple& a, + const std::tuple& b) { + cv::Point2f img_center(1440.0 / 2, 1080.0 / 2); // TODO + + auto const& [a_, a_center] = a; + auto const& [b_, b_center] = b; + + auto distance_1 = cv::norm(a_center - img_center); + auto distance_2 = cv::norm(b_center - img_center); + return distance_1 < distance_2; + }); + sq_armor_list->sort([](const auto& a, const auto& b) { return a.priority < b.priority; }); + + auto iterator = sq_armor_list->begin(); + auto filter_flag = snapshot_manager_->GetId(); + + while (!world_exe::enumeration::IsFlagContains(filter_flag, iterator->armor.id)) { + ++iterator; + if (iterator == sq_armor_list->end()) return nullptr; + } + return snapshot_manager_->GetSingleSnapshot(iterator->armor.id); } private: - const std::shared_ptr snapshot_manager_; -}; \ No newline at end of file +}; diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp index b6ac9ff..d7b3d59 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -4,8 +4,17 @@ #include "interfaces/armor_in_image.hpp" #include "interfaces/time_stamped.hpp" #include "util/index.hpp" +#include +#include namespace world_exe::tongji::identifier { + +struct SPArmor { + const data::ArmorImageSpacing& armor; + const cv::Point2f center; + const int priority = 0; +}; + class IdentifiedArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { public: explicit IdentifiedArmor(const std::vector& armors) { @@ -23,8 +32,25 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface return armors_[util::enumeration::GetIndex(armor_id)]; } + std::shared_ptr> get_sq_armor() { + if (armor_list != nullptr) return armor_list; + + armor_list = std::make_shared>(); + for (const auto& car : armors_) + for (const auto& armor : car) + armor_list->emplace_back( + armor, (armor.image_points[0] + armor.image_points[3]) / 2, 0); + + return armor_list; + } + + static IdentifiedArmor DecorateIArmorInImage(const interfaces::IArmorInImage& armor) { + throw std::runtime_error("Not implemented"); + } + private: std::time_t time_stamp_ { 0 }; std::array, 8> armors_; + std::shared_ptr> armor_list = nullptr; }; } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager.cpp index a660c9b..64b5fa8 100644 --- a/src/tongji/predictor/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager.cpp @@ -32,6 +32,15 @@ class TargetSnapshotManager::Impl { return std::make_shared(result, time_stamp); } + const std::unique_ptr GetSingleSnapshot( + const enumeration::ArmorIdFlag& id) const { + auto it = snapshots_.find(id); + if (it != snapshots_.end()) { + return std::make_unique(it->second); + } + return nullptr; + } + private: std::unordered_map snapshots_; enumeration::ArmorIdFlag id_; @@ -44,6 +53,11 @@ TargetSnapshotManager::~TargetSnapshotManager() = default; const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } +const std::unique_ptr TargetSnapshotManager::GetSingleSnapshot( + const enumeration::ArmorIdFlag& id) const { + return pimpl_->GetSingleSnapshot(id); +} + std ::shared_ptr TargetSnapshotManager::Predictor( const std ::time_t& time_stamp) const { return pimpl_->Predictor(time_stamp); diff --git a/src/tongji/predictor/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager.hpp index 02394fd..1353394 100644 --- a/src/tongji/predictor/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager.hpp @@ -6,6 +6,7 @@ #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" #include "tongji/predictor/live_target.hpp" +#include "tongji/predictor/target_snapshot.hpp" namespace world_exe::tongji::predictor { @@ -17,6 +18,9 @@ class TargetSnapshotManager final : public interfaces::IPredictor { const enumeration ::ArmorIdFlag& GetId() const override; + const std::unique_ptr GetSingleSnapshot( + const enumeration::ArmorIdFlag& id) const; + std ::shared_ptr Predictor( const std ::time_t& time_stamp) const override; From 3e0fe3d491c78b9fdda824700cb807bf2741b25b Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 3 Oct 2025 04:34:53 +0800 Subject: [PATCH 34/93] fix(tracker): fix data structure in tracker.hpp --- src/tongji/fire_controller/tracker.hpp | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index 29091bb..050593f 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -26,18 +26,13 @@ class DefaultTracker final { const auto& sq_armor_list = armors.get_sq_armor(); - sq_armor_list->sort( - [](const std::tuple& a, - const std::tuple& b) { - cv::Point2f img_center(1440.0 / 2, 1080.0 / 2); // TODO - - auto const& [a_, a_center] = a; - auto const& [b_, b_center] = b; - - auto distance_1 = cv::norm(a_center - img_center); - auto distance_2 = cv::norm(b_center - img_center); - return distance_1 < distance_2; - }); + sq_armor_list->sort([](const world_exe::tongji::identifier::SPArmor& a, + const world_exe::tongji::identifier::SPArmor& b) { + cv::Point2f img_center(1440.0 / 2, 1080.0 / 2); // TODO + auto distance_1 = cv::norm(a.center - img_center); + auto distance_2 = cv::norm(b.center - img_center); + return distance_1 < distance_2; + }); sq_armor_list->sort([](const auto& a, const auto& b) { return a.priority < b.priority; }); auto iterator = sq_armor_list->begin(); From 64e372a38eb15e2bf27422c138fca809038f2bb9 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 3 Oct 2025 04:35:28 +0800 Subject: [PATCH 35/93] fix(tracker): fix data structure in tracker.hpp --- src/tongji/fire_controller/tracker.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index 050593f..e5ffdfb 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -1,12 +1,14 @@ #pragma once +#include + +#include + #include "../predictor/target_snapshot.hpp" #include "../predictor/target_snapshot_manager.hpp" #include "enum/enum_tools.hpp" #include "interfaces/car_state.hpp" #include "tongji/identifier/identified_armor.hpp" -#include -#include class DefaultTracker final { using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; From 44a79ff965e86e3d192cd780c0c86c07d21bfd92 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 4 Oct 2025 03:06:25 +0800 Subject: [PATCH 36/93] feat(fire_controller): add aim point selection and trajectory classes --- .../fire_controller/aim_point_chooser.hpp | 87 +++++++++++++++++++ .../fire_controller/fire_control_solver.hpp | 87 +++++++++++++++++++ src/tongji/fire_controller/trajectory.hpp | 20 +++-- src/tongji/predictor/live_target.hpp | 82 +---------------- src/tongji/predictor/live_target_manager.cpp | 14 +-- src/tongji/predictor/target_snapshot.hpp | 22 ++++- 6 files changed, 217 insertions(+), 95 deletions(-) create mode 100644 src/tongji/fire_controller/aim_point_chooser.hpp create mode 100644 src/tongji/fire_controller/fire_control_solver.hpp diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp new file mode 100644 index 0000000..4922113 --- /dev/null +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -0,0 +1,87 @@ +#pragma once + +#include "enum/car_id.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::fire_control { + +class AimPointChooser { +public: + AimPointChooser( + const double& comming_angle = 60 / 57.3, const double& leaving_angle = 20 / 57.3) + : comming_angle_(comming_angle) + , leaving_angle_(leaving_angle) { } + + std::pair ChooseAimArmor(const Eigen::Vector& ekf_x, + const std::vector& xyza_list, const enumeration::CarIDFlag& single_id) { + const auto armor_num = xyza_list.size(); + int chosen_id = -1; + + // 整车旋转中心的球坐标yaw + const auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); + + // 如果delta_angle为0，则该装甲板中心和整车中心的连线在世界坐标系的xy平面过原点 + std::vector delta_angle_list; + for (int i = 0; i < armor_num; i++) { + auto delta_angle = util::math::clamp_pm_pi(xyza_list[i][3] - center_yaw); + delta_angle_list.emplace_back(delta_angle); + } + + // 不考虑小陀螺 + if (std::abs(ekf_x[8]) <= 2 && single_id != enumeration::CarIDFlag::Outpost) { + // 选择在可射击范围内的装甲板 + std::vector id_list; + for (int i = 0; i < armor_num; i++) { + if (std::abs(delta_angle_list[i]) > 60 / 57.3) continue; + id_list.push_back(i); + } + + if (id_list.size() == 1) { + chosen_id = id_list[0]; + lock_id_ = -1; + } else if (id_list.size() > 1) { + const int id0 = id_list[0], id1 = id_list[1]; + // 未处于锁定模式时，选择delta_angle绝对值较小的装甲板，进入锁定模式 + if (lock_id_ != id0 && lock_id_ != id1) + lock_id_ = (std::abs(delta_angle_list[id0]) < std::abs(delta_angle_list[id1])) + ? id0 + : id1; + chosen_id = lock_id_; + } else { + chosen_id = -1; + } + } else { + // 小陀螺 + double coming_angle = + (single_id == enumeration::CarIDFlag::Outpost) ? 70 / 57.3 : comming_angle_; + double leaving_angle = + (single_id == enumeration::CarIDFlag::Outpost) ? 30 / 57.3 : leaving_angle_; + + // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 + for (int i = 0; i < armor_num; i++) { + if (std::abs(delta_angle_list[i]) > coming_angle) continue; + if ((ekf_x[7] > 0 && delta_angle_list[i] < leaving_angle) + || (ekf_x[7] < 0 && delta_angle_list[i] > -leaving_angle)) { + chosen_id = i; + break; + } + } + } + + if (chosen_id == -1) { + return { false, xyza_list[0] }; + } + + return { + true, + xyza_list[chosen_id], + }; + } + +private: + double comming_angle_ = 60 / 57.3; + double leaving_angle_ = 20 / 57.3; + int lock_id_ = -1; +}; + +} diff --git a/src/tongji/fire_controller/fire_control_solver.hpp b/src/tongji/fire_controller/fire_control_solver.hpp new file mode 100644 index 0000000..dfec413 --- /dev/null +++ b/src/tongji/fire_controller/fire_control_solver.hpp @@ -0,0 +1,87 @@ +#pragma once + +#include +#include +#include +#include + +#include "tongji/fire_controller/aim_point_chooser.hpp" +#include "tongji/fire_controller/trajectory.hpp" +#include "tongji/predictor/target_snapshot.hpp" + +namespace world_exe::tongji::fire_control { + +struct ControlCommand { + bool solvable; + double yaw; + double pitch; + double horizon_distance = 0; // 无人机专有 +}; + +class FireControlSolver { +public: + FireControlSolver(std::unique_ptr snapshot, + const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, + const double& gravity = 9.7833) + : snapshot_(std::move(snapshot)) + , aim_point_chooser_(std::make_unique()) + , bullet_speed_(bullet_speed) + , g_(gravity) { } + + ControlCommand GenerateCommand() { + const auto& [valid0, aim_point0] = aim_point_chooser_->ChooseAimArmor( + snapshot_->GetEkfX(), snapshot_->GetXYZAList(0), snapshot_->GetID()); + if (!valid0) return { false, 0, 0 }; + + const auto& xyz0 = aim_point0.head(3); + const auto& d0 = std::sqrt(xyz0.x() * xyz0.x() + xyz0.y() * xyz0.y()); + const auto trajectory0 = TrajectorySolver::SolveTrajectory(bullet_speed_, d0, xyz0[2], g_); + + // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) + bool converged = false; + double prev_fly_time = trajectory0.fly_time; + + // 预测目标在 future + prev_fly_time 时刻的位置 + Eigen::Vector4d final_aim_point; + auto current_trajectory = trajectory0; + + for (int i = 0; i < 10; ++i) { + auto dt = prev_fly_time; + const auto& [valid, aim_point] = aim_point_chooser_->ChooseAimArmor( + snapshot_->Predict(dt), snapshot_->GetXYZAList(dt), snapshot_->GetID()); + + if (!valid) return { false, 0., 0. }; + + const auto& xyz = aim_point.head(3); + const auto& d = std::sqrt(xyz.x() * xyz.x() + xyz.y() * xyz.y()); + current_trajectory = TrajectorySolver::SolveTrajectory(bullet_speed_, d, xyz.z(), g_); + + if (!current_trajectory.solvable) return { false, 0., 0. }; + + // 检查收敛条件 + if (std::abs(current_trajectory.fly_time - prev_fly_time) < 0.001) { + converged = true; + final_aim_point = aim_point; + break; + } + prev_fly_time = current_trajectory.fly_time; + } + + if (!converged) return { false, 0., 0. }; + + // 计算最终角度 + Eigen::Vector3d final_xyz = final_aim_point.head(3); + const double yaw = std::atan2(final_xyz.y(), final_xyz.x()) + yaw_offset_; + const double pitch = + -(current_trajectory.pitch + pitch_offset_); // 世界坐标系下pitch向上为负 + return { true, yaw, pitch }; + } + +private: + double bullet_speed_; + double yaw_offset_, pitch_offset_; + const double g_; + std::unique_ptr snapshot_; + std::unique_ptr aim_point_chooser_; +}; +} \ No newline at end of file diff --git a/src/tongji/fire_controller/trajectory.hpp b/src/tongji/fire_controller/trajectory.hpp index 3799c2c..b5d9586 100644 --- a/src/tongji/fire_controller/trajectory.hpp +++ b/src/tongji/fire_controller/trajectory.hpp @@ -3,39 +3,41 @@ #include namespace world_exe::tongji::fire_control { -struct TrajectorySolver { +struct TrajectoryResult { bool solvable = true; double fly_time; double pitch; // 抬头为正 +}; +struct TrajectorySolver { // 不考虑空气阻力 // v0 子弹初速度大小，单位：m/s // d 目标水平距离，单位：m // h 目标竖直高度，单位：m // g 重力加速度，单位：m/s^2 //(g·x²)/(2v₀²)·u² - x·u + (g·x²)/(2v₀²) + y = 0(其中u = tan(θ)) - TrajectorySolver(const double& v0, const double& d, const double& h, const double& g) { + static auto SolveTrajectory(const double& v0, const double& d, const double& h, const double& g) + -> TrajectoryResult const { auto a = g * d * d / (2 * v0 * v0); auto b = -d; auto c = a + h; auto delta = b * b - 4 * a * c; if (delta < 0) { - solvable = false; - return; + return { .solvable = false, .fly_time = 0, .pitch = 0 }; } - solvable = true; - auto tan_pitch_1 = (-b + std::sqrt(delta)) / (2 * a); auto tan_pitch_2 = (-b - std::sqrt(delta)) / (2 * a); auto pitch_1 = std::atan(tan_pitch_1); auto pitch_2 = std::atan(tan_pitch_2); auto t_1 = d / (v0 * std::cos(pitch_1)); auto t_2 = d / (v0 * std::cos(pitch_2)); - - pitch = (t_1 < t_2) ? pitch_1 : pitch_2; - fly_time = (t_1 < t_2) ? t_1 : t_2; + return { + .solvable = true, + .fly_time = (t_1 < t_2) ? t_1 : t_2, + .pitch = (t_1 < t_2) ? pitch_1 : pitch_2, + }; } }; diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index c9fece5..cff0951 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -4,13 +4,10 @@ #include #include #include -#include -#include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" #include "predict_model.hpp" #include "util/extended_kalman_filter.hpp" -#include "util/math.hpp" namespace world_exe::tongji::predictor { @@ -34,7 +31,7 @@ class LiveTarget { LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const enumeration::CarIDFlag& car_id, const std::time_t& t) - : last_time_stamp_(t) + : time_stamp_(t) , model_(car_id) { // x vx y vy z vz a w r l h @@ -60,77 +57,7 @@ class LiveTarget { Eigen::VectorXd GetEkfX() const { return ekf_.x; } Eigen::VectorXd GetP0Dig() const { return model_.GetP0Dig(); } const PredictModel& GetModel() const { return model_; } - std::time_t GetTimeStamp() const { return last_time_stamp_; } - - data::ArmorGimbalControlSpacing GetTargetArmorGimbalControlSpacings() { - const auto& ekf_x = GetEkfX(); - const auto& xyza_list = model_.GetArmorXYZAList(ekf_x); - - Eigen::Vector4d xyza; - data::ArmorGimbalControlSpacing armor; - - if (!status_.jumped) { - // 如果装甲板未发生过跳变，则只有当前装甲板的位置已知 - xyza = xyza_list.at(0); - } - - // 整车旋转中心的球坐标yaw - auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); - std::vector delta_angle_list; - for (int i = 0; i < model_.GetArmorNum(); i++) { - auto delta_angle = util::math::clamp_pm_pi(xyza_list[i][3] - center_yaw); - delta_angle_list.emplace_back(delta_angle); - } - - // 不考虑小陀螺 - if (std::abs(ekf_x[8]) <= 2 && model_.GetID() != enumeration::CarIDFlag::Outpost) { - // 选择在可射击范围内的装甲板 - std::vector id_list; - for (int i = 0; i < model_.GetArmorNum(); i++) { - if (std::abs(delta_angle_list[i]) > 60 / 57.3) continue; - id_list.push_back(i); - } - - // 锁定模式：防止在两个都呈45度的装甲板之间来回切换 - if (id_list.size() > 1) { - int id0 = id_list[0], id1 = id_list[1]; - - // 未处于锁定模式时，选择delta_angle绝对值较小的装甲板，进入锁定模式 - if (status_.lock_id_ != id0 && status_.lock_id_ != id1) - status_.lock_id_ = - (std::abs(delta_angle_list[id0]) < std::abs(delta_angle_list[id1])) ? id0 - : id1; - xyza = xyza_list.at(status_.lock_id_); - } - - // 只有一个装甲板在可射击范围内时，退出锁定模式 - status_.lock_id_ = -1; - xyza = xyza_list.at(id_list[0]); - } - - double coming_angle, leaving_angle; - if (model_.GetID() == enumeration::CarIDFlag::Outpost) { - coming_angle = 70 / 57.3; - leaving_angle = 30 / 57.3; - } else { - coming_angle = comming_angle_; - leaving_angle = leaving_angle_; - } - - // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 - for (int i = 0; i < model_.GetArmorNum(); i++) { - if (std::abs(delta_angle_list[i]) > coming_angle) continue; - if (ekf_x[7] > 0 && delta_angle_list[i] < leaving_angle) xyza = xyza_list[i]; - else if (ekf_x[7] < 0 && delta_angle_list[i] > -leaving_angle) xyza = xyza_list[i]; - } - - armor.id = model_.GetID(); - armor.position = xyza.head<3>(); - const auto& angle = xyza[3]; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - - return armor; - } + std::time_t GetTimeStamp() const { return time_stamp_; } void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { @@ -198,11 +125,8 @@ class LiveTarget { this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; } - std::time_t last_time_stamp_; + std::time_t time_stamp_; util::ExtendedKalmanFilter ekf_; PredictModel model_; - - double comming_angle_ = 60 / 57.3; - double leaving_angle_ = 20 / 57.3; }; } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index 8d662af..f2e1972 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -29,12 +29,14 @@ class LiveTargetManager::Impl { std::unordered_map> result; - for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { - auto it = targets_.find(id); - if (it != targets_.end() && it->second) { - result[id].emplace_back(it->second->GetTargetArmorGimbalControlSpacings()); - } - } + // TODO + + // for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { + // auto it = targets_.find(id); + // if (it != targets_.end() && it->second) { + // result[id].emplace_back(it->second->GetArmorGimbalControlSpacings()); + // } + // } return std::make_shared(result, time_stamp); } diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index 0dc4a61..a38c705 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -1,5 +1,8 @@ #pragma once +#include + +#include "data/armor_gimbal_control_spacing.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/predict_model.hpp" #include "util/extended_kalman_filter.hpp" @@ -10,7 +13,8 @@ class TargetSnapshot { public: TargetSnapshot(const LiveTarget& target) : model_(target.GetModel()) - , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) { } + , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) + , time_stamp_(target.GetTimeStamp()) { } std::vector GetArmorGimbalControlSpacings() const { std::vector armors; @@ -28,9 +32,25 @@ class TargetSnapshot { return armors; } + auto GetXYZAList(const double& dt) -> std::vector const { + return model_.GetArmorXYZAList(this->Predict(dt)); + } + + auto GetTimeStamp() const { return time_stamp_; } + + auto GetID() const { return model_.GetID(); } + + auto GetEkfX() const { return ekf_.x; } + + auto Predict(const double& dt) -> Eigen::Vector const { + auto predicted_x = model_.f(ekf_.x, dt); + return predicted_x; + } + private: PredictModel model_; util::ExtendedKalmanFilter ekf_; + const std::time_t time_stamp_; }; } \ No newline at end of file From 2dc6cd82b37ba01d9c1b89d10577a43f8e9ca890 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 4 Oct 2025 05:17:37 +0800 Subject: [PATCH 37/93] feat(fire_control): add fire command validation class fire_judge --- .../fire_controller/aim_point_chooser.hpp | 4 +- src/tongji/fire_controller/fire_judge.hpp | 49 +++++++++++++++++++ src/tongji/fire_controller/firecontrol.hpp | 14 ------ .../fire_controller/firecontrol_impl.cpp | 12 ----- ...rol_solver.hpp => gimbal_angle_solver.hpp} | 26 +++++----- 5 files changed, 66 insertions(+), 39 deletions(-) create mode 100644 src/tongji/fire_controller/fire_judge.hpp delete mode 100644 src/tongji/fire_controller/firecontrol.hpp delete mode 100644 src/tongji/fire_controller/firecontrol_impl.cpp rename src/tongji/fire_controller/{fire_control_solver.hpp => gimbal_angle_solver.hpp} (84%) diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp index 4922113..b457a76 100644 --- a/src/tongji/fire_controller/aim_point_chooser.hpp +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -79,8 +79,8 @@ class AimPointChooser { } private: - double comming_angle_ = 60 / 57.3; - double leaving_angle_ = 20 / 57.3; + double comming_angle_ = 60 / 57.3; // degree + double leaving_angle_ = 20 / 57.3; // degree int lock_id_ = -1; }; diff --git a/src/tongji/fire_controller/fire_judge.hpp b/src/tongji/fire_controller/fire_judge.hpp new file mode 100644 index 0000000..9776c5f --- /dev/null +++ b/src/tongji/fire_controller/fire_judge.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include +#include + +#include "tongji/fire_controller/gimbal_angle_solver.hpp" + +namespace world_exe::tongji::fire_control { + +class FireJudge { +public: + explicit FireJudge(const bool& auto_fire, const double& first_tolerance = 5, + const double& second_tolerance = 2, const double& judge_distance = 3) + : auto_fire_(auto_fire) + , last_command_ { false, 0, 0 } + , first_tolerance_(first_tolerance) + , second_tolerance_(second_tolerance) + , judge_distance_(judge_distance) { } + + bool ShouldFire(const GimbalCommand& command, const Eigen::Vector3d& valid_target_pos, + const Eigen::Vector3d& gimbal_pos) { + + if (!command.valid || !auto_fire_) return false; + const auto& tolerance = std::sqrt(valid_target_pos.x() * valid_target_pos.x() + + valid_target_pos.y() * valid_target_pos.y()) + > judge_distance_ + ? second_tolerance_ + : first_tolerance_; + + if (std::abs(last_command_.yaw - command.yaw) + < tolerance * 2 // 此时认为command突变不应该射击 + && std::abs(gimbal_pos[0] - last_command_.yaw) < tolerance && command.valid) { + last_command_ = command; + return true; + } + last_command_ = command; + return false; + } + +private: + bool auto_fire_; + GimbalCommand last_command_; + + double first_tolerance_ { 5 }; // 近距离射击容差，degree + double second_tolerance_ { 2 }; // 远距离射击容差，degree + double judge_distance_ { 3 }; // 距离判断阈值 +}; + +} \ No newline at end of file diff --git a/src/tongji/fire_controller/firecontrol.hpp b/src/tongji/fire_controller/firecontrol.hpp deleted file mode 100644 index cb81d96..0000000 --- a/src/tongji/fire_controller/firecontrol.hpp +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include "interfaces/fire_controller.hpp" -namespace world_exe::tongji::fire_controller { -class FireControl final : interfaces::IFireControl { - - const data::FireControl CalculateTarget(const std::time_t& time_duration) const override; - - const enumeration::CarIDFlag GetAttackCarId() const override; - - struct Impl; - std::unique_ptr pimpl_; -}; -} diff --git a/src/tongji/fire_controller/firecontrol_impl.cpp b/src/tongji/fire_controller/firecontrol_impl.cpp deleted file mode 100644 index 43c55f4..0000000 --- a/src/tongji/fire_controller/firecontrol_impl.cpp +++ /dev/null @@ -1,12 +0,0 @@ - -#include "./firecontrol.hpp" -#include "tongji/fire_controller/tracker.hpp" - -using namespace world_exe::tongji::fire_controller; -class FireControl::Impl { - // 后续转依赖注入 -public: - explicit Impl(DefaultTracker tracker) { } - - // impl interface here -}; diff --git a/src/tongji/fire_controller/fire_control_solver.hpp b/src/tongji/fire_controller/gimbal_angle_solver.hpp similarity index 84% rename from src/tongji/fire_controller/fire_control_solver.hpp rename to src/tongji/fire_controller/gimbal_angle_solver.hpp index dfec413..d8bca8f 100644 --- a/src/tongji/fire_controller/fire_control_solver.hpp +++ b/src/tongji/fire_controller/gimbal_angle_solver.hpp @@ -11,16 +11,16 @@ namespace world_exe::tongji::fire_control { -struct ControlCommand { - bool solvable; +struct GimbalCommand { + bool valid; double yaw; double pitch; double horizon_distance = 0; // 无人机专有 }; -class FireControlSolver { +class GimbleAngleSolver { public: - FireControlSolver(std::unique_ptr snapshot, + GimbleAngleSolver(std::unique_ptr snapshot, const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) : snapshot_(std::move(snapshot)) @@ -28,7 +28,7 @@ class FireControlSolver { , bullet_speed_(bullet_speed) , g_(gravity) { } - ControlCommand GenerateCommand() { + GimbalCommand Solve() { const auto& [valid0, aim_point0] = aim_point_chooser_->ChooseAimArmor( snapshot_->GetEkfX(), snapshot_->GetXYZAList(0), snapshot_->GetID()); if (!valid0) return { false, 0, 0 }; @@ -42,7 +42,7 @@ class FireControlSolver { double prev_fly_time = trajectory0.fly_time; // 预测目标在 future + prev_fly_time 时刻的位置 - Eigen::Vector4d final_aim_point; + auto current_trajectory = trajectory0; for (int i = 0; i < 10; ++i) { @@ -60,8 +60,8 @@ class FireControlSolver { // 检查收敛条件 if (std::abs(current_trajectory.fly_time - prev_fly_time) < 0.001) { - converged = true; - final_aim_point = aim_point; + converged = true; + final_aim_point_ = aim_point; break; } prev_fly_time = current_trajectory.fly_time; @@ -70,17 +70,21 @@ class FireControlSolver { if (!converged) return { false, 0., 0. }; // 计算最终角度 - Eigen::Vector3d final_xyz = final_aim_point.head(3); + Eigen::Vector3d final_xyz = final_aim_point_.head(3); const double yaw = std::atan2(final_xyz.y(), final_xyz.x()) + yaw_offset_; const double pitch = -(current_trajectory.pitch + pitch_offset_); // 世界坐标系下pitch向上为负 return { true, yaw, pitch }; } + auto GetAimPoint() -> Eigen::Vector4d const { return final_aim_point_; } + private: - double bullet_speed_; - double yaw_offset_, pitch_offset_; + double bullet_speed_; // m/s + double yaw_offset_, pitch_offset_; // degree const double g_; + Eigen::Vector4d final_aim_point_; + std::unique_ptr snapshot_; std::unique_ptr aim_point_chooser_; }; From c44607aa7ad81341a6caa61589049326f3687076 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 5 Oct 2025 03:50:54 +0800 Subject: [PATCH 38/93] refactor(control): rename classes and refine fire logic MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - fire_judge → fire_decision - gimble_angle_solver → aim_solver - Improve fire validation and decision process --- src/tongji/fire_controller/aim_solver.hpp | 92 +++++++++++++++++++ .../{fire_judge.hpp => fire_decision.hpp} | 22 ++--- .../fire_controller/gimbal_angle_solver.hpp | 91 ------------------ src/tongji/fire_controller/trajectory.hpp | 2 +- src/tongji/predictor/target_snapshot.hpp | 2 +- 5 files changed, 105 insertions(+), 104 deletions(-) create mode 100644 src/tongji/fire_controller/aim_solver.hpp rename src/tongji/fire_controller/{fire_judge.hpp => fire_decision.hpp} (61%) delete mode 100644 src/tongji/fire_controller/gimbal_angle_solver.hpp diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp new file mode 100644 index 0000000..d33d529 --- /dev/null +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -0,0 +1,92 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "tongji/fire_controller/aim_point_chooser.hpp" +#include "tongji/fire_controller/trajectory.hpp" +#include "tongji/predictor/target_snapshot.hpp" + +namespace world_exe::tongji::fire_control { + +struct AimSolution { + bool valid; + double yaw; + double pitch; + Eigen::Vector4d aim_point; // 最终瞄准点（世界坐标 + 装甲板yaw） + double horizon_distance = 0; // 无人机专有 +}; + +class AimingSolver { +public: + AimingSolver(std::unique_ptr snapshot, const double& bullet_speed, + const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) + : snapshot_(std::move(snapshot)) + , aim_point_chooser_(std::make_unique()) + , bullet_speed_(bullet_speed) + , yaw_offset_(yaw_offset / 57.3) // degree to rad + , pitch_offset_(pitch_offset / 57.3) // degree to rad + , g_(gravity) { } + + AimSolution SolveAimSolution(const double& time_delay) { + if (!snapshot_) return { false, 0, 0, {}, 0 }; + + // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) + double prev_fly_time = 0; + Eigen::Vector4d final_aim_point; + TrajectoryResult final_trajectory; + bool converged = false; + + // 预测目标在未来 dt时间后的位置 + for (int i = 0; i < 10; ++i) { + double dt = time_delay + prev_fly_time; + const auto aim = SelectPredictedAim(dt); + if (!aim.has_value()) return { false, 0, 0, {}, 0 }; // failed: no valid aim point + + const auto traj = SolveTrajectory(aim->head(3)); + if (!traj.has_value()) return { false, 0, 0, {}, 0 }; // failed: trajectory unsolvable + + if (i > 0 && std::abs(traj->fly_time - prev_fly_time) < 0.001) { + final_aim_point = *aim; + final_trajectory = *traj; + converged = true; + break; + } + prev_fly_time = traj->fly_time; + } + if (!converged) return { false, 0, 0, {}, 0 }; // failed: trajectory did not converge + + const auto xyz = final_aim_point.head(3); + const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; + const double pitch = -(final_trajectory.pitch + pitch_offset_); + return { true, yaw, pitch, final_aim_point }; + } + + void UpdateSnapshot(std::unique_ptr snapshot) { + snapshot_ = std::move(snapshot); + } + +private: + std::optional SelectPredictedAim(double dt) const { + const auto& [selectable, aim_point] = aim_point_chooser_->ChooseAimArmor( + snapshot_->Predict(dt), snapshot_->GetPredictedXYZAList(dt), snapshot_->GetID()); + return selectable ? std::optional { aim_point } : std::nullopt; + } + + std::optional SolveTrajectory(const Eigen::Vector3d& xyz) const { + double d = std::hypot(xyz.x(), xyz.y()); + auto result = TrajectorySolver::SolveTrajectory(bullet_speed_, d, xyz.z(), g_); + return result.solvable ? std::optional { result } : std::nullopt; + } + + double bullet_speed_; // m/s + double yaw_offset_, pitch_offset_; + const double g_; + + std::unique_ptr aim_point_chooser_; + std::unique_ptr snapshot_; +}; +} \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_judge.hpp b/src/tongji/fire_controller/fire_decision.hpp similarity index 61% rename from src/tongji/fire_controller/fire_judge.hpp rename to src/tongji/fire_controller/fire_decision.hpp index 9776c5f..8dda984 100644 --- a/src/tongji/fire_controller/fire_judge.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -3,43 +3,43 @@ #include #include -#include "tongji/fire_controller/gimbal_angle_solver.hpp" +#include "tongji/fire_controller/aim_solver.hpp" namespace world_exe::tongji::fire_control { -class FireJudge { +class FireDecision { public: - explicit FireJudge(const bool& auto_fire, const double& first_tolerance = 5, + explicit FireDecision(const bool& auto_fire, const double& first_tolerance = 5, const double& second_tolerance = 2, const double& judge_distance = 3) : auto_fire_(auto_fire) - , last_command_ { false, 0, 0 } + , last_aim_solution_ { false, 0, 0 } , first_tolerance_(first_tolerance) , second_tolerance_(second_tolerance) , judge_distance_(judge_distance) { } - bool ShouldFire(const GimbalCommand& command, const Eigen::Vector3d& valid_target_pos, + bool ShouldFire(const AimSolution& aimsolution, const Eigen::Vector3d& valid_target_pos, const Eigen::Vector3d& gimbal_pos) { - if (!command.valid || !auto_fire_) return false; + if (!aimsolution.valid || !auto_fire_) return false; const auto& tolerance = std::sqrt(valid_target_pos.x() * valid_target_pos.x() + valid_target_pos.y() * valid_target_pos.y()) > judge_distance_ ? second_tolerance_ : first_tolerance_; - if (std::abs(last_command_.yaw - command.yaw) + if (std::abs(last_aim_solution_.yaw - aimsolution.yaw) < tolerance * 2 // 此时认为command突变不应该射击 - && std::abs(gimbal_pos[0] - last_command_.yaw) < tolerance && command.valid) { - last_command_ = command; + && std::abs(gimbal_pos[0] - last_aim_solution_.yaw) < tolerance && last_aim_solution_.valid) { + last_aim_solution_ = aimsolution; return true; } - last_command_ = command; + last_aim_solution_ = aimsolution; return false; } private: bool auto_fire_; - GimbalCommand last_command_; + AimSolution last_aim_solution_; double first_tolerance_ { 5 }; // 近距离射击容差，degree double second_tolerance_ { 2 }; // 远距离射击容差，degree diff --git a/src/tongji/fire_controller/gimbal_angle_solver.hpp b/src/tongji/fire_controller/gimbal_angle_solver.hpp deleted file mode 100644 index d8bca8f..0000000 --- a/src/tongji/fire_controller/gimbal_angle_solver.hpp +++ /dev/null @@ -1,91 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "tongji/fire_controller/aim_point_chooser.hpp" -#include "tongji/fire_controller/trajectory.hpp" -#include "tongji/predictor/target_snapshot.hpp" - -namespace world_exe::tongji::fire_control { - -struct GimbalCommand { - bool valid; - double yaw; - double pitch; - double horizon_distance = 0; // 无人机专有 -}; - -class GimbleAngleSolver { -public: - GimbleAngleSolver(std::unique_ptr snapshot, - const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, - const double& gravity = 9.7833) - : snapshot_(std::move(snapshot)) - , aim_point_chooser_(std::make_unique()) - , bullet_speed_(bullet_speed) - , g_(gravity) { } - - GimbalCommand Solve() { - const auto& [valid0, aim_point0] = aim_point_chooser_->ChooseAimArmor( - snapshot_->GetEkfX(), snapshot_->GetXYZAList(0), snapshot_->GetID()); - if (!valid0) return { false, 0, 0 }; - - const auto& xyz0 = aim_point0.head(3); - const auto& d0 = std::sqrt(xyz0.x() * xyz0.x() + xyz0.y() * xyz0.y()); - const auto trajectory0 = TrajectorySolver::SolveTrajectory(bullet_speed_, d0, xyz0[2], g_); - - // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) - bool converged = false; - double prev_fly_time = trajectory0.fly_time; - - // 预测目标在 future + prev_fly_time 时刻的位置 - - auto current_trajectory = trajectory0; - - for (int i = 0; i < 10; ++i) { - auto dt = prev_fly_time; - const auto& [valid, aim_point] = aim_point_chooser_->ChooseAimArmor( - snapshot_->Predict(dt), snapshot_->GetXYZAList(dt), snapshot_->GetID()); - - if (!valid) return { false, 0., 0. }; - - const auto& xyz = aim_point.head(3); - const auto& d = std::sqrt(xyz.x() * xyz.x() + xyz.y() * xyz.y()); - current_trajectory = TrajectorySolver::SolveTrajectory(bullet_speed_, d, xyz.z(), g_); - - if (!current_trajectory.solvable) return { false, 0., 0. }; - - // 检查收敛条件 - if (std::abs(current_trajectory.fly_time - prev_fly_time) < 0.001) { - converged = true; - final_aim_point_ = aim_point; - break; - } - prev_fly_time = current_trajectory.fly_time; - } - - if (!converged) return { false, 0., 0. }; - - // 计算最终角度 - Eigen::Vector3d final_xyz = final_aim_point_.head(3); - const double yaw = std::atan2(final_xyz.y(), final_xyz.x()) + yaw_offset_; - const double pitch = - -(current_trajectory.pitch + pitch_offset_); // 世界坐标系下pitch向上为负 - return { true, yaw, pitch }; - } - - auto GetAimPoint() -> Eigen::Vector4d const { return final_aim_point_; } - -private: - double bullet_speed_; // m/s - double yaw_offset_, pitch_offset_; // degree - const double g_; - Eigen::Vector4d final_aim_point_; - - std::unique_ptr snapshot_; - std::unique_ptr aim_point_chooser_; -}; -} \ No newline at end of file diff --git a/src/tongji/fire_controller/trajectory.hpp b/src/tongji/fire_controller/trajectory.hpp index b5d9586..2b9c5e1 100644 --- a/src/tongji/fire_controller/trajectory.hpp +++ b/src/tongji/fire_controller/trajectory.hpp @@ -6,7 +6,7 @@ namespace world_exe::tongji::fire_control { struct TrajectoryResult { bool solvable = true; double fly_time; - double pitch; // 抬头为正 + double pitch; // 抬头为正,rad }; struct TrajectorySolver { diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index a38c705..75a5e96 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -32,7 +32,7 @@ class TargetSnapshot { return armors; } - auto GetXYZAList(const double& dt) -> std::vector const { + auto GetPredictedXYZAList(const double& dt) -> std::vector const { return model_.GetArmorXYZAList(this->Predict(dt)); } From 3aa3c9d5f36b5119a121828a709e77f1e8dc6a26 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 5 Oct 2025 04:41:23 +0800 Subject: [PATCH 39/93] feat(fire_control): implement basic module interfaces --- .../fire_controller/fire_controller.cpp | 78 +++++++++++++++++++ .../fire_controller/fire_controller.hpp | 22 ++++++ src/tongji/fire_controller/tracker.hpp | 9 ++- 3 files changed, 108 insertions(+), 1 deletion(-) create mode 100644 src/tongji/fire_controller/fire_controller.cpp create mode 100644 src/tongji/fire_controller/fire_controller.hpp diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp new file mode 100644 index 0000000..b9eba13 --- /dev/null +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -0,0 +1,78 @@ +#include "fire_controller.hpp" + +#include +#include +#include +#include + +#include + +#include "enum/car_id.hpp" +#include "tongji/fire_controller/aim_solver.hpp" +#include "tongji/fire_controller/fire_decision.hpp" +#include "tongji/fire_controller/tracker.hpp" +#include "tongji/identifier/identified_armor.hpp" +#include "tongji/predictor/target_snapshot_manager.hpp" +#include "tongji/predictor/time_stamp.hpp" + +namespace world_exe::tongji::fire_control { +class FireController::Impl { +public: + Impl(const double& control_delay_in_second, const double& bullet_speed) + : control_delay_(control_delay_in_second) + , bullet_speed_(bullet_speed) { } + + // TODO:std::time_t + const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { + if (!identified_armors_ || !tracker_ || !predictor_ || !aim_solver_ || !fire_decision_) + return { .fire_allowance = false }; + + auto snapshot = tracker_->CalculateTarget(*identified_armors_, predictor_); + if (!snapshot) return { .fire_allowance = false }; + + aim_solver_->UpdateSnapshot(std::move(snapshot)); + const auto& aim_solution = + aim_solver_->SolveAimSolution(static_cast(time_duration)); + if (!aim_solution.valid) return { .fire_allowance = false }; + + const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); + const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_); + return { .time_stamp = time_stamp_.GetTimeStamp(), + .gimbal_dir = Eigen::Vector3d(aim_solution.yaw, aim_solution.pitch, 0), + .fire_allowance = fireable }; + } + + const enumeration ::CarIDFlag GetAttackCarId() const { + if (!tracker_) return enumeration::CarIDFlag::Unknow; + return tracker_->GetCurrentTargetID(); + } + + void UpdateArmors(const identifier::IdentifiedArmor& armors) { identified_armors_ = armors; } + void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { gimbal_pos_ = gimbal_pos; }; + void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_.SetTimeStamp(time_stamp); } + +private: + Eigen::Vector3d gimbal_pos_ { Eigen::Vector3d::Zero() }; + double control_delay_; + double bullet_speed_; + + mutable std::optional identified_armors_; + std::unique_ptr tracker_; + std::shared_ptr predictor_; + std::unique_ptr aim_solver_; + std::unique_ptr fire_decision_; + predictor::TimeStamp time_stamp_ { 0 }; +}; + +FireController::FireController() + : pimpl_() { } + +const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { + return pimpl_->CalculateTarget(time_duration); +} + +const enumeration ::CarIDFlag FireController::GetAttackCarId() const { + return pimpl_->GetAttackCarId(); +} + +} \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp new file mode 100644 index 0000000..8b2e658 --- /dev/null +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include + +#include "interfaces/fire_controller.hpp" + +namespace world_exe::tongji::fire_control { + +class FireController final : public interfaces::IFireControl { +public: + FireController(); + + const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; + + const enumeration ::CarIDFlag GetAttackCarId() const override; + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} \ No newline at end of file diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index e5ffdfb..b065bfd 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -6,6 +6,7 @@ #include "../predictor/target_snapshot.hpp" #include "../predictor/target_snapshot_manager.hpp" +#include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "interfaces/car_state.hpp" #include "tongji/identifier/identified_armor.hpp" @@ -44,8 +45,14 @@ class DefaultTracker final { ++iterator; if (iterator == sq_armor_list->end()) return nullptr; } - return snapshot_manager_->GetSingleSnapshot(iterator->armor.id); + auto snapshot = snapshot_manager_->GetSingleSnapshot(iterator->armor.id); + if (!snapshot) return nullptr; + tracking_car_id_ = snapshot->GetID(); + return snapshot; } + world_exe::enumeration::CarIDFlag GetCurrentTargetID() const { return tracking_car_id_; } + private: + world_exe::enumeration::CarIDFlag tracking_car_id_; }; From 890a40a7f5fffcdd8c21d9366a59f5f672668a43 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 6 Oct 2025 23:58:13 +0800 Subject: [PATCH 40/93] fix(tracking): improve live_target convergence logic --- src/tongji/predictor/live_target.hpp | 72 ++++++++++++++-------------- 1 file changed, 37 insertions(+), 35 deletions(-) diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index cff0951..3edc46b 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "enum/car_id.hpp" #include "predict_model.hpp" @@ -11,23 +12,23 @@ namespace world_exe::tongji::predictor { -struct TargetStatus { - bool jumped = false; - bool switched = false; - bool converged = false; - bool diverged = false; - bool lost = false; - bool reidentified = false; - int last_id = -1; - double lock_id_ = -1; - int switch_count = 0; - int update_count = 0; - int lost_count = 0; -}; +// struct TargetStatus { +// bool jumped = false; +// bool switched = false; +// bool converged = false; +// bool diverged = false; +// bool lost = false; +// bool reidentified = false; +// int last_id = -1; +// double lock_id_ = -1; +// int switch_count = 0; +// int update_count = 0; +// int lost_count = 0; +// }; class LiveTarget { public: - TargetStatus status_; + // TargetStatus status_; LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const enumeration::CarIDFlag& car_id, const std::time_t& t) @@ -61,35 +62,30 @@ class LiveTarget { void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { - // 装甲板匹配 int id = model_.MatchArmor(ekf_.x, armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); - if (id != 0) status_.jumped = true; - status_.switched = (id != status_.last_id); - if (status_.switched) status_.switch_count++; + last_id = id; + update_count++; - status_.last_id = id; Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id, dt); - status_.update_count++; } private: - bool Converged() { - if (model_.GetID() != enumeration::CarIDFlag::Outpost && status_.update_count > 3 - && !this->Diverged()) { - status_.converged = true; - } - + bool EvaluateConvergence() { // 前哨站特殊判断 - if (model_.GetID() == enumeration::CarIDFlag::Outpost && status_.update_count > 10 - && !this->Diverged()) { - status_.converged = true; - } - return status_.converged; + const int required_count = (model_.GetID() == enumeration::CarIDFlag::Outpost) ? 10 : 3; + if (update_count < required_count) return false; + if (EvaluateDivergence()) return false; + + auto nis_failures = + std::accumulate(ekf_.recent_nis_failures.begin(), ekf_.recent_nis_failures.end(), 0); + if (nis_failures > 0.4 * ekf_.window_size) return false; + + return true; } - bool Diverged() const { + bool EvaluateDivergence() const { auto r_ok = ekf_.x[8] > 0.05 && ekf_.x[8] < 0.5; auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; @@ -120,13 +116,19 @@ class LiveTarget { ekf_.Update(dt, A, Q, f, z, H, R, h, z_subtract); // 前哨站转速特判 - if (this->Converged() && model_.GetID() == enumeration::CarIDFlag::Outpost - && std::abs(this->ekf_.x[7]) > 2) - this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; + // TODO + // if (this->Converged() && model_.GetID() == enumeration::CarIDFlag::Outpost + // && std::abs(this->ekf_.x[7]) > 2) + // this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; } std::time_t time_stamp_; util::ExtendedKalmanFilter ekf_; PredictModel model_; + + int last_id = -1; + int update_count = 0; + + // bool converged_ { false }; }; } \ No newline at end of file From e599cb4f60738f4bc5c63a27fe821590ae86df55 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 7 Oct 2025 05:13:58 +0800 Subject: [PATCH 41/93] feat: complete core logic across multiple modules - State Machine: add convergence detection logic - LiveTargetManager: implement add/remove operations for LiveTarget - IdentifiedArmor: add GetDetectedIDs() method for ID retrieval - Tracker: complete state update logic implementation - FireControl: add reset function for SnapshotManager --- src/tongji/fire_controller/aim_solver.hpp | 13 +- .../fire_controller/fire_controller.cpp | 43 +++++- .../fire_controller/fire_controller.hpp | 3 + src/tongji/fire_controller/tracker.hpp | 142 ++++++++++++++++-- src/tongji/identifier/identified_armor.hpp | 13 ++ src/tongji/predictor/live_target.hpp | 45 +++--- src/tongji/predictor/live_target_manager.cpp | 43 +++++- src/tongji/predictor/predict_model.hpp | 5 + src/tongji/predictor/target_snapshot.hpp | 8 +- .../state_machine/car_state_manager.hpp | 58 +++---- src/tongji/state_machine/state_machine.cpp | 46 ++++-- src/tongji/state_machine/state_machine.hpp | 5 +- 12 files changed, 320 insertions(+), 104 deletions(-) diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index d33d529..12fec38 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -22,15 +22,18 @@ struct AimSolution { class AimingSolver { public: - AimingSolver(std::unique_ptr snapshot, const double& bullet_speed, + AimingSolver( const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) - : snapshot_(std::move(snapshot)) - , aim_point_chooser_(std::make_unique()) + : aim_point_chooser_(std::make_unique()) , bullet_speed_(bullet_speed) , yaw_offset_(yaw_offset / 57.3) // degree to rad , pitch_offset_(pitch_offset / 57.3) // degree to rad , g_(gravity) { } + void UpdateSnapshot(std::unique_ptr snapshot) { + snapshot_ = std::move(snapshot); + } + AimSolution SolveAimSolution(const double& time_delay) { if (!snapshot_) return { false, 0, 0, {}, 0 }; @@ -65,10 +68,6 @@ class AimingSolver { return { true, yaw, pitch, final_aim_point }; } - void UpdateSnapshot(std::unique_ptr snapshot) { - snapshot_ = std::move(snapshot); - } - private: std::optional SelectPredictedAim(double dt) const { const auto& [selectable, aim_point] = aim_point_chooser_->ChooseAimArmor( diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index b9eba13..114de5d 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -14,39 +14,64 @@ #include "tongji/identifier/identified_armor.hpp" #include "tongji/predictor/target_snapshot_manager.hpp" #include "tongji/predictor/time_stamp.hpp" +#include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::fire_control { + +using TargetSnapshotManager = predictor::TargetSnapshotManager; + class FireController::Impl { public: - Impl(const double& control_delay_in_second, const double& bullet_speed) + Impl(bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, + double yaw_offset, double pitch_offset) : control_delay_(control_delay_in_second) - , bullet_speed_(bullet_speed) { } + , bullet_speed_(bullet_speed) + , tracker_(std::make_unique()) + , predictor_(nullptr) // TODO + , aim_solver_(std::make_unique(bullet_speed, yaw_offset, pitch_offset)) + , fire_decision_(std::make_unique(auto_fire)) + , state_machine_(std::make_unique()) { } // TODO:std::time_t const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { if (!identified_armors_ || !tracker_ || !predictor_ || !aim_solver_ || !fire_decision_) return { .fire_allowance = false }; - auto snapshot = tracker_->CalculateTarget(*identified_armors_, predictor_); - if (!snapshot) return { .fire_allowance = false }; + auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, predictor_); + bool found = (snapshot != nullptr); + tracker_->UpdateState(found); + if (!found) return { .fire_allowance = false }; aim_solver_->UpdateSnapshot(std::move(snapshot)); + const auto& aim_solution = - aim_solver_->SolveAimSolution(static_cast(time_duration)); + aim_solver_->SolveAimSolution(static_cast(time_duration)); // TODO if (!aim_solution.valid) return { .fire_allowance = false }; + state_machine_->Update(identified_armors_->GetDetectedIDs(), time_stamp_.GetTimeStamp()); + + const auto allowed_id = state_machine_->GetAllowdToFires(); // TODO + const auto current_id = tracker_->GetCurrentTargetID(); + const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); - const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_); + const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_) + && state_machine_->IsAllowedToFire(current_id); + return { .time_stamp = time_stamp_.GetTimeStamp(), .gimbal_dir = Eigen::Vector3d(aim_solution.yaw, aim_solution.pitch, 0), .fire_allowance = fireable }; } + // TODO const enumeration ::CarIDFlag GetAttackCarId() const { if (!tracker_) return enumeration::CarIDFlag::Unknow; return tracker_->GetCurrentTargetID(); } + void SetSnapshotManager(std::shared_ptr manager) { + predictor_ = std::move(manager); + } + void UpdateArmors(const identifier::IdentifiedArmor& armors) { identified_armors_ = armors; } void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { gimbal_pos_ = gimbal_pos; }; void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_.SetTimeStamp(time_stamp); } @@ -61,6 +86,8 @@ class FireController::Impl { std::shared_ptr predictor_; std::unique_ptr aim_solver_; std::unique_ptr fire_decision_; + std::unique_ptr state_machine_; + predictor::TimeStamp time_stamp_ { 0 }; }; @@ -71,6 +98,10 @@ const data ::FireControl FireController::CalculateTarget(const std ::time_t& tim return pimpl_->CalculateTarget(time_duration); } +void FireController::SetSnapshotManager(std::shared_ptr manager) { + pimpl_->SetSnapshotManager(std::move(manager)); +} + const enumeration ::CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 8b2e658..8458a07 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -3,6 +3,7 @@ #include #include "interfaces/fire_controller.hpp" +#include "tongji/predictor/target_snapshot_manager.hpp" namespace world_exe::tongji::fire_control { @@ -14,6 +15,8 @@ class FireController final : public interfaces::IFireControl { const enumeration ::CarIDFlag GetAttackCarId() const override; + void SetSnapshotManager(std::shared_ptr manager); + private: class Impl; std::unique_ptr pimpl_; diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index b065bfd..9e871e0 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -6,10 +6,22 @@ #include "../predictor/target_snapshot.hpp" #include "../predictor/target_snapshot_manager.hpp" +#include "enum/armor_id.hpp" #include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "interfaces/car_state.hpp" #include "tongji/identifier/identified_armor.hpp" +#include "tongji/predictor/time_stamp.hpp" + +namespace world_exe::tongji::fire_control { + +enum class TrackState { + Lost, // + Detecting, // + Tracking, // + TempLost, // + Switching // +}; class DefaultTracker final { using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; @@ -18,11 +30,12 @@ class DefaultTracker final { using EnemiesState = world_exe::interfaces::ICarState; public: - DefaultTracker() = default; + DefaultTracker() + : last_timestamp_(0) { } ~DefaultTracker() = default; - auto CalculateTarget( // + auto SelectTrackingTarget( // ArmorInImage& armors, // const std::shared_ptr& snapshot_manager_ // ) noexcept -> std::unique_ptr { @@ -38,21 +51,128 @@ class DefaultTracker final { }); sq_armor_list->sort([](const auto& a, const auto& b) { return a.priority < b.priority; }); - auto iterator = sq_armor_list->begin(); + // auto iterator = sq_armor_list->begin(); auto filter_flag = snapshot_manager_->GetId(); + for (const auto& armor : *sq_armor_list) { + if (!enumeration::IsFlagContains(filter_flag, armor.armor.id)) continue; + + auto snapshot = snapshot_manager_->GetSingleSnapshot(armor.armor.id); + if (!snapshot) continue; - while (!world_exe::enumeration::IsFlagContains(filter_flag, iterator->armor.id)) { - ++iterator; - if (iterator == sq_armor_list->end()) return nullptr; + if (state_ == TrackState::Tracking && snapshot->GetPriority() < current_priority_) { + SetState(TrackState::Switching); + temp_lost_count_ = 0; + } + + auto now = predictor::TimeStamp(std::time(nullptr)); + if (state_ != TrackState::Lost && now.SecondsSince(last_timestamp_) > 0.1) { + SetState(TrackState::Lost); + ResetTracking(); + return nullptr; + } + + last_timestamp_ = now; + tracking_car_id_ = snapshot->GetID(); + current_priority_ = snapshot->GetPriority(); + return snapshot; } - auto snapshot = snapshot_manager_->GetSingleSnapshot(iterator->armor.id); - if (!snapshot) return nullptr; - tracking_car_id_ = snapshot->GetID(); - return snapshot; + ResetTracking(); + return nullptr; } world_exe::enumeration::CarIDFlag GetCurrentTargetID() const { return tracking_car_id_; } + void UpdateState(bool found) { + switch (state_) { + case TrackState::Lost: { + if (found) { + SetState(TrackState::Detecting); + detect_count_++; + } else { + ResetTracking(); + } + break; + } + + case TrackState::Detecting: { + if (found) { + detect_count_++; + if (detect_count_ >= min_detect_count_) SetState(TrackState::Tracking); + } else { + detect_count_ = 0; + SetState((pre_state_ == TrackState::Switching) ? TrackState::Switching + : TrackState::Lost); + } + break; + } + + case TrackState::Tracking: { + if (!found) { + temp_lost_count_ = 1; + SetState(TrackState::TempLost); + } + break; + } + + case TrackState::Switching: { + if (found) { + SetState(TrackState::Detecting); + } else { + temp_lost_count_++; + if (temp_lost_count_ > max_switch_count_) { + SetState(TrackState::Lost); + ResetTracking(); + }; + } + break; + } + + case TrackState::TempLost: { + if (found) { + SetState(TrackState::Tracking); + } else { + temp_lost_count_++; + max_temp_lost_count_ = (tracking_car_id_ == enumeration::ArmorIdFlag::Outpost) + ? outpost_max_temp_lost_count_ + : normal_max_temp_lost_count_; + + if (temp_lost_count_ > max_temp_lost_count_) { + SetState(TrackState::Lost); + ResetTracking(); + }; + } + break; + } + } + } + + TrackState GetState() const { return state_; } + private: - world_exe::enumeration::CarIDFlag tracking_car_id_; + void SetState(TrackState new_state) { + pre_state_ = state_; + state_ = new_state; + } + + void ResetTracking() { + tracking_car_id_ = enumeration::CarIDFlag::None; + current_priority_ = 100; + } + + int current_priority_ = 100; + world_exe::enumeration::CarIDFlag tracking_car_id_ { enumeration::CarIDFlag::None }; + TrackState state_ = TrackState::Lost; + TrackState pre_state_ = TrackState::Lost; + + int detect_count_ = 0; + int temp_lost_count_ = 0; + int max_temp_lost_count_ = 15; + const int min_detect_count_ = 5; + const int outpost_max_temp_lost_count_ = 75; + const int normal_max_temp_lost_count_ = max_temp_lost_count_; + const int max_switch_count_ = 200; + + predictor::TimeStamp last_timestamp_; }; + +} \ No newline at end of file diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp index d7b3d59..950fb32 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -1,9 +1,11 @@ #pragma once #include "enum/armor_id.hpp" +#include "enum/car_id.hpp" #include "interfaces/armor_in_image.hpp" #include "interfaces/time_stamped.hpp" #include "util/index.hpp" +#include #include #include @@ -48,6 +50,17 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface throw std::runtime_error("Not implemented"); } + auto GetDetectedIDs() { + enumeration::CarIDFlag result; + for (int i = 0; i < 8; i++) { + if (!armors_[i].empty()) { + result = static_cast( + static_cast(result) | static_cast(armors_[i][0].id)); + } + } + return result; + } + private: std::time_t time_stamp_ { 0 }; std::array, 8> armors_; diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index 3edc46b..2f5eed1 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -8,31 +8,16 @@ #include "enum/car_id.hpp" #include "predict_model.hpp" +#include "tongji/state_machine/car_state_manager.hpp" #include "util/extended_kalman_filter.hpp" namespace world_exe::tongji::predictor { -// struct TargetStatus { -// bool jumped = false; -// bool switched = false; -// bool converged = false; -// bool diverged = false; -// bool lost = false; -// bool reidentified = false; -// int last_id = -1; -// double lock_id_ = -1; -// int switch_count = 0; -// int update_count = 0; -// int lost_count = 0; -// }; - class LiveTarget { public: - // TargetStatus status_; - LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - const enumeration::CarIDFlag& car_id, const std::time_t& t) - : time_stamp_(t) + const enumeration::CarIDFlag& car_id) + : last_see_time_stamp_(std::time(nullptr)) , model_(car_id) { // x vx y vy z vz a w r l h @@ -52,13 +37,13 @@ class LiveTarget { Eigen::MatrixXd P0 = model_.GetP0Dig().asDiagonal(); ekf_ = util::ExtendedKalmanFilter( - x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） + x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） } Eigen::VectorXd GetEkfX() const { return ekf_.x; } Eigen::VectorXd GetP0Dig() const { return model_.GetP0Dig(); } const PredictModel& GetModel() const { return model_; } - std::time_t GetTimeStamp() const { return time_stamp_; } + predictor::TimeStamp LastSeen() const { return predictor::TimeStamp(last_see_time_stamp_); } void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { @@ -69,10 +54,14 @@ class LiveTarget { update_count++; Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id, dt); + + last_see_time_stamp_ = std::time(nullptr); } + bool IsConverged() const { return EvaluateConvergence(); } + private: - bool EvaluateConvergence() { + bool EvaluateConvergence() const { // 前哨站特殊判断 const int required_count = (model_.GetID() == enumeration::CarIDFlag::Outpost) ? 10 : 3; if (update_count < required_count) return false; @@ -116,19 +105,19 @@ class LiveTarget { ekf_.Update(dt, A, Q, f, z, H, R, h, z_subtract); // 前哨站转速特判 - // TODO - // if (this->Converged() && model_.GetID() == enumeration::CarIDFlag::Outpost - // && std::abs(this->ekf_.x[7]) > 2) - // this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; + if (model_.GetID() == enumeration::CarIDFlag::Outpost && EvaluateConvergence()) { + constexpr double max_outpost_w = 2.51; + if (std::abs(ekf_.x[7]) > 2.0) { + ekf_.x[7] = ekf_.x[7] > 0 ? max_outpost_w : -max_outpost_w; + } + } } - std::time_t time_stamp_; + std::time_t last_see_time_stamp_; util::ExtendedKalmanFilter ekf_; PredictModel model_; int last_id = -1; int update_count = 0; - - // bool converged_ { false }; }; } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index f2e1972..698d9a2 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -1,10 +1,12 @@ #include "live_target_manager.hpp" #include +#include #include #include #include "enum/armor_id.hpp" +#include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "interfaces/predictor_update_package.hpp" #include "tongji/predictor/in_gimbal_control_armor.hpp" @@ -16,24 +18,30 @@ namespace world_exe::tongji::predictor { class LiveTargetManager::Impl { public: - Impl() = default; + Impl(double timeout_sec = 0.1) + : timeout_sec_(timeout_sec) { } void RegisterTarget(enumeration::ArmorIdFlag id, std::shared_ptr target) { targets_[id] = std::move(target); } void RemoveTarget(enumeration::ArmorIdFlag id) { targets_.erase(id); } + bool HasTarget(enumeration::ArmorIdFlag id) const { return targets_.count(id) > 0; } + std::shared_ptr GetTarget(enumeration::ArmorIdFlag id) const { + auto it = targets_.find(id); + return (it != targets_.end()) ? it->second : nullptr; + } std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { std::unordered_map> result; + throw std::runtime_error("Not implemented"); // TODO - // for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { // auto it = targets_.find(id); - // if (it != targets_.end() && it->second) { + // if (it != targets_.end() && it->second && it->second->IsConverged()) { // result[id].emplace_back(it->second->GetArmorGimbalControlSpacings()); // } // } @@ -47,7 +55,7 @@ class LiveTargetManager::Impl { for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { auto it = targets_.find(id); - if (it != targets_.end() && it->second) { + if (it != targets_.end() && it->second && it->second->IsConverged()) { snapshot_map[id] = it->second; } } @@ -55,7 +63,15 @@ class LiveTargetManager::Impl { return std::make_shared(flag, snapshot_map); } - void Update(std::shared_ptr data, const double& dt) { + void Update(std::shared_ptr data, double dt) { + const auto now = predictor::TimeStamp(std::time(nullptr)); + RemoveLostTargets(now); + UpdateTargets(data, dt); + } + +private: + void UpdateTargets( + const std::shared_ptr& data, double dt) { const auto& transform = data->GetTransform(); const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); @@ -74,8 +90,23 @@ class LiveTargetManager::Impl { } } -private: + void RemoveLostTargets(const predictor::TimeStamp& now) { + for (auto it = targets_.begin(); it != targets_.end();) { + if (IsTargetLost(it->second, now)) { + it = targets_.erase(it); + } else { + ++it; + } + } + } + + bool IsTargetLost( + const std::shared_ptr& target, const predictor::TimeStamp& now) const { + return now.SecondsSince(target->LastSeen()) > timeout_sec_; + } + std::unordered_map> targets_; + const double timeout_sec_ { 0.1 }; }; LiveTargetManager::LiveTargetManager() = default; diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/predict_model.hpp index 8fe4b43..6697f3b 100644 --- a/src/tongji/predictor/predict_model.hpp +++ b/src/tongji/predictor/predict_model.hpp @@ -46,11 +46,14 @@ class PredictModel { } else { armor_num_ = 4; } + + // TODO :add priority } auto GetP0Dig() const { return P0_dig_; } auto GetRadius() const { return radius_; } auto GetID() const { return car_id_; } + auto GetPriority() const { return priority_; } int GetArmorNum() const { return armor_num_; } // 防止夹角求和出现异常值 @@ -69,6 +72,7 @@ class PredictModel { return x_prior; }; + //TODO int MatchArmor(const Eigen::VectorXd& x, const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) const { @@ -248,6 +252,7 @@ class PredictModel { } private: + int priority_; int armor_num_; enumeration::CarIDFlag car_id_; diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index 75a5e96..b390a36 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -5,6 +5,7 @@ #include "data/armor_gimbal_control_spacing.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/predict_model.hpp" +#include "tongji/predictor/time_stamp.hpp" #include "util/extended_kalman_filter.hpp" namespace world_exe::tongji::predictor { @@ -14,7 +15,7 @@ class TargetSnapshot { TargetSnapshot(const LiveTarget& target) : model_(target.GetModel()) , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) - , time_stamp_(target.GetTimeStamp()) { } + , time_stamp_(target.LastSeen()) { } std::vector GetArmorGimbalControlSpacings() const { std::vector armors; @@ -37,10 +38,9 @@ class TargetSnapshot { } auto GetTimeStamp() const { return time_stamp_; } - auto GetID() const { return model_.GetID(); } - auto GetEkfX() const { return ekf_.x; } + auto GetPriority() const { return model_.GetPriority(); } auto Predict(const double& dt) -> Eigen::Vector const { auto predicted_x = model_.f(ekf_.x, dt); @@ -50,7 +50,7 @@ class TargetSnapshot { private: PredictModel model_; util::ExtendedKalmanFilter ekf_; - const std::time_t time_stamp_; + TimeStamp time_stamp_; }; } \ No newline at end of file diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index 219e457..4e940bb 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -6,57 +6,61 @@ namespace world_exe::tongji::car_state { +using TimeStamp = predictor::TimeStamp; class CarStateManager { public: CarStateManager(int switch_threshold = 5) - : switch_threshold_(switch_threshold) { } + : switch_threshold_(switch_threshold) + , last_seen_(std::time(nullptr)) { } - void Update(bool detected, std::time_t now_raw) { + void Update(bool detected, const TimeStamp& now) { if (detected) { count_ = std::min(count_ + 1, switch_threshold_); - auto now = predictor::TimeStamp(now_raw); last_seen_ = now; + update_count_++; + if (update_count_ >= converge_threshold_ && is_diverged_) is_converged_ = true; } else { count_ = std::max(count_ - 1, 0); } is_locked_ = (count_ >= switch_threshold_); } - bool IsLost(const predictor::TimeStamp& now, double timeout_sec) const { - return now.SecondsSince(last_seen_) > timeout_sec; - } + bool IsConverged() const { return is_converged_ && !is_diverged_; } - bool IsLocked() const { return is_locked_; } - bool IsConverged() const { return is_converged_; } - bool IsDiverged() const { return is_diverged_; } + bool IsLost(const TimeStamp& now) const { + return is_locked_ && now.SecondsSince(last_seen_) > timeout_sec_; + } - void SetPriority(int p); - void SetThreshold(const int& value) { switch_threshold_ = value; } - void SetDiverged(bool diverged) { - is_diverged_ = diverged; - if (diverged) is_converged_ = false; + void Reset() { + count_ = 0; + update_count_ = 0; + is_locked_ = false; + is_converged_ = false; + is_diverged_ = true; + priority_ = default_priority_; + last_seen_ = predictor::TimeStamp(0); } + void SetPriority(const int& p) { priority_ = p; } int GetPriority() const { return priority_; } - predictor::TimeStamp LastSeen() const { return last_seen_; } + void SetThreshold(const int& value) { switch_threshold_ = value; } - void IncrementUpdateCount() { - update_count_++; - if (update_count_ > 3 && !is_diverged_) { - is_converged_ = true; - } - } + predictor::TimeStamp LastSeen() const { return last_seen_; } private: int count_ = 0; int switch_threshold_; - int update_count_ = 0; + int converge_threshold_ = 3; + int update_count_ = 0; + double timeout_sec_; + + bool is_locked_ = false; + bool is_converged_ = false; + bool is_diverged_ = true; + int priority_ = 100; + int default_priority_ = 100; - bool is_locked_ = false; - bool is_converged_ = false; - bool is_diverged_ = false; - int priority_ = 100; // 默认最低优先级 - predictor::TimeStamp last_seen_ { 0 }; + predictor::TimeStamp last_seen_; }; } \ No newline at end of file diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 442148d..3afd297 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -1,36 +1,56 @@ #include "state_machine.hpp" +#include + #include "enum/car_id.hpp" +#include "tongji/state_machine/car_state_manager.hpp" +#include "util/index.hpp" namespace world_exe::tongji::state_machine { +using TimeStamp = car_state::TimeStamp; + StateMachine::StateMachine(int switch_threshold) - : switch_threshold_(switch_threshold) { - for (auto& manager : car_states_) { - manager = car_state::CarStateManager(switch_threshold_); - } + : switch_threshold_(switch_threshold) + , car_states_ {} { + std::generate(car_states_.begin(), car_states_.end(), + [threshold = switch_threshold]() { return car_state::CarStateManager(threshold); }); } const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return tracing_state_; } +bool StateMachine::IsAllowedToFire(enumeration::CarIDFlag id) const { return tracing_state_ == id; } + +const car_state::CarStateManager& StateMachine::GetState(enumeration::CarIDFlag single_id) const { + return car_states_[util::enumeration::GetIndex(single_id)]; +} + const interfaces::ICarState& StateMachine::Update( - const enumeration::CarIDFlag& car_detected, std::time_t now) { - tracing_state_ = enumeration::CarIDFlag::None; + const enumeration::CarIDFlag& car_detected, const std::time_t& now) { + tracing_state_ = enumeration::CarIDFlag::None; + int best_priority = std::numeric_limits::max(); for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { auto id = static_cast(1 << i); bool detected = (static_cast(car_detected) >> i) & 0x01; - car_states_[i].Update(detected, now); + auto& state = car_states_[i]; + state.Update(detected, now); + + if (!state.IsConverged()) { + state.Reset(); + continue; + } - if (detected) { - car_states_[i].IncrementUpdateCount(); + if (state.IsLost(now)) { + state.Reset(); + continue; } - if (car_states_[i].IsLocked() && car_states_[i].IsConverged() - && !car_states_[i].IsDiverged()) { - tracing_state_ = - static_cast(static_cast(tracing_state_) | (1 << i)); + int p = state.GetPriority(); + if (p < best_priority) { + best_priority = p; + tracing_state_ = id; } } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 8fb9872..a10d1eb 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -12,9 +12,10 @@ class StateMachine final : public interfaces::ICarState { StateMachine(int switch_threshold = 5); const enumeration::CarIDFlag& GetAllowdToFires() const override; - + bool IsAllowedToFire(enumeration::CarIDFlag id) const; + const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const; const interfaces::ICarState& Update( - const enumeration::CarIDFlag& car_detected, std::time_t now); + const enumeration::CarIDFlag& car_detected, const std::time_t& now); private: enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; From b11971e3d19cae7ad5fdc48372ee713accba50f2 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 8 Oct 2025 02:12:52 +0800 Subject: [PATCH 42/93] chore(state_machine):refactor StateMachine to PIMPL pattern and do some unimportant cleanup --- .../fire_controller/aim_point_chooser.hpp | 10 +- src/tongji/fire_controller/aim_solver.hpp | 18 ++-- .../fire_controller/fire_controller.cpp | 35 ++++--- src/tongji/predictor/live_target_manager.cpp | 1 - src/tongji/state_machine/state_machine.cpp | 96 ++++++++++++------- src/tongji/state_machine/state_machine.hpp | 15 +-- 6 files changed, 104 insertions(+), 71 deletions(-) diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp index b457a76..5ba8b72 100644 --- a/src/tongji/fire_controller/aim_point_chooser.hpp +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -5,6 +5,8 @@ namespace world_exe::tongji::fire_control { +using CarIDFlag = enumeration::CarIDFlag; + class AimPointChooser { public: AimPointChooser( @@ -13,7 +15,7 @@ class AimPointChooser { , leaving_angle_(leaving_angle) { } std::pair ChooseAimArmor(const Eigen::Vector& ekf_x, - const std::vector& xyza_list, const enumeration::CarIDFlag& single_id) { + const std::vector& xyza_list, const CarIDFlag& single_id) { const auto armor_num = xyza_list.size(); int chosen_id = -1; @@ -28,7 +30,7 @@ class AimPointChooser { } // 不考虑小陀螺 - if (std::abs(ekf_x[8]) <= 2 && single_id != enumeration::CarIDFlag::Outpost) { + if (std::abs(ekf_x[8]) <= 2 && single_id != CarIDFlag::Outpost) { // 选择在可射击范围内的装甲板 std::vector id_list; for (int i = 0; i < armor_num; i++) { @@ -53,9 +55,9 @@ class AimPointChooser { } else { // 小陀螺 double coming_angle = - (single_id == enumeration::CarIDFlag::Outpost) ? 70 / 57.3 : comming_angle_; + (single_id == CarIDFlag::Outpost) ? 70 / 57.3 : comming_angle_; double leaving_angle = - (single_id == enumeration::CarIDFlag::Outpost) ? 30 / 57.3 : leaving_angle_; + (single_id == CarIDFlag::Outpost) ? 30 / 57.3 : leaving_angle_; // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 for (int i = 0; i < armor_num; i++) { diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index 12fec38..c462bf1 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -12,6 +12,8 @@ namespace world_exe::tongji::fire_control { +using TargetSnapshot = predictor::TargetSnapshot; + struct AimSolution { bool valid; double yaw; @@ -22,20 +24,20 @@ struct AimSolution { class AimingSolver { public: - AimingSolver( const double& bullet_speed, - const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) + AimingSolver(const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, + const double& gravity = 9.7833) : aim_point_chooser_(std::make_unique()) , bullet_speed_(bullet_speed) , yaw_offset_(yaw_offset / 57.3) // degree to rad , pitch_offset_(pitch_offset / 57.3) // degree to rad , g_(gravity) { } - void UpdateSnapshot(std::unique_ptr snapshot) { + void UpdateSnapshot(std::unique_ptr snapshot) { snapshot_ = std::move(snapshot); } AimSolution SolveAimSolution(const double& time_delay) { - if (!snapshot_) return { false, 0, 0, {}, 0 }; + if (!snapshot_) return { false, 0, 0, { }, 0 }; // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) double prev_fly_time = 0; @@ -47,10 +49,10 @@ class AimingSolver { for (int i = 0; i < 10; ++i) { double dt = time_delay + prev_fly_time; const auto aim = SelectPredictedAim(dt); - if (!aim.has_value()) return { false, 0, 0, {}, 0 }; // failed: no valid aim point + if (!aim.has_value()) return { false, 0, 0, { }, 0 }; // failed: no valid aim point const auto traj = SolveTrajectory(aim->head(3)); - if (!traj.has_value()) return { false, 0, 0, {}, 0 }; // failed: trajectory unsolvable + if (!traj.has_value()) return { false, 0, 0, { }, 0 }; // failed: trajectory unsolvable if (i > 0 && std::abs(traj->fly_time - prev_fly_time) < 0.001) { final_aim_point = *aim; @@ -60,7 +62,7 @@ class AimingSolver { } prev_fly_time = traj->fly_time; } - if (!converged) return { false, 0, 0, {}, 0 }; // failed: trajectory did not converge + if (!converged) return { false, 0, 0, { }, 0 }; // failed: trajectory did not converge const auto xyz = final_aim_point.head(3); const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; @@ -86,6 +88,6 @@ class AimingSolver { const double g_; std::unique_ptr aim_point_chooser_; - std::unique_ptr snapshot_; + std::unique_ptr snapshot_; }; } \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 114de5d..7639411 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -19,6 +19,9 @@ namespace world_exe::tongji::fire_control { using TargetSnapshotManager = predictor::TargetSnapshotManager; +using StateMachine = state_machine::StateMachine; +using IdentifiedArmor = identifier::IdentifiedArmor; +using CarIDFlag = enumeration::CarIDFlag; class FireController::Impl { public: @@ -27,17 +30,19 @@ class FireController::Impl { : control_delay_(control_delay_in_second) , bullet_speed_(bullet_speed) , tracker_(std::make_unique()) - , predictor_(nullptr) // TODO + , snapshot_manager_(nullptr) // TODO:A new snapshot manager needs to be created based on the + // input of each frame of the camera , aim_solver_(std::make_unique(bullet_speed, yaw_offset, pitch_offset)) , fire_decision_(std::make_unique(auto_fire)) - , state_machine_(std::make_unique()) { } + , state_machine_(std::make_unique()) { } // TODO:std::time_t const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { - if (!identified_armors_ || !tracker_ || !predictor_ || !aim_solver_ || !fire_decision_) + if (!identified_armors_ || !tracker_ || !snapshot_manager_ || !aim_solver_ + || !fire_decision_) return { .fire_allowance = false }; - auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, predictor_); + auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, snapshot_manager_); bool found = (snapshot != nullptr); tracker_->UpdateState(found); if (!found) return { .fire_allowance = false }; @@ -56,23 +61,23 @@ class FireController::Impl { const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_) && state_machine_->IsAllowedToFire(current_id); - + return { .time_stamp = time_stamp_.GetTimeStamp(), .gimbal_dir = Eigen::Vector3d(aim_solution.yaw, aim_solution.pitch, 0), .fire_allowance = fireable }; } // TODO - const enumeration ::CarIDFlag GetAttackCarId() const { - if (!tracker_) return enumeration::CarIDFlag::Unknow; + const CarIDFlag GetAttackCarId() const { + if (!tracker_) return CarIDFlag::Unknow; return tracker_->GetCurrentTargetID(); } - void SetSnapshotManager(std::shared_ptr manager) { - predictor_ = std::move(manager); + void SetSnapshotManager(std::shared_ptr manager) { + snapshot_manager_ = std::move(manager); } - void UpdateArmors(const identifier::IdentifiedArmor& armors) { identified_armors_ = armors; } + void UpdateArmors(const IdentifiedArmor& armors) { identified_armors_ = armors; } void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { gimbal_pos_ = gimbal_pos; }; void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_.SetTimeStamp(time_stamp); } @@ -81,12 +86,12 @@ class FireController::Impl { double control_delay_; double bullet_speed_; - mutable std::optional identified_armors_; + mutable std::optional identified_armors_; std::unique_ptr tracker_; - std::shared_ptr predictor_; + std::shared_ptr snapshot_manager_; std::unique_ptr aim_solver_; std::unique_ptr fire_decision_; - std::unique_ptr state_machine_; + std::unique_ptr state_machine_; predictor::TimeStamp time_stamp_ { 0 }; }; @@ -98,11 +103,11 @@ const data ::FireControl FireController::CalculateTarget(const std ::time_t& tim return pimpl_->CalculateTarget(time_duration); } -void FireController::SetSnapshotManager(std::shared_ptr manager) { +void FireController::SetSnapshotManager(std::shared_ptr manager) { pimpl_->SetSnapshotManager(std::move(manager)); } -const enumeration ::CarIDFlag FireController::GetAttackCarId() const { +const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index 698d9a2..7bdc12c 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -6,7 +6,6 @@ #include #include "enum/armor_id.hpp" -#include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "interfaces/predictor_update_package.hpp" #include "tongji/predictor/in_gimbal_control_armor.hpp" diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 3afd297..c72ffa6 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -1,59 +1,83 @@ #include "state_machine.hpp" -#include +#include #include "enum/car_id.hpp" #include "tongji/state_machine/car_state_manager.hpp" #include "util/index.hpp" namespace world_exe::tongji::state_machine { - using TimeStamp = car_state::TimeStamp; -StateMachine::StateMachine(int switch_threshold) - : switch_threshold_(switch_threshold) - , car_states_ {} { - std::generate(car_states_.begin(), car_states_.end(), - [threshold = switch_threshold]() { return car_state::CarStateManager(threshold); }); -} +class StateMachine::Impl { +public: + Impl() + : car_states_ { } { + std::generate( + car_states_.begin(), car_states_.end(), []() { return car_state::CarStateManager(); }); + } -const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return tracing_state_; } + const enumeration::CarIDFlag& GetAllowdToFires() const { return tracing_state_; } -bool StateMachine::IsAllowedToFire(enumeration::CarIDFlag id) const { return tracing_state_ == id; } + bool IsAllowedToFire(enumeration::CarIDFlag id) const { return tracing_state_ == id; } -const car_state::CarStateManager& StateMachine::GetState(enumeration::CarIDFlag single_id) const { - return car_states_[util::enumeration::GetIndex(single_id)]; -} + const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const { + return car_states_[util::enumeration::GetIndex(single_id)]; + } + void Update(const enumeration::CarIDFlag& car_detected, const std::time_t& now) { + tracing_state_ = enumeration::CarIDFlag::None; + int best_priority = std::numeric_limits::max(); + + for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { + auto id = static_cast(1 << i); + bool detected = (static_cast(car_detected) >> i) & 0x01; + + auto& state = car_states_[i]; + state.Update(detected, now); + + if (!state.IsConverged()) { + state.Reset(); + continue; + } + + if (state.IsLost(now)) { + state.Reset(); + continue; + } + + int p = state.GetPriority(); + if (p < best_priority) { + best_priority = p; + tracing_state_ = id; + } + } + } -const interfaces::ICarState& StateMachine::Update( - const enumeration::CarIDFlag& car_detected, const std::time_t& now) { - tracing_state_ = enumeration::CarIDFlag::None; - int best_priority = std::numeric_limits::max(); +private: + enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; + std::array car_states_; +}; - for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { - auto id = static_cast(1 << i); - bool detected = (static_cast(car_detected) >> i) & 0x01; +StateMachine::StateMachine() + : pimpl_(std::make_unique()) { } - auto& state = car_states_[i]; - state.Update(detected, now); +StateMachine::~StateMachine() = default; - if (!state.IsConverged()) { - state.Reset(); - continue; - } +const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { + return pimpl_->GetAllowdToFires(); +} - if (state.IsLost(now)) { - state.Reset(); - continue; - } +bool StateMachine::IsAllowedToFire(enumeration::CarIDFlag id) const { + return pimpl_->IsAllowedToFire(id); +} - int p = state.GetPriority(); - if (p < best_priority) { - best_priority = p; - tracing_state_ = id; - } - } +const car_state::CarStateManager& StateMachine::GetState(enumeration::CarIDFlag single_id) const { + return pimpl_->GetState(single_id); +} +const interfaces::ICarState& StateMachine::Update( + const enumeration::CarIDFlag& car_detected, const std::time_t& now) { + pimpl_->Update(car_detected, now); return *this; } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index a10d1eb..4b6fa86 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -1,25 +1,26 @@ #pragma once #include +#include -#include "car_state_manager.hpp" #include "enum/car_id.hpp" #include "interfaces/car_state.hpp" +#include "tongji/state_machine/car_state_manager.hpp" namespace world_exe::tongji::state_machine { class StateMachine final : public interfaces::ICarState { public: - StateMachine(int switch_threshold = 5); + StateMachine(); + ~StateMachine(); - const enumeration::CarIDFlag& GetAllowdToFires() const override; - bool IsAllowedToFire(enumeration::CarIDFlag id) const; + const enumeration ::CarIDFlag& GetAllowdToFires() const override; const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const; + bool IsAllowedToFire(enumeration::CarIDFlag id) const; const interfaces::ICarState& Update( const enumeration::CarIDFlag& car_detected, const std::time_t& now); private: - enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; - std::array car_states_; - int switch_threshold_; + class Impl; + std::unique_ptr pimpl_; }; } \ No newline at end of file From faaa5b1629de59e8344d7933f366adfa40596790 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 8 Oct 2025 05:47:31 +0800 Subject: [PATCH 43/93] feat(tracking): complete LiveTargetManager update logic --- .../fire_controller/fire_controller.cpp | 20 ++---- src/tongji/predictor/live_target.hpp | 21 +++++- src/tongji/predictor/live_target_manager.cpp | 64 +++++++++++++------ src/tongji/predictor/predict_model.hpp | 3 +- src/tongji/state_machine/state_machine.cpp | 19 ++---- src/tongji/state_machine/state_machine.hpp | 1 - 6 files changed, 76 insertions(+), 52 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 7639411..43f859b 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -55,23 +55,17 @@ class FireController::Impl { state_machine_->Update(identified_armors_->GetDetectedIDs(), time_stamp_.GetTimeStamp()); - const auto allowed_id = state_machine_->GetAllowdToFires(); // TODO - const auto current_id = tracker_->GetCurrentTargetID(); - const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); - const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_) - && state_machine_->IsAllowedToFire(current_id); + const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_); + + if (fireable) attacked_cars_ = snapshot->GetID(); return { .time_stamp = time_stamp_.GetTimeStamp(), .gimbal_dir = Eigen::Vector3d(aim_solution.yaw, aim_solution.pitch, 0), .fire_allowance = fireable }; } - // TODO - const CarIDFlag GetAttackCarId() const { - if (!tracker_) return CarIDFlag::Unknow; - return tracker_->GetCurrentTargetID(); - } + const CarIDFlag GetAttackCarId() const { return attacked_cars_; } void SetSnapshotManager(std::shared_ptr manager) { snapshot_manager_ = std::move(manager); @@ -86,6 +80,8 @@ class FireController::Impl { double control_delay_; double bullet_speed_; + mutable CarIDFlag attacked_cars_ { CarIDFlag::None }; // TODO + mutable std::optional identified_armors_; std::unique_ptr tracker_; std::shared_ptr snapshot_manager_; @@ -107,8 +103,6 @@ void FireController::SetSnapshotManager(std::shared_ptr m pimpl_->SetSnapshotManager(std::move(manager)); } -const CarIDFlag FireController::GetAttackCarId() const { - return pimpl_->GetAttackCarId(); -} +const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } } \ No newline at end of file diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index 2f5eed1..e0dc168 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -6,9 +6,10 @@ #include #include +#include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" #include "predict_model.hpp" -#include "tongji/state_machine/car_state_manager.hpp" +#include "tongji/predictor/time_stamp.hpp" #include "util/extended_kalman_filter.hpp" namespace world_exe::tongji::predictor { @@ -37,7 +38,7 @@ class LiveTarget { Eigen::MatrixXd P0 = model_.GetP0Dig().asDiagonal(); ekf_ = util::ExtendedKalmanFilter( - x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） + x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） } Eigen::VectorXd GetEkfX() const { return ekf_.x; } @@ -45,6 +46,22 @@ class LiveTarget { const PredictModel& GetModel() const { return model_; } predictor::TimeStamp LastSeen() const { return predictor::TimeStamp(last_see_time_stamp_); } + std::vector GetArmorGimbalControlSpacings() const { + std::vector armors; + for (int id = 0; id < model_.GetArmorNum(); id++) { + auto angle = + util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto xyz = model_.h_armor_xyz(this->ekf_.x, id); + + data::ArmorGimbalControlSpacing armor; + armor.id = model_.GetID(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armors.emplace_back(std::move(armor)); + } + return armors; + } + void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { // 装甲板匹配 diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index 7bdc12c..504f828 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -1,11 +1,12 @@ #include "live_target_manager.hpp" +#include #include -#include #include #include #include "enum/armor_id.hpp" +#include "enum/car_id.hpp" #include "enum/enum_tools.hpp" #include "interfaces/predictor_update_package.hpp" #include "tongji/predictor/in_gimbal_control_armor.hpp" @@ -36,14 +37,13 @@ class LiveTargetManager::Impl { std::unordered_map> result; - throw std::runtime_error("Not implemented"); - // TODO - // for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { - // auto it = targets_.find(id); - // if (it != targets_.end() && it->second && it->second->IsConverged()) { - // result[id].emplace_back(it->second->GetArmorGimbalControlSpacings()); - // } - // } + for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { + auto it = targets_.find(id); + if (it != targets_.end() && it->second && it->second->IsConverged()) { + auto spacings = it->second->GetArmorGimbalControlSpacings(); + result[id] = std::move(spacings); + } + } return std::make_shared(result, time_stamp); } @@ -68,23 +68,48 @@ class LiveTargetManager::Impl { UpdateTargets(data, dt); } + auto GetLiveTargetIDs() -> enumeration::CarIDFlag const { + enumeration::CarIDFlag result = enumeration::CarIDFlag::None; + for (const auto& [id, target] : targets_) { + if (target && target->IsConverged()) { + result = static_cast( + static_cast(result) | static_cast(id)); + } + } + return result; + } + private: void UpdateTargets( const std::shared_ptr& data, double dt) { - const auto& transform = data->GetTransform(); - const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); + const Eigen::Affine3d transform = data->GetTransform(); + const Eigen::Matrix3d rotation_matrix = transform.linear(); + const Eigen::Quaterniond rotation_quat(rotation_matrix); - for (const auto& [id, target] : targets_) { - const auto armors = data->GetArmors()->GetArmors(id); + const auto& armors_interface = data->GetArmors(); + for (int i; i < 8; i++) { + auto id = static_cast( + static_cast(enumeration::CarIDFlag::Hero) << i); + const auto& armors = armors_interface->GetArmors(id); if (armors.empty()) continue; - for (auto armor : armors) { - const auto& armor_in_world_position = transform * armor.position; - const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; - target->Update(dt, armor_in_world_position, - util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), - util::math::xyz2ypd(armor_in_world_position)); + if (!HasTarget(id)) { + const auto& armor = armors.front(); + const Eigen::Vector3d position_in_world = transform * armor.position; + const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX + + auto target = std::make_shared(position_in_world, ypr_in_world, id); + RegisterTarget(id, target); + } + + auto target = GetTarget(id); + for (const auto& armor : armors) { + const Eigen::Vector3d position_in_world = transform * armor.position; + const Eigen::Vector3d ypr = + util::math::quaternion_to_euler(Eigen::Quaterniond(rotation_matrix), 2, 1, 0); + + target->Update(dt, position_in_world, ypr, util::math::xyz2ypd(position_in_world)); } } } @@ -120,5 +145,4 @@ std ::shared_ptr LiveTargetManager::GetPredictor( const enumeration ::ArmorIdFlag& id) const { return pimpl_->GetPredictor(id); } - } \ No newline at end of file diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/predict_model.hpp index 6697f3b..109fee0 100644 --- a/src/tongji/predictor/predict_model.hpp +++ b/src/tongji/predictor/predict_model.hpp @@ -47,7 +47,7 @@ class PredictModel { armor_num_ = 4; } - // TODO :add priority + // TODO :init priority } auto GetP0Dig() const { return P0_dig_; } @@ -72,7 +72,6 @@ class PredictModel { return x_prior; }; - //TODO int MatchArmor(const Eigen::VectorXd& x, const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) const { diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index c72ffa6..2ce500e 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -1,6 +1,7 @@ #include "state_machine.hpp" #include +#include #include "enum/car_id.hpp" #include "tongji/state_machine/car_state_manager.hpp" @@ -16,17 +17,14 @@ class StateMachine::Impl { std::generate( car_states_.begin(), car_states_.end(), []() { return car_state::CarStateManager(); }); } - const enumeration::CarIDFlag& GetAllowdToFires() const { return tracing_state_; } - bool IsAllowedToFire(enumeration::CarIDFlag id) const { return tracing_state_ == id; } - const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const { return car_states_[util::enumeration::GetIndex(single_id)]; } + void Update(const enumeration::CarIDFlag& car_detected, const std::time_t& now) { - tracing_state_ = enumeration::CarIDFlag::None; - int best_priority = std::numeric_limits::max(); + tracing_state_ = enumeration::CarIDFlag::None; for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { auto id = static_cast(1 << i); @@ -45,11 +43,8 @@ class StateMachine::Impl { continue; } - int p = state.GetPriority(); - if (p < best_priority) { - best_priority = p; - tracing_state_ = id; - } + tracing_state_ = static_cast( + static_cast(tracing_state_) | static_cast(id)); } } @@ -67,10 +62,6 @@ const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return pimpl_->GetAllowdToFires(); } -bool StateMachine::IsAllowedToFire(enumeration::CarIDFlag id) const { - return pimpl_->IsAllowedToFire(id); -} - const car_state::CarStateManager& StateMachine::GetState(enumeration::CarIDFlag single_id) const { return pimpl_->GetState(single_id); } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 4b6fa86..780fe07 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -15,7 +15,6 @@ class StateMachine final : public interfaces::ICarState { const enumeration ::CarIDFlag& GetAllowdToFires() const override; const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const; - bool IsAllowedToFire(enumeration::CarIDFlag id) const; const interfaces::ICarState& Update( const enumeration::CarIDFlag& car_detected, const std::time_t& now); From 08b7ebd390dbd4584d05135950ba03d8e2ceb40f Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 8 Oct 2025 10:11:49 +0800 Subject: [PATCH 44/93] feat(fire_control):enhance CalculateTarget logic and add miss public interfaces declaration --- .../fire_controller/fire_controller.cpp | 56 ++++++++++--------- .../fire_controller/fire_controller.hpp | 13 +++-- src/tongji/fire_controller/tracker.hpp | 1 - src/tongji/predictor/live_target_manager.cpp | 32 +++++++---- src/tongji/predictor/live_target_manager.hpp | 6 ++ 5 files changed, 67 insertions(+), 41 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 43f859b..75d3b21 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -12,6 +12,7 @@ #include "tongji/fire_controller/fire_decision.hpp" #include "tongji/fire_controller/tracker.hpp" #include "tongji/identifier/identified_armor.hpp" +#include "tongji/predictor/live_target_manager.hpp" #include "tongji/predictor/target_snapshot_manager.hpp" #include "tongji/predictor/time_stamp.hpp" #include "tongji/state_machine/state_machine.hpp" @@ -22,6 +23,8 @@ using TargetSnapshotManager = predictor::TargetSnapshotManager; using StateMachine = state_machine::StateMachine; using IdentifiedArmor = identifier::IdentifiedArmor; using CarIDFlag = enumeration::CarIDFlag; +using LiveTargetManager = predictor::LiveTargetManager; +using TimeStamp = predictor::TimeStamp; class FireController::Impl { public: @@ -30,31 +33,30 @@ class FireController::Impl { : control_delay_(control_delay_in_second) , bullet_speed_(bullet_speed) , tracker_(std::make_unique()) - , snapshot_manager_(nullptr) // TODO:A new snapshot manager needs to be created based on the - // input of each frame of the camera , aim_solver_(std::make_unique(bullet_speed, yaw_offset, pitch_offset)) , fire_decision_(std::make_unique(auto_fire)) - , state_machine_(std::make_unique()) { } + , live_target_manager_(std::make_shared()) { } // TODO:std::time_t const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { - if (!identified_armors_ || !tracker_ || !snapshot_manager_ || !aim_solver_ - || !fire_decision_) + if (!identified_armors_ || !tracker_ || !aim_solver_ || !fire_decision_ + || !live_target_manager_) return { .fire_allowance = false }; + + auto snapshot_manager = std::dynamic_pointer_cast( + live_target_manager_->GetPredictor(live_target_manager_->GetLiveTargetIDs())); + auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, snapshot_manager); - auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, snapshot_manager_); - bool found = (snapshot != nullptr); + bool found = (snapshot != nullptr); tracker_->UpdateState(found); if (!found) return { .fire_allowance = false }; aim_solver_->UpdateSnapshot(std::move(snapshot)); - const auto& aim_solution = - aim_solver_->SolveAimSolution(static_cast(time_duration)); // TODO + const auto& aim_solution = aim_solver_->SolveAimSolution(static_cast( + time_duration)); // TODO:std::time_t should not be converted directly to double type if (!aim_solution.valid) return { .fire_allowance = false }; - state_machine_->Update(identified_armors_->GetDetectedIDs(), time_stamp_.GetTimeStamp()); - const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_); @@ -67,42 +69,46 @@ class FireController::Impl { const CarIDFlag GetAttackCarId() const { return attacked_cars_; } - void SetSnapshotManager(std::shared_ptr manager) { - snapshot_manager_ = std::move(manager); - } - - void UpdateArmors(const IdentifiedArmor& armors) { identified_armors_ = armors; } + void UpdateIdentifiedArmors(const IdentifiedArmor& armors) { identified_armors_ = armors; } void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { gimbal_pos_ = gimbal_pos; }; void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_.SetTimeStamp(time_stamp); } + TimeStamp GetTimeStamp() const { return time_stamp_; } private: Eigen::Vector3d gimbal_pos_ { Eigen::Vector3d::Zero() }; double control_delay_; double bullet_speed_; - mutable CarIDFlag attacked_cars_ { CarIDFlag::None }; // TODO + mutable CarIDFlag attacked_cars_ { + CarIDFlag::None + }; // TODO:Mutable should not be used to break const encapsulation mutable std::optional identified_armors_; std::unique_ptr tracker_; - std::shared_ptr snapshot_manager_; + std::shared_ptr live_target_manager_; std::unique_ptr aim_solver_; std::unique_ptr fire_decision_; - std::unique_ptr state_machine_; - predictor::TimeStamp time_stamp_ { 0 }; + predictor::TimeStamp time_stamp_ { std::time(nullptr) }; }; -FireController::FireController() - : pimpl_() { } +FireController::FireController(bool auto_fire, const double& control_delay_in_second, + const double& bullet_speed, double yaw_offset, double pitch_offset) + : pimpl_(std::make_unique( + auto_fire, control_delay_in_second, bullet_speed, yaw_offset, pitch_offset)) { } const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { return pimpl_->CalculateTarget(time_duration); } -void FireController::SetSnapshotManager(std::shared_ptr manager) { - pimpl_->SetSnapshotManager(std::move(manager)); +const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } + +void FireController::UpdateIdentifiedArmors(const IdentifiedArmor& armors) { + return pimpl_->UpdateIdentifiedArmors(armors); } -const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } +void FireController::UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { + return pimpl_->UpdateGimbalPosition(gimbal_pos); +}; } \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 8458a07..c5fedd4 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -3,19 +3,24 @@ #include #include "interfaces/fire_controller.hpp" -#include "tongji/predictor/target_snapshot_manager.hpp" +#include "tongji/identifier/identified_armor.hpp" +#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::fire_control { class FireController final : public interfaces::IFireControl { public: - FireController(); + FireController(bool auto_fire, const double& control_delay_in_second, + const double& bullet_speed, double yaw_offset, double pitch_offset); const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; - const enumeration ::CarIDFlag GetAttackCarId() const override; - void SetSnapshotManager(std::shared_ptr manager); + void UpdateIdentifiedArmors(const identifier::IdentifiedArmor& armors); + void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos); + + void SetTimeStamp(const std::time_t& time_stamp); + predictor::TimeStamp GetTimeStamp() const; private: class Impl; diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index 9e871e0..361a3da 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -51,7 +51,6 @@ class DefaultTracker final { }); sq_armor_list->sort([](const auto& a, const auto& b) { return a.priority < b.priority; }); - // auto iterator = sq_armor_list->begin(); auto filter_flag = snapshot_manager_->GetId(); for (const auto& armor : *sq_armor_list) { if (!enumeration::IsFlagContains(filter_flag, armor.armor.id)) continue; diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index 504f828..aaae1be 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -21,17 +21,6 @@ class LiveTargetManager::Impl { Impl(double timeout_sec = 0.1) : timeout_sec_(timeout_sec) { } - void RegisterTarget(enumeration::ArmorIdFlag id, std::shared_ptr target) { - targets_[id] = std::move(target); - } - - void RemoveTarget(enumeration::ArmorIdFlag id) { targets_.erase(id); } - bool HasTarget(enumeration::ArmorIdFlag id) const { return targets_.count(id) > 0; } - std::shared_ptr GetTarget(enumeration::ArmorIdFlag id) const { - auto it = targets_.find(id); - return (it != targets_.end()) ? it->second : nullptr; - } - std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { std::unordered_map> @@ -66,6 +55,7 @@ class LiveTargetManager::Impl { const auto now = predictor::TimeStamp(std::time(nullptr)); RemoveLostTargets(now); UpdateTargets(data, dt); + // TODO:update the state_machine } auto GetLiveTargetIDs() -> enumeration::CarIDFlag const { @@ -80,6 +70,16 @@ class LiveTargetManager::Impl { } private: + void RegisterTarget(enumeration::ArmorIdFlag id, std::shared_ptr target) { + targets_[id] = std::move(target); + } + void RemoveTarget(enumeration::ArmorIdFlag id) { targets_.erase(id); } + bool HasTarget(enumeration::ArmorIdFlag id) const { return targets_.count(id) > 0; } + std::shared_ptr GetTarget(enumeration::ArmorIdFlag id) const { + auto it = targets_.find(id); + return (it != targets_.end()) ? it->second : nullptr; + } + void UpdateTargets( const std::shared_ptr& data, double dt) { const Eigen::Affine3d transform = data->GetTransform(); @@ -145,4 +145,14 @@ std ::shared_ptr LiveTargetManager::GetPredictor( const enumeration ::ArmorIdFlag& id) const { return pimpl_->GetPredictor(id); } + +auto LiveTargetManager::GetLiveTargetIDs() -> enumeration::CarIDFlag const { + return pimpl_->GetLiveTargetIDs(); +} + +void LiveTargetManager::Update( + std::shared_ptr data, double dt) { + return pimpl_->Update(data, dt); +} + } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.hpp b/src/tongji/predictor/live_target_manager.hpp index 33937ab..f64af9f 100644 --- a/src/tongji/predictor/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager.hpp @@ -3,6 +3,8 @@ #include #include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "interfaces/predictor_update_package.hpp" #include "interfaces/target_predictor.hpp" namespace world_exe::tongji::predictor { @@ -18,6 +20,10 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { std ::shared_ptr GetPredictor( const enumeration ::ArmorIdFlag& id) const override; + auto GetLiveTargetIDs() -> enumeration::CarIDFlag const; + + void Update(std::shared_ptr data, double dt) ; + private: class Impl; std::unique_ptr pimpl_; From 7e3b7437316cf879a9998e1bf85ae5448cbdb5ce Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 8 Oct 2025 11:50:49 +0800 Subject: [PATCH 45/93] refactor(state_machine): complete state machine update logic --- .../fire_controller/fire_controller.cpp | 18 ++++++++------- .../fire_controller/fire_controller.hpp | 6 +++-- src/tongji/predictor/live_target_manager.cpp | 23 +++++++++++++++---- src/tongji/predictor/live_target_manager.hpp | 6 +++-- 4 files changed, 36 insertions(+), 17 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 75d3b21..e065cb6 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -28,21 +28,22 @@ using TimeStamp = predictor::TimeStamp; class FireController::Impl { public: - Impl(bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, - double yaw_offset, double pitch_offset) + Impl(std::shared_ptr state_machine, bool auto_fire, + const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, + double pitch_offset) : control_delay_(control_delay_in_second) , bullet_speed_(bullet_speed) , tracker_(std::make_unique()) , aim_solver_(std::make_unique(bullet_speed, yaw_offset, pitch_offset)) , fire_decision_(std::make_unique(auto_fire)) - , live_target_manager_(std::make_shared()) { } + , live_target_manager_(std::make_shared(state_machine)) { } // TODO:std::time_t const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { if (!identified_armors_ || !tracker_ || !aim_solver_ || !fire_decision_ || !live_target_manager_) return { .fire_allowance = false }; - + auto snapshot_manager = std::dynamic_pointer_cast( live_target_manager_->GetPredictor(live_target_manager_->GetLiveTargetIDs())); auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, snapshot_manager); @@ -92,10 +93,11 @@ class FireController::Impl { predictor::TimeStamp time_stamp_ { std::time(nullptr) }; }; -FireController::FireController(bool auto_fire, const double& control_delay_in_second, - const double& bullet_speed, double yaw_offset, double pitch_offset) - : pimpl_(std::make_unique( - auto_fire, control_delay_in_second, bullet_speed, yaw_offset, pitch_offset)) { } +FireController::FireController(std::shared_ptr state_machine, bool auto_fire, + const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, + double pitch_offset) + : pimpl_(std::make_unique(state_machine, auto_fire, control_delay_in_second, bullet_speed, + yaw_offset, pitch_offset)) { } const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { return pimpl_->CalculateTarget(time_duration); diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index c5fedd4..6258923 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -5,13 +5,15 @@ #include "interfaces/fire_controller.hpp" #include "tongji/identifier/identified_armor.hpp" #include "tongji/predictor/time_stamp.hpp" +#include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::fire_control { class FireController final : public interfaces::IFireControl { public: - FireController(bool auto_fire, const double& control_delay_in_second, - const double& bullet_speed, double yaw_offset, double pitch_offset); + FireController(std::shared_ptr state_machine, bool auto_fire, + const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, + double pitch_offset); const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index aaae1be..c5e6ba2 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -12,14 +12,17 @@ #include "tongji/predictor/in_gimbal_control_armor.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/target_snapshot_manager.hpp" +#include "tongji/predictor/time_stamp.hpp" +#include "tongji/state_machine/state_machine.hpp" #include "util/index.hpp" namespace world_exe::tongji::predictor { class LiveTargetManager::Impl { public: - Impl(double timeout_sec = 0.1) - : timeout_sec_(timeout_sec) { } + Impl(std::shared_ptr state_machine, double timeout_sec = 0.1) + : timeout_sec_(timeout_sec) + , state_machine_(state_machine) { } std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { @@ -54,7 +57,7 @@ class LiveTargetManager::Impl { void Update(std::shared_ptr data, double dt) { const auto now = predictor::TimeStamp(std::time(nullptr)); RemoveLostTargets(now); - UpdateTargets(data, dt); + AddNewTargets(data, dt); // TODO:update the state_machine } @@ -80,7 +83,7 @@ class LiveTargetManager::Impl { return (it != targets_.end()) ? it->second : nullptr; } - void UpdateTargets( + void AddNewTargets( const std::shared_ptr& data, double dt) { const Eigen::Affine3d transform = data->GetTransform(); const Eigen::Matrix3d rotation_matrix = transform.linear(); @@ -94,6 +97,8 @@ class LiveTargetManager::Impl { if (armors.empty()) continue; + UpdateStateMachine(id, std::time(nullptr)); + if (!HasTarget(id)) { const auto& armor = armors.front(); const Eigen::Vector3d position_in_world = transform * armor.position; @@ -101,6 +106,7 @@ class LiveTargetManager::Impl { auto target = std::make_shared(position_in_world, ypr_in_world, id); RegisterTarget(id, target); + continue; } auto target = GetTarget(id); @@ -124,16 +130,23 @@ class LiveTargetManager::Impl { } } + void UpdateStateMachine(enumeration::CarIDFlag cars_detected, TimeStamp now) { + state_machine_->Update(cars_detected, now.GetTimeStamp()); + } + bool IsTargetLost( const std::shared_ptr& target, const predictor::TimeStamp& now) const { return now.SecondsSince(target->LastSeen()) > timeout_sec_; } std::unordered_map> targets_; + std::shared_ptr state_machine_; const double timeout_sec_ { 0.1 }; }; -LiveTargetManager::LiveTargetManager() = default; +LiveTargetManager::LiveTargetManager( + std::shared_ptr state_machine, double timeout_sec) + : pimpl_(std::make_unique(state_machine, timeout_sec)) { } LiveTargetManager::~LiveTargetManager() = default; std ::shared_ptr LiveTargetManager::Predict( diff --git a/src/tongji/predictor/live_target_manager.hpp b/src/tongji/predictor/live_target_manager.hpp index f64af9f..7a1c978 100644 --- a/src/tongji/predictor/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager.hpp @@ -6,12 +6,14 @@ #include "enum/car_id.hpp" #include "interfaces/predictor_update_package.hpp" #include "interfaces/target_predictor.hpp" +#include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::predictor { class LiveTargetManager final : public interfaces::ITargetPredictor { public: - LiveTargetManager(); + LiveTargetManager( + std::shared_ptr state_machine, double timeout_sec = 0.1); ~LiveTargetManager(); std ::shared_ptr Predict( @@ -22,7 +24,7 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { auto GetLiveTargetIDs() -> enumeration::CarIDFlag const; - void Update(std::shared_ptr data, double dt) ; + void Update(std::shared_ptr data, double dt); private: class Impl; From 010b46631be6d204256fccf670a762d617957887 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 8 Oct 2025 14:13:20 +0800 Subject: [PATCH 46/93] perf(filter): make Kalman filter type inference compile-time Replace runtime type checks with templates for better performance. --- .../predictor/extended_kalman_filter.hpp | 39 ++++--- src/tongji/predictor/live_target.hpp | 33 +++--- src/tongji/predictor/predict_model.hpp | 91 +++++++++------- src/tongji/predictor/target_snapshot.hpp | 4 +- src/util/extended_kalman_filter.hpp | 101 ------------------ 5 files changed, 96 insertions(+), 172 deletions(-) delete mode 100644 src/util/extended_kalman_filter.hpp diff --git a/src/tongji/predictor/extended_kalman_filter.hpp b/src/tongji/predictor/extended_kalman_filter.hpp index 32450db..f016491 100644 --- a/src/tongji/predictor/extended_kalman_filter.hpp +++ b/src/tongji/predictor/extended_kalman_filter.hpp @@ -7,17 +7,29 @@ #include namespace world_exe::tongji::predictor { +template // class ExtendedKalmanFilter { public: - Eigen::VectorXd x; - Eigen::MatrixXd P; + using XVec = Eigen::Matrix; + using ZVec = Eigen::Matrix; + using AMat = Eigen::Matrix; + using PMat = Eigen::Matrix; + using PDig = Eigen::Matrix; + using RMat = Eigen::Matrix; + using RDig = Eigen::Matrix; + using QMat = Eigen::Matrix; + using HMat = Eigen::Matrix; + + XVec x; + PMat P; ExtendedKalmanFilter() = default; ExtendedKalmanFilter( - const Eigen::VectorXd& x0, const Eigen::MatrixXd& P0, - std::function x_add = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; }) { + const XVec& x0, const PMat& P0, + std::function x_add = [](const XVec& a, const XVec& b) { + return a + b; + }) { data["residual_yaw"] = 0.0; data["residual_pitch"] = 0.0; data["residual_distance"] = 0.0; @@ -29,13 +41,12 @@ class ExtendedKalmanFilter { data["recent_nis_failures"] = 0.0; } - Eigen::VectorXd Update( - const double& dt, const Eigen::MatrixXd& A, const Eigen::MatrixXd& Q, - std::function f, - const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, - std::function h, - std::function z_subtract = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { + XVec Update( + const double& dt, const AMat& A, const QMat& Q, + std::function f, const ZVec& z, const HMat& H, + const RMat& R, std::function h, + std::function z_subtract = + [](const ZVec& a, const ZVec& b) { return a - b; }) { auto x_n = f(x, dt); @@ -90,8 +101,8 @@ class ExtendedKalmanFilter { double last_nis; private: - Eigen::MatrixXd I; - std::function x_add; + Eigen::Matrix I; + std::function x_add; int nees_count_ = 0; int nis_count_ = 0; diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp index e0dc168..d25bfdb 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target.hpp @@ -1,16 +1,15 @@ #pragma once #include -#include #include #include #include #include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" +#include "extended_kalman_filter.hpp" #include "predict_model.hpp" #include "tongji/predictor/time_stamp.hpp" -#include "util/extended_kalman_filter.hpp" namespace world_exe::tongji::predictor { @@ -32,17 +31,17 @@ class LiveTarget { armor_xyz_in_world[1] + model_.GetRadius() * std::sin(armor_ypr_in_world[0]); auto center_z = armor_xyz_in_world[2]; - Eigen::VectorXd x0(11); + ExtendedKalmanFilter<11, 4>::XVec x0; x0 << center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, model_.GetRadius(), 0, 0; - Eigen::MatrixXd P0 = model_.GetP0Dig().asDiagonal(); - ekf_ = util::ExtendedKalmanFilter( + ExtendedKalmanFilter<11, 4>::PMat P0 = model_.GetP0Dig().asDiagonal(); + ekf_ = ExtendedKalmanFilter<11, 4>( x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） } - Eigen::VectorXd GetEkfX() const { return ekf_.x; } - Eigen::VectorXd GetP0Dig() const { return model_.GetP0Dig(); } + ExtendedKalmanFilter<11, 4>::XVec GetEkfX() const { return ekf_.x; } + ExtendedKalmanFilter<11, 4>::PDig GetP0Dig() const { return model_.GetP0Dig(); } const PredictModel& GetModel() const { return model_; } predictor::TimeStamp LastSeen() const { return predictor::TimeStamp(last_see_time_stamp_); } @@ -104,19 +103,19 @@ class LiveTarget { const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, const int& id, const double& dt) { // 观测jacobi - auto H = model_.H(ekf_.x, id); - auto R = model_.R(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); - auto A = model_.A(dt); - auto Q = model_.Q(dt); - auto f = model_.f; - auto h = [this, id](const Eigen::VectorXd& x) { return model_.h(x, id); }; + auto H = model_.H(ekf_.x, id); + auto R = model_.R(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); + auto A = model_.A(dt); + auto Q = model_.Q(dt); + auto f = model_.f; + auto h = [this, id](const ExtendedKalmanFilter<11, 4>::XVec& x) { return model_.h(x, id); }; auto z_subtract = model_.z_subtract; - const Eigen::VectorXd& ypd = armor_ypd_in_world; - const Eigen::VectorXd& ypr = armor_ypr_in_world; + const Eigen::Vector3d& ypd = armor_ypd_in_world; + const Eigen::Vector3d& ypr = armor_ypr_in_world; // 获得观测量 - Eigen::VectorXd z(4); + ExtendedKalmanFilter<11, 4>::ZVec z(4); z << ypd[0], ypd[1], ypd[2], ypr[0]; ekf_.Update(dt, A, Q, f, z, H, R, h, z_subtract); @@ -131,7 +130,7 @@ class LiveTarget { } std::time_t last_see_time_stamp_; - util::ExtendedKalmanFilter ekf_; + ExtendedKalmanFilter<11, 4> ekf_; PredictModel model_; int last_id = -1; diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/predict_model.hpp index 109fee0..a9942a4 100644 --- a/src/tongji/predictor/predict_model.hpp +++ b/src/tongji/predictor/predict_model.hpp @@ -2,10 +2,12 @@ #include +#include #include #include #include "enum/car_id.hpp" +#include "tongji/predictor/extended_kalman_filter.hpp" #include "util/math.hpp" namespace world_exe::tongji::predictor { @@ -57,23 +59,27 @@ class PredictModel { int GetArmorNum() const { return armor_num_; } // 防止夹角求和出现异常值 - std::function x_add = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { - Eigen::VectorXd c = a + b; - c(6) = util::math::clamp_pm_pi(c(6)); + std::function::XVec( + const ExtendedKalmanFilter<11, 4>::XVec&, const ExtendedKalmanFilter<11, 4>::XVec&)> + x_add = [](const ExtendedKalmanFilter<11, 4>::XVec& a, + const ExtendedKalmanFilter<11, 4>::XVec& b) { + ExtendedKalmanFilter<11, 4>::XVec c = a + b; + c(6) = util::math::clamp_pm_pi(c(6)); return c; }; // 防止夹角求和出现异常值 - std::function f = - [=, this](const Eigen::VectorXd& x, const double& dt) -> Eigen::VectorXd { - Eigen::VectorXd x_prior = A(dt) * x; - x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); + std::function::XVec( + const ExtendedKalmanFilter<11, 4>::XVec& x, const double& dt)> + f = [this](const ExtendedKalmanFilter<11, 4>::XVec& x, + const double& dt) -> ExtendedKalmanFilter<11, 4>::XVec { + ExtendedKalmanFilter<11, 4>::XVec x_prior = A(dt) * x; + x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); return x_prior; }; - int MatchArmor(const Eigen::VectorXd& x, const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, + int MatchArmor(const ExtendedKalmanFilter<11, 4>::XVec& x, + const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) const { const auto& xyza_list = GetArmorXYZAList(x); @@ -107,7 +113,7 @@ class PredictModel { } // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { + Eigen::Vector3d h_armor_xyz(const ExtendedKalmanFilter<11, 4>::XVec& x, int id) const { auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); @@ -119,7 +125,7 @@ class PredictModel { return { armor_x, armor_y, armor_z }; } - Eigen::MatrixXd H(const Eigen::VectorXd& x, int id) const { + ExtendedKalmanFilter<11, 4>::HMat H(const ExtendedKalmanFilter<11, 4>::XVec& x, int id) const { auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); @@ -135,7 +141,7 @@ class PredictModel { auto dz_dh = (use_l_h) ? 1.0 : 0.0; // clang-format off - Eigen::MatrixXd H_armor_xyza{ + Eigen::MatrixXd H_armor_xyza{ {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, @@ -146,7 +152,7 @@ class PredictModel { Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); // clang-format off - Eigen::MatrixXd H_armor_ypda{ + Eigen::Matrix H_armor_ypda{ {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, @@ -157,13 +163,13 @@ class PredictModel { return H_armor_ypda * H_armor_xyza; } - Eigen::MatrixXd R(const Eigen::Vector3d& armor_xyz_in_world, + ExtendedKalmanFilter<11, 4>::RMat R(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, int id) { // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; auto center_yaw = std::atan2(armor_xyz_in_world(1), armor_xyz_in_world(0)); auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world(0) - center_yaw); - Eigen::VectorXd R_dig(4); + ExtendedKalmanFilter<11, 4>::RDig R_dig(4); R_dig << 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, log(std::abs(armor_ypd_in_world(2)) + 1) / 200 + 9e-2; @@ -171,10 +177,11 @@ class PredictModel { return R_dig.asDiagonal(); } - Eigen::MatrixXd A(double dt) const { + ExtendedKalmanFilter<11, 4>::AMat A(double dt) const { // 状态转移矩阵 + ExtendedKalmanFilter<11, 4>::AMat _A; // clang-format off - return (Eigen::MatrixXd(11, 11) << + _A<< 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, @@ -185,11 +192,13 @@ class PredictModel { 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1).finished(); + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; + //clang-format on + return _A; } - //clang-format on + - Eigen::MatrixXd Q(double dt) const { +ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { // Piecewise White Noise Model // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb double v1, v2; @@ -205,8 +214,10 @@ class PredictModel { auto c = dt * dt; // 预测过程噪声偏差的方差 + Eigen::Matrix _Q; // clang-format off - return (Eigen::MatrixXd(11, 11) << + _Q + << a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, @@ -217,29 +228,33 @@ class PredictModel { 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(); + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; // clang-format on + return _Q; } - std::function h = [this](const Eigen::VectorXd& x, - int id) { - Eigen::Vector3d xyz = this->h_armor_xyz(x, id); - Eigen::Vector3d ypd = util::math::xyz2ypd(xyz); - double angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); - return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; - }; + std::function::ZVec(const ExtendedKalmanFilter<11, 4>::XVec&, int)> + h = [this](const ExtendedKalmanFilter<11, 4>::XVec& x, int id) { + Eigen::Vector3d xyz = this->h_armor_xyz(x, id); + Eigen::Vector3d ypd = util::math::xyz2ypd(xyz); + double angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); + return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; + }; // 防止夹角求差出现异常值 - std::function z_subtract = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { - Eigen::VectorXd c = a - b; - c(0) = util::math::clamp_pm_pi(c(0)); - c(1) = util::math::clamp_pm_pi(c(1)); - c(3) = util::math::clamp_pm_pi(c(3)); + std::function::ZVec( + const ExtendedKalmanFilter<11, 4>::ZVec&, const ExtendedKalmanFilter<11, 4>::ZVec&)> + z_subtract = [](const ExtendedKalmanFilter<11, 4>::ZVec& a, + const ExtendedKalmanFilter<11, 4>::ZVec& b) { + ExtendedKalmanFilter<11, 4>::ZVec c = a - b; + c(0) = util::math::clamp_pm_pi(c(0)); + c(1) = util::math::clamp_pm_pi(c(1)); + c(3) = util::math::clamp_pm_pi(c(3)); return c; }; - std::vector GetArmorXYZAList(const Eigen::VectorXd& ekf_x) const { + std::vector GetArmorXYZAList( + const ExtendedKalmanFilter<11, 4>::XVec& ekf_x) const { std::vector _armor_xyza_list; for (int i = 0; i < armor_num_; i++) { @@ -255,7 +270,7 @@ class PredictModel { int armor_num_; enumeration::CarIDFlag car_id_; - Eigen::VectorXd P0_dig_; + ExtendedKalmanFilter<11, 4>::PDig P0_dig_; double radius_; }; diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index b390a36..ffe37b6 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -3,10 +3,10 @@ #include #include "data/armor_gimbal_control_spacing.hpp" +#include "extended_kalman_filter.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/predict_model.hpp" #include "tongji/predictor/time_stamp.hpp" -#include "util/extended_kalman_filter.hpp" namespace world_exe::tongji::predictor { @@ -49,7 +49,7 @@ class TargetSnapshot { private: PredictModel model_; - util::ExtendedKalmanFilter ekf_; + ExtendedKalmanFilter<11, 4> ekf_; TimeStamp time_stamp_; }; diff --git a/src/util/extended_kalman_filter.hpp b/src/util/extended_kalman_filter.hpp deleted file mode 100644 index 73fc86b..0000000 --- a/src/util/extended_kalman_filter.hpp +++ /dev/null @@ -1,101 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -namespace world_exe::util { -class ExtendedKalmanFilter { -public: - Eigen::VectorXd x; - Eigen::MatrixXd P; - - ExtendedKalmanFilter() = default; - - ExtendedKalmanFilter( - const Eigen::VectorXd& x0, const Eigen::MatrixXd& P0, - std::function x_add = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; }) { - data["residual_yaw"] = 0.0; - data["residual_pitch"] = 0.0; - data["residual_distance"] = 0.0; - data["residual_angle"] = 0.0; - data["nis"] = 0.0; - data["nees"] = 0.0; - data["nis_fail"] = 0.0; - data["nees_fail"] = 0.0; - data["recent_nis_failures"] = 0.0; - } - - Eigen::VectorXd Update( - const double& dt, const Eigen::MatrixXd& A, const Eigen::MatrixXd& Q, - std::function f, - const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, - std::function h, - std::function z_subtract = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { - - auto x_n = f(x, dt); - - auto P_n = A * P * A.transpose() + Q; - - auto residual = z_subtract(z, h(x_n)); - - auto S = H * P * H.transpose() + R; - - auto K = P_n * H.transpose() * S.inverse(); - - x = x_add(x, K * residual); - - // Stable Compution of the Posterior Covariance - // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb - P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose(); - - /// 卡方检验 - // 新增检验 - double nis = residual.transpose() * S.inverse() * residual; - - // 卡方检验阈值（自由度=4，取置信水平95%） - constexpr double nis_threshold = 0.711; - - if (nis > nis_threshold) nis_count_++, data["nis_fail"] = 1; - total_count_++; - last_nis = nis; - - recent_nis_failures.push_back(nis > nis_threshold ? 1 : 0); - - if (recent_nis_failures.size() > window_size) { - recent_nis_failures.pop_front(); - } - - int recent_failures = - std::accumulate(recent_nis_failures.begin(), recent_nis_failures.end(), 0); - double recent_rate = static_cast(recent_failures) / recent_nis_failures.size(); - - data["residual_yaw"] = residual[0]; - data["residual_pitch"] = residual[1]; - data["residual_distance"] = residual[2]; - data["residual_angle"] = residual[3]; - data["nis"] = nis; - data["recent_nis_failures"] = recent_rate; - - return x; - } - - std::map data; // 卡方检验数据 - std::deque recent_nis_failures { 0 }; - size_t window_size = 100; - double last_nis; - -private: - Eigen::MatrixXd I; - std::function x_add; - - int nees_count_ = 0; - int nis_count_ = 0; - int total_count_ = 0; -}; - -} // namespace tools From ab6f47abac2572dc1caf8db014727119bc8a85d6 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 9 Oct 2025 23:09:13 +0800 Subject: [PATCH 47/93] feat(decider): implement priority assessment logic --- src/tongji/decider/armor_info.hpp | 26 ++++ src/tongji/decider/decider.cpp | 131 ++++++++++++++++++ src/tongji/decider/decider.hpp | 30 ++++ .../fire_controller/fire_controller.cpp | 15 +- .../fire_controller/fire_controller.hpp | 4 +- src/tongji/fire_controller/tracker.hpp | 98 +++++++------ src/tongji/identifier/identified_armor.hpp | 22 +-- src/tongji/predictor/predict_model.hpp | 4 - src/tongji/predictor/target_snapshot.hpp | 1 - .../state_machine/car_state_manager.hpp | 13 +- 10 files changed, 265 insertions(+), 79 deletions(-) create mode 100644 src/tongji/decider/armor_info.hpp create mode 100644 src/tongji/decider/decider.cpp create mode 100644 src/tongji/decider/decider.hpp diff --git a/src/tongji/decider/armor_info.hpp b/src/tongji/decider/armor_info.hpp new file mode 100644 index 0000000..a5abb07 --- /dev/null +++ b/src/tongji/decider/armor_info.hpp @@ -0,0 +1,26 @@ + + +#pragma once + +#include "data/armor_image_spaceing.hpp" + +namespace world_exe::tongji::decider { + +enum class ArmorPriority { + First = 1, // + Second, // + Third, // + Forth, // + Fifth // +}; + +struct ArmorInfo { + ArmorInfo(const data::ArmorImageSpacing& armor_spacing, + const ArmorPriority& priority = ArmorPriority::Fifth) + : armor_spacing(armor_spacing) + , priority(priority) { } + + data::ArmorImageSpacing armor_spacing; + ArmorPriority priority; +}; +} \ No newline at end of file diff --git a/src/tongji/decider/decider.cpp b/src/tongji/decider/decider.cpp new file mode 100644 index 0000000..14e338d --- /dev/null +++ b/src/tongji/decider/decider.cpp @@ -0,0 +1,131 @@ +#include "decider.hpp" + +#include +#include +#include +#include +#include +#include + +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "tongji/decider/armor_info.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::decider { + +class Decider::Impl { +public: + explicit Impl(PriorityMode mode = PriorityMode::MODE_ONE) + : mode_(mode) { } + + void SetInvincibleArmors(const enumeration::ArmorIdFlag& armors) { + invincible_armor_.clear(); + for (const auto id : util::enumeration::ExpandArmorIdFlags(armors)) { + invincible_armor_.emplace(std::move(armors)); + } + } + + void SetPriority(const std::vector& detected_result) const { + const PriorityMap& priority_map = (mode_ == PriorityMode::MODE_ONE) ? mode1 : mode2; + if (!detected_result.empty()) { + for (auto armor : detected_result) { + armor.priority = priority_map.at(armor.armor_spacing.id); + } + } + } + + enumeration::ArmorIdFlag GetSortedArmor(std::vector& armors) const { + if (armors.empty()) return enumeration::ArmorIdFlag::None; + + // 对每个 DetectionResult 调用 armor_filter 和 set_priority + // ArmorFilter(armors); + // SetPriority(armors); + + // 对每个 DetectionResult 中的 armors 进行排序 + cv::Point2d img_center(1440.0 / 2, 1080.0 / 2); // TODO + std::sort(armors.begin(), armors.end(), [&](const auto& a, const auto& b) { + auto center_a = + (a.armor_spacing.image_points[0] + a.armor_spacing.image_points[3]) * 0.5f; + auto center_b = + (b.armor_spacing.image_points[0] + b.armor_spacing.image_points[3]) * 0.5f; + return cv::norm(center_a - img_center) < cv::norm(center_b - img_center); + }); + + std::sort(armors.begin(), armors.end(), + [](const auto& a, const auto& b) { return a.priority < b.priority; }); + return armors.front().armor_spacing.id; + } + + bool ArmorFilter(std::vector& armors) const { + if (armors.empty()) return true; + + // 25赛季没有5号装甲板 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [](const auto& armor) { return armor.armor_spacing.id == ArmorId::InfantryV; })); + // 不打前哨站 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [](const auto& armor) { return armor.armor_spacing.id == ArmorId::Outpost; })); + // 过滤掉刚复活无敌的装甲板 + armors.erase( + std::remove_if(armors.begin(), armors.end(), + [&](const auto& armor) { return invincible_armor_.count(armor.armor_spacing.id); }), + armors.end()); + + return armors.empty(); + } + +private: + using ArmorPriority = world_exe::tongji::decider::ArmorPriority; + using ArmorId = world_exe::enumeration::ArmorIdFlag; + using ArmorInfo = world_exe::tongji::decider::ArmorInfo; + + using PriorityMap = std::unordered_map; + + const PriorityMap mode1 = { + { ArmorId::Hero, ArmorPriority::Second }, // + { ArmorId::Engineer, ArmorPriority::Forth }, // + { ArmorId::InfantryIII, ArmorPriority::First }, // + { ArmorId::InfantryIV, ArmorPriority::First }, // + { ArmorId::InfantryV, ArmorPriority::Third }, // + { ArmorId::Sentry, ArmorPriority::Third }, // + { ArmorId::Outpost, ArmorPriority::Fifth }, // + { ArmorId::Base, ArmorPriority::Fifth }, // + { ArmorId::Unknow, ArmorPriority::Fifth } // + }; + + const PriorityMap mode2 = { + { ArmorId::Hero, ArmorPriority::Second }, // + { ArmorId::Engineer, ArmorPriority::Forth }, // + { ArmorId::InfantryIII, ArmorPriority::First }, // + { ArmorId::InfantryIV, ArmorPriority::First }, // + { ArmorId::InfantryV, ArmorPriority::Third }, // + { ArmorId::Sentry, ArmorPriority::Third }, // + { ArmorId::Outpost, ArmorPriority::Fifth }, // + { ArmorId::Base, ArmorPriority::Fifth }, // + { ArmorId::Unknow, ArmorPriority::Fifth } // + }; + + std::unordered_set invincible_armor_; + PriorityMode mode_; +}; + +Decider::Decider(PriorityMode mode) + : pimpl_(std::make_unique(mode)) { } + +void Decider::SetInvincibleArmors(const enumeration::ArmorIdFlag& armors) { + return pimpl_->SetInvincibleArmors(armors); +} + +void Decider::SetPriority(const std::vector& detected_result) const { + return pimpl_->SetPriority(detected_result); +} + +bool Decider::ArmorFilter(std::vector& armors) { return pimpl_->ArmorFilter(armors); } + +enumeration::CarIDFlag Decider::GetSortedArmor(std::vector& armors) const { + return pimpl_->GetSortedArmor(armors); +} + +Decider::~Decider() = default; +} diff --git a/src/tongji/decider/decider.hpp b/src/tongji/decider/decider.hpp new file mode 100644 index 0000000..9717d04 --- /dev/null +++ b/src/tongji/decider/decider.hpp @@ -0,0 +1,30 @@ + + +#pragma once + +#include +#include + +#include "enum/armor_id.hpp" +#include "tongji/decider/armor_info.hpp" + +namespace world_exe::tongji::decider { + +enum PriorityMode { MODE_ONE = 1, MODE_TWO }; + +class Decider { +public: + Decider(PriorityMode mode = PriorityMode::MODE_ONE); + ~Decider(); + + void SetInvincibleArmors(const enumeration::ArmorIdFlag& armors); + void SetPriority(const std::vector& detected_result) const; + enumeration::ArmorIdFlag GetSortedArmor(std::vector& armors) const; + bool ArmorFilter(std::vector& armors); + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index e065cb6..29a14f2 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -8,6 +8,7 @@ #include #include "enum/car_id.hpp" +#include "interfaces/armor_in_image.hpp" #include "tongji/fire_controller/aim_solver.hpp" #include "tongji/fire_controller/fire_decision.hpp" #include "tongji/fire_controller/tracker.hpp" @@ -33,7 +34,7 @@ class FireController::Impl { double pitch_offset) : control_delay_(control_delay_in_second) , bullet_speed_(bullet_speed) - , tracker_(std::make_unique()) + , tracker_(std::make_unique()) , aim_solver_(std::make_unique(bullet_speed, yaw_offset, pitch_offset)) , fire_decision_(std::make_unique(auto_fire)) , live_target_manager_(std::make_shared(state_machine)) { } @@ -46,7 +47,7 @@ class FireController::Impl { auto snapshot_manager = std::dynamic_pointer_cast( live_target_manager_->GetPredictor(live_target_manager_->GetLiveTargetIDs())); - auto snapshot = tracker_->SelectTrackingTarget(*identified_armors_, snapshot_manager); + auto snapshot = tracker_->SelectTrackingTarget(identified_armors_, snapshot_manager); bool found = (snapshot != nullptr); tracker_->UpdateState(found); @@ -70,7 +71,9 @@ class FireController::Impl { const CarIDFlag GetAttackCarId() const { return attacked_cars_; } - void UpdateIdentifiedArmors(const IdentifiedArmor& armors) { identified_armors_ = armors; } + void UpdateIdentifiedArmors(std::shared_ptr armors) { + identified_armors_ = armors; + } void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { gimbal_pos_ = gimbal_pos; }; void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_.SetTimeStamp(time_stamp); } TimeStamp GetTimeStamp() const { return time_stamp_; } @@ -84,8 +87,8 @@ class FireController::Impl { CarIDFlag::None }; // TODO:Mutable should not be used to break const encapsulation - mutable std::optional identified_armors_; - std::unique_ptr tracker_; + std::shared_ptr identified_armors_; + std::unique_ptr tracker_; std::shared_ptr live_target_manager_; std::unique_ptr aim_solver_; std::unique_ptr fire_decision_; @@ -105,7 +108,7 @@ const data ::FireControl FireController::CalculateTarget(const std ::time_t& tim const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } -void FireController::UpdateIdentifiedArmors(const IdentifiedArmor& armors) { +void FireController::UpdateIdentifiedArmors(std::shared_ptr armors) { return pimpl_->UpdateIdentifiedArmors(armors); } diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 6258923..bbbe7a0 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -2,8 +2,8 @@ #include +#include "interfaces/armor_in_image.hpp" #include "interfaces/fire_controller.hpp" -#include "tongji/identifier/identified_armor.hpp" #include "tongji/predictor/time_stamp.hpp" #include "tongji/state_machine/state_machine.hpp" @@ -18,7 +18,7 @@ class FireController final : public interfaces::IFireControl { const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; - void UpdateIdentifiedArmors(const identifier::IdentifiedArmor& armors); + void UpdateIdentifiedArmors(std::shared_ptr armors); void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos); void SetTimeStamp(const std::time_t& time_stamp); diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index 361a3da..57b9920 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -1,15 +1,21 @@ #pragma once +#include #include #include +#include #include "../predictor/target_snapshot.hpp" #include "../predictor/target_snapshot_manager.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" #include "enum/enum_tools.hpp" +#include "interfaces/armor_in_image.hpp" #include "interfaces/car_state.hpp" +#include "tongji/decider/armor_info.hpp" +#include "tongji/decider/decider.hpp" +#include "tongji/fire_controller/aim_point_chooser.hpp" #include "tongji/identifier/identified_armor.hpp" #include "tongji/predictor/time_stamp.hpp" @@ -23,62 +29,75 @@ enum class TrackState { Switching // }; -class DefaultTracker final { +class Tracker final { using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; using TargetSnapshot = world_exe::tongji::predictor::TargetSnapshot; using ArmorInImage = world_exe::tongji::identifier::IdentifiedArmor; using EnemiesState = world_exe::interfaces::ICarState; public: - DefaultTracker() - : last_timestamp_(0) { } + Tracker() + : last_timestamp_(std::time(nullptr)) + , decider_(std::make_unique()) { } - ~DefaultTracker() = default; + ~Tracker() = default; - auto SelectTrackingTarget( // - ArmorInImage& armors, // - const std::shared_ptr& snapshot_manager_ // - ) noexcept -> std::unique_ptr { + auto SelectTrackingTarget(std::shared_ptr armors_in_image, + const std::shared_ptr& snapshot_manager_) noexcept + -> std::unique_ptr { - const auto& sq_armor_list = armors.get_sq_armor(); + auto detected_ids = enumeration::CarIDFlag::None; + auto armor_info_list = std::vector { }; - sq_armor_list->sort([](const world_exe::tongji::identifier::SPArmor& a, - const world_exe::tongji::identifier::SPArmor& b) { - cv::Point2f img_center(1440.0 / 2, 1080.0 / 2); // TODO - auto distance_1 = cv::norm(a.center - img_center); - auto distance_2 = cv::norm(b.center - img_center); - return distance_1 < distance_2; - }); - sq_armor_list->sort([](const auto& a, const auto& b) { return a.priority < b.priority; }); + for (uint32_t i = 0; i < static_cast(CarIDFlag::Count); ++i) { + auto id = static_cast(static_cast(CarIDFlag::Hero) << i); - auto filter_flag = snapshot_manager_->GetId(); - for (const auto& armor : *sq_armor_list) { - if (!enumeration::IsFlagContains(filter_flag, armor.armor.id)) continue; + if (armors_in_image->GetArmors(id).empty()) continue; - auto snapshot = snapshot_manager_->GetSingleSnapshot(armor.armor.id); - if (!snapshot) continue; + detected_ids = static_cast( + static_cast(detected_ids) | static_cast(id)); - if (state_ == TrackState::Tracking && snapshot->GetPriority() < current_priority_) { - SetState(TrackState::Switching); - temp_lost_count_ = 0; + for (const auto& armor : armors_in_image->GetArmors(id)) { + auto armor_info = decider::ArmorInfo(armor); + armor_info_list.emplace_back(armor_info); } + } + + decider_->SetInvincibleArmors(invincible_armors_); + decider_->ArmorFilter(armor_info_list); + decider_->SetPriority(armor_info_list); + + auto sorted_id = decider_->GetSortedArmor(armor_info_list); + if (sorted_id == enumeration::ArmorIdFlag::None) { + return nullptr; + } - auto now = predictor::TimeStamp(std::time(nullptr)); - if (state_ != TrackState::Lost && now.SecondsSince(last_timestamp_) > 0.1) { - SetState(TrackState::Lost); - ResetTracking(); - return nullptr; - } + auto snapshot = snapshot_manager_->GetSingleSnapshot(sorted_id); + + if (state_ == TrackState::Tracking) { + SetState(TrackState::Switching); + temp_lost_count_ = 0; + } - last_timestamp_ = now; - tracking_car_id_ = snapshot->GetID(); - current_priority_ = snapshot->GetPriority(); - return snapshot; + auto now = predictor::TimeStamp(std::time(nullptr)); + if (state_ != TrackState::Lost && now.SecondsSince(last_timestamp_) > 0.1) { + SetState(TrackState::Lost); + ResetTracking(); + return nullptr; } + + last_timestamp_ = now; + tracking_car_id_ = snapshot->GetID(); + return snapshot; + ResetTracking(); return nullptr; } + void SetInvincibleArmors(const CarIDFlag& invincible_armors) { + invincible_armors_ = invincible_armors; + } + world_exe::enumeration::CarIDFlag GetCurrentTargetID() const { return tracking_car_id_; } void UpdateState(bool found) { @@ -153,15 +172,12 @@ class DefaultTracker final { state_ = new_state; } - void ResetTracking() { - tracking_car_id_ = enumeration::CarIDFlag::None; - current_priority_ = 100; - } + void ResetTracking() { tracking_car_id_ = enumeration::CarIDFlag::None; } - int current_priority_ = 100; world_exe::enumeration::CarIDFlag tracking_car_id_ { enumeration::CarIDFlag::None }; TrackState state_ = TrackState::Lost; TrackState pre_state_ = TrackState::Lost; + CarIDFlag invincible_armors_ { CarIDFlag::None }; int detect_count_ = 0; int temp_lost_count_ = 0; @@ -172,6 +188,8 @@ class DefaultTracker final { const int max_switch_count_ = 200; predictor::TimeStamp last_timestamp_; + + std::unique_ptr decider_; }; } \ No newline at end of file diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp index 950fb32..ceabe04 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -1,20 +1,20 @@ #pragma once +#include +#include +#include + #include "enum/armor_id.hpp" #include "enum/car_id.hpp" #include "interfaces/armor_in_image.hpp" #include "interfaces/time_stamped.hpp" #include "util/index.hpp" -#include -#include -#include namespace world_exe::tongji::identifier { struct SPArmor { const data::ArmorImageSpacing& armor; const cv::Point2f center; - const int priority = 0; }; class IdentifiedArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { @@ -34,18 +34,6 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface return armors_[util::enumeration::GetIndex(armor_id)]; } - std::shared_ptr> get_sq_armor() { - if (armor_list != nullptr) return armor_list; - - armor_list = std::make_shared>(); - for (const auto& car : armors_) - for (const auto& armor : car) - armor_list->emplace_back( - armor, (armor.image_points[0] + armor.image_points[3]) / 2, 0); - - return armor_list; - } - static IdentifiedArmor DecorateIArmorInImage(const interfaces::IArmorInImage& armor) { throw std::runtime_error("Not implemented"); } @@ -62,7 +50,7 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface } private: - std::time_t time_stamp_ { 0 }; + std::time_t time_stamp_ { std::time(nullptr) }; std::array, 8> armors_; std::shared_ptr> armor_list = nullptr; }; diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/predict_model.hpp index a9942a4..db04478 100644 --- a/src/tongji/predictor/predict_model.hpp +++ b/src/tongji/predictor/predict_model.hpp @@ -48,14 +48,11 @@ class PredictModel { } else { armor_num_ = 4; } - - // TODO :init priority } auto GetP0Dig() const { return P0_dig_; } auto GetRadius() const { return radius_; } auto GetID() const { return car_id_; } - auto GetPriority() const { return priority_; } int GetArmorNum() const { return armor_num_; } // 防止夹角求和出现异常值 @@ -266,7 +263,6 @@ ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { } private: - int priority_; int armor_num_; enumeration::CarIDFlag car_id_; diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index ffe37b6..5dd41b9 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -40,7 +40,6 @@ class TargetSnapshot { auto GetTimeStamp() const { return time_stamp_; } auto GetID() const { return model_.GetID(); } auto GetEkfX() const { return ekf_.x; } - auto GetPriority() const { return model_.GetPriority(); } auto Predict(const double& dt) -> Eigen::Vector const { auto predicted_x = model_.f(ekf_.x, dt); diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index 4e940bb..a4717e8 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -37,13 +37,9 @@ class CarStateManager { is_locked_ = false; is_converged_ = false; is_diverged_ = true; - priority_ = default_priority_; last_seen_ = predictor::TimeStamp(0); } - void SetPriority(const int& p) { priority_ = p; } - int GetPriority() const { return priority_; } - void SetThreshold(const int& value) { switch_threshold_ = value; } predictor::TimeStamp LastSeen() const { return last_seen_; } @@ -55,11 +51,10 @@ class CarStateManager { int update_count_ = 0; double timeout_sec_; - bool is_locked_ = false; - bool is_converged_ = false; - bool is_diverged_ = true; - int priority_ = 100; - int default_priority_ = 100; + bool is_locked_ = false; + bool is_converged_ = false; + bool is_diverged_ = true; + ; predictor::TimeStamp last_seen_; }; From 344ee501100b68adc19fb600b9ff3b7b57996e58 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 9 Oct 2025 23:53:13 +0800 Subject: [PATCH 48/93] chore(state): remove redundant logic from state machine --- src/tongji/decider/decider.cpp | 6 +++--- src/tongji/decider/decider.hpp | 2 +- src/tongji/fire_controller/tracker.hpp | 4 ++-- src/tongji/state_machine/car_state_manager.hpp | 9 ++------- src/tongji/state_machine/state_machine.cpp | 7 +------ 5 files changed, 9 insertions(+), 19 deletions(-) diff --git a/src/tongji/decider/decider.cpp b/src/tongji/decider/decider.cpp index 14e338d..ec9fb35 100644 --- a/src/tongji/decider/decider.cpp +++ b/src/tongji/decider/decider.cpp @@ -26,10 +26,10 @@ class Decider::Impl { } } - void SetPriority(const std::vector& detected_result) const { + void SetPriority( std::vector& detected_result) const { const PriorityMap& priority_map = (mode_ == PriorityMode::MODE_ONE) ? mode1 : mode2; if (!detected_result.empty()) { - for (auto armor : detected_result) { + for (auto& armor : detected_result) { armor.priority = priority_map.at(armor.armor_spacing.id); } } @@ -117,7 +117,7 @@ void Decider::SetInvincibleArmors(const enumeration::ArmorIdFlag& armors) { return pimpl_->SetInvincibleArmors(armors); } -void Decider::SetPriority(const std::vector& detected_result) const { +void Decider::SetPriority( std::vector& detected_result) const { return pimpl_->SetPriority(detected_result); } diff --git a/src/tongji/decider/decider.hpp b/src/tongji/decider/decider.hpp index 9717d04..0ff0287 100644 --- a/src/tongji/decider/decider.hpp +++ b/src/tongji/decider/decider.hpp @@ -18,7 +18,7 @@ class Decider { ~Decider(); void SetInvincibleArmors(const enumeration::ArmorIdFlag& armors); - void SetPriority(const std::vector& detected_result) const; + void SetPriority(std::vector& detected_result) const; enumeration::ArmorIdFlag GetSortedArmor(std::vector& armors) const; bool ArmorFilter(std::vector& armors); diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/fire_controller/tracker.hpp index 57b9920..68b5d4a 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/fire_controller/tracker.hpp @@ -62,9 +62,9 @@ class Tracker final { armor_info_list.emplace_back(armor_info); } } - + decider_->SetInvincibleArmors(invincible_armors_); - decider_->ArmorFilter(armor_info_list); + auto is_empty = decider_->ArmorFilter(armor_info_list); decider_->SetPriority(armor_info_list); auto sorted_id = decider_->GetSortedArmor(armor_info_list); diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index a4717e8..9c40d4b 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -18,7 +18,7 @@ class CarStateManager { count_ = std::min(count_ + 1, switch_threshold_); last_seen_ = now; update_count_++; - if (update_count_ >= converge_threshold_ && is_diverged_) is_converged_ = true; + if (update_count_ >= converge_threshold_ && !is_diverged_) is_converged_ = true; } else { count_ = std::max(count_ - 1, 0); } @@ -27,17 +27,13 @@ class CarStateManager { bool IsConverged() const { return is_converged_ && !is_diverged_; } - bool IsLost(const TimeStamp& now) const { - return is_locked_ && now.SecondsSince(last_seen_) > timeout_sec_; - } - void Reset() { count_ = 0; update_count_ = 0; is_locked_ = false; is_converged_ = false; is_diverged_ = true; - last_seen_ = predictor::TimeStamp(0); + last_seen_ = predictor::TimeStamp(std::time(nullptr)); } void SetThreshold(const int& value) { switch_threshold_ = value; } @@ -54,7 +50,6 @@ class CarStateManager { bool is_locked_ = false; bool is_converged_ = false; bool is_diverged_ = true; - ; predictor::TimeStamp last_seen_; }; diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 2ce500e..fc0ffa8 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -22,7 +22,7 @@ class StateMachine::Impl { const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const { return car_states_[util::enumeration::GetIndex(single_id)]; } - + void Update(const enumeration::CarIDFlag& car_detected, const std::time_t& now) { tracing_state_ = enumeration::CarIDFlag::None; @@ -38,11 +38,6 @@ class StateMachine::Impl { continue; } - if (state.IsLost(now)) { - state.Reset(); - continue; - } - tracing_state_ = static_cast( static_cast(tracing_state_) | static_cast(id)); } From 6b295089a4eb7ac9051d32cdd03a9febd5e0a7c4 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 9 Oct 2025 23:55:35 +0800 Subject: [PATCH 49/93] chore: remove miscellaneous trivial cruft --- src/tongji/state_machine/car_state_manager.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp index 9c40d4b..6592a63 100644 --- a/src/tongji/state_machine/car_state_manager.hpp +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -45,7 +45,6 @@ class CarStateManager { int switch_threshold_; int converge_threshold_ = 3; int update_count_ = 0; - double timeout_sec_; bool is_locked_ = false; bool is_converged_ = false; From 919cae35bfdf31e0112bd3ef31c4b25844de23c7 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 10 Oct 2025 21:47:36 +0800 Subject: [PATCH 50/93] chore(fire controller): remove redundant timestamp setter function --- src/tongji/fire_controller/fire_controller.cpp | 11 +++++------ src/tongji/fire_controller/fire_controller.hpp | 3 +-- src/tongji/fire_controller/fire_decision.hpp | 5 +++-- 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 29a14f2..b36c965 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -60,7 +60,7 @@ class FireController::Impl { if (!aim_solution.valid) return { .fire_allowance = false }; const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); - const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_pos_); + const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_yaw_); if (fireable) attacked_cars_ = snapshot->GetID(); @@ -74,12 +74,11 @@ class FireController::Impl { void UpdateIdentifiedArmors(std::shared_ptr armors) { identified_armors_ = armors; } - void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { gimbal_pos_ = gimbal_pos; }; - void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_.SetTimeStamp(time_stamp); } + void UpdateGimbalPosition(const double gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; TimeStamp GetTimeStamp() const { return time_stamp_; } private: - Eigen::Vector3d gimbal_pos_ { Eigen::Vector3d::Zero() }; + double gimbal_yaw_; double control_delay_; double bullet_speed_; @@ -112,8 +111,8 @@ void FireController::UpdateIdentifiedArmors(std::shared_ptrUpdateIdentifiedArmors(armors); } -void FireController::UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos) { - return pimpl_->UpdateGimbalPosition(gimbal_pos); +void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { + return pimpl_->UpdateGimbalPosition(gimbal_yaw); }; } \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index bbbe7a0..be9ddea 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -19,9 +19,8 @@ class FireController final : public interfaces::IFireControl { const enumeration ::CarIDFlag GetAttackCarId() const override; void UpdateIdentifiedArmors(std::shared_ptr armors); - void UpdateGimbalPosition(const Eigen::Vector3d& gimbal_pos); + void UpdateGimbalPosition(const double& gimbal_yaw); - void SetTimeStamp(const std::time_t& time_stamp); predictor::TimeStamp GetTimeStamp() const; private: diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index 8dda984..e6790db 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -18,7 +18,7 @@ class FireDecision { , judge_distance_(judge_distance) { } bool ShouldFire(const AimSolution& aimsolution, const Eigen::Vector3d& valid_target_pos, - const Eigen::Vector3d& gimbal_pos) { + const double& gimbal_yaw) { if (!aimsolution.valid || !auto_fire_) return false; const auto& tolerance = std::sqrt(valid_target_pos.x() * valid_target_pos.x() @@ -29,7 +29,8 @@ class FireDecision { if (std::abs(last_aim_solution_.yaw - aimsolution.yaw) < tolerance * 2 // 此时认为command突变不应该射击 - && std::abs(gimbal_pos[0] - last_aim_solution_.yaw) < tolerance && last_aim_solution_.valid) { + && std::abs(gimbal_yaw - last_aim_solution_.yaw) < tolerance + && last_aim_solution_.valid) { last_aim_solution_ = aimsolution; return true; } From 511127574a8774dfa776a329ce439980a3c0a6b1 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 13 Oct 2025 11:34:28 +0800 Subject: [PATCH 51/93] refactor: complete initial architectural restructuring - Reimplement predictor module with clear dependency boundaries - Rewrite fire_control module to follow proper dependency injection --- .../aim_point_chooser.hpp | 0 .../aim_solver.hpp | 41 ++-- .../trajectory.hpp | 2 +- src/tongji/decider/armor_info.hpp | 26 --- src/tongji/decider/decider.cpp | 131 ------------- src/tongji/decider/decider.hpp | 30 --- .../fire_controller/fire_controller.cpp | 110 ++++++----- .../fire_controller/fire_controller.hpp | 10 +- src/tongji/fire_controller/fire_decision.hpp | 24 +-- src/tongji/identifier/armor_filter.hpp | 45 +++++ src/tongji/identifier/identified_armor.hpp | 22 --- src/tongji/predictor/decider.cpp | 78 ++++++++ src/tongji/predictor/decider.hpp | 35 ++++ .../predictor/extended_kalman_filter.hpp | 2 +- src/tongji/predictor/live_target_manager.cpp | 176 ++++++++---------- src/tongji/predictor/live_target_manager.hpp | 14 +- src/tongji/predictor/target_snapshot.hpp | 31 ++- .../predictor/target_snapshot_manager.cpp | 87 ++++++--- .../predictor/target_snapshot_manager.hpp | 17 +- .../tracker.hpp | 112 +++++------ .../state_machine/car_state_manager.hpp | 55 ------ src/tongji/state_machine/state_machine.cpp | 62 ++---- src/tongji/state_machine/state_machine.hpp | 6 +- 23 files changed, 483 insertions(+), 633 deletions(-) rename src/tongji/{fire_controller => armor_solver}/aim_point_chooser.hpp (100%) rename src/tongji/{fire_controller => armor_solver}/aim_solver.hpp (65%) rename src/tongji/{fire_controller => armor_solver}/trajectory.hpp (96%) delete mode 100644 src/tongji/decider/armor_info.hpp delete mode 100644 src/tongji/decider/decider.cpp delete mode 100644 src/tongji/decider/decider.hpp create mode 100644 src/tongji/identifier/armor_filter.hpp create mode 100644 src/tongji/predictor/decider.cpp create mode 100644 src/tongji/predictor/decider.hpp rename src/tongji/{fire_controller => predictor}/tracker.hpp (56%) delete mode 100644 src/tongji/state_machine/car_state_manager.hpp diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/armor_solver/aim_point_chooser.hpp similarity index 100% rename from src/tongji/fire_controller/aim_point_chooser.hpp rename to src/tongji/armor_solver/aim_point_chooser.hpp diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/armor_solver/aim_solver.hpp similarity index 65% rename from src/tongji/fire_controller/aim_solver.hpp rename to src/tongji/armor_solver/aim_solver.hpp index c462bf1..db34fe3 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/armor_solver/aim_solver.hpp @@ -4,13 +4,12 @@ #include #include #include -#include -#include "tongji/fire_controller/aim_point_chooser.hpp" -#include "tongji/fire_controller/trajectory.hpp" +#include "tongji/armor_solver/aim_point_chooser.hpp" +#include "tongji/armor_solver/trajectory.hpp" #include "tongji/predictor/target_snapshot.hpp" -namespace world_exe::tongji::fire_control { +namespace world_exe::tongji::armor_solver { using TargetSnapshot = predictor::TargetSnapshot; @@ -24,20 +23,16 @@ struct AimSolution { class AimingSolver { public: - AimingSolver(const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, - const double& gravity = 9.7833) - : aim_point_chooser_(std::make_unique()) - , bullet_speed_(bullet_speed) + AimingSolver( + const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) + : aim_point_chooser_(std::make_unique()) , yaw_offset_(yaw_offset / 57.3) // degree to rad , pitch_offset_(pitch_offset / 57.3) // degree to rad , g_(gravity) { } - void UpdateSnapshot(std::unique_ptr snapshot) { - snapshot_ = std::move(snapshot); - } - - AimSolution SolveAimSolution(const double& time_delay) { - if (!snapshot_) return { false, 0, 0, { }, 0 }; + AimSolution SolveAimSolution(const std::shared_ptr& snapshot, + const double& bullet_speed, const double& time_delay) { + if (!snapshot) return { false, 0, 0, { }, 0 }; // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) double prev_fly_time = 0; @@ -48,10 +43,10 @@ class AimingSolver { // 预测目标在未来 dt时间后的位置 for (int i = 0; i < 10; ++i) { double dt = time_delay + prev_fly_time; - const auto aim = SelectPredictedAim(dt); + const auto aim = SelectPredictedAim(snapshot, dt); if (!aim.has_value()) return { false, 0, 0, { }, 0 }; // failed: no valid aim point - const auto traj = SolveTrajectory(aim->head(3)); + const auto traj = SolveTrajectory(aim->head(3), bullet_speed); if (!traj.has_value()) return { false, 0, 0, { }, 0 }; // failed: trajectory unsolvable if (i > 0 && std::abs(traj->fly_time - prev_fly_time) < 0.001) { @@ -71,23 +66,23 @@ class AimingSolver { } private: - std::optional SelectPredictedAim(double dt) const { + std::optional SelectPredictedAim( + const std::shared_ptr& snapshot, const double& dt) const { const auto& [selectable, aim_point] = aim_point_chooser_->ChooseAimArmor( - snapshot_->Predict(dt), snapshot_->GetPredictedXYZAList(dt), snapshot_->GetID()); + snapshot->Predict(dt), snapshot->GetPredictedXYZAList(dt), snapshot->GetID()); return selectable ? std::optional { aim_point } : std::nullopt; } - std::optional SolveTrajectory(const Eigen::Vector3d& xyz) const { + std::optional SolveTrajectory( + const Eigen::Vector3d& xyz, const double& bullet_speed) const { double d = std::hypot(xyz.x(), xyz.y()); - auto result = TrajectorySolver::SolveTrajectory(bullet_speed_, d, xyz.z(), g_); + auto result = TrajectorySolver::SolveTrajectory(bullet_speed, d, xyz.z(), g_); return result.solvable ? std::optional { result } : std::nullopt; } - double bullet_speed_; // m/s double yaw_offset_, pitch_offset_; const double g_; - std::unique_ptr aim_point_chooser_; - std::unique_ptr snapshot_; + std::unique_ptr aim_point_chooser_; }; } \ No newline at end of file diff --git a/src/tongji/fire_controller/trajectory.hpp b/src/tongji/armor_solver/trajectory.hpp similarity index 96% rename from src/tongji/fire_controller/trajectory.hpp rename to src/tongji/armor_solver/trajectory.hpp index 2b9c5e1..d4f9e68 100644 --- a/src/tongji/fire_controller/trajectory.hpp +++ b/src/tongji/armor_solver/trajectory.hpp @@ -1,7 +1,7 @@ #pragma once #include -namespace world_exe::tongji::fire_control { +namespace world_exe::tongji::armor_solver { struct TrajectoryResult { bool solvable = true; diff --git a/src/tongji/decider/armor_info.hpp b/src/tongji/decider/armor_info.hpp deleted file mode 100644 index a5abb07..0000000 --- a/src/tongji/decider/armor_info.hpp +++ /dev/null @@ -1,26 +0,0 @@ - - -#pragma once - -#include "data/armor_image_spaceing.hpp" - -namespace world_exe::tongji::decider { - -enum class ArmorPriority { - First = 1, // - Second, // - Third, // - Forth, // - Fifth // -}; - -struct ArmorInfo { - ArmorInfo(const data::ArmorImageSpacing& armor_spacing, - const ArmorPriority& priority = ArmorPriority::Fifth) - : armor_spacing(armor_spacing) - , priority(priority) { } - - data::ArmorImageSpacing armor_spacing; - ArmorPriority priority; -}; -} \ No newline at end of file diff --git a/src/tongji/decider/decider.cpp b/src/tongji/decider/decider.cpp deleted file mode 100644 index ec9fb35..0000000 --- a/src/tongji/decider/decider.cpp +++ /dev/null @@ -1,131 +0,0 @@ -#include "decider.hpp" - -#include -#include -#include -#include -#include -#include - -#include "enum/armor_id.hpp" -#include "enum/car_id.hpp" -#include "tongji/decider/armor_info.hpp" -#include "util/index.hpp" - -namespace world_exe::tongji::decider { - -class Decider::Impl { -public: - explicit Impl(PriorityMode mode = PriorityMode::MODE_ONE) - : mode_(mode) { } - - void SetInvincibleArmors(const enumeration::ArmorIdFlag& armors) { - invincible_armor_.clear(); - for (const auto id : util::enumeration::ExpandArmorIdFlags(armors)) { - invincible_armor_.emplace(std::move(armors)); - } - } - - void SetPriority( std::vector& detected_result) const { - const PriorityMap& priority_map = (mode_ == PriorityMode::MODE_ONE) ? mode1 : mode2; - if (!detected_result.empty()) { - for (auto& armor : detected_result) { - armor.priority = priority_map.at(armor.armor_spacing.id); - } - } - } - - enumeration::ArmorIdFlag GetSortedArmor(std::vector& armors) const { - if (armors.empty()) return enumeration::ArmorIdFlag::None; - - // 对每个 DetectionResult 调用 armor_filter 和 set_priority - // ArmorFilter(armors); - // SetPriority(armors); - - // 对每个 DetectionResult 中的 armors 进行排序 - cv::Point2d img_center(1440.0 / 2, 1080.0 / 2); // TODO - std::sort(armors.begin(), armors.end(), [&](const auto& a, const auto& b) { - auto center_a = - (a.armor_spacing.image_points[0] + a.armor_spacing.image_points[3]) * 0.5f; - auto center_b = - (b.armor_spacing.image_points[0] + b.armor_spacing.image_points[3]) * 0.5f; - return cv::norm(center_a - img_center) < cv::norm(center_b - img_center); - }); - - std::sort(armors.begin(), armors.end(), - [](const auto& a, const auto& b) { return a.priority < b.priority; }); - return armors.front().armor_spacing.id; - } - - bool ArmorFilter(std::vector& armors) const { - if (armors.empty()) return true; - - // 25赛季没有5号装甲板 - armors.erase(std::remove_if(armors.begin(), armors.end(), - [](const auto& armor) { return armor.armor_spacing.id == ArmorId::InfantryV; })); - // 不打前哨站 - armors.erase(std::remove_if(armors.begin(), armors.end(), - [](const auto& armor) { return armor.armor_spacing.id == ArmorId::Outpost; })); - // 过滤掉刚复活无敌的装甲板 - armors.erase( - std::remove_if(armors.begin(), armors.end(), - [&](const auto& armor) { return invincible_armor_.count(armor.armor_spacing.id); }), - armors.end()); - - return armors.empty(); - } - -private: - using ArmorPriority = world_exe::tongji::decider::ArmorPriority; - using ArmorId = world_exe::enumeration::ArmorIdFlag; - using ArmorInfo = world_exe::tongji::decider::ArmorInfo; - - using PriorityMap = std::unordered_map; - - const PriorityMap mode1 = { - { ArmorId::Hero, ArmorPriority::Second }, // - { ArmorId::Engineer, ArmorPriority::Forth }, // - { ArmorId::InfantryIII, ArmorPriority::First }, // - { ArmorId::InfantryIV, ArmorPriority::First }, // - { ArmorId::InfantryV, ArmorPriority::Third }, // - { ArmorId::Sentry, ArmorPriority::Third }, // - { ArmorId::Outpost, ArmorPriority::Fifth }, // - { ArmorId::Base, ArmorPriority::Fifth }, // - { ArmorId::Unknow, ArmorPriority::Fifth } // - }; - - const PriorityMap mode2 = { - { ArmorId::Hero, ArmorPriority::Second }, // - { ArmorId::Engineer, ArmorPriority::Forth }, // - { ArmorId::InfantryIII, ArmorPriority::First }, // - { ArmorId::InfantryIV, ArmorPriority::First }, // - { ArmorId::InfantryV, ArmorPriority::Third }, // - { ArmorId::Sentry, ArmorPriority::Third }, // - { ArmorId::Outpost, ArmorPriority::Fifth }, // - { ArmorId::Base, ArmorPriority::Fifth }, // - { ArmorId::Unknow, ArmorPriority::Fifth } // - }; - - std::unordered_set invincible_armor_; - PriorityMode mode_; -}; - -Decider::Decider(PriorityMode mode) - : pimpl_(std::make_unique(mode)) { } - -void Decider::SetInvincibleArmors(const enumeration::ArmorIdFlag& armors) { - return pimpl_->SetInvincibleArmors(armors); -} - -void Decider::SetPriority( std::vector& detected_result) const { - return pimpl_->SetPriority(detected_result); -} - -bool Decider::ArmorFilter(std::vector& armors) { return pimpl_->ArmorFilter(armors); } - -enumeration::CarIDFlag Decider::GetSortedArmor(std::vector& armors) const { - return pimpl_->GetSortedArmor(armors); -} - -Decider::~Decider() = default; -} diff --git a/src/tongji/decider/decider.hpp b/src/tongji/decider/decider.hpp deleted file mode 100644 index 0ff0287..0000000 --- a/src/tongji/decider/decider.hpp +++ /dev/null @@ -1,30 +0,0 @@ - - -#pragma once - -#include -#include - -#include "enum/armor_id.hpp" -#include "tongji/decider/armor_info.hpp" - -namespace world_exe::tongji::decider { - -enum PriorityMode { MODE_ONE = 1, MODE_TWO }; - -class Decider { -public: - Decider(PriorityMode mode = PriorityMode::MODE_ONE); - ~Decider(); - - void SetInvincibleArmors(const enumeration::ArmorIdFlag& armors); - void SetPriority(std::vector& detected_result) const; - enumeration::ArmorIdFlag GetSortedArmor(std::vector& armors) const; - bool ArmorFilter(std::vector& armors); - -private: - class Impl; - std::unique_ptr pimpl_; -}; - -} \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index b36c965..b4f79e6 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -1,21 +1,14 @@ #include "fire_controller.hpp" -#include -#include -#include #include +#include -#include - -#include "enum/car_id.hpp" -#include "interfaces/armor_in_image.hpp" -#include "tongji/fire_controller/aim_solver.hpp" +#include "data/fire_control.hpp" +#include "interfaces/target_predictor.hpp" #include "tongji/fire_controller/fire_decision.hpp" -#include "tongji/fire_controller/tracker.hpp" #include "tongji/identifier/identified_armor.hpp" #include "tongji/predictor/live_target_manager.hpp" #include "tongji/predictor/target_snapshot_manager.hpp" -#include "tongji/predictor/time_stamp.hpp" #include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::fire_control { @@ -29,77 +22,88 @@ using TimeStamp = predictor::TimeStamp; class FireController::Impl { public: - Impl(std::shared_ptr state_machine, bool auto_fire, - const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, - double pitch_offset) + Impl(bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, + double yaw_offset, double pitch_offset, + std::shared_ptr state_machine, + std::shared_ptr live_target_manager) : control_delay_(control_delay_in_second) , bullet_speed_(bullet_speed) - , tracker_(std::make_unique()) - , aim_solver_(std::make_unique(bullet_speed, yaw_offset, pitch_offset)) , fire_decision_(std::make_unique(auto_fire)) - , live_target_manager_(std::make_shared(state_machine)) { } + , state_machine_(state_machine) + , live_target_manager_(std::move(live_target_manager)) { } // TODO:std::time_t const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { - if (!identified_armors_ || !tracker_ || !aim_solver_ || !fire_decision_ - || !live_target_manager_) + + if (!identified_armors_ || !fire_decision_ || !state_machine_ || !live_target_manager_) return { .fire_allowance = false }; - auto snapshot_manager = std::dynamic_pointer_cast( - live_target_manager_->GetPredictor(live_target_manager_->GetLiveTargetIDs())); - auto snapshot = tracker_->SelectTrackingTarget(identified_armors_, snapshot_manager); + auto converged_cars = state_machine_->GetAllowdToFires(); + auto snapshot_manager = live_target_manager_->GetPredictor(converged_cars); + if (!snapshot_manager) + return data::FireControl { .time_stamp = time_stamp_.GetTimeStamp(), + .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), + .fire_allowance = false }; - bool found = (snapshot != nullptr); - tracker_->UpdateState(found); - if (!found) return { .fire_allowance = false }; + auto armors_in_gimbal_control = snapshot_manager->Predictor(control_delay_); + allowed_target_id_ = state_machine_->GetAllowdToFires(); - aim_solver_->UpdateSnapshot(std::move(snapshot)); + auto target_gimbal_spacing = + armors_in_gimbal_control->GetArmors(allowed_target_id_).front(); - const auto& aim_solution = aim_solver_->SolveAimSolution(static_cast( - time_duration)); // TODO:std::time_t should not be converted directly to double type - if (!aim_solution.valid) return { .fire_allowance = false }; + auto gimbal_command = + std::dynamic_pointer_cast(snapshot_manager)->GetGimbalCommand(); - const Eigen::Vector3d target_pos = aim_solution.aim_point.head<3>(); - const bool fireable = fire_decision_->ShouldFire(aim_solution, target_pos, gimbal_yaw_); + auto fire_command = + fire_decision_->ShouldFire(gimbal_command, target_gimbal_spacing.position); + firable_ = fire_command; - if (fireable) attacked_cars_ = snapshot->GetID(); + data::FireControl result; + result.fire_allowance = fire_command; + result.gimbal_dir << gimbal_command.yaw, gimbal_command.pitch, 0; + result.time_stamp = time_stamp_.GetTimeStamp(); + return result; + } - return { .time_stamp = time_stamp_.GetTimeStamp(), - .gimbal_dir = Eigen::Vector3d(aim_solution.yaw, aim_solution.pitch, 0), - .fire_allowance = fireable }; + const CarIDFlag GetAttackCarId() const { + if (firable_) { } + return allowed_target_id_; } - const CarIDFlag GetAttackCarId() const { return attacked_cars_; } + void Update(std::shared_ptr armors, const double& gimbal_yaw) { + time_stamp_.SetTimeStamp(std::time(nullptr)); + UpdateIdentifiedArmor(armors); + UpdateGimbalPosition(gimbal_yaw); + } - void UpdateIdentifiedArmors(std::shared_ptr armors) { +private: + void UpdateIdentifiedArmor(std::shared_ptr armors) { identified_armors_ = armors; } - void UpdateGimbalPosition(const double gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; + void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; TimeStamp GetTimeStamp() const { return time_stamp_; } -private: double gimbal_yaw_; double control_delay_; double bullet_speed_; - mutable CarIDFlag attacked_cars_ { - CarIDFlag::None - }; // TODO:Mutable should not be used to break const encapsulation + mutable CarIDFlag allowed_target_id_; + mutable double firable_ { false }; std::shared_ptr identified_armors_; - std::unique_ptr tracker_; - std::shared_ptr live_target_manager_; - std::unique_ptr aim_solver_; - std::unique_ptr fire_decision_; + std::unique_ptr fire_decision_; predictor::TimeStamp time_stamp_ { std::time(nullptr) }; + + std::shared_ptr state_machine_; + std::shared_ptr live_target_manager_; }; -FireController::FireController(std::shared_ptr state_machine, bool auto_fire, +FireController::FireController(std::shared_ptr state_machine, bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, - double pitch_offset) - : pimpl_(std::make_unique(state_machine, auto_fire, control_delay_in_second, bullet_speed, - yaw_offset, pitch_offset)) { } + double pitch_offset, std::shared_ptr live_target_manager) + : pimpl_(std::make_unique(auto_fire, control_delay_in_second, bullet_speed, yaw_offset, + pitch_offset, state_machine, live_target_manager)) { } const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { return pimpl_->CalculateTarget(time_duration); @@ -107,12 +111,4 @@ const data ::FireControl FireController::CalculateTarget(const std ::time_t& tim const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } -void FireController::UpdateIdentifiedArmors(std::shared_ptr armors) { - return pimpl_->UpdateIdentifiedArmors(armors); -} - -void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { - return pimpl_->UpdateGimbalPosition(gimbal_yaw); -}; - } \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index be9ddea..eed2838 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -3,23 +3,23 @@ #include #include "interfaces/armor_in_image.hpp" +#include "interfaces/car_state.hpp" #include "interfaces/fire_controller.hpp" +#include "interfaces/target_predictor.hpp" #include "tongji/predictor/time_stamp.hpp" -#include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::fire_control { class FireController final : public interfaces::IFireControl { public: - FireController(std::shared_ptr state_machine, bool auto_fire, + FireController(std::shared_ptr state_machine, bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, - double pitch_offset); + double pitch_offset, std::shared_ptr live_target_manager); const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; - void UpdateIdentifiedArmors(std::shared_ptr armors); - void UpdateGimbalPosition(const double& gimbal_yaw); + void Update(std::shared_ptr armors, const double& gimbal_yaw); predictor::TimeStamp GetTimeStamp() const; diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index e6790db..7c94956 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -3,7 +3,7 @@ #include #include -#include "tongji/fire_controller/aim_solver.hpp" +#include "tongji/predictor/target_snapshot_manager.hpp" namespace world_exe::tongji::fire_control { @@ -12,35 +12,37 @@ class FireDecision { explicit FireDecision(const bool& auto_fire, const double& first_tolerance = 5, const double& second_tolerance = 2, const double& judge_distance = 3) : auto_fire_(auto_fire) - , last_aim_solution_ { false, 0, 0 } + , last_gimbal_command_({ std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN() }) , first_tolerance_(first_tolerance) , second_tolerance_(second_tolerance) , judge_distance_(judge_distance) { } - bool ShouldFire(const AimSolution& aimsolution, const Eigen::Vector3d& valid_target_pos, - const double& gimbal_yaw) { + bool ShouldFire( + predictor::GimbalCommand gimbal_command, const Eigen::Vector3d& valid_target_pos) { - if (!aimsolution.valid || !auto_fire_) return false; + if (!auto_fire_) return false; const auto& tolerance = std::sqrt(valid_target_pos.x() * valid_target_pos.x() + valid_target_pos.y() * valid_target_pos.y()) > judge_distance_ ? second_tolerance_ : first_tolerance_; - if (std::abs(last_aim_solution_.yaw - aimsolution.yaw) + if (std::abs(last_gimbal_command_.yaw - gimbal_yaw_) < tolerance * 2 // 此时认为command突变不应该射击 - && std::abs(gimbal_yaw - last_aim_solution_.yaw) < tolerance - && last_aim_solution_.valid) { - last_aim_solution_ = aimsolution; + && std::abs(gimbal_yaw_ - last_gimbal_command_.yaw) < tolerance) { + last_gimbal_command_ = gimbal_command; return true; } - last_aim_solution_ = aimsolution; + last_gimbal_command_ = gimbal_command; return false; } private: bool auto_fire_; - AimSolution last_aim_solution_; + predictor::GimbalCommand last_gimbal_command_; + + double gimbal_yaw_; double first_tolerance_ { 5 }; // 近距离射击容差，degree double second_tolerance_ { 2 }; // 远距离射击容差，degree diff --git a/src/tongji/identifier/armor_filter.hpp b/src/tongji/identifier/armor_filter.hpp new file mode 100644 index 0000000..21699e0 --- /dev/null +++ b/src/tongji/identifier/armor_filter.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::identifier { + +class ArmorFilter { +public: + explicit ArmorFilter() + : invincible_armor_({ }) { } + std::vector FilterArmor( + std::vector armors) const { + + // 25赛季没有5号装甲板 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [](const auto& armor) { return armor.id == enumeration::ArmorIdFlag::InfantryV; })); + // 不打前哨站 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [](const auto& armor) { return armor.id == enumeration::ArmorIdFlag::Outpost; })); + // 过滤掉刚复活无敌的装甲板 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [&](const auto& armor) { return invincible_armor_.count(armor.id); }), + armors.end()); + + return std::move(armors); + } + + void Update(enumeration::CarIDFlag ids) { + invincible_armor_.clear(); + for (auto id : util::enumeration::ExpandArmorIdFlags(ids)) { + invincible_armor_.emplace(std::move(id)); + } + } + +private: + std::unordered_set invincible_armor_; +}; +} \ No newline at end of file diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp index ceabe04..14295e5 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -1,22 +1,12 @@ #pragma once -#include -#include -#include - #include "enum/armor_id.hpp" -#include "enum/car_id.hpp" #include "interfaces/armor_in_image.hpp" #include "interfaces/time_stamped.hpp" #include "util/index.hpp" namespace world_exe::tongji::identifier { -struct SPArmor { - const data::ArmorImageSpacing& armor; - const cv::Point2f center; -}; - class IdentifiedArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { public: explicit IdentifiedArmor(const std::vector& armors) { @@ -38,20 +28,8 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface throw std::runtime_error("Not implemented"); } - auto GetDetectedIDs() { - enumeration::CarIDFlag result; - for (int i = 0; i < 8; i++) { - if (!armors_[i].empty()) { - result = static_cast( - static_cast(result) | static_cast(armors_[i][0].id)); - } - } - return result; - } - private: std::time_t time_stamp_ { std::time(nullptr) }; std::array, 8> armors_; - std::shared_ptr> armor_list = nullptr; }; } \ No newline at end of file diff --git a/src/tongji/predictor/decider.cpp b/src/tongji/predictor/decider.cpp new file mode 100644 index 0000000..4db8573 --- /dev/null +++ b/src/tongji/predictor/decider.cpp @@ -0,0 +1,78 @@ +#include "decider.hpp" + +#include +#include +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" + +namespace world_exe::tongji::predictor { + +class Decider::Impl { +public: + explicit Impl(PriorityMode mode = PriorityMode::MODE_ONE) + : mode_(mode) { } + + enumeration::ArmorIdFlag GetBestArmor(std::vector& armors) const { + if (armors.empty()) return enumeration::ArmorIdFlag::None; + + const PriorityMap& priority_map = (mode_ == PriorityMode::MODE_ONE) ? mode1 : mode2; + std::vector> armors_list; + for (const auto& armor : armors) { + armors_list.emplace_back(armor.id, priority_map.at(armor.id)); + } + + cv::Point2d img_center(1440.0 / 2, 1080.0 / 2); // TODO + std::sort(armors.begin(), armors.end(), [&](const auto& a, const auto& b) { + auto center_a = (a.image_points[0] + a.image_points[3]) * 0.5f; + auto center_b = (b.image_points[0] + b.image_points[3]) * 0.5f; + return cv::norm(center_a - img_center) < cv::norm(center_b - img_center); + }); + + std::sort(armors_list.begin(), armors_list.end(), + [](const auto& a, const auto& b) { return a.second < b.second; }); + return armors.front().id; + } + +private: + using ArmorPriority = world_exe::tongji::predictor::ArmorPriority; + using ArmorId = world_exe::enumeration::ArmorIdFlag; + + using PriorityMap = std::unordered_map; + + const PriorityMap mode1 = { + { ArmorId::Hero, ArmorPriority::Second }, // + { ArmorId::Engineer, ArmorPriority::Forth }, // + { ArmorId::InfantryIII, ArmorPriority::First }, // + { ArmorId::InfantryIV, ArmorPriority::First }, // + { ArmorId::InfantryV, ArmorPriority::Third }, // + { ArmorId::Sentry, ArmorPriority::Third }, // + { ArmorId::Outpost, ArmorPriority::Fifth }, // + { ArmorId::Base, ArmorPriority::Fifth }, // + { ArmorId::Unknow, ArmorPriority::Fifth } // + }; + + const PriorityMap mode2 = { + { ArmorId::Hero, ArmorPriority::Second }, // + { ArmorId::Engineer, ArmorPriority::Forth }, // + { ArmorId::InfantryIII, ArmorPriority::First }, // + { ArmorId::InfantryIV, ArmorPriority::First }, // + { ArmorId::InfantryV, ArmorPriority::Third }, // + { ArmorId::Sentry, ArmorPriority::Third }, // + { ArmorId::Outpost, ArmorPriority::Fifth }, // + { ArmorId::Base, ArmorPriority::Fifth }, // + { ArmorId::Unknow, ArmorPriority::Fifth } // + }; + PriorityMode mode_; +}; + +Decider::Decider(PriorityMode mode) + : pimpl_(std::make_unique(mode)) { } +Decider::~Decider() = default; + +enumeration::ArmorIdFlag Decider::GetBestArmor(std::vector& armors) const { + return pimpl_->GetBestArmor(armors); +} +} diff --git a/src/tongji/predictor/decider.hpp b/src/tongji/predictor/decider.hpp new file mode 100644 index 0000000..0a44730 --- /dev/null +++ b/src/tongji/predictor/decider.hpp @@ -0,0 +1,35 @@ + + +#pragma once + +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" + +namespace world_exe::tongji::predictor { + +enum PriorityMode { MODE_ONE = 1, MODE_TWO }; + +enum class ArmorPriority { + First = 1, // + Second, // + Third, // + Forth, // + Fifth // +}; + +class Decider { +public: + Decider(PriorityMode mode = PriorityMode::MODE_ONE); + ~Decider(); + + enumeration::ArmorIdFlag GetBestArmor(std::vector& armors) const; + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/extended_kalman_filter.hpp b/src/tongji/predictor/extended_kalman_filter.hpp index f016491..9f80180 100644 --- a/src/tongji/predictor/extended_kalman_filter.hpp +++ b/src/tongji/predictor/extended_kalman_filter.hpp @@ -16,7 +16,7 @@ class ExtendedKalmanFilter { using PMat = Eigen::Matrix; using PDig = Eigen::Matrix; using RMat = Eigen::Matrix; - using RDig = Eigen::Matrix; + using RDig = Eigen::Matrix; using QMat = Eigen::Matrix; using HMat = Eigen::Matrix; diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp index c5e6ba2..1b989b9 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager.cpp @@ -1,28 +1,34 @@ #include "live_target_manager.hpp" #include +#include #include #include -#include #include "enum/armor_id.hpp" #include "enum/car_id.hpp" -#include "enum/enum_tools.hpp" #include "interfaces/predictor_update_package.hpp" #include "tongji/predictor/in_gimbal_control_armor.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/target_snapshot_manager.hpp" -#include "tongji/predictor/time_stamp.hpp" -#include "tongji/state_machine/state_machine.hpp" +#include "tongji/predictor/tracker.hpp" #include "util/index.hpp" +#include "util/math.hpp" namespace world_exe::tongji::predictor { class LiveTargetManager::Impl { public: - Impl(std::shared_ptr state_machine, double timeout_sec = 0.1) - : timeout_sec_(timeout_sec) - , state_machine_(state_machine) { } + Impl(const double& time_delay, const double& yaw_offset, const double& pitch_offset, + double timeout_sec = 0.1) + : targets_map_() + , tracker_(std::make_unique()) + , last_update_timestamp_(std::time(nullptr)) + , tracking_id_(enumeration::CarIDFlag::None) + , time_delay_(time_delay) + , yaw_offset_(yaw_offset) + , pitch_offset_(pitch_offset) + , timeout_sec_(timeout_sec) { } std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { @@ -30,142 +36,118 @@ class LiveTargetManager::Impl { result; for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { - auto it = targets_.find(id); - if (it != targets_.end() && it->second && it->second->IsConverged()) { + auto it = targets_map_.find(id); + if (it != targets_map_.end() && it->second && it->second->IsConverged()) { auto spacings = it->second->GetArmorGimbalControlSpacings(); - result[id] = std::move(spacings); + result[id] = spacings; } } return std::make_shared(result, time_stamp); - } + } // TODO: ** 目前 ** 我认为这个函数是多余的 std::shared_ptr GetPredictor( const enumeration::ArmorIdFlag& flag) const { - std::unordered_map> snapshot_map; - for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { - auto it = targets_.find(id); - if (it != targets_.end() && it->second && it->second->IsConverged()) { - snapshot_map[id] = it->second; - } - } + if (targets_map_.empty()) return nullptr; // TODO - return std::make_shared(flag, snapshot_map); + return std::make_shared( + flag, targets_map_, last_update_timestamp_, bullet_speed_, yaw_offset_, pitch_offset_); } - void Update(std::shared_ptr data, double dt) { - const auto now = predictor::TimeStamp(std::time(nullptr)); - RemoveLostTargets(now); - AddNewTargets(data, dt); - // TODO:update the state_machine + void Update(std::shared_ptr data, + const std::shared_ptr& armors_in_image, const std::time_t& now, + const double& bullet_speed) { + + UpdateTimeStamp(data->GetTimeStamped().GetTimeStamp()); + UpdateTargetMap(data, now); + UpdateTarget(data, armors_in_image, now); + UpdateBulletSpeed(bullet_speed); } - auto GetLiveTargetIDs() -> enumeration::CarIDFlag const { - enumeration::CarIDFlag result = enumeration::CarIDFlag::None; - for (const auto& [id, target] : targets_) { - if (target && target->IsConverged()) { - result = static_cast( - static_cast(result) | static_cast(id)); - } + auto GetAllowedTargetID() const -> enumeration::CarIDFlag const { + if (targets_map_.at(tracking_id_)->IsConverged()) { + return tracking_id_; } - return result; + return enumeration::CarIDFlag::None; } private: - void RegisterTarget(enumeration::ArmorIdFlag id, std::shared_ptr target) { - targets_[id] = std::move(target); - } - void RemoveTarget(enumeration::ArmorIdFlag id) { targets_.erase(id); } - bool HasTarget(enumeration::ArmorIdFlag id) const { return targets_.count(id) > 0; } - std::shared_ptr GetTarget(enumeration::ArmorIdFlag id) const { - auto it = targets_.find(id); - return (it != targets_.end()) ? it->second : nullptr; - } - - void AddNewTargets( - const std::shared_ptr& data, double dt) { + void UpdateBulletSpeed(const double& bullet_speed) { bullet_speed_ = bullet_speed; } + void UpdateTimeStamp(const time_t& time_stamp) { last_update_timestamp_ = time_stamp; } + void UpdateTargetMap( + std::shared_ptr data, const std::time_t& now) { const Eigen::Affine3d transform = data->GetTransform(); const Eigen::Matrix3d rotation_matrix = transform.linear(); - const Eigen::Quaterniond rotation_quat(rotation_matrix); + const auto armors_interface = data->GetArmors(); - const auto& armors_interface = data->GetArmors(); - for (int i; i < 8; i++) { + targets_map_.clear(); + for (int i; i < static_cast(enumeration::CarIDFlag::Count); i++) { auto id = static_cast( static_cast(enumeration::CarIDFlag::Hero) << i); - const auto& armors = armors_interface->GetArmors(id); - - if (armors.empty()) continue; - UpdateStateMachine(id, std::time(nullptr)); + const auto& armors_list = armors_interface->GetArmors(id); + if (armors_list.empty()) return; - if (!HasTarget(id)) { - const auto& armor = armors.front(); - const Eigen::Vector3d position_in_world = transform * armor.position; - const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX + const auto& armor = armors_list.front(); + const Eigen::Vector3d xyz_in_world = transform * armor.position; + const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX + targets_map_[id] = + std::move(std::make_shared(xyz_in_world, ypr_in_world, id)); + } + } - auto target = std::make_shared(position_in_world, ypr_in_world, id); - RegisterTarget(id, target); - continue; - } + void UpdateTarget(std::shared_ptr data, + const std::shared_ptr& armors_in_image, const std::time_t& now) { + const Eigen::Affine3d transform = data->GetTransform(); + const Eigen::Matrix3d rotation_matrix = transform.linear(); + const auto armors_interface = data->GetArmors(); - auto target = GetTarget(id); - for (const auto& armor : armors) { - const Eigen::Vector3d position_in_world = transform * armor.position; - const Eigen::Vector3d ypr = - util::math::quaternion_to_euler(Eigen::Quaterniond(rotation_matrix), 2, 1, 0); + tracking_id_ = tracker_->SelectTrackingTargetID(armors_in_image, now); - target->Update(dt, position_in_world, ypr, util::math::xyz2ypd(position_in_world)); - } - } - } + const auto& armors_list = armors_interface->GetArmors(tracking_id_); + if (armors_list.empty()) return; - void RemoveLostTargets(const predictor::TimeStamp& now) { - for (auto it = targets_.begin(); it != targets_.end();) { - if (IsTargetLost(it->second, now)) { - it = targets_.erase(it); - } else { - ++it; - } - } + const auto& armor = armors_list.front(); + const Eigen::Vector3d xyz_in_world = transform * armor.position; + const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX + targets_map_[tracking_id_]->Update(static_cast(now - last_update_timestamp_), + xyz_in_world, ypr_in_world, util::math::xyz2ypd(xyz_in_world)); } - void UpdateStateMachine(enumeration::CarIDFlag cars_detected, TimeStamp now) { - state_machine_->Update(cars_detected, now.GetTimeStamp()); - } + std::unordered_map> targets_map_; + std::unique_ptr tracker_; + std::time_t last_update_timestamp_; + enumeration::CarIDFlag tracking_id_; - bool IsTargetLost( - const std::shared_ptr& target, const predictor::TimeStamp& now) const { - return now.SecondsSince(target->LastSeen()) > timeout_sec_; - } + double bullet_speed_; + const double time_delay_; + const double yaw_offset_; + const double pitch_offset_; - std::unordered_map> targets_; - std::shared_ptr state_machine_; - const double timeout_sec_ { 0.1 }; + const double timeout_sec_; }; -LiveTargetManager::LiveTargetManager( - std::shared_ptr state_machine, double timeout_sec) - : pimpl_(std::make_unique(state_machine, timeout_sec)) { } +LiveTargetManager::LiveTargetManager(const double& time_delay, const double& yaw_offset, + const double& pitch_offset, double timeout_sec) + : pimpl_(std::make_unique(time_delay, yaw_offset, pitch_offset, timeout_sec)) { } LiveTargetManager::~LiveTargetManager() = default; std ::shared_ptr LiveTargetManager::Predict( const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { return pimpl_->Predict(id, time_stamp); } - std ::shared_ptr LiveTargetManager::GetPredictor( const enumeration ::ArmorIdFlag& id) const { return pimpl_->GetPredictor(id); } -auto LiveTargetManager::GetLiveTargetIDs() -> enumeration::CarIDFlag const { - return pimpl_->GetLiveTargetIDs(); +void LiveTargetManager::Update(std::shared_ptr data, + const std::shared_ptr& armors_in_image, const std::time_t& now, + const double& bullet_speed) { + return pimpl_->Update(data, armors_in_image, now, bullet_speed); } - -void LiveTargetManager::Update( - std::shared_ptr data, double dt) { - return pimpl_->Update(data, dt); +auto LiveTargetManager::GetAllowedTargetID() const -> enumeration::ArmorIdFlag const { + return pimpl_->GetAllowedTargetID(); } - } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.hpp b/src/tongji/predictor/live_target_manager.hpp index 7a1c978..88ebfce 100644 --- a/src/tongji/predictor/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager.hpp @@ -3,28 +3,28 @@ #include #include "enum/armor_id.hpp" -#include "enum/car_id.hpp" +#include "interfaces/armor_in_image.hpp" #include "interfaces/predictor_update_package.hpp" #include "interfaces/target_predictor.hpp" -#include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::predictor { class LiveTargetManager final : public interfaces::ITargetPredictor { public: - LiveTargetManager( - std::shared_ptr state_machine, double timeout_sec = 0.1); + LiveTargetManager(const double& time_delay, const double& yaw_offset, + const double& pitch_offset, double timeout_sec = 0.1); ~LiveTargetManager(); std ::shared_ptr Predict( const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; - std ::shared_ptr GetPredictor( const enumeration ::ArmorIdFlag& id) const override; - auto GetLiveTargetIDs() -> enumeration::CarIDFlag const; + void Update(std::shared_ptr data, + const std::shared_ptr& armors_in_image, const std::time_t& now, + const double& bullet_speed); - void Update(std::shared_ptr data, double dt); + auto GetAllowedTargetID() const -> enumeration::ArmorIdFlag const; private: class Impl; diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp index 5dd41b9..e1a73af 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot.hpp @@ -2,7 +2,6 @@ #include -#include "data/armor_gimbal_control_spacing.hpp" #include "extended_kalman_filter.hpp" #include "tongji/predictor/live_target.hpp" #include "tongji/predictor/predict_model.hpp" @@ -17,21 +16,21 @@ class TargetSnapshot { , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) , time_stamp_(target.LastSeen()) { } - std::vector GetArmorGimbalControlSpacings() const { - std::vector armors; - for (int id = 0; id < model_.GetArmorNum(); id++) { - auto angle = - util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); - auto xyz = model_.h_armor_xyz(this->ekf_.x, id); - - data::ArmorGimbalControlSpacing armor; - armor.id = model_.GetID(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - armors.emplace_back(std::move(armor)); - } - return armors; - } + // std::vector GetArmorGimbalControlSpacings() const { + // std::vector armors; + // for (int id = 0; id < model_.GetArmorNum(); id++) { + // auto angle = + // util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + // auto xyz = model_.h_armor_xyz(this->ekf_.x, id); + + // data::ArmorGimbalControlSpacing armor; + // armor.id = model_.GetID(); + // armor.position = xyz; + // armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + // armors.emplace_back(std::move(armor)); + // } + // return armors; + // } auto GetPredictedXYZAList(const double& dt) -> std::vector const { return model_.GetArmorXYZAList(this->Predict(dt)); diff --git a/src/tongji/predictor/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager.cpp index 64b5fa8..f2e3240 100644 --- a/src/tongji/predictor/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager.cpp @@ -2,64 +2,99 @@ #include #include +#include #include "data/armor_gimbal_control_spacing.hpp" #include "enum/enum_tools.hpp" #include "in_gimbal_control_armor.hpp" +#include "tongji/armor_solver/aim_solver.hpp" +#include "tongji/predictor/live_target.hpp" #include "tongji/predictor/target_snapshot.hpp" namespace world_exe::tongji::predictor { + class TargetSnapshotManager::Impl { public: Impl(const enumeration::ArmorIdFlag& id, - const std::unordered_map>& snapshots) - : id_(id) { - for (const auto& [flag, target] : snapshots) { - snapshots_.emplace(flag, TargetSnapshot(*target)); - } - } + const std::unordered_map>& + live_target_map, + const std::time_t& now, const double& bullet_speed, const double& yaw_offset, + const double& pitch_offset) + : aim_solver_(std::make_unique(yaw_offset, pitch_offset)) + + , now_(now) + , ids_(id) + , bullet_speed_(bullet_speed) + , snapshots_(BuildSnapshots(live_target_map)) + , gimbal_command_({ std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN() }) { } - const enumeration::ArmorIdFlag& GetId() const { return id_; } + const enumeration::ArmorIdFlag& GetId() const { return ids_; } std::shared_ptr Predictor( const std::time_t& time_stamp) const { + std::unordered_map> result; - for (const auto& [flag, snapshot] : snapshots_) { - result[flag] = snapshot.GetArmorGimbalControlSpacings(); + for (const auto& [id, snapshot] : snapshots_) { + auto aim_solution = + aim_solver_->SolveAimSolution(std::make_unique(snapshot), + bullet_speed_, snapshot.GetTimeStamp().GetTimeStamp()); + + if (!aim_solution.valid) continue; + + auto target_pos = aim_solution.aim_point.head<3>(); + auto armor_yaw = aim_solution.aim_point[3]; + result.emplace(id, + std::vector { + data::ArmorGimbalControlSpacing { id, target_pos, + util::math::euler_to_quaternion(armor_yaw, 15. / 180. * CV_PI, 0) } }); + + gimbal_command_.yaw = aim_solution.yaw; + gimbal_command_.pitch = aim_solution.pitch; } return std::make_shared(result, time_stamp); } - const std::unique_ptr GetSingleSnapshot( - const enumeration::ArmorIdFlag& id) const { - auto it = snapshots_.find(id); - if (it != snapshots_.end()) { - return std::make_unique(it->second); + auto GetGimbalCommand() const -> GimbalCommand const { return gimbal_command_; } + +private: + static std::unordered_map BuildSnapshots( + const std::unordered_map>& input) { + std::unordered_map result; + for (const auto& [id, target] : input) { + if (target) { + result.emplace(id, TargetSnapshot(*target)); + } } - return nullptr; + return result; } -private: - std::unordered_map snapshots_; - enumeration::ArmorIdFlag id_; + std::unique_ptr aim_solver_; + const std::unordered_map snapshots_; + const std::time_t& now_; + const enumeration::ArmorIdFlag ids_; + const double bullet_speed_; + mutable GimbalCommand gimbal_command_; }; TargetSnapshotManager::TargetSnapshotManager(const enumeration::ArmorIdFlag& id, - const std::unordered_map> snapshots) - : pimpl_(std::make_unique(id, snapshots)) { } + const std::unordered_map>& + live_target_map, + const std::time_t& now, const double& bullet_speed, const double& yaw_offset, + const double& pitch_offset) + : pimpl_(std::make_unique( + id, live_target_map, now, bullet_speed, yaw_offset, pitch_offset)) { } TargetSnapshotManager::~TargetSnapshotManager() = default; const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } - -const std::unique_ptr TargetSnapshotManager::GetSingleSnapshot( - const enumeration::ArmorIdFlag& id) const { - return pimpl_->GetSingleSnapshot(id); -} - std ::shared_ptr TargetSnapshotManager::Predictor( const std ::time_t& time_stamp) const { return pimpl_->Predictor(time_stamp); } + +auto TargetSnapshotManager::GetGimbalCommand() const -> GimbalCommand const { + return pimpl_->GetGimbalCommand(); +} } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager.hpp index 1353394..34e2616 100644 --- a/src/tongji/predictor/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager.hpp @@ -6,24 +6,29 @@ #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" #include "tongji/predictor/live_target.hpp" -#include "tongji/predictor/target_snapshot.hpp" namespace world_exe::tongji::predictor { +struct GimbalCommand { + double yaw; + double pitch; +}; + class TargetSnapshotManager final : public interfaces::IPredictor { public: TargetSnapshotManager(const enumeration::ArmorIdFlag& id, - const std::unordered_map> snapshots); + const std::unordered_map>& + live_target_map, + const std::time_t& now, const double& bullet_speed, const double& yaw_offset, + const double& pitch_offset); ~TargetSnapshotManager(); const enumeration ::ArmorIdFlag& GetId() const override; - - const std::unique_ptr GetSingleSnapshot( - const enumeration::ArmorIdFlag& id) const; - std ::shared_ptr Predictor( const std ::time_t& time_stamp) const override; + auto GetGimbalCommand() const -> GimbalCommand const; + private: class Impl; std::unique_ptr pimpl_; diff --git a/src/tongji/fire_controller/tracker.hpp b/src/tongji/predictor/tracker.hpp similarity index 56% rename from src/tongji/fire_controller/tracker.hpp rename to src/tongji/predictor/tracker.hpp index 68b5d4a..1af1f6b 100644 --- a/src/tongji/fire_controller/tracker.hpp +++ b/src/tongji/predictor/tracker.hpp @@ -1,25 +1,19 @@ #pragma once -#include #include - #include +#include #include -#include "../predictor/target_snapshot.hpp" -#include "../predictor/target_snapshot_manager.hpp" #include "enum/armor_id.hpp" -#include "enum/car_id.hpp" -#include "enum/enum_tools.hpp" -#include "interfaces/armor_in_image.hpp" -#include "interfaces/car_state.hpp" -#include "tongji/decider/armor_info.hpp" -#include "tongji/decider/decider.hpp" -#include "tongji/fire_controller/aim_point_chooser.hpp" +#include "tongji/identifier/armor_filter.hpp" #include "tongji/identifier/identified_armor.hpp" +#include "tongji/predictor/decider.hpp" +#include "tongji/predictor/target_snapshot.hpp" +#include "tongji/predictor/target_snapshot_manager.hpp" #include "tongji/predictor/time_stamp.hpp" -namespace world_exe::tongji::fire_control { +namespace world_exe::tongji::predictor { enum class TrackState { Lost, // @@ -33,81 +27,55 @@ class Tracker final { using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; using TargetSnapshot = world_exe::tongji::predictor::TargetSnapshot; using ArmorInImage = world_exe::tongji::identifier::IdentifiedArmor; - using EnemiesState = world_exe::interfaces::ICarState; public: Tracker() - : last_timestamp_(std::time(nullptr)) - , decider_(std::make_unique()) { } + : armor_filter_(std::make_unique()) + , decider_(std::make_unique()) + , last_track_timestamp_(std::time(nullptr)) { } ~Tracker() = default; - auto SelectTrackingTarget(std::shared_ptr armors_in_image, - const std::shared_ptr& snapshot_manager_) noexcept - -> std::unique_ptr { - - auto detected_ids = enumeration::CarIDFlag::None; - auto armor_info_list = std::vector { }; + auto SelectTrackingTargetID(const std::shared_ptr& armors_in_image, + const std::time_t& now) noexcept -> enumeration::ArmorIdFlag const { + CheckCameraOffline(now); + last_track_timestamp_.SetTimeStamp(now); - for (uint32_t i = 0; i < static_cast(CarIDFlag::Count); ++i) { - auto id = static_cast(static_cast(CarIDFlag::Hero) << i); + auto filtered_ids = enumeration::ArmorIdFlag::None; + auto detected_ids = enumeration::ArmorIdFlag::None; + std::vector filtered_armors; + for (uint32_t i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { + auto id = static_cast( + static_cast(enumeration::ArmorIdFlag::Hero) << i); if (armors_in_image->GetArmors(id).empty()) continue; - detected_ids = static_cast( + // 图像中出现的装甲板 + auto armors = armors_in_image->GetArmors(id); + detected_ids = static_cast( static_cast(detected_ids) | static_cast(id)); - for (const auto& armor : armors_in_image->GetArmors(id)) { - auto armor_info = decider::ArmorInfo(armor); - armor_info_list.emplace_back(armor_info); + // 对从图像识别到的装甲板进行过滤 + filtered_armors = std::move(armor_filter_->FilterArmor(std::move(armors))); + if (!filtered_armors.empty()) { + filtered_ids = + static_cast(static_cast(filtered_ids) + | static_cast(filtered_armors.at(0).id)); } } - decider_->SetInvincibleArmors(invincible_armors_); - auto is_empty = decider_->ArmorFilter(armor_info_list); - decider_->SetPriority(armor_info_list); - - auto sorted_id = decider_->GetSortedArmor(armor_info_list); - if (sorted_id == enumeration::ArmorIdFlag::None) { - return nullptr; - } - - auto snapshot = snapshot_manager_->GetSingleSnapshot(sorted_id); - - if (state_ == TrackState::Tracking) { - SetState(TrackState::Switching); - temp_lost_count_ = 0; - } - - auto now = predictor::TimeStamp(std::time(nullptr)); - if (state_ != TrackState::Lost && now.SecondsSince(last_timestamp_) > 0.1) { - SetState(TrackState::Lost); - ResetTracking(); - return nullptr; - } - - last_timestamp_ = now; - tracking_car_id_ = snapshot->GetID(); - return snapshot; + UpdateState(!(detected_ids == enumeration::ArmorIdFlag::None)); - ResetTracking(); - return nullptr; + tracking_car_id_ = decider_->GetBestArmor(filtered_armors); + return tracking_car_id_; } - void SetInvincibleArmors(const CarIDFlag& invincible_armors) { - invincible_armors_ = invincible_armors; - } - - world_exe::enumeration::CarIDFlag GetCurrentTargetID() const { return tracking_car_id_; } - void UpdateState(bool found) { switch (state_) { case TrackState::Lost: { if (found) { SetState(TrackState::Detecting); - detect_count_++; - } else { - ResetTracking(); + detect_count_ = 1; } break; } @@ -167,6 +135,14 @@ class Tracker final { TrackState GetState() const { return state_; } private: + void CheckCameraOffline(const std::time_t& now) { + // TODO:If the underlying timestamp is std::time_t, then this if branch will never be + // entered + if (state_ != TrackState::Lost + && static_cast(now - last_track_timestamp_.GetTimeStamp()) < 0.1) + SetState(TrackState::Lost); + } + void SetState(TrackState new_state) { pre_state_ = state_; state_ = new_state; @@ -177,7 +153,9 @@ class Tracker final { world_exe::enumeration::CarIDFlag tracking_car_id_ { enumeration::CarIDFlag::None }; TrackState state_ = TrackState::Lost; TrackState pre_state_ = TrackState::Lost; - CarIDFlag invincible_armors_ { CarIDFlag::None }; + + std::unique_ptr armor_filter_; + std::unique_ptr decider_; int detect_count_ = 0; int temp_lost_count_ = 0; @@ -187,9 +165,7 @@ class Tracker final { const int normal_max_temp_lost_count_ = max_temp_lost_count_; const int max_switch_count_ = 200; - predictor::TimeStamp last_timestamp_; - - std::unique_ptr decider_; + predictor::TimeStamp last_track_timestamp_; }; } \ No newline at end of file diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp deleted file mode 100644 index 6592a63..0000000 --- a/src/tongji/state_machine/car_state_manager.hpp +++ /dev/null @@ -1,55 +0,0 @@ -#pragma once - -#include - -#include "tongji/predictor/time_stamp.hpp" - -namespace world_exe::tongji::car_state { - -using TimeStamp = predictor::TimeStamp; -class CarStateManager { -public: - CarStateManager(int switch_threshold = 5) - : switch_threshold_(switch_threshold) - , last_seen_(std::time(nullptr)) { } - - void Update(bool detected, const TimeStamp& now) { - if (detected) { - count_ = std::min(count_ + 1, switch_threshold_); - last_seen_ = now; - update_count_++; - if (update_count_ >= converge_threshold_ && !is_diverged_) is_converged_ = true; - } else { - count_ = std::max(count_ - 1, 0); - } - is_locked_ = (count_ >= switch_threshold_); - } - - bool IsConverged() const { return is_converged_ && !is_diverged_; } - - void Reset() { - count_ = 0; - update_count_ = 0; - is_locked_ = false; - is_converged_ = false; - is_diverged_ = true; - last_seen_ = predictor::TimeStamp(std::time(nullptr)); - } - - void SetThreshold(const int& value) { switch_threshold_ = value; } - - predictor::TimeStamp LastSeen() const { return last_seen_; } - -private: - int count_ = 0; - int switch_threshold_; - int converge_threshold_ = 3; - int update_count_ = 0; - - bool is_locked_ = false; - bool is_converged_ = false; - bool is_diverged_ = true; - - predictor::TimeStamp last_seen_; -}; -} \ No newline at end of file diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index fc0ffa8..b6bb318 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -1,70 +1,38 @@ #include "state_machine.hpp" -#include -#include +#include #include "enum/car_id.hpp" -#include "tongji/state_machine/car_state_manager.hpp" -#include "util/index.hpp" +#include "interfaces/target_predictor.hpp" +#include "tongji/predictor/live_target_manager.hpp" namespace world_exe::tongji::state_machine { -using TimeStamp = car_state::TimeStamp; class StateMachine::Impl { public: - Impl() - : car_states_ { } { - std::generate( - car_states_.begin(), car_states_.end(), []() { return car_state::CarStateManager(); }); - } - const enumeration::CarIDFlag& GetAllowdToFires() const { return tracing_state_; } - - const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const { - return car_states_[util::enumeration::GetIndex(single_id)]; - } - - void Update(const enumeration::CarIDFlag& car_detected, const std::time_t& now) { - tracing_state_ = enumeration::CarIDFlag::None; - - for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { - auto id = static_cast(1 << i); - bool detected = (static_cast(car_detected) >> i) & 0x01; - - auto& state = car_states_[i]; - state.Update(detected, now); + Impl(std::shared_ptr live_target_manager) + : live_target_manager_(std::move(live_target_manager)) { } - if (!state.IsConverged()) { - state.Reset(); - continue; - } + const enumeration::CarIDFlag& GetAllowdToFires() const { return target_ids_; } - tracing_state_ = static_cast( - static_cast(tracing_state_) | static_cast(id)); - } + void Update() { + auto live_target_manager = + std::dynamic_pointer_cast(live_target_manager_); + target_ids_ = live_target_manager->GetAllowedTargetID(); } private: - enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; - std::array car_states_; + std::shared_ptr live_target_manager_; + enumeration::CarIDFlag target_ids_; }; -StateMachine::StateMachine() - : pimpl_(std::make_unique()) { } - +StateMachine::StateMachine( + std::shared_ptr live_target_manager) + : pimpl_(std::make_unique(live_target_manager)) { } StateMachine::~StateMachine() = default; const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return pimpl_->GetAllowdToFires(); } -const car_state::CarStateManager& StateMachine::GetState(enumeration::CarIDFlag single_id) const { - return pimpl_->GetState(single_id); -} - -const interfaces::ICarState& StateMachine::Update( - const enumeration::CarIDFlag& car_detected, const std::time_t& now) { - pimpl_->Update(car_detected, now); - return *this; -} - } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 780fe07..c3417e3 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -5,7 +5,7 @@ #include "enum/car_id.hpp" #include "interfaces/car_state.hpp" -#include "tongji/state_machine/car_state_manager.hpp" +#include "interfaces/target_predictor.hpp" namespace world_exe::tongji::state_machine { class StateMachine final : public interfaces::ICarState { @@ -14,9 +14,7 @@ class StateMachine final : public interfaces::ICarState { ~StateMachine(); const enumeration ::CarIDFlag& GetAllowdToFires() const override; - const car_state::CarStateManager& GetState(enumeration::CarIDFlag single_id) const; - const interfaces::ICarState& Update( - const enumeration::CarIDFlag& car_detected, const std::time_t& now); + StateMachine(std::shared_ptr live_target_manager); private: class Impl; From 7b15810673fbdd19c2b4460391e00151f5f2ccbe Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 13 Oct 2025 12:17:48 +0800 Subject: [PATCH 52/93] refactor(project): reorganize directory structure to match new architecture --- src/tongji/fire_controller/fire_controller.cpp | 10 +++++----- src/tongji/fire_controller/fire_controller.hpp | 2 +- src/tongji/fire_controller/fire_decision.hpp | 2 +- src/tongji/identifier/identifier.cpp | 2 +- src/tongji/predictor/in_gimbal_control_armor.hpp | 2 +- .../{ => kalman_filter}/extended_kalman_filter.hpp | 3 ++- .../predictor/{ => kalman_filter}/predict_model.hpp | 2 +- .../predictor/{ => live_target_manager}/decider.cpp | 0 .../predictor/{ => live_target_manager}/decider.hpp | 0 .../{ => live_target_manager}/live_target.hpp | 8 ++++---- .../live_target_manager.cpp | 8 ++++---- .../live_target_manager.hpp | 0 .../predictor/{ => live_target_manager}/tracker.hpp | 12 ++++++------ .../target_snapshot_manager}/aim_point_chooser.hpp | 0 .../target_snapshot_manager}/aim_solver.hpp | 6 +++--- .../target_snapshot.hpp | 8 ++++---- .../target_snapshot_manager.cpp | 8 ++++---- .../target_snapshot_manager.hpp | 2 +- .../target_snapshot_manager}/trajectory.hpp | 0 src/tongji/state_machine/state_machine.cpp | 2 +- src/tongji/{predictor => time_stamp}/time_stamp.hpp | 0 21 files changed, 39 insertions(+), 38 deletions(-) rename src/tongji/predictor/{ => kalman_filter}/extended_kalman_filter.hpp (99%) rename src/tongji/predictor/{ => kalman_filter}/predict_model.hpp (99%) rename src/tongji/predictor/{ => live_target_manager}/decider.cpp (100%) rename src/tongji/predictor/{ => live_target_manager}/decider.hpp (100%) rename src/tongji/predictor/{ => live_target_manager}/live_target.hpp (95%) rename src/tongji/predictor/{ => live_target_manager}/live_target_manager.cpp (96%) rename src/tongji/predictor/{ => live_target_manager}/live_target_manager.hpp (100%) rename src/tongji/predictor/{ => live_target_manager}/tracker.hpp (95%) rename src/tongji/{armor_solver => predictor/target_snapshot_manager}/aim_point_chooser.hpp (100%) rename src/tongji/{armor_solver => predictor/target_snapshot_manager}/aim_solver.hpp (95%) rename src/tongji/predictor/{ => target_snapshot_manager}/target_snapshot.hpp (89%) rename src/tongji/predictor/{ => target_snapshot_manager}/target_snapshot_manager.cpp (95%) rename src/tongji/predictor/{ => target_snapshot_manager}/target_snapshot_manager.hpp (94%) rename src/tongji/{armor_solver => predictor/target_snapshot_manager}/trajectory.hpp (100%) rename src/tongji/{predictor => time_stamp}/time_stamp.hpp (100%) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index b4f79e6..b3eccd7 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -3,13 +3,13 @@ #include #include +#include "../identifier/identified_armor.hpp" +#include "../predictor/live_target_manager/live_target_manager.hpp" +#include "../predictor/target_snapshot_manager/target_snapshot_manager.hpp" +#include "../state_machine/state_machine.hpp" #include "data/fire_control.hpp" +#include "fire_decision.hpp" #include "interfaces/target_predictor.hpp" -#include "tongji/fire_controller/fire_decision.hpp" -#include "tongji/identifier/identified_armor.hpp" -#include "tongji/predictor/live_target_manager.hpp" -#include "tongji/predictor/target_snapshot_manager.hpp" -#include "tongji/state_machine/state_machine.hpp" namespace world_exe::tongji::fire_control { diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index eed2838..f193145 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -2,11 +2,11 @@ #include +#include "../time_stamp/time_stamp.hpp" #include "interfaces/armor_in_image.hpp" #include "interfaces/car_state.hpp" #include "interfaces/fire_controller.hpp" #include "interfaces/target_predictor.hpp" -#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::fire_control { diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index 7c94956..46088e8 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -3,7 +3,7 @@ #include #include -#include "tongji/predictor/target_snapshot_manager.hpp" +#include "tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp" namespace world_exe::tongji::fire_control { diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index c314f74..f70af8f 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -19,7 +19,7 @@ #include "enum/armor_id.hpp" #include "identified_armor.hpp" #include "interfaces/armor_in_image.hpp" -#include "tongji/identifier/classifier.hpp" +#include "../identifier/classifier.hpp" #include "util/logger.hpp" #include "util/stringifier.hpp" diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index 117203a..cdf4981 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -4,10 +4,10 @@ #include #include +#include "../time_stamp/time_stamp.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "enum/armor_id.hpp" #include "interfaces/armor_in_gimbal_control.hpp" -#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::predictor { diff --git a/src/tongji/predictor/extended_kalman_filter.hpp b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp similarity index 99% rename from src/tongji/predictor/extended_kalman_filter.hpp rename to src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp index 9f80180..ce90cfd 100644 --- a/src/tongji/predictor/extended_kalman_filter.hpp +++ b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp @@ -1,11 +1,12 @@ #pragma once -#include #include #include #include #include +#include + namespace world_exe::tongji::predictor { template // class ExtendedKalmanFilter { diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/kalman_filter/predict_model.hpp similarity index 99% rename from src/tongji/predictor/predict_model.hpp rename to src/tongji/predictor/kalman_filter/predict_model.hpp index db04478..5af8395 100644 --- a/src/tongji/predictor/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -7,7 +7,7 @@ #include #include "enum/car_id.hpp" -#include "tongji/predictor/extended_kalman_filter.hpp" +#include "extended_kalman_filter.hpp" #include "util/math.hpp" namespace world_exe::tongji::predictor { diff --git a/src/tongji/predictor/decider.cpp b/src/tongji/predictor/live_target_manager/decider.cpp similarity index 100% rename from src/tongji/predictor/decider.cpp rename to src/tongji/predictor/live_target_manager/decider.cpp diff --git a/src/tongji/predictor/decider.hpp b/src/tongji/predictor/live_target_manager/decider.hpp similarity index 100% rename from src/tongji/predictor/decider.hpp rename to src/tongji/predictor/live_target_manager/decider.hpp diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target_manager/live_target.hpp similarity index 95% rename from src/tongji/predictor/live_target.hpp rename to src/tongji/predictor/live_target_manager/live_target.hpp index d25bfdb..710c87c 100644 --- a/src/tongji/predictor/live_target.hpp +++ b/src/tongji/predictor/live_target_manager/live_target.hpp @@ -5,11 +5,11 @@ #include #include +#include "../../time_stamp/time_stamp.hpp" +#include "../kalman_filter/extended_kalman_filter.hpp" +#include "../kalman_filter/predict_model.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" -#include "extended_kalman_filter.hpp" -#include "predict_model.hpp" -#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::predictor { @@ -36,7 +36,7 @@ class LiveTarget { 0, 0; ExtendedKalmanFilter<11, 4>::PMat P0 = model_.GetP0Dig().asDiagonal(); - ekf_ = ExtendedKalmanFilter<11, 4>( + ekf_ = ExtendedKalmanFilter<11, 4>( x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） } diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp similarity index 96% rename from src/tongji/predictor/live_target_manager.cpp rename to src/tongji/predictor/live_target_manager/live_target_manager.cpp index 1b989b9..ad20219 100644 --- a/src/tongji/predictor/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -5,13 +5,13 @@ #include #include +#include "../in_gimbal_control_armor.hpp" +#include "../target_snapshot_manager/target_snapshot_manager.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" #include "interfaces/predictor_update_package.hpp" -#include "tongji/predictor/in_gimbal_control_armor.hpp" -#include "tongji/predictor/live_target.hpp" -#include "tongji/predictor/target_snapshot_manager.hpp" -#include "tongji/predictor/tracker.hpp" +#include "live_target.hpp" +#include "tracker.hpp" #include "util/index.hpp" #include "util/math.hpp" diff --git a/src/tongji/predictor/live_target_manager.hpp b/src/tongji/predictor/live_target_manager/live_target_manager.hpp similarity index 100% rename from src/tongji/predictor/live_target_manager.hpp rename to src/tongji/predictor/live_target_manager/live_target_manager.hpp diff --git a/src/tongji/predictor/tracker.hpp b/src/tongji/predictor/live_target_manager/tracker.hpp similarity index 95% rename from src/tongji/predictor/tracker.hpp rename to src/tongji/predictor/live_target_manager/tracker.hpp index 1af1f6b..a03c12d 100644 --- a/src/tongji/predictor/tracker.hpp +++ b/src/tongji/predictor/live_target_manager/tracker.hpp @@ -5,13 +5,13 @@ #include #include +#include "../../identifier/armor_filter.hpp" +#include "../../identifier/identified_armor.hpp" +#include "../../time_stamp/time_stamp.hpp" +#include "../target_snapshot_manager/target_snapshot.hpp" +#include "../target_snapshot_manager/target_snapshot_manager.hpp" +#include "decider.hpp" #include "enum/armor_id.hpp" -#include "tongji/identifier/armor_filter.hpp" -#include "tongji/identifier/identified_armor.hpp" -#include "tongji/predictor/decider.hpp" -#include "tongji/predictor/target_snapshot.hpp" -#include "tongji/predictor/target_snapshot_manager.hpp" -#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::predictor { diff --git a/src/tongji/armor_solver/aim_point_chooser.hpp b/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp similarity index 100% rename from src/tongji/armor_solver/aim_point_chooser.hpp rename to src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp diff --git a/src/tongji/armor_solver/aim_solver.hpp b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp similarity index 95% rename from src/tongji/armor_solver/aim_solver.hpp rename to src/tongji/predictor/target_snapshot_manager/aim_solver.hpp index db34fe3..732c8a2 100644 --- a/src/tongji/armor_solver/aim_solver.hpp +++ b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp @@ -5,9 +5,9 @@ #include #include -#include "tongji/armor_solver/aim_point_chooser.hpp" -#include "tongji/armor_solver/trajectory.hpp" -#include "tongji/predictor/target_snapshot.hpp" +#include "aim_point_chooser.hpp" +#include "target_snapshot.hpp" +#include "trajectory.hpp" namespace world_exe::tongji::armor_solver { diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp similarity index 89% rename from src/tongji/predictor/target_snapshot.hpp rename to src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp index e1a73af..f055594 100644 --- a/src/tongji/predictor/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp @@ -2,10 +2,10 @@ #include -#include "extended_kalman_filter.hpp" -#include "tongji/predictor/live_target.hpp" -#include "tongji/predictor/predict_model.hpp" -#include "tongji/predictor/time_stamp.hpp" +#include "../../time_stamp/time_stamp.hpp" +#include "../kalman_filter/extended_kalman_filter.hpp" +#include "../kalman_filter/predict_model.hpp" +#include "../live_target_manager/live_target.hpp" namespace world_exe::tongji::predictor { diff --git a/src/tongji/predictor/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp similarity index 95% rename from src/tongji/predictor/target_snapshot_manager.cpp rename to src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp index f2e3240..a1d9614 100644 --- a/src/tongji/predictor/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -4,12 +4,12 @@ #include #include +#include "../in_gimbal_control_armor.hpp" +#include "../live_target_manager/live_target.hpp" +#include "../target_snapshot_manager/target_snapshot.hpp" +#include "aim_solver.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "enum/enum_tools.hpp" -#include "in_gimbal_control_armor.hpp" -#include "tongji/armor_solver/aim_solver.hpp" -#include "tongji/predictor/live_target.hpp" -#include "tongji/predictor/target_snapshot.hpp" namespace world_exe::tongji::predictor { diff --git a/src/tongji/predictor/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp similarity index 94% rename from src/tongji/predictor/target_snapshot_manager.hpp rename to src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp index 34e2616..3176e9a 100644 --- a/src/tongji/predictor/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp @@ -3,9 +3,9 @@ #include #include +#include "../live_target_manager/live_target.hpp" #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" -#include "tongji/predictor/live_target.hpp" namespace world_exe::tongji::predictor { diff --git a/src/tongji/armor_solver/trajectory.hpp b/src/tongji/predictor/target_snapshot_manager/trajectory.hpp similarity index 100% rename from src/tongji/armor_solver/trajectory.hpp rename to src/tongji/predictor/target_snapshot_manager/trajectory.hpp diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index b6bb318..990bf99 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -4,7 +4,7 @@ #include "enum/car_id.hpp" #include "interfaces/target_predictor.hpp" -#include "tongji/predictor/live_target_manager.hpp" +#include "tongji/predictor/live_target_manager/live_target_manager.hpp" namespace world_exe::tongji::state_machine { diff --git a/src/tongji/predictor/time_stamp.hpp b/src/tongji/time_stamp/time_stamp.hpp similarity index 100% rename from src/tongji/predictor/time_stamp.hpp rename to src/tongji/time_stamp/time_stamp.hpp From 9ccbf6526fbe1f4e73dcd69b97e886675f6a47b3 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 13 Oct 2025 13:39:42 +0800 Subject: [PATCH 53/93] refactor(namespace): update namespaces to align with new architecture --- src/tongji/fire_controller/fire_controller.cpp | 4 ++-- src/tongji/fire_controller/fire_controller.hpp | 2 +- src/tongji/predictor/in_gimbal_control_armor.hpp | 2 +- src/tongji/predictor/live_target_manager/live_target.hpp | 2 +- src/tongji/predictor/live_target_manager/tracker.hpp | 3 ++- .../predictor/target_snapshot_manager/aim_point_chooser.hpp | 2 +- src/tongji/predictor/target_snapshot_manager/aim_solver.hpp | 6 +++--- .../predictor/target_snapshot_manager/target_snapshot.hpp | 2 +- .../target_snapshot_manager/target_snapshot_manager.cpp | 5 ++--- src/tongji/predictor/target_snapshot_manager/trajectory.hpp | 2 +- src/tongji/time_stamp/time_stamp.hpp | 2 +- 11 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index b3eccd7..6443340 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -18,7 +18,7 @@ using StateMachine = state_machine::StateMachine; using IdentifiedArmor = identifier::IdentifiedArmor; using CarIDFlag = enumeration::CarIDFlag; using LiveTargetManager = predictor::LiveTargetManager; -using TimeStamp = predictor::TimeStamp; +using TimeStamp = time_stamp::TimeStamp; class FireController::Impl { public: @@ -93,7 +93,7 @@ class FireController::Impl { std::shared_ptr identified_armors_; std::unique_ptr fire_decision_; - predictor::TimeStamp time_stamp_ { std::time(nullptr) }; + time_stamp::TimeStamp time_stamp_ { std::time(nullptr) }; std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index f193145..19343e0 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -21,7 +21,7 @@ class FireController final : public interfaces::IFireControl { void Update(std::shared_ptr armors, const double& gimbal_yaw); - predictor::TimeStamp GetTimeStamp() const; + time_stamp::TimeStamp GetTimeStamp() const; private: class Impl; diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index cdf4981..c012806 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -28,7 +28,7 @@ class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } private: - TimeStamp time_stamp_; + time_stamp::TimeStamp time_stamp_; std::unordered_map> armors_map_; static const std::vector empty; diff --git a/src/tongji/predictor/live_target_manager/live_target.hpp b/src/tongji/predictor/live_target_manager/live_target.hpp index 710c87c..d9369a4 100644 --- a/src/tongji/predictor/live_target_manager/live_target.hpp +++ b/src/tongji/predictor/live_target_manager/live_target.hpp @@ -43,7 +43,7 @@ class LiveTarget { ExtendedKalmanFilter<11, 4>::XVec GetEkfX() const { return ekf_.x; } ExtendedKalmanFilter<11, 4>::PDig GetP0Dig() const { return model_.GetP0Dig(); } const PredictModel& GetModel() const { return model_; } - predictor::TimeStamp LastSeen() const { return predictor::TimeStamp(last_see_time_stamp_); } + time_stamp::TimeStamp LastSeen() const { return time_stamp::TimeStamp(last_see_time_stamp_); } std::vector GetArmorGimbalControlSpacings() const { std::vector armors; diff --git a/src/tongji/predictor/live_target_manager/tracker.hpp b/src/tongji/predictor/live_target_manager/tracker.hpp index a03c12d..c30b2eb 100644 --- a/src/tongji/predictor/live_target_manager/tracker.hpp +++ b/src/tongji/predictor/live_target_manager/tracker.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -165,7 +166,7 @@ class Tracker final { const int normal_max_temp_lost_count_ = max_temp_lost_count_; const int max_switch_count_ = 200; - predictor::TimeStamp last_track_timestamp_; + time_stamp::TimeStamp last_track_timestamp_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp b/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp index 5ba8b72..2f2c15d 100644 --- a/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp +++ b/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp @@ -3,7 +3,7 @@ #include "enum/car_id.hpp" #include "util/math.hpp" -namespace world_exe::tongji::fire_control { +namespace world_exe::tongji::predictor { using CarIDFlag = enumeration::CarIDFlag; diff --git a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp index 732c8a2..4c9122e 100644 --- a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp +++ b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp @@ -9,7 +9,7 @@ #include "target_snapshot.hpp" #include "trajectory.hpp" -namespace world_exe::tongji::armor_solver { +namespace world_exe::tongji::predictor { using TargetSnapshot = predictor::TargetSnapshot; @@ -25,7 +25,7 @@ class AimingSolver { public: AimingSolver( const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) - : aim_point_chooser_(std::make_unique()) + : aim_point_chooser_(std::make_unique()) , yaw_offset_(yaw_offset / 57.3) // degree to rad , pitch_offset_(pitch_offset / 57.3) // degree to rad , g_(gravity) { } @@ -83,6 +83,6 @@ class AimingSolver { double yaw_offset_, pitch_offset_; const double g_; - std::unique_ptr aim_point_chooser_; + std::unique_ptr aim_point_chooser_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp index f055594..a1fa6bf 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp @@ -48,7 +48,7 @@ class TargetSnapshot { private: PredictModel model_; ExtendedKalmanFilter<11, 4> ekf_; - TimeStamp time_stamp_; + time_stamp::TimeStamp time_stamp_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp index a1d9614..ad9a586 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -6,7 +6,6 @@ #include "../in_gimbal_control_armor.hpp" #include "../live_target_manager/live_target.hpp" -#include "../target_snapshot_manager/target_snapshot.hpp" #include "aim_solver.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "enum/enum_tools.hpp" @@ -20,7 +19,7 @@ class TargetSnapshotManager::Impl { live_target_map, const std::time_t& now, const double& bullet_speed, const double& yaw_offset, const double& pitch_offset) - : aim_solver_(std::make_unique(yaw_offset, pitch_offset)) + : aim_solver_(std::make_unique(yaw_offset, pitch_offset)) , now_(now) , ids_(id) @@ -71,7 +70,7 @@ class TargetSnapshotManager::Impl { return result; } - std::unique_ptr aim_solver_; + std::unique_ptr aim_solver_; const std::unordered_map snapshots_; const std::time_t& now_; const enumeration::ArmorIdFlag ids_; diff --git a/src/tongji/predictor/target_snapshot_manager/trajectory.hpp b/src/tongji/predictor/target_snapshot_manager/trajectory.hpp index d4f9e68..ff5adc1 100644 --- a/src/tongji/predictor/target_snapshot_manager/trajectory.hpp +++ b/src/tongji/predictor/target_snapshot_manager/trajectory.hpp @@ -1,7 +1,7 @@ #pragma once #include -namespace world_exe::tongji::armor_solver { +namespace world_exe::tongji::predictor { struct TrajectoryResult { bool solvable = true; diff --git a/src/tongji/time_stamp/time_stamp.hpp b/src/tongji/time_stamp/time_stamp.hpp index 90deacf..aa93ae0 100644 --- a/src/tongji/time_stamp/time_stamp.hpp +++ b/src/tongji/time_stamp/time_stamp.hpp @@ -2,7 +2,7 @@ #include "interfaces/time_stamped.hpp" -namespace world_exe::tongji::predictor { +namespace world_exe::tongji::time_stamp { class TimeStamp : public interfaces::ITimeStamped { public: TimeStamp(const std::time_t& time_stamp) From 3bfab3562e906fd8b9d102aae4bf1eb3e465937e Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 13 Oct 2025 15:17:53 +0800 Subject: [PATCH 54/93] fix(initialization): repair incomplete constructor implementations --- src/tongji/fire_controller/fire_controller.cpp | 16 +++++++++------- src/tongji/fire_controller/fire_decision.hpp | 1 + .../live_target_manager/live_target_manager.cpp | 5 +---- .../predictor/live_target_manager/tracker.hpp | 3 ++- .../target_snapshot_manager.cpp | 3 +-- 5 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 6443340..b178e0b 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -10,6 +10,7 @@ #include "data/fire_control.hpp" #include "fire_decision.hpp" #include "interfaces/target_predictor.hpp" +#include "tongji/time_stamp/time_stamp.hpp" namespace world_exe::tongji::fire_control { @@ -28,6 +29,9 @@ class FireController::Impl { std::shared_ptr live_target_manager) : control_delay_(control_delay_in_second) , bullet_speed_(bullet_speed) + , allowed_target_id_(CarIDFlag::None) + , firable_(false) + , time_stamp_(std::time(nullptr)) , fire_decision_(std::make_unique(auto_fire)) , state_machine_(state_machine) , live_target_manager_(std::move(live_target_manager)) { } @@ -66,8 +70,8 @@ class FireController::Impl { } const CarIDFlag GetAttackCarId() const { - if (firable_) { } - return allowed_target_id_; + if (firable_) return allowed_target_id_; + return CarIDFlag::None; } void Update(std::shared_ptr armors, const double& gimbal_yaw) { @@ -86,15 +90,13 @@ class FireController::Impl { double gimbal_yaw_; double control_delay_; double bullet_speed_; + std::shared_ptr identified_armors_; mutable CarIDFlag allowed_target_id_; - mutable double firable_ { false }; - - std::shared_ptr identified_armors_; + mutable double firable_; + time_stamp::TimeStamp time_stamp_; std::unique_ptr fire_decision_; - time_stamp::TimeStamp time_stamp_ { std::time(nullptr) }; - std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; }; diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index 46088e8..74ceaac 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -14,6 +14,7 @@ class FireDecision { : auto_fire_(auto_fire) , last_gimbal_command_({ std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN() }) + , gimbal_yaw_(std::numeric_limits::quiet_NaN()) , first_tolerance_(first_tolerance) , second_tolerance_(second_tolerance) , judge_distance_(judge_distance) { } diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp index ad20219..bf1da76 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -27,8 +27,7 @@ class LiveTargetManager::Impl { , tracking_id_(enumeration::CarIDFlag::None) , time_delay_(time_delay) , yaw_offset_(yaw_offset) - , pitch_offset_(pitch_offset) - , timeout_sec_(timeout_sec) { } + , pitch_offset_(pitch_offset) { } std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { @@ -124,8 +123,6 @@ class LiveTargetManager::Impl { const double time_delay_; const double yaw_offset_; const double pitch_offset_; - - const double timeout_sec_; }; LiveTargetManager::LiveTargetManager(const double& time_delay, const double& yaw_offset, diff --git a/src/tongji/predictor/live_target_manager/tracker.hpp b/src/tongji/predictor/live_target_manager/tracker.hpp index c30b2eb..0b834a6 100644 --- a/src/tongji/predictor/live_target_manager/tracker.hpp +++ b/src/tongji/predictor/live_target_manager/tracker.hpp @@ -140,7 +140,7 @@ class Tracker final { // TODO:If the underlying timestamp is std::time_t, then this if branch will never be // entered if (state_ != TrackState::Lost - && static_cast(now - last_track_timestamp_.GetTimeStamp()) < 0.1) + && static_cast(now - last_track_timestamp_.GetTimeStamp()) < timeout_sec_) SetState(TrackState::Lost); } @@ -165,6 +165,7 @@ class Tracker final { const int outpost_max_temp_lost_count_ = 75; const int normal_max_temp_lost_count_ = max_temp_lost_count_; const int max_switch_count_ = 200; + const double timeout_sec_ = 0.1; time_stamp::TimeStamp last_track_timestamp_; }; diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp index ad9a586..e26d631 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -20,11 +20,10 @@ class TargetSnapshotManager::Impl { const std::time_t& now, const double& bullet_speed, const double& yaw_offset, const double& pitch_offset) : aim_solver_(std::make_unique(yaw_offset, pitch_offset)) - + , snapshots_(BuildSnapshots(live_target_map)) , now_(now) , ids_(id) , bullet_speed_(bullet_speed) - , snapshots_(BuildSnapshots(live_target_map)) , gimbal_command_({ std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN() }) { } From 50f1976e687a0fd4e79392cd2d47b00ec3037854 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 21 Oct 2025 23:58:34 +0800 Subject: [PATCH 55/93] refactor(filter): rewrite Kalman filter with template implementation - Reimplement Kalman filter as template class for type safety and performance - Refactor prediction and update methods with improved numerical stability - Adapt predictor modules to work with new templated filter interface --- .gitignore | 5 +- .../kalman_filter/extended_kalman_filter.hpp | 168 ++++++----- .../predictor/kalman_filter/predict_model.hpp | 273 +++++++++--------- .../live_target_manager/live_target.hpp | 88 +++--- .../target_snapshot.hpp | 11 +- src/tongji/solver/solved_armor.hpp | 4 +- 6 files changed, 297 insertions(+), 252 deletions(-) diff --git a/.gitignore b/.gitignore index a57b149..7102495 100644 --- a/.gitignore +++ b/.gitignore @@ -3,4 +3,7 @@ build install log .cache -.devcontainer \ No newline at end of file +.devcontainer +docker-compose.yml +Dockerfile +.env \ No newline at end of file diff --git a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp index ce90cfd..ec494a4 100644 --- a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp +++ b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp @@ -1,110 +1,144 @@ #pragma once +#include #include -#include -#include #include #include namespace world_exe::tongji::predictor { -template // -class ExtendedKalmanFilter { +template +concept EKFModelTypes = requires { + { T::xn } -> std::same_as; + { T::zn } -> std::same_as; + + typename T::XVec; + typename T::ZVec; + typename T::AMat; + typename T::PMat; + typename T::RMat; + typename T::QMat; + typename T::HMat; +}; + +template class ExtendedKalmanFilter { public: - using XVec = Eigen::Matrix; - using ZVec = Eigen::Matrix; - using AMat = Eigen::Matrix; - using PMat = Eigen::Matrix; - using PDig = Eigen::Matrix; - using RMat = Eigen::Matrix; - using RDig = Eigen::Matrix; - using QMat = Eigen::Matrix; - using HMat = Eigen::Matrix; + static constexpr int xn = EKFModel::xn; + static constexpr int zn = EKFModel::zn; + + using XVec = EKFModel::XVec; + using ZVec = EKFModel::ZVec; + using AMat = EKFModel::AMat; + using PMat = EKFModel::PMat; + using PDig = EKFModel::PDig; + using RMat = EKFModel::RMat; + using RDig = EKFModel::RDig; + using QMat = EKFModel::QMat; + using HMat = EKFModel::HMat; XVec x; PMat P; - ExtendedKalmanFilter() = default; - - ExtendedKalmanFilter( - const XVec& x0, const PMat& P0, - std::function x_add = [](const XVec& a, const XVec& b) { - return a + b; - }) { - data["residual_yaw"] = 0.0; - data["residual_pitch"] = 0.0; - data["residual_distance"] = 0.0; - data["residual_angle"] = 0.0; - data["nis"] = 0.0; - data["nees"] = 0.0; - data["nis_fail"] = 0.0; - data["nees_fail"] = 0.0; - data["recent_nis_failures"] = 0.0; + ExtendedKalmanFilter(const XVec& x0, const PMat& P0, const EKFModel& model) + : x(x0) + , P(P0) + , model_(model) { + // data["residual_yaw"] = 0.0; + // data["residual_pitch"] = 0.0; + // data["residual_distance"] = 0.0; + // data["residual_angle"] = 0.0; + // data["nis"] = 0.0; + // data["nees"] = 0.0; + // data["nis_fail"] = 0.0; + // data["nees_fail"] = 0.0; + // data["recent_nis_failures"] = 0.0; } - XVec Update( - const double& dt, const AMat& A, const QMat& Q, - std::function f, const ZVec& z, const HMat& H, - const RMat& R, std::function h, - std::function z_subtract = - [](const ZVec& a, const ZVec& b) { return a - b; }) { + // 无副作用，不修改x，仅预测 + auto PredictOnce(const double& dt) const -> const std::pair { + const auto A = model_.A(dt); + const auto Q = model_.Q(dt); + const auto x_n = model_.f(x, dt); + const auto P_n = A * P * A.transpose() + Q; - auto x_n = f(x, dt); + return { x_n, P_n }; + } - auto P_n = A * P * A.transpose() + Q; + auto Update(const double& dt, const ZVec& z, const HMat& H, const RMat& R, const int& id) + -> const XVec { + const auto [x_prior, P_prior] = PredictOnce(dt); + const auto z_prior = model_.h(x_prior, id); - auto residual = z_subtract(z, h(x_n)); + const auto residual = model_.z_subtract(z, z_prior); - auto S = H * P * H.transpose() + R; + const auto S = H * P_prior * H.transpose() + R; - auto K = P_n * H.transpose() * S.inverse(); + const auto K = P_prior * H.transpose() * S.inverse(); - x = x_add(x, K * residual); + x = x_prior; + x = model_.x_add(x, K * residual); // Stable Compution of the Posterior Covariance // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose(); - /// 卡方检验 - // 新增检验 - double nis = residual.transpose() * S.inverse() * residual; + const auto P_inverse = P_prior.inverse(); + const auto error = x - x_prior; - // 卡方检验阈值（自由度=4，取置信水平95%） - constexpr double nis_threshold = 0.711; + RecordStatistics(residual, S, error, P_inverse); - if (nis > nis_threshold) nis_count_++, data["nis_fail"] = 1; - total_count_++; - last_nis = nis; + return x; + } - recent_nis_failures.push_back(nis > nis_threshold ? 1 : 0); + auto IsDiverse() const -> bool const { return CalculateFailureRate() >= max_failure_rate; } - if (recent_nis_failures.size() > window_size) { - recent_nis_failures.pop_front(); - } +private: + auto CalculateFailureRate() const -> bool const { + int failures = std::accumulate(recent_nis_failures.begin(), recent_nis_failures.end(), 0); + return (double)failures / window_size; + } + + auto GetRecentNISFailureRate() const -> double const { + if (recent_nis_failures.empty()) return 0.0; int recent_failures = std::accumulate(recent_nis_failures.begin(), recent_nis_failures.end(), 0); - double recent_rate = static_cast(recent_failures) / recent_nis_failures.size(); + return (double)recent_failures / recent_nis_failures.size(); + } - data["residual_yaw"] = residual[0]; - data["residual_pitch"] = residual[1]; - data["residual_distance"] = residual[2]; - data["residual_angle"] = residual[3]; - data["nis"] = nis; - data["recent_nis_failures"] = recent_rate; + auto RecordStatistics( + const ZVec& residual, const RMat& S, const XVec& error, const PMat& P_inverse) -> void { - return x; + /// 卡方检验 + // 新增检验 + double nis = residual.transpose() * S.inverse() * residual; + double nees = (error).transpose() * P.inverse() * error; + + total_count_++; + last_nis = nis; + last_nees = nees; + + if (nis > nis_threshold_) nis_count_++; + if (nees > nees_threshold_) nees_count_++; + recent_nis_failures.push_back(nis > nis_threshold_ ? 1 : 0); + if (recent_nis_failures.size() > window_size) { + recent_nis_failures.pop_front(); + } } - std::map data; // 卡方检验数据 + const Eigen::Matrix I; + const EKFModel& model_; + + // 卡方检验阈值（自由度=4，取置信水平95%） + const double nis_threshold_ = 0.711; + const double nees_threshold_ = 0.711; + const double max_failure_rate = 0.4; + + // std::map data; // 卡方检验数据 std::deque recent_nis_failures { 0 }; size_t window_size = 100; double last_nis; - -private: - Eigen::Matrix I; - std::function x_add; - + double last_nees; int nees_count_ = 0; int nis_count_ = 0; int total_count_ = 0; diff --git a/src/tongji/predictor/kalman_filter/predict_model.hpp b/src/tongji/predictor/kalman_filter/predict_model.hpp index 5af8395..03d7734 100644 --- a/src/tongji/predictor/kalman_filter/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -1,20 +1,37 @@ #pragma once -#include +#include #include #include #include #include "enum/car_id.hpp" -#include "extended_kalman_filter.hpp" #include "util/math.hpp" - namespace world_exe::tongji::predictor { -class PredictModel { +template +concept PositiveInteger = std::integral && T > 0; + +template + requires PositiveInteger && PositiveInteger + +class EKFModel { public: - PredictModel(const enumeration::CarIDFlag& car_id) + static constexpr int xn = N_STATE; + static constexpr int zn = N_MEAS; + + using XVec = Eigen::Matrix; + using ZVec = Eigen::Matrix; + using AMat = Eigen::Matrix; + using PMat = Eigen::Matrix; + using PDig = Eigen::Matrix; + using RMat = Eigen::Matrix; + using RDig = Eigen::Matrix; + using QMat = Eigen::Matrix; + using HMat = Eigen::Matrix; + + EKFModel(const enumeration::CarIDFlag& car_id) : car_id_(car_id) { bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII @@ -50,34 +67,58 @@ class PredictModel { } } - auto GetP0Dig() const { return P0_dig_; } - auto GetRadius() const { return radius_; } - auto GetID() const { return car_id_; } - int GetArmorNum() const { return armor_num_; } + auto GetP0Dig() const -> const PDig { return P0_dig_; } + auto GetRadius() const -> const double { return radius_; } + auto GetID() const -> const auto { return car_id_; } + auto GetArmorNum() const -> const int { return armor_num_; } // 防止夹角求和出现异常值 - std::function::XVec( - const ExtendedKalmanFilter<11, 4>::XVec&, const ExtendedKalmanFilter<11, 4>::XVec&)> - x_add = [](const ExtendedKalmanFilter<11, 4>::XVec& a, - const ExtendedKalmanFilter<11, 4>::XVec& b) { - ExtendedKalmanFilter<11, 4>::XVec c = a + b; - c(6) = util::math::clamp_pm_pi(c(6)); - return c; - }; + constexpr auto x_add(const XVec& a, const XVec& b) const -> const auto { + XVec c = a + b; + c(6) = util ::math::clamp_pm_pi(c(6)); + return c; + } + + constexpr auto z_substract(const ZVec& a, const ZVec& b) const -> const auto { + auto c = a - b; + c(0) = util::math::clamp_pm_pi(c(0)); + c(1) = util::math::clamp_pm_pi(c(1)); + c(3) = util::math::clamp_pm_pi(c(3)); + return c; + } + + auto A(double dt) const -> auto const { + // 状态转移矩阵 + AMat _A; + // clang-format off + _A<< + 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; + //clang-format on + return _A; + } + // 防止夹角求和出现异常值 - std::function::XVec( - const ExtendedKalmanFilter<11, 4>::XVec& x, const double& dt)> - f = [this](const ExtendedKalmanFilter<11, 4>::XVec& x, - const double& dt) -> ExtendedKalmanFilter<11, 4>::XVec { - ExtendedKalmanFilter<11, 4>::XVec x_prior = A(dt) * x; - x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); + auto f(const XVec& x, const double& dt)const ->const auto{ + XVec x_prior = this->A(dt) * x; + x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); return x_prior; }; - int MatchArmor(const ExtendedKalmanFilter<11, 4>::XVec& x, - const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - const Eigen::Vector3d& armor_ypd_in_world) const { + + auto MatchArmor(const XVec& x, const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) const + ->const int { const auto& xyza_list = GetArmorXYZAList(x); std::vector> xyza_i_list; @@ -110,7 +151,7 @@ class PredictModel { } // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const ExtendedKalmanFilter<11, 4>::XVec& x, int id) const { + auto h_armor_xyz(const XVec& x, int id) const ->const auto { auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); @@ -119,54 +160,56 @@ class PredictModel { auto armor_y = x(2) - r * std::sin(angle); auto armor_z = (use_l_h) ? x(4) + x(10) : x(4); - return { armor_x, armor_y, armor_z }; + return Eigen::Vector3d { armor_x, armor_y, armor_z }; } - ExtendedKalmanFilter<11, 4>::HMat H(const ExtendedKalmanFilter<11, 4>::XVec& x, int id) const { + constexpr auto H(const XVec& x, int id) const->HMat const { auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); - auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); + auto cos_angle=std::cos(angle); + auto sin_angle=std::sin(angle); + auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); auto r = (use_l_h) ? x(8) + x(9) : x(8); - auto dx_da = r * std::sin(angle); - auto dy_da = -r * std::cos(angle); - auto dx_dr = -std::cos(angle); - auto dy_dr = -std::sin(angle); - auto dx_dl = (use_l_h) ? -std::cos(angle) : 0.0; - auto dy_dl = (use_l_h) ? -std::sin(angle) : 0.0; + auto dx_da = r * sin_angle; + auto dy_da = -r *cos_angle; + + auto dx_dr = -cos_angle; + auto dy_dr = -sin_angle; + auto dx_dl = (use_l_h) ? -cos_angle : 0.0; + auto dy_dl = (use_l_h) ? -sin_angle : 0.0; auto dz_dh = (use_l_h) ? 1.0 : 0.0; // clang-format off - Eigen::MatrixXd H_armor_xyza{ - {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, - {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, - {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, - {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} - }; + Eigen::Matrix + H_armor_xyza{ + {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, + {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, + {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, + {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0} + }; // clang-format on - Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); - Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); + auto armor_xyz = h_armor_xyz(x, id); + auto H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); // clang-format off - Eigen::Matrix H_armor_ypda{ - {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, - {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, - {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, - { 0, 0, 0, 1} - }; + Eigen::MatrixH_armor_ypda; + H_armor_ypda<< + H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0, + H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0, + H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0, + 0 , 0, 0, 1; // clang-format on - return H_armor_ypda * H_armor_xyza; } - ExtendedKalmanFilter<11, 4>::RMat R(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, - int id) { + auto R(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, + const Eigen::Vector3d& armor_ypd_in_world, int id) const -> RMat const { // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; auto center_yaw = std::atan2(armor_xyz_in_world(1), armor_xyz_in_world(0)); auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world(0) - center_yaw); - ExtendedKalmanFilter<11, 4>::RDig R_dig(4); + RDig R_dig(4); R_dig << 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, log(std::abs(armor_ypd_in_world(2)) + 1) / 200 + 9e-2; @@ -174,89 +217,63 @@ class PredictModel { return R_dig.asDiagonal(); } - ExtendedKalmanFilter<11, 4>::AMat A(double dt) const { - // 状态转移矩阵 - ExtendedKalmanFilter<11, 4>::AMat _A; - // clang-format off - _A<< - 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; - //clang-format on - return _A; - } - - -ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { + auto Q(const double& dt) const -> const auto { // Piecewise White Noise Model // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb - double v1, v2; - if (car_id_ == enumeration::CarIDFlag::Outpost) { - v1 = 10; // 前哨站加速度方差 - v2 = 0.1; // 前哨站角加速度方差 - } else { - v1 = 100; // 加速度方差 - v2 = 400; // 角加速度方差 - } - auto a = dt * dt * dt * dt / 4; - auto b = dt * dt * dt / 2; - auto c = dt * dt; - - // 预测过程噪声偏差的方差 - Eigen::Matrix _Q; - // clang-format off - _Q + double v1, v2; + if (car_id_ == enumeration::CarIDFlag::Outpost) { + v1 = 10; // 前哨站加速度方差 + v2 = 0.1; // 前哨站角加速度方差 + } else { + v1 = 100; // 加速度方差 + v2 = 400; // 角加速度方差 + } + auto a = dt * dt * dt * dt / 4; + auto b = dt * dt * dt / 2; + auto c = dt * dt; + + // 预测过程噪声偏差的方差 + QMat _Q; + // clang-format off + _Q << - a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0, - 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; + a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, a * v2, b * v2, 0, 0, 0, + 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; // clang-format on return _Q; } - std::function::ZVec(const ExtendedKalmanFilter<11, 4>::XVec&, int)> - h = [this](const ExtendedKalmanFilter<11, 4>::XVec& x, int id) { - Eigen::Vector3d xyz = this->h_armor_xyz(x, id); - Eigen::Vector3d ypd = util::math::xyz2ypd(xyz); - double angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); - return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; - }; + constexpr auto h(const XVec& x, const int& id) const -> const auto { + auto xyz = this->h_armor_xyz(x, id); + auto ypd = util::math::xyz2ypd(xyz); + auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); + return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; + } // 防止夹角求差出现异常值 - std::function::ZVec( - const ExtendedKalmanFilter<11, 4>::ZVec&, const ExtendedKalmanFilter<11, 4>::ZVec&)> - z_subtract = [](const ExtendedKalmanFilter<11, 4>::ZVec& a, - const ExtendedKalmanFilter<11, 4>::ZVec& b) { - ExtendedKalmanFilter<11, 4>::ZVec c = a - b; - c(0) = util::math::clamp_pm_pi(c(0)); - c(1) = util::math::clamp_pm_pi(c(1)); - c(3) = util::math::clamp_pm_pi(c(3)); - return c; - }; + constexpr auto z_subtract(const ZVec& a, const ZVec& b) const -> const auto { + ZVec c = a - b; + c(0) = util::math::clamp_pm_pi(c(0)); + c(1) = util::math::clamp_pm_pi(c(1)); + c(3) = util::math::clamp_pm_pi(c(3)); + return c; + }; - std::vector GetArmorXYZAList( - const ExtendedKalmanFilter<11, 4>::XVec& ekf_x) const { + constexpr auto GetArmorXYZAList(const XVec& ekf_x) const -> const auto { std::vector _armor_xyza_list; for (int i = 0; i < armor_num_; i++) { - auto angle = util::math::clamp_pm_pi(ekf_x(6) + i * 2 * CV_PI / armor_num_); - Eigen::Vector3d xyz = h_armor_xyz(ekf_x, i); + auto angle = util::math::clamp_pm_pi(ekf_x(6) + i * 2 * CV_PI / armor_num_); + auto xyz = h_armor_xyz(ekf_x, i); _armor_xyza_list.push_back({ xyz(0), xyz(1), xyz(2), angle }); } return _armor_xyza_list; @@ -265,9 +282,7 @@ ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { private: int armor_num_; enumeration::CarIDFlag car_id_; - - ExtendedKalmanFilter<11, 4>::PDig P0_dig_; + PDig P0_dig_; double radius_; }; - -} \ No newline at end of file +} diff --git a/src/tongji/predictor/live_target_manager/live_target.hpp b/src/tongji/predictor/live_target_manager/live_target.hpp index d9369a4..58f0a20 100644 --- a/src/tongji/predictor/live_target_manager/live_target.hpp +++ b/src/tongji/predictor/live_target_manager/live_target.hpp @@ -1,9 +1,10 @@ #pragma once -#include #include #include -#include +#include + +#include #include "../../time_stamp/time_stamp.hpp" #include "../kalman_filter/extended_kalman_filter.hpp" @@ -15,11 +16,13 @@ namespace world_exe::tongji::predictor { class LiveTarget { public: + using PredictorModel = EKFModel<11, 4>; + using EKF = ExtendedKalmanFilter; + LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const enumeration::CarIDFlag& car_id) : last_see_time_stamp_(std::time(nullptr)) , model_(car_id) { - // x vx y vy z vz a w r l h // a: angle // w: angular velocity @@ -31,26 +34,24 @@ class LiveTarget { armor_xyz_in_world[1] + model_.GetRadius() * std::sin(armor_ypr_in_world[0]); auto center_z = armor_xyz_in_world[2]; - ExtendedKalmanFilter<11, 4>::XVec x0; - x0 << center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, model_.GetRadius(), - 0, 0; + EKF::XVec x0 { center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, + model_.GetRadius(), 0, 0 }; - ExtendedKalmanFilter<11, 4>::PMat P0 = model_.GetP0Dig().asDiagonal(); - ekf_ = ExtendedKalmanFilter<11, 4>( - x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） + EKF::PMat P0 = model_.GetP0Dig().asDiagonal(); + ekf_.emplace(x0, P0, model_); // 初始化滤波器（预测量、预测量协方差） } - ExtendedKalmanFilter<11, 4>::XVec GetEkfX() const { return ekf_.x; } - ExtendedKalmanFilter<11, 4>::PDig GetP0Dig() const { return model_.GetP0Dig(); } - const PredictModel& GetModel() const { return model_; } + EKF::XVec GetEkfX() const { return ekf_->x; } + EKF::PDig GetP0Dig() const { return model_.GetP0Dig(); } + const PredictorModel& GetModel() const { return model_; } time_stamp::TimeStamp LastSeen() const { return time_stamp::TimeStamp(last_see_time_stamp_); } std::vector GetArmorGimbalControlSpacings() const { std::vector armors; for (int id = 0; id < model_.GetArmorNum(); id++) { auto angle = - util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); - auto xyz = model_.h_armor_xyz(this->ekf_.x, id); + util::math::clamp_pm_pi(this->ekf_->x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto xyz = model_.h_armor_xyz(this->ekf_->x, id); data::ArmorGimbalControlSpacing armor; armor.id = model_.GetID(); @@ -65,75 +66,64 @@ class LiveTarget { const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { // 装甲板匹配 int id = - model_.MatchArmor(ekf_.x, armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); - last_id = id; - update_count++; + model_.MatchArmor(ekf_->x, armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); + last_id_ = id; + update_count_++; Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id, dt); last_see_time_stamp_ = std::time(nullptr); } - bool IsConverged() const { return EvaluateConvergence(); } - -private: - bool EvaluateConvergence() const { + bool IsConverged() const { + if (!ekf_.has_value()) return false; // 前哨站特殊判断 const int required_count = (model_.GetID() == enumeration::CarIDFlag::Outpost) ? 10 : 3; - if (update_count < required_count) return false; - if (EvaluateDivergence()) return false; - - auto nis_failures = - std::accumulate(ekf_.recent_nis_failures.begin(), ekf_.recent_nis_failures.end(), 0); - if (nis_failures > 0.4 * ekf_.window_size) return false; - + if (update_count_ < required_count || IsDivergened()) return false; return true; } - bool EvaluateDivergence() const { - auto r_ok = ekf_.x[8] > 0.05 && ekf_.x[8] < 0.5; - auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; +private: + bool IsDivergened() const { + auto r_ok = ekf_->x[8] > 0.05 && ekf_->x[8] < 0.5; + auto l_ok = ekf_->x[8] + ekf_->x[9] > 0.05 && ekf_->x[8] + ekf_->x[9] < 0.5; if (r_ok && l_ok) return false; - // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); + // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_->x[8], ekf_->x[9]); return true; } - + // TODO:need to update correctly void Update_ypda(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, const int& id, const double& dt) { // 观测jacobi - auto H = model_.H(ekf_.x, id); + auto H = model_.H(ekf_->x, id); auto R = model_.R(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); - auto A = model_.A(dt); - auto Q = model_.Q(dt); - auto f = model_.f; - auto h = [this, id](const ExtendedKalmanFilter<11, 4>::XVec& x) { return model_.h(x, id); }; - auto z_subtract = model_.z_subtract; const Eigen::Vector3d& ypd = armor_ypd_in_world; const Eigen::Vector3d& ypr = armor_ypr_in_world; // 获得观测量 - ExtendedKalmanFilter<11, 4>::ZVec z(4); + EKF::ZVec z(4); z << ypd[0], ypd[1], ypd[2], ypr[0]; - ekf_.Update(dt, A, Q, f, z, H, R, h, z_subtract); + ekf_->Update(dt, z, H, R, id); // 前哨站转速特判 - if (model_.GetID() == enumeration::CarIDFlag::Outpost && EvaluateConvergence()) { + if (model_.GetID() == enumeration::CarIDFlag::Outpost) { constexpr double max_outpost_w = 2.51; - if (std::abs(ekf_.x[7]) > 2.0) { - ekf_.x[7] = ekf_.x[7] > 0 ? max_outpost_w : -max_outpost_w; + if (std::abs(ekf_->x[7]) > 2.0) { + ekf_->x[7] = ekf_->x[7] > 0 ? max_outpost_w : -max_outpost_w; } } } std::time_t last_see_time_stamp_; - ExtendedKalmanFilter<11, 4> ekf_; - PredictModel model_; + PredictorModel model_; + std::optional ekf_; - int last_id = -1; - int update_count = 0; + int last_id_ = -1; + int update_count_ = 0; + const double max_allowed_failure_rate_ = 0.4; }; -} \ No newline at end of file +} diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp index a1fa6bf..363314e 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp @@ -11,9 +11,12 @@ namespace world_exe::tongji::predictor { class TargetSnapshot { public: + using PredictorModel = EKFModel<11, 4>; + using EKF = ExtendedKalmanFilter; + TargetSnapshot(const LiveTarget& target) : model_(target.GetModel()) - , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) + , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_) , time_stamp_(target.LastSeen()) { } // std::vector GetArmorGimbalControlSpacings() const { @@ -46,9 +49,9 @@ class TargetSnapshot { } private: - PredictModel model_; - ExtendedKalmanFilter<11, 4> ekf_; + PredictorModel model_; + ExtendedKalmanFilter ekf_; time_stamp::TimeStamp time_stamp_; }; -} \ No newline at end of file +} diff --git a/src/tongji/solver/solved_armor.hpp b/src/tongji/solver/solved_armor.hpp index 43a79eb..052490b 100644 --- a/src/tongji/solver/solved_armor.hpp +++ b/src/tongji/solver/solved_armor.hpp @@ -27,7 +27,7 @@ class SolvedArmor final : public interfaces::IArmorInCamera, public interfaces:: } private: - std::time_t time_stamp_ { 0 }; + std::time_t time_stamp_ { std::time(nullptr) }; std::array, 8> armors_; }; -} \ No newline at end of file +} From 94e19fc36545b3d5d44fe2e3c2a53785d8cbb005 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 23 Oct 2025 22:44:21 +0800 Subject: [PATCH 56/93] fix(pimpl): correct PIMPL implementation issues --- src/tongji/fire_controller/fire_controller.cpp | 10 +++++----- src/tongji/fire_controller/fire_controller.hpp | 4 ++-- src/tongji/identifier/identifier.cpp | 4 ++-- src/tongji/identifier/identifier.hpp | 5 +++-- .../live_target_manager/live_target_manager.cpp | 6 +++--- .../live_target_manager/live_target_manager.hpp | 5 +++-- .../target_snapshot_manager.cpp | 6 +++--- .../target_snapshot_manager.hpp | 6 +++--- src/tongji/solver/solver.cpp | 6 +++--- src/tongji/solver/solver.hpp | 6 +++--- src/tongji/state_machine/state_machine.cpp | 6 +++--- src/tongji/state_machine/state_machine.hpp | 5 +++-- 12 files changed, 36 insertions(+), 33 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index b178e0b..7fb6b06 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -21,10 +21,10 @@ using CarIDFlag = enumeration::CarIDFlag; using LiveTargetManager = predictor::LiveTargetManager; using TimeStamp = time_stamp::TimeStamp; -class FireController::Impl { +class FireControllerImpl { public: - Impl(bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, - double yaw_offset, double pitch_offset, + FireControllerImpl(bool auto_fire, const double& control_delay_in_second, + const double& bullet_speed, double yaw_offset, double pitch_offset, std::shared_ptr state_machine, std::shared_ptr live_target_manager) : control_delay_(control_delay_in_second) @@ -104,8 +104,8 @@ class FireController::Impl { FireController::FireController(std::shared_ptr state_machine, bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, double pitch_offset, std::shared_ptr live_target_manager) - : pimpl_(std::make_unique(auto_fire, control_delay_in_second, bullet_speed, yaw_offset, - pitch_offset, state_machine, live_target_manager)) { } + : pimpl_(std::make_unique(auto_fire, control_delay_in_second, bullet_speed, + yaw_offset, pitch_offset, state_machine, live_target_manager)) { } const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { return pimpl_->CalculateTarget(time_duration); diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 19343e0..ab76f3d 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -10,6 +10,7 @@ namespace world_exe::tongji::fire_control { +class FireControllerImpl; class FireController final : public interfaces::IFireControl { public: FireController(std::shared_ptr state_machine, bool auto_fire, @@ -24,8 +25,7 @@ class FireController final : public interfaces::IFireControl { time_stamp::TimeStamp GetTimeStamp() const; private: - class Impl; - std::unique_ptr pimpl_; + std::unique_ptr pimpl_; }; } \ No newline at end of file diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index f70af8f..0d0f50d 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -25,9 +25,9 @@ namespace world_exe::tongji::identifier { -class Identifier::Impl { +class IdentifierImpl { public: - explicit Impl(const std::string& model_path, const int& model_image_width, + explicit IdentifierImpl(const std::string& model_path, const int& model_image_width, const int& model_image_height, const double& threshold, const double& max_angle_error, const double& min_lightbar_ratio, const double& max_lightbar_ratio, const double& min_lightbar_length, const double& max_armor_ratio, diff --git a/src/tongji/identifier/identifier.hpp b/src/tongji/identifier/identifier.hpp index 01e73a4..f3b246a 100644 --- a/src/tongji/identifier/identifier.hpp +++ b/src/tongji/identifier/identifier.hpp @@ -37,6 +37,8 @@ struct Lightbar { this->ratio = length / width; } }; + +class IdentifierImpl; class Identifier final : public interfaces::IIdentifier { public: Identifier(const std::string& model_path, int model_image_width, int model_image_height); @@ -48,8 +50,7 @@ class Identifier final : public interfaces::IIdentifier { void SetTargetColor(Color target_color); private: - class Impl; - std::unique_ptr pimpl_; + std::unique_ptr pimpl_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp index bf1da76..c26efed 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -17,9 +17,9 @@ namespace world_exe::tongji::predictor { -class LiveTargetManager::Impl { +class LiveTargetManagerImpl { public: - Impl(const double& time_delay, const double& yaw_offset, const double& pitch_offset, + LiveTargetManagerImpl(const double& time_delay, const double& yaw_offset, const double& pitch_offset, double timeout_sec = 0.1) : targets_map_() , tracker_(std::make_unique()) @@ -127,7 +127,7 @@ class LiveTargetManager::Impl { LiveTargetManager::LiveTargetManager(const double& time_delay, const double& yaw_offset, const double& pitch_offset, double timeout_sec) - : pimpl_(std::make_unique(time_delay, yaw_offset, pitch_offset, timeout_sec)) { } + : pimpl_(std::make_unique(time_delay, yaw_offset, pitch_offset, timeout_sec)) { } LiveTargetManager::~LiveTargetManager() = default; std ::shared_ptr LiveTargetManager::Predict( diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.hpp b/src/tongji/predictor/live_target_manager/live_target_manager.hpp index 88ebfce..1d830e0 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.hpp @@ -9,6 +9,7 @@ namespace world_exe::tongji::predictor { + class LiveTargetManagerImpl; class LiveTargetManager final : public interfaces::ITargetPredictor { public: LiveTargetManager(const double& time_delay, const double& yaw_offset, @@ -27,8 +28,8 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { auto GetAllowedTargetID() const -> enumeration::ArmorIdFlag const; private: - class Impl; - std::unique_ptr pimpl_; + + std::unique_ptr pimpl_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp index e26d631..fa1c521 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -12,9 +12,9 @@ namespace world_exe::tongji::predictor { -class TargetSnapshotManager::Impl { +class TargetSnapshotManagerImpl { public: - Impl(const enumeration::ArmorIdFlag& id, + TargetSnapshotManagerImpl(const enumeration::ArmorIdFlag& id, const std::unordered_map>& live_target_map, const std::time_t& now, const double& bullet_speed, const double& yaw_offset, @@ -82,7 +82,7 @@ TargetSnapshotManager::TargetSnapshotManager(const enumeration::ArmorIdFlag& id, live_target_map, const std::time_t& now, const double& bullet_speed, const double& yaw_offset, const double& pitch_offset) - : pimpl_(std::make_unique( + : pimpl_(std::make_unique( id, live_target_map, now, bullet_speed, yaw_offset, pitch_offset)) { } TargetSnapshotManager::~TargetSnapshotManager() = default; diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp index 3176e9a..e438fb6 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp @@ -8,7 +8,7 @@ #include "interfaces/predictor.hpp" namespace world_exe::tongji::predictor { - + class TargetSnapshotManagerImpl; struct GimbalCommand { double yaw; double pitch; @@ -30,7 +30,7 @@ class TargetSnapshotManager final : public interfaces::IPredictor { auto GetGimbalCommand() const -> GimbalCommand const; private: - class Impl; - std::unique_ptr pimpl_; + + std::unique_ptr pimpl_; }; } diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 2202025..119da52 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -14,9 +14,9 @@ #include "util/math.hpp" namespace world_exe::tongji::solver { -class Solver::Impl { +class SolverImpl { public: - Impl(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, + SolverImpl(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, Eigen::Vector3d t_camera2gimbal) : R_camera2gimbal_(R_camera2gimbal) , R_gimbal2world_(R_gimbal2world) @@ -300,7 +300,7 @@ class Solver::Impl { Solver::Solver(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, Eigen::Vector3d t_camera2gimbal) - : pimpl_(std::make_unique(R_camera2gimbal, R_gimbal2world, t_camera2gimbal)) { } + : pimpl_(std::make_unique(R_camera2gimbal, R_gimbal2world, t_camera2gimbal)) { } Solver::~Solver() = default; const std::time_t& Solver::GetTimeStamp() const { return pimpl_->GetTimeStamp(); } diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index 5f6c2f4..02ae0a4 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -4,7 +4,7 @@ #include "interfaces/time_stamped.hpp" namespace world_exe::tongji::solver { - + class SolverImpl; class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStamped { public: explicit Solver(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, @@ -17,8 +17,8 @@ class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStam const std::time_t& GetTimeStamp() const override; private: - class Impl; - std::unique_ptr pimpl_; + + std::unique_ptr pimpl_; }; } \ No newline at end of file diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 990bf99..c6a70ea 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -8,9 +8,9 @@ namespace world_exe::tongji::state_machine { -class StateMachine::Impl { +class StateMachineImpl { public: - Impl(std::shared_ptr live_target_manager) + StateMachineImpl(std::shared_ptr live_target_manager) : live_target_manager_(std::move(live_target_manager)) { } const enumeration::CarIDFlag& GetAllowdToFires() const { return target_ids_; } @@ -28,7 +28,7 @@ class StateMachine::Impl { StateMachine::StateMachine( std::shared_ptr live_target_manager) - : pimpl_(std::make_unique(live_target_manager)) { } + : pimpl_(std::make_unique(live_target_manager)) { } StateMachine::~StateMachine() = default; const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index c3417e3..54a6c80 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -8,6 +8,7 @@ #include "interfaces/target_predictor.hpp" namespace world_exe::tongji::state_machine { + class StateMachineImpl; class StateMachine final : public interfaces::ICarState { public: StateMachine(); @@ -17,7 +18,7 @@ class StateMachine final : public interfaces::ICarState { StateMachine(std::shared_ptr live_target_manager); private: - class Impl; - std::unique_ptr pimpl_; + + std::unique_ptr pimpl_; }; } \ No newline at end of file From 3e50b686846956566967355fd9a38f5c6f8e1180 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 23 Oct 2025 22:59:36 +0800 Subject: [PATCH 57/93] fix(pimpl): correct modern C++ PIMPL implementation --- src/tongji/fire_controller/fire_controller.hpp | 5 +++++ src/tongji/identifier/identifier.hpp | 5 +++++ .../predictor/live_target_manager/live_target_manager.hpp | 8 ++++++-- .../target_snapshot_manager/target_snapshot_manager.hpp | 8 ++++++-- src/tongji/solver/solver.hpp | 8 ++++++-- src/tongji/state_machine/state_machine.hpp | 8 ++++++-- 6 files changed, 34 insertions(+), 8 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index ab76f3d..e20b920 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -24,6 +24,11 @@ class FireController final : public interfaces::IFireControl { time_stamp::TimeStamp GetTimeStamp() const; + FireController(const FireController&) = delete; + FireController& operator=(const FireController&) = delete; + FireController(FireController&&) noexcept = default; + FireController& operator=(FireController&&) noexcept = default; + private: std::unique_ptr pimpl_; }; diff --git a/src/tongji/identifier/identifier.hpp b/src/tongji/identifier/identifier.hpp index f3b246a..726b32e 100644 --- a/src/tongji/identifier/identifier.hpp +++ b/src/tongji/identifier/identifier.hpp @@ -49,6 +49,11 @@ class Identifier final : public interfaces::IIdentifier { void SetTargetColor(Color target_color); + Identifier(const Identifier&) = delete; + Identifier& operator=(const Identifier&) = delete; + Identifier(Identifier&&) noexcept = default; + Identifier& operator=(Identifier&&) noexcept = default; + private: std::unique_ptr pimpl_; }; diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.hpp b/src/tongji/predictor/live_target_manager/live_target_manager.hpp index 1d830e0..5c3e315 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.hpp @@ -9,7 +9,7 @@ namespace world_exe::tongji::predictor { - class LiveTargetManagerImpl; +class LiveTargetManagerImpl; class LiveTargetManager final : public interfaces::ITargetPredictor { public: LiveTargetManager(const double& time_delay, const double& yaw_offset, @@ -27,8 +27,12 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { auto GetAllowedTargetID() const -> enumeration::ArmorIdFlag const; + LiveTargetManager(const LiveTargetManager&) = delete; + LiveTargetManager& operator=(const LiveTargetManager&) = delete; + LiveTargetManager(LiveTargetManager&&) noexcept = default; + LiveTargetManager& operator=(LiveTargetManager&&) noexcept = default; + private: - std::unique_ptr pimpl_; }; diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp index e438fb6..2c17fef 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp @@ -8,7 +8,7 @@ #include "interfaces/predictor.hpp" namespace world_exe::tongji::predictor { - class TargetSnapshotManagerImpl; +class TargetSnapshotManagerImpl; struct GimbalCommand { double yaw; double pitch; @@ -29,8 +29,12 @@ class TargetSnapshotManager final : public interfaces::IPredictor { auto GetGimbalCommand() const -> GimbalCommand const; + TargetSnapshotManager(const TargetSnapshotManager&) = delete; + TargetSnapshotManager& operator=(const TargetSnapshotManager&) = delete; + TargetSnapshotManager(TargetSnapshotManager&&) noexcept = default; + TargetSnapshotManager& operator=(TargetSnapshotManager&&) noexcept = default; + private: - std::unique_ptr pimpl_; }; } diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index 02ae0a4..41e9566 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -4,7 +4,7 @@ #include "interfaces/time_stamped.hpp" namespace world_exe::tongji::solver { - class SolverImpl; +class SolverImpl; class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStamped { public: explicit Solver(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, @@ -16,8 +16,12 @@ class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStam const std::time_t& GetTimeStamp() const override; + Solver(const Solver&) = delete; + Solver& operator=(const Solver&) = delete; + Solver(Solver&&) noexcept = default; + Solver& operator=(Solver&&) noexcept = default; + private: - std::unique_ptr pimpl_; }; diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 54a6c80..b5d1f73 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -8,7 +8,7 @@ #include "interfaces/target_predictor.hpp" namespace world_exe::tongji::state_machine { - class StateMachineImpl; +class StateMachineImpl; class StateMachine final : public interfaces::ICarState { public: StateMachine(); @@ -17,8 +17,12 @@ class StateMachine final : public interfaces::ICarState { const enumeration ::CarIDFlag& GetAllowdToFires() const override; StateMachine(std::shared_ptr live_target_manager); + StateMachine(const StateMachine&) = delete; + StateMachine& operator=(const StateMachine&) = delete; + StateMachine(StateMachine&&) noexcept = default; + StateMachine& operator=(StateMachine&&) noexcept = default; + private: - std::unique_ptr pimpl_; }; } \ No newline at end of file From 3d6ce85f29ca0fe4c3fc48c8fc34288e627fb658 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 24 Oct 2025 01:27:14 +0800 Subject: [PATCH 58/93] refactor(config): load identifier parameters from YAML configuration --- configs/example.yaml | 16 +++++++++ src/tongji/identifier/classifier.hpp | 19 ++++++---- src/tongji/identifier/identifier.cpp | 52 +++++++++++++++------------- src/tongji/identifier/identifier.hpp | 3 +- src/tongji/solver/solver.cpp | 2 +- 5 files changed, 60 insertions(+), 32 deletions(-) create mode 100644 configs/example.yaml diff --git a/configs/example.yaml b/configs/example.yaml new file mode 100644 index 0000000..db7735d --- /dev/null +++ b/configs/example.yaml @@ -0,0 +1,16 @@ +#####-----classfier parameters-----##### +classify_model: +model_image_width: +model_image_height: + +#####-----identifier parameters-----##### +threshold: 150 +max_angle_error: 45 #degree +min_lightbar_ratio: 1.5 +max_lightbar_ratio: 20 +min_lightbar_length: 8 +max_armor_ratio: 5 +min_armor_ratio: 1 +max_side_ratio: 1.5 +min_confidence: 0.8 +max_rectangular_error: 25 #degree \ No newline at end of file diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp index 47924f3..7bcf8bd 100644 --- a/src/tongji/identifier/classifier.hpp +++ b/src/tongji/identifier/classifier.hpp @@ -5,20 +5,27 @@ #include #include #include +#include +#include +#include #include "enum/armor_id.hpp" namespace world_exe::tongji::identifier { class Classifier final { public: - explicit Classifier( - const std::string& model_path, int model_image_width, int model_image_height) - : model_image_width_(model_image_width) - , model_image_height_(model_image_height) { + explicit Classifier(const std::string& config_path) { + const auto yaml = YAML::Load(config_path); + const auto model_path = yaml["classify_model"].as(); + model_image_width_ = yaml["model_image_width"].as(); + model_image_height_ = yaml["model_image_height"].as(); + net_ = cv::dnn::readNetFromONNX(model_path); auto ovmodel = core_.read_model(model_path); compiled_model_ = core_.compile_model( ovmodel, "AUTO", ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY)); + + // TODO:需要对模型做适配 } void Classify(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag& armor_id, @@ -147,7 +154,7 @@ class Classifier final { ov::Core core_; ov::CompiledModel compiled_model_; - const int model_image_height_ = 640; - const int model_image_width_ = 640; + int model_image_height_ = 640; + int model_image_width_ = 640; }; } \ No newline at end of file diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index 0d0f50d..a8f9faf 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include @@ -14,12 +14,14 @@ #include #include #include +#include +#include +#include "../identifier/classifier.hpp" #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" #include "identified_armor.hpp" #include "interfaces/armor_in_image.hpp" -#include "../identifier/classifier.hpp" #include "util/logger.hpp" #include "util/stringifier.hpp" @@ -27,28 +29,25 @@ namespace world_exe::tongji::identifier { class IdentifierImpl { public: - explicit IdentifierImpl(const std::string& model_path, const int& model_image_width, - const int& model_image_height, const double& threshold, const double& max_angle_error, - const double& min_lightbar_ratio, const double& max_lightbar_ratio, - const double& min_lightbar_length, const double& max_armor_ratio, - const double& min_armor_ratio, const double& max_side_ratio, const double& min_confidence, - const double& max_rectangular_error, const std::string& save_path, const bool& debug = true, - const bool& record = true) - : classifier_(model_path, model_image_width, model_image_height) - , threshold_(threshold) - , max_angle_error_(max_angle_error / 57.3) // degree to rad - , min_lightbar_ratio_(min_lightbar_ratio) - , max_lightbar_ratio_(max_lightbar_ratio) - , min_lightbar_length_(min_lightbar_length) - , max_armor_ratio_(max_armor_ratio) - , min_armor_ratio_(min_armor_ratio) - , max_side_ratio_(max_side_ratio) - , min_confidence_(min_confidence) - , max_rectangular_error_(max_rectangular_error / 57.3) // degree to rad - , save_path_(std::move(save_path)) + explicit IdentifierImpl(const std::string& config_path, const std::string& save_path, + const bool& debug, const bool& record) + : classifier_(std::make_unique(config_path)) + , save_path_(save_path) , debug_(debug) , record_(record) , target_color_(blue) { + const auto yaml = YAML::Load(config_path); + + threshold_ = yaml["threshold"].as(); + max_angle_error_ = yaml["max_angle_error"].as() / 57.3; // degree to rad + min_lightbar_ratio_ = yaml["min_lightbar_ratio"].as(); + max_lightbar_ratio_ = yaml["max_lightbar_ratio"].as(); + min_lightbar_length_ = yaml["min_lightbar_length"].as(); + max_armor_ratio_ = yaml["max_armor_ratio"].as(); + min_armor_ratio_ = yaml["min_armor_ratio"].as(); + max_side_ratio_ = yaml["max_side_ratio"].as(); + min_confidence_ = yaml["min_confidence"].as(); + max_rectangular_error_ = yaml["max_rectangular_error"].as() / 57.3; // degree to rad if (!std::filesystem::exists(save_path_)) std::filesystem::create_directories(save_path_); } @@ -131,7 +130,7 @@ class IdentifierImpl { armor_candidate.pattern = GetPattern(bgr_img, *left, *right); - classifier_.Classify( + classifier_->Classify( armor_candidate.pattern, armor_candidate.id, armor_candidate.confidence); if (!CheckName( @@ -322,10 +321,10 @@ class IdentifierImpl { } private: - Classifier classifier_; + std::unique_ptr classifier_; double threshold_; - double max_angle_error_; + double max_angle_error_; // rad double min_lightbar_ratio_, max_lightbar_ratio_; double min_lightbar_length_; double min_armor_ratio_, max_armor_ratio_; @@ -340,6 +339,11 @@ class IdentifierImpl { std::string save_path_; }; +Identifier::Identifier(const std::string& config_path, const std::string& save_path, + const bool& debug, const bool& record) + : pimpl_(std::make_unique(config_path, save_path, debug, record)) { } +Identifier::~Identifier() = default; + const std::tuple, enumeration::CarIDFlag> Identifier::identify(const cv::Mat& input_image) { return pimpl_->Identify(input_image); diff --git a/src/tongji/identifier/identifier.hpp b/src/tongji/identifier/identifier.hpp index 726b32e..8ca44ce 100644 --- a/src/tongji/identifier/identifier.hpp +++ b/src/tongji/identifier/identifier.hpp @@ -41,7 +41,8 @@ struct Lightbar { class IdentifierImpl; class Identifier final : public interfaces::IIdentifier { public: - Identifier(const std::string& model_path, int model_image_width, int model_image_height); + explicit Identifier(const std::string& config_path, const std::string& save_path, + const bool& debug = true, const bool& record = false); ~Identifier(); const std::tuple, enumeration::CarIDFlag> diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 119da52..7469209 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -69,7 +69,7 @@ class SolverImpl { armor_in_camera.orientation = orientation_ros; if (armor_in_camera.position.norm() > MaxArmorDistance) { - return {}; + return { }; } return armor_in_camera; } From b95cf4c2c21432356e09d851e275b31f1808f2a4 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 24 Oct 2025 02:42:42 +0800 Subject: [PATCH 59/93] refactor(config):Convert fire_controller and predictor constructor to load parameters from YAML config --- configs/example.yaml | 27 +++++++++++- env/ubuntu22.04.sh | 2 +- .../fire_controller/fire_controller.cpp | 25 ++++++----- .../fire_controller/fire_controller.hpp | 7 ++-- src/tongji/fire_controller/fire_decision.hpp | 19 +++++---- .../live_target_manager.cpp | 20 ++++----- .../live_target_manager.hpp | 4 +- .../aim_point_chooser.hpp | 17 ++++---- .../target_snapshot_manager/aim_solver.hpp | 16 ++++--- .../target_snapshot_manager.cpp | 42 +++++++++++++------ .../target_snapshot_manager.hpp | 5 +-- 11 files changed, 119 insertions(+), 65 deletions(-) diff --git a/configs/example.yaml b/configs/example.yaml index db7735d..7d2d05c 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -13,4 +13,29 @@ max_armor_ratio: 5 min_armor_ratio: 1 max_side_ratio: 1.5 min_confidence: 0.8 -max_rectangular_error: 25 #degree \ No newline at end of file +max_rectangular_error: 25 #degree + +#####-----fire_decision parameters-----##### +auto_fire: true +first_tolerance: 100 # 近距离射击容差，degree +second_tolerance: 100 # 远距离射击容差，degree +judge_distance: 2 #距离判断阈值 + +#####-----fire_controller parameters-----##### + + +#####-----target_snapshot_manager parameters-----##### +decision_speed: 7 # rad/s +high_speed_delay_time: 0.050 # s +low_speed_delay_time: 0.015 # s + +#####-----live_target_manager parameters-----##### +yaw_offset: 1.5 # degree +pitch_offset: -0.5 # degree + +#####-----aime_point_chooser parameters-----##### +comming_angle: 60 # degree +leaving_angle: 20 # degree + + + diff --git a/env/ubuntu22.04.sh b/env/ubuntu22.04.sh index c838f97..0a0e144 100644 --- a/env/ubuntu22.04.sh +++ b/env/ubuntu22.04.sh @@ -13,7 +13,7 @@ rm GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB echo "deb https://apt.repos.intel.com/openvino ubuntu22 main" | sudo tee /etc/apt/sources.list.d/intel-openvino.list sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys BAC6F0C353D04109 sudo apt-get update -sudo apt-get install -y libtbb-dev libeigen3-dev libopencv-dev openvino gcc-13 g++-13 libceres-dev libdwarf-dev libbackward-cpp-dev binutils-dev libdw-dev libunwind-dev libfmt-dev libspdlog-dev +sudo apt-get install -y libtbb-dev libeigen3-dev libopencv-dev openvino gcc-13 g++-13 libceres-dev libdwarf-dev libbackward-cpp-dev binutils-dev libdw-dev libunwind-dev libfmt-dev libspdlog-dev libyaml-cpp-dev sudo ln -s /usr/include/libdwarf/libdwarf.h /usr/include/libdwarf.h sudo ln -s /usr/include/libdwarf/dwarf.h /usr/include/dwarf.h diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 7fb6b06..d654ae8 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -1,7 +1,8 @@ #include "fire_controller.hpp" #include -#include + +#include #include "../identifier/identified_armor.hpp" #include "../predictor/live_target_manager/live_target_manager.hpp" @@ -23,8 +24,8 @@ using TimeStamp = time_stamp::TimeStamp; class FireControllerImpl { public: - FireControllerImpl(bool auto_fire, const double& control_delay_in_second, - const double& bullet_speed, double yaw_offset, double pitch_offset, + FireControllerImpl(const std::string& config_path, const double& control_delay_in_second, + const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, std::shared_ptr state_machine, std::shared_ptr live_target_manager) : control_delay_(control_delay_in_second) @@ -32,9 +33,12 @@ class FireControllerImpl { , allowed_target_id_(CarIDFlag::None) , firable_(false) , time_stamp_(std::time(nullptr)) - , fire_decision_(std::make_unique(auto_fire)) + , fire_decision_(std::make_unique(config_path)) , state_machine_(state_machine) - , live_target_manager_(std::move(live_target_manager)) { } + , live_target_manager_(live_target_manager) { + + auto yaml = YAML::LoadFile(config_path); + } // TODO:std::time_t const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { @@ -101,11 +105,12 @@ class FireControllerImpl { std::shared_ptr live_target_manager_; }; -FireController::FireController(std::shared_ptr state_machine, bool auto_fire, - const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, - double pitch_offset, std::shared_ptr live_target_manager) - : pimpl_(std::make_unique(auto_fire, control_delay_in_second, bullet_speed, - yaw_offset, pitch_offset, state_machine, live_target_manager)) { } +FireController::FireController(const std::string& config_path, + const double& control_delay_in_second, const double& bullet_speed, const double& yaw_offset, + const double& pitch_offset, std::shared_ptr state_machine, + std::shared_ptr live_target_manager) + : pimpl_(std::make_unique(config_path, control_delay_in_second, + bullet_speed, yaw_offset, pitch_offset, state_machine, live_target_manager)) { } const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { return pimpl_->CalculateTarget(time_duration); diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index e20b920..ccae0db 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -13,9 +13,10 @@ namespace world_exe::tongji::fire_control { class FireControllerImpl; class FireController final : public interfaces::IFireControl { public: - FireController(std::shared_ptr state_machine, bool auto_fire, - const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, - double pitch_offset, std::shared_ptr live_target_manager); + FireController(const std::string& config_path, const double& control_delay_in_second, + const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, + std::shared_ptr state_machine, + std::shared_ptr live_target_manager); const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index 74ceaac..ca47c40 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -3,21 +3,24 @@ #include #include +#include + #include "tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp" namespace world_exe::tongji::fire_control { class FireDecision { public: - explicit FireDecision(const bool& auto_fire, const double& first_tolerance = 5, - const double& second_tolerance = 2, const double& judge_distance = 3) - : auto_fire_(auto_fire) - , last_gimbal_command_({ std::numeric_limits::quiet_NaN(), + explicit FireDecision(const std::string& config_path) + : last_gimbal_command_({ std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN() }) - , gimbal_yaw_(std::numeric_limits::quiet_NaN()) - , first_tolerance_(first_tolerance) - , second_tolerance_(second_tolerance) - , judge_distance_(judge_distance) { } + , gimbal_yaw_(std::numeric_limits::quiet_NaN()) { + auto yaml = YAML::LoadFile(config_path); + auto_fire_ = yaml["auto_fire"].as(); + first_tolerance_ = yaml["first_tolerance"].as() / 57.3; // degree to rad + second_tolerance_ = yaml["second_tolerance"].as() / 57.3; // degree to rad + judge_distance_ = yaml["judge_distance"].as(); + } bool ShouldFire( predictor::GimbalCommand gimbal_command, const Eigen::Vector3d& valid_target_pos) { diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp index c26efed..5a2642b 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -5,6 +5,8 @@ #include #include +#include + #include "../in_gimbal_control_armor.hpp" #include "../target_snapshot_manager/target_snapshot_manager.hpp" #include "enum/armor_id.hpp" @@ -19,15 +21,14 @@ namespace world_exe::tongji::predictor { class LiveTargetManagerImpl { public: - LiveTargetManagerImpl(const double& time_delay, const double& yaw_offset, const double& pitch_offset, - double timeout_sec = 0.1) + LiveTargetManagerImpl( + const std::string& config_path, const double& time_delay, const double& timeout_sec) : targets_map_() , tracker_(std::make_unique()) , last_update_timestamp_(std::time(nullptr)) , tracking_id_(enumeration::CarIDFlag::None) , time_delay_(time_delay) - , yaw_offset_(yaw_offset) - , pitch_offset_(pitch_offset) { } + , config_path_(config_path) { } std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { @@ -51,7 +52,7 @@ class LiveTargetManagerImpl { if (targets_map_.empty()) return nullptr; // TODO return std::make_shared( - flag, targets_map_, last_update_timestamp_, bullet_speed_, yaw_offset_, pitch_offset_); + config_path_, flag, targets_map_, last_update_timestamp_, bullet_speed_); } void Update(std::shared_ptr data, @@ -121,13 +122,12 @@ class LiveTargetManagerImpl { double bullet_speed_; const double time_delay_; - const double yaw_offset_; - const double pitch_offset_; + const std::string config_path_; }; -LiveTargetManager::LiveTargetManager(const double& time_delay, const double& yaw_offset, - const double& pitch_offset, double timeout_sec) - : pimpl_(std::make_unique(time_delay, yaw_offset, pitch_offset, timeout_sec)) { } +LiveTargetManager::LiveTargetManager( + const std::string& config_path, const double& time_delay, const double& timeout_sec) + : pimpl_(std::make_unique(config_path, time_delay, timeout_sec)) { } LiveTargetManager::~LiveTargetManager() = default; std ::shared_ptr LiveTargetManager::Predict( diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.hpp b/src/tongji/predictor/live_target_manager/live_target_manager.hpp index 5c3e315..039f725 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.hpp @@ -12,8 +12,8 @@ namespace world_exe::tongji::predictor { class LiveTargetManagerImpl; class LiveTargetManager final : public interfaces::ITargetPredictor { public: - LiveTargetManager(const double& time_delay, const double& yaw_offset, - const double& pitch_offset, double timeout_sec = 0.1); + LiveTargetManager( + const std::string& config_path, const double& time_delay, const double& timeout_sec=0.1); ~LiveTargetManager(); std ::shared_ptr Predict( diff --git a/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp b/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp index 2f2c15d..6d46bee 100644 --- a/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp +++ b/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp @@ -3,16 +3,19 @@ #include "enum/car_id.hpp" #include "util/math.hpp" +#include + namespace world_exe::tongji::predictor { using CarIDFlag = enumeration::CarIDFlag; class AimPointChooser { public: - AimPointChooser( - const double& comming_angle = 60 / 57.3, const double& leaving_angle = 20 / 57.3) - : comming_angle_(comming_angle) - , leaving_angle_(leaving_angle) { } + AimPointChooser(const std::string& config_path) { + auto yaml = YAML::LoadFile(config_path); + comming_angle_ = yaml["comming_angle"].as() / 57.3; // degree to rad + leaving_angle_ = yaml["leaving_angle"].as() / 57.3; // degree to rad + } std::pair ChooseAimArmor(const Eigen::Vector& ekf_x, const std::vector& xyza_list, const CarIDFlag& single_id) { @@ -54,10 +57,8 @@ class AimPointChooser { } } else { // 小陀螺 - double coming_angle = - (single_id == CarIDFlag::Outpost) ? 70 / 57.3 : comming_angle_; - double leaving_angle = - (single_id == CarIDFlag::Outpost) ? 30 / 57.3 : leaving_angle_; + double coming_angle = (single_id == CarIDFlag::Outpost) ? 70 / 57.3 : comming_angle_; + double leaving_angle = (single_id == CarIDFlag::Outpost) ? 30 / 57.3 : leaving_angle_; // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 for (int i = 0; i < armor_num; i++) { diff --git a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp index 4c9122e..79857b3 100644 --- a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp +++ b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp @@ -5,6 +5,8 @@ #include #include +#include + #include "aim_point_chooser.hpp" #include "target_snapshot.hpp" #include "trajectory.hpp" @@ -23,12 +25,14 @@ struct AimSolution { class AimingSolver { public: - AimingSolver( - const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) - : aim_point_chooser_(std::make_unique()) - , yaw_offset_(yaw_offset / 57.3) // degree to rad - , pitch_offset_(pitch_offset / 57.3) // degree to rad - , g_(gravity) { } + AimingSolver(const std::string& config_path, const double& gravity = 9.7833) + : aim_point_chooser_(std::make_unique(config_path)) + , g_(gravity) { + + auto yaml = YAML::LoadFile(config_path); + yaw_offset_ = yaml["yaw_offset"].as() / 57.3; // degree to rad + pitch_offset_ = yaml["pitch_offset"].as() / 57.3; // degree to rad + } AimSolution SolveAimSolution(const std::shared_ptr& snapshot, const double& bullet_speed, const double& time_delay) { diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp index fa1c521..7483d22 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -4,6 +4,8 @@ #include #include +#include + #include "../in_gimbal_control_armor.hpp" #include "../live_target_manager/live_target.hpp" #include "aim_solver.hpp" @@ -14,18 +16,24 @@ namespace world_exe::tongji::predictor { class TargetSnapshotManagerImpl { public: - TargetSnapshotManagerImpl(const enumeration::ArmorIdFlag& id, + TargetSnapshotManagerImpl(const std::string& config_path, const enumeration::ArmorIdFlag& id, const std::unordered_map>& live_target_map, - const std::time_t& now, const double& bullet_speed, const double& yaw_offset, - const double& pitch_offset) - : aim_solver_(std::make_unique(yaw_offset, pitch_offset)) + const std::time_t& now, const double& bullet_speed) + : aim_solver_(std::make_unique(config_path)) , snapshots_(BuildSnapshots(live_target_map)) , now_(now) , ids_(id) , bullet_speed_(bullet_speed) , gimbal_command_({ std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN() }) { } + std::numeric_limits::quiet_NaN() }) { + + auto yaml = YAML::LoadFile(config_path); + + decision_speed_ = yaml["decision_speed"].as(); + high_speed_delay_time_ = yaml["high_speed_delay_time"].as(); + low_speed_delay_time_ = yaml["low_speed_delay_time"].as(); + } const enumeration::ArmorIdFlag& GetId() const { return ids_; } @@ -34,11 +42,15 @@ class TargetSnapshotManagerImpl { std::unordered_map> result; - + // TODO:time_delay for (const auto& [id, snapshot] : snapshots_) { - auto aim_solution = - aim_solver_->SolveAimSolution(std::make_unique(snapshot), - bullet_speed_, snapshot.GetTimeStamp().GetTimeStamp()); + auto ekf_x = snapshot.GetEkfX(); + const auto delay_time = + ekf_x[7] > decision_speed_ ? high_speed_delay_time_ : low_speed_delay_time_; + const auto dt = control_delay_time_ + delay_time; // TODO:add delta(now)? + + auto aim_solution = aim_solver_->SolveAimSolution( + std::make_unique(snapshot), bullet_speed_, dt); if (!aim_solution.valid) continue; @@ -75,15 +87,19 @@ class TargetSnapshotManagerImpl { const enumeration::ArmorIdFlag ids_; const double bullet_speed_; mutable GimbalCommand gimbal_command_; + double decision_speed_; + double high_speed_delay_time_; + double low_speed_delay_time_; + double control_delay_time_; }; -TargetSnapshotManager::TargetSnapshotManager(const enumeration::ArmorIdFlag& id, +TargetSnapshotManager::TargetSnapshotManager(const std::string& config_path, + const enumeration::ArmorIdFlag& id, const std::unordered_map>& live_target_map, - const std::time_t& now, const double& bullet_speed, const double& yaw_offset, - const double& pitch_offset) + const std::time_t& now, const double& bullet_speed) : pimpl_(std::make_unique( - id, live_target_map, now, bullet_speed, yaw_offset, pitch_offset)) { } + config_path, id, live_target_map, now, bullet_speed)) { } TargetSnapshotManager::~TargetSnapshotManager() = default; const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp index 2c17fef..3187731 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp @@ -16,11 +16,10 @@ struct GimbalCommand { class TargetSnapshotManager final : public interfaces::IPredictor { public: - TargetSnapshotManager(const enumeration::ArmorIdFlag& id, + TargetSnapshotManager(const std::string& config_path, const enumeration::ArmorIdFlag& id, const std::unordered_map>& live_target_map, - const std::time_t& now, const double& bullet_speed, const double& yaw_offset, - const double& pitch_offset); + const std::time_t& now, const double& bullet_speed); ~TargetSnapshotManager(); const enumeration ::ArmorIdFlag& GetId() const override; From 5793805e82d22aa502897066e588fbe987160344 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 25 Oct 2025 17:37:27 +0800 Subject: [PATCH 60/93] refactor(api): return timestamp by value instead of reference --- include/interfaces/armor_in_camera.hpp | 2 +- .../interfaces/armor_in_gimbal_control.hpp | 2 +- include/interfaces/armor_in_image.hpp | 2 +- .../interfaces/predictor_update_package.hpp | 2 +- include/interfaces/time_stamped.hpp | 5 +-- .../HZA/identifier/time_stamped_detail.cpp | 14 ++++----- .../HZA/identifier/time_stamped_detail.hpp | 4 +-- .../fire_controller/fire_controller.cpp | 31 +++++++++---------- .../fire_controller/fire_controller.hpp | 3 +- src/tongji/identifier/identified_armor.hpp | 9 +++--- .../predictor/live_target_manager/tracker.hpp | 19 ++++++------ src/tongji/solver/solved_armor.hpp | 15 +++++---- src/tongji/solver/solver.cpp | 2 +- src/tongji/solver/solver.hpp | 2 +- src/tongji/time_stamp/time_stamp.hpp | 29 ++++++++++++----- src/v1/identifier/identifier_armor.hpp | 2 +- src/v1/pnpsolver/armor_pnp_solver.cpp | 4 +-- src/v1/pnpsolver/armor_pnp_solver.hpp | 2 +- src/v1/predictor/predict_time_stamp.hpp | 2 +- src/v1/sync/syncer.cpp | 2 +- 20 files changed, 77 insertions(+), 76 deletions(-) diff --git a/include/interfaces/armor_in_camera.hpp b/include/interfaces/armor_in_camera.hpp index dbb3b94..d376a85 100644 --- a/include/interfaces/armor_in_camera.hpp +++ b/include/interfaces/armor_in_camera.hpp @@ -16,7 +16,7 @@ namespace world_exe::interfaces { class IArmorInCamera { public: /// 获取时间戳，标志其内容装甲板的准确时间点 - COMBINE_TIME_STAMPED; + virtual const ITimeStamped& GetTimeStamped() const = 0; /// 获取某个车辆ID的装甲板集合 virtual const std::vector& GetArmors( diff --git a/include/interfaces/armor_in_gimbal_control.hpp b/include/interfaces/armor_in_gimbal_control.hpp index f2da7be..757142c 100644 --- a/include/interfaces/armor_in_gimbal_control.hpp +++ b/include/interfaces/armor_in_gimbal_control.hpp @@ -17,7 +17,7 @@ class IArmorInGimbalControl { public: /// 获取时间戳，标志其内容装甲板的准确时间点 - COMBINE_TIME_STAMPED; + virtual const ITimeStamped& GetTimeStamped() const = 0; /// 获取某个车辆ID的装甲板集合 virtual const std::vector& GetArmors( diff --git a/include/interfaces/armor_in_image.hpp b/include/interfaces/armor_in_image.hpp index ba3ecec..61f758e 100644 --- a/include/interfaces/armor_in_image.hpp +++ b/include/interfaces/armor_in_image.hpp @@ -21,7 +21,7 @@ class IArmorInImage { public: /// 获取时间戳，标志其内容装甲板的准确时间点 - COMBINE_TIME_STAMPED; + virtual const ITimeStamped& GetTimeStamped() const = 0; /** * @brief 获取某个车辆ID的装甲板集合 diff --git a/include/interfaces/predictor_update_package.hpp b/include/interfaces/predictor_update_package.hpp index ea0eda6..9d2e86b 100644 --- a/include/interfaces/predictor_update_package.hpp +++ b/include/interfaces/predictor_update_package.hpp @@ -16,7 +16,7 @@ class IPreDictorUpdatePackage { /** * @brief 传感器数据获取时的时间戳 */ - COMBINE_TIME_STAMPED; + virtual const ITimeStamped& GetTimeStamped() const = 0; /** * @brief 求解好的装甲板三维信息 * diff --git a/include/interfaces/time_stamped.hpp b/include/interfaces/time_stamped.hpp index 28bfe28..c074e20 100644 --- a/include/interfaces/time_stamped.hpp +++ b/include/interfaces/time_stamped.hpp @@ -1,16 +1,13 @@ #pragma once #include - -#define COMBINE_TIME_STAMPED virtual const ITimeStamped& GetTimeStamped() const = 0 - namespace world_exe::interfaces { /** * @brief 时间戳，通常是SteadyClock */ class ITimeStamped { public: - virtual const std::time_t& GetTimeStamp() const = 0; + virtual const std::time_t GetTimeStamp() const = 0; virtual ~ITimeStamped() = default; }; diff --git a/src/playground/HZA/identifier/time_stamped_detail.cpp b/src/playground/HZA/identifier/time_stamped_detail.cpp index f77d1a4..1ea13df 100644 --- a/src/playground/HZA/identifier/time_stamped_detail.cpp +++ b/src/playground/HZA/identifier/time_stamped_detail.cpp @@ -1,10 +1,8 @@ #include "time_stamped_detail.hpp" -namespace world_exe::interfaces::detail -{ - const std::time_t& TimeStamped::GetTimeStamp() const - { - //不是很清楚具体实现是什么 - return time_stamp_; - } -} \ No newline at end of file +namespace world_exe::interfaces::detail { +const std::time_t TimeStamped::GetTimeStamp() const { + // 不是很清楚具体实现是什么 + return time_stamp_; +} +} \ No newline at end of file diff --git a/src/playground/HZA/identifier/time_stamped_detail.hpp b/src/playground/HZA/identifier/time_stamped_detail.hpp index 1ae4c19..455d1f4 100644 --- a/src/playground/HZA/identifier/time_stamped_detail.hpp +++ b/src/playground/HZA/identifier/time_stamped_detail.hpp @@ -1,12 +1,12 @@ #pragma once #include "interfaces/time_stamped.hpp" -#include + namespace world_exe::interfaces::detail { class TimeStamped : public world_exe::interfaces::ITimeStamped { public: TimeStamped() = default; virtual ~TimeStamped() = default; - const std::time_t& GetTimeStamp() const override; + const std::time_t GetTimeStamp() const override; private: std::time_t time_stamp_; diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index d654ae8..800231c 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -1,5 +1,6 @@ #include "fire_controller.hpp" +#include #include #include @@ -24,20 +25,18 @@ using TimeStamp = time_stamp::TimeStamp; class FireControllerImpl { public: - FireControllerImpl(const std::string& config_path, const double& control_delay_in_second, - const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, + FireControllerImpl(const std::string& config_path, std::shared_ptr state_machine, std::shared_ptr live_target_manager) - : control_delay_(control_delay_in_second) - , bullet_speed_(bullet_speed) - , allowed_target_id_(CarIDFlag::None) + : allowed_target_id_(CarIDFlag::None) , firable_(false) - , time_stamp_(std::time(nullptr)) , fire_decision_(std::make_unique(config_path)) , state_machine_(state_machine) , live_target_manager_(live_target_manager) { - auto yaml = YAML::LoadFile(config_path); + auto yaml = YAML::LoadFile(config_path); + control_delay_ = yaml["control_delay"].as(); + bullet_speed_ = yaml["bullet_speed"].as(); } // TODO:std::time_t @@ -49,9 +48,11 @@ class FireControllerImpl { auto converged_cars = state_machine_->GetAllowdToFires(); auto snapshot_manager = live_target_manager_->GetPredictor(converged_cars); if (!snapshot_manager) - return data::FireControl { .time_stamp = time_stamp_.GetTimeStamp(), + return data::FireControl { + .time_stamp = TimeStamp(std::chrono::steady_clock::now()).GetTimeStamp(), .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), - .fire_allowance = false }; + .fire_allowance = false + }; auto armors_in_gimbal_control = snapshot_manager->Predictor(control_delay_); allowed_target_id_ = state_machine_->GetAllowdToFires(); @@ -69,7 +70,7 @@ class FireControllerImpl { data::FireControl result; result.fire_allowance = fire_command; result.gimbal_dir << gimbal_command.yaw, gimbal_command.pitch, 0; - result.time_stamp = time_stamp_.GetTimeStamp(); + result.time_stamp = TimeStamp(std::chrono::steady_clock::now()).GetTimeStamp(); return result; } @@ -79,7 +80,6 @@ class FireControllerImpl { } void Update(std::shared_ptr armors, const double& gimbal_yaw) { - time_stamp_.SetTimeStamp(std::time(nullptr)); UpdateIdentifiedArmor(armors); UpdateGimbalPosition(gimbal_yaw); } @@ -89,7 +89,6 @@ class FireControllerImpl { identified_armors_ = armors; } void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; - TimeStamp GetTimeStamp() const { return time_stamp_; } double gimbal_yaw_; double control_delay_; @@ -99,18 +98,16 @@ class FireControllerImpl { mutable CarIDFlag allowed_target_id_; mutable double firable_; - time_stamp::TimeStamp time_stamp_; std::unique_ptr fire_decision_; std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; }; FireController::FireController(const std::string& config_path, - const double& control_delay_in_second, const double& bullet_speed, const double& yaw_offset, - const double& pitch_offset, std::shared_ptr state_machine, + std::shared_ptr state_machine, std::shared_ptr live_target_manager) - : pimpl_(std::make_unique(config_path, control_delay_in_second, - bullet_speed, yaw_offset, pitch_offset, state_machine, live_target_manager)) { } + : pimpl_( + std::make_unique(config_path, state_machine, live_target_manager)) { } const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { return pimpl_->CalculateTarget(time_duration); diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index ccae0db..8ef5630 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -13,8 +13,7 @@ namespace world_exe::tongji::fire_control { class FireControllerImpl; class FireController final : public interfaces::IFireControl { public: - FireController(const std::string& config_path, const double& control_delay_in_second, - const double& bullet_speed, const double& yaw_offset, const double& pitch_offset, + FireController(const std::string& config_path, std::shared_ptr state_machine, std::shared_ptr live_target_manager); diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp index 14295e5..9a658f4 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -3,11 +3,12 @@ #include "enum/armor_id.hpp" #include "interfaces/armor_in_image.hpp" #include "interfaces/time_stamped.hpp" +#include "tongji/time_stamp/time_stamp.hpp" #include "util/index.hpp" namespace world_exe::tongji::identifier { -class IdentifiedArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { +class IdentifiedArmor final : public interfaces::IArmorInImage { public: explicit IdentifiedArmor(const std::vector& armors) { for (const auto& armor : armors) { @@ -15,9 +16,7 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface } } - const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const override { @@ -29,7 +28,7 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface } private: - std::time_t time_stamp_ { std::time(nullptr) }; + time_stamp::TimeStamp time_stamp_ { std::chrono::steady_clock::now() }; std::array, 8> armors_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager/tracker.hpp b/src/tongji/predictor/live_target_manager/tracker.hpp index 0b834a6..6a37b20 100644 --- a/src/tongji/predictor/live_target_manager/tracker.hpp +++ b/src/tongji/predictor/live_target_manager/tracker.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -8,13 +9,13 @@ #include "../../identifier/armor_filter.hpp" #include "../../identifier/identified_armor.hpp" -#include "../../time_stamp/time_stamp.hpp" #include "../target_snapshot_manager/target_snapshot.hpp" #include "../target_snapshot_manager/target_snapshot_manager.hpp" #include "decider.hpp" #include "enum/armor_id.hpp" namespace world_exe::tongji::predictor { +using namespace std::chrono_literals; enum class TrackState { Lost, // @@ -33,14 +34,14 @@ class Tracker final { Tracker() : armor_filter_(std::make_unique()) , decider_(std::make_unique()) - , last_track_timestamp_(std::time(nullptr)) { } + , last_track_timestamp_(std::chrono::steady_clock::now()) { } ~Tracker() = default; auto SelectTrackingTargetID(const std::shared_ptr& armors_in_image, const std::time_t& now) noexcept -> enumeration::ArmorIdFlag const { - CheckCameraOffline(now); - last_track_timestamp_.SetTimeStamp(now); + CheckCameraOffline(); + last_track_timestamp_ = std::chrono::steady_clock::now(); auto filtered_ids = enumeration::ArmorIdFlag::None; auto detected_ids = enumeration::ArmorIdFlag::None; @@ -136,11 +137,9 @@ class Tracker final { TrackState GetState() const { return state_; } private: - void CheckCameraOffline(const std::time_t& now) { - // TODO:If the underlying timestamp is std::time_t, then this if branch will never be - // entered + void CheckCameraOffline() { if (state_ != TrackState::Lost - && static_cast(now - last_track_timestamp_.GetTimeStamp()) < timeout_sec_) + && (std::chrono::steady_clock::now() - last_track_timestamp_) < timeout_sec_) SetState(TrackState::Lost); } @@ -165,9 +164,9 @@ class Tracker final { const int outpost_max_temp_lost_count_ = 75; const int normal_max_temp_lost_count_ = max_temp_lost_count_; const int max_switch_count_ = 200; - const double timeout_sec_ = 0.1; + static constexpr auto timeout_sec_ = 100ms; - time_stamp::TimeStamp last_track_timestamp_; + std::chrono::steady_clock::time_point last_track_timestamp_; }; } \ No newline at end of file diff --git a/src/tongji/solver/solved_armor.hpp b/src/tongji/solver/solved_armor.hpp index 052490b..d44b45c 100644 --- a/src/tongji/solver/solved_armor.hpp +++ b/src/tongji/solver/solved_armor.hpp @@ -1,33 +1,32 @@ #pragma once #include +#include #include #include "interfaces/armor_in_camera.hpp" #include "interfaces/time_stamped.hpp" +#include "tongji/time_stamp/time_stamp.hpp" #include "util/index.hpp" namespace world_exe::tongji::solver { -class SolvedArmor final : public interfaces::IArmorInCamera, public interfaces::ITimeStamped { +class SolvedArmor final : public interfaces::IArmorInCamera { public: - explicit SolvedArmor(const std::vector& armors) { + explicit SolvedArmor(const std::vector& armors) + : time_stamp_(std::chrono::steady_clock::now()) { for (const auto& armor : armors) { armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); } - time_stamp_ = 0; } - const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; - const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const override { return armors_[util::enumeration::GetIndex(armor_id)]; } + const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } private: - std::time_t time_stamp_ { std::time(nullptr) }; std::array, 8> armors_; + time_stamp::TimeStamp time_stamp_; }; } diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 7469209..21b2e40 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -303,7 +303,7 @@ Solver::Solver(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, : pimpl_(std::make_unique(R_camera2gimbal, R_gimbal2world, t_camera2gimbal)) { } Solver::~Solver() = default; -const std::time_t& Solver::GetTimeStamp() const { return pimpl_->GetTimeStamp(); } +const std::time_t Solver::GetTimeStamp() const { return pimpl_->GetTimeStamp(); } std::shared_ptr Solver::SolvePnp( std::shared_ptr armors_in_image) { diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index 41e9566..83118f3 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -14,7 +14,7 @@ class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStam std::shared_ptr SolvePnp( std::shared_ptr armor) override; - const std::time_t& GetTimeStamp() const override; + const std::time_t GetTimeStamp() const override; Solver(const Solver&) = delete; Solver& operator=(const Solver&) = delete; diff --git a/src/tongji/time_stamp/time_stamp.hpp b/src/tongji/time_stamp/time_stamp.hpp index aa93ae0..454bac0 100644 --- a/src/tongji/time_stamp/time_stamp.hpp +++ b/src/tongji/time_stamp/time_stamp.hpp @@ -1,21 +1,34 @@ #pragma once +#include +#include +#include + #include "interfaces/time_stamped.hpp" namespace world_exe::tongji::time_stamp { -class TimeStamp : public interfaces::ITimeStamped { +using namespace std::chrono; +class TimeStamp final : public interfaces::ITimeStamped { public: - TimeStamp(const std::time_t& time_stamp) - : time_stamp_(time_stamp) { } + using StorageType = int64_t; + using ClockType = std::chrono::steady_clock; + using TimeDuration = nanoseconds; + + explicit TimeStamp(const steady_clock::time_point& tp) + : value_ns_(duration_cast(tp.time_since_epoch()).count()) { } + explicit TimeStamp(const StorageType& ns_count) + : value_ns_(ns_count) { } - void SetTimeStamp(const std::time_t& time_stamp) { time_stamp_ = time_stamp; } + auto DeltaTime(const TimeStamp& other) const -> double { + TimeDuration diff_ns = TimeDuration(value_ns_ - other.value_ns_); + return std::chrono::duration(diff_ns).count(); + } - double SecondsSince(const TimeStamp& other) const { - return std::difftime(time_stamp_, other.time_stamp_); + const std::time_t GetTimeStamp() const override { + return std::bit_cast(value_ns_); } - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; private: - std::time_t time_stamp_; + const StorageType value_ns_; }; } \ No newline at end of file diff --git a/src/v1/identifier/identifier_armor.hpp b/src/v1/identifier/identifier_armor.hpp index d6a6877..7bfbb69 100644 --- a/src/v1/identifier/identifier_armor.hpp +++ b/src/v1/identifier/identifier_armor.hpp @@ -15,7 +15,7 @@ class IdentifierArmor : public interfaces::IArmorInImage, public interfaces::ITi const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + const std::time_t GetTimeStamp() const override { return time_stamp_; }; void SetArmors(const std::vector& armors) { for (auto& armors : armors_) diff --git a/src/v1/pnpsolver/armor_pnp_solver.cpp b/src/v1/pnpsolver/armor_pnp_solver.cpp index 7f2185c..9feb630 100644 --- a/src/v1/pnpsolver/armor_pnp_solver.cpp +++ b/src/v1/pnpsolver/armor_pnp_solver.cpp @@ -63,7 +63,7 @@ class ArmorInCamera final : public world_exe::interfaces::IArmorInCamera, const world_exe::enumeration::ArmorIdFlag& armor_id) const override { return armors[world_exe::util::enumeration::GetIndex(armor_id)]; } - const std::time_t& GetTimeStamp() const override { return time_stampe; } + const std::time_t GetTimeStamp() const override { return time_stampe; } std::time_t time_stampe = 0; std::vector @@ -86,7 +86,7 @@ std::shared_ptr ArmorIPPEPnPSolver::Solve return armors_; } -const std::time_t& ArmorIPPEPnPSolver::GetTimeStamp() const { return time_point_; } +const std::time_t ArmorIPPEPnPSolver::GetTimeStamp() const { return time_point_; } ArmorIPPEPnPSolver::ArmorIPPEPnPSolver(const std::vector& LargeArmorObjectPoints, const std::vector& NormalArmorObjectPoints) diff --git a/src/v1/pnpsolver/armor_pnp_solver.hpp b/src/v1/pnpsolver/armor_pnp_solver.hpp index 9f0684f..2760ad6 100644 --- a/src/v1/pnpsolver/armor_pnp_solver.hpp +++ b/src/v1/pnpsolver/armor_pnp_solver.hpp @@ -16,7 +16,7 @@ class ArmorIPPEPnPSolver final : public interfaces::IPnpSolver, public interface std::shared_ptr SolvePnp( std::shared_ptr armor) override; - const std::time_t& GetTimeStamp() const override; + const std::time_t GetTimeStamp() const override; private: class Impl; diff --git a/src/v1/predictor/predict_time_stamp.hpp b/src/v1/predictor/predict_time_stamp.hpp index 03cb579..fe818c4 100644 --- a/src/v1/predictor/predict_time_stamp.hpp +++ b/src/v1/predictor/predict_time_stamp.hpp @@ -11,7 +11,7 @@ class PredictTimeStamp : public interfaces::ITimeStamped { inline void SetTimeStamp(const time_t& time_stamp) { time_stamp_ = time_stamp; } - const std::time_t& GetTimeStamp() const override { return time_stamp_; }; + const std::time_t GetTimeStamp() const override { return time_stamp_; }; private: std::time_t time_stamp_; diff --git a/src/v1/sync/syncer.cpp b/src/v1/sync/syncer.cpp index 03557cb..b90f5b4 100644 --- a/src/v1/sync/syncer.cpp +++ b/src/v1/sync/syncer.cpp @@ -16,7 +16,7 @@ struct Syncer::Impl final : public interfaces::IPreDictorUpdatePackage, const ITimeStamped& GetTimeStamped() const override { return *this; } - const std::time_t& GetTimeStamp() const override { return last_update_time_; } + const std::time_t GetTimeStamp() const override { return last_update_time_; } std::shared_ptr GetArmors() const override { if (armors_loaded_ == nullptr) return std::make_shared(); From 892f37c0ccb34df8b6e9db82ea564cbc5d24f827 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 26 Oct 2025 21:15:06 +0800 Subject: [PATCH 61/93] refactor(coordinates): define explicit coordinate systems and update solver --- configs/example.yaml | 4 +- .../fire_controller/fire_controller.cpp | 9 +- src/tongji/fire_controller/fire_decision.hpp | 13 +- .../predictor/kalman_filter/predict_model.hpp | 18 +- .../live_target_manager/live_target.hpp | 32 +-- .../live_target_manager.cpp | 12 +- .../target_snapshot_manager/aim_solver.hpp | 19 +- .../target_snapshot.hpp | 2 +- src/tongji/solver/solver.cpp | 197 ++++++++---------- src/tongji/solver/solver.hpp | 3 +- src/util/coordinate.hpp | 8 - 11 files changed, 140 insertions(+), 177 deletions(-) diff --git a/configs/example.yaml b/configs/example.yaml index 7d2d05c..dc92c3e 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -37,5 +37,7 @@ pitch_offset: -0.5 # degree comming_angle: 60 # degree leaving_angle: 20 # degree - +#####-----solver parameters-----##### +R_muzzle2camera: +t_muzzle2camera: diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 800231c..754851b 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -54,17 +54,16 @@ class FireControllerImpl { .fire_allowance = false }; - auto armors_in_gimbal_control = snapshot_manager->Predictor(control_delay_); - allowed_target_id_ = state_machine_->GetAllowdToFires(); + auto armors_in_gimbal = snapshot_manager->Predictor(control_delay_); + allowed_target_id_ = state_machine_->GetAllowdToFires(); - auto target_gimbal_spacing = - armors_in_gimbal_control->GetArmors(allowed_target_id_).front(); + auto target_gimbal_spacing = armors_in_gimbal->GetArmors(allowed_target_id_).front(); auto gimbal_command = std::dynamic_pointer_cast(snapshot_manager)->GetGimbalCommand(); auto fire_command = - fire_decision_->ShouldFire(gimbal_command, target_gimbal_spacing.position); + fire_decision_->ShouldFire(gimbal_yaw_,gimbal_command, target_gimbal_spacing.position); firable_ = fire_command; data::FireControl result; diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index ca47c40..2c7f80f 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -13,8 +13,7 @@ class FireDecision { public: explicit FireDecision(const std::string& config_path) : last_gimbal_command_({ std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN() }) - , gimbal_yaw_(std::numeric_limits::quiet_NaN()) { + std::numeric_limits::quiet_NaN() }) { auto yaml = YAML::LoadFile(config_path); auto_fire_ = yaml["auto_fire"].as(); first_tolerance_ = yaml["first_tolerance"].as() / 57.3; // degree to rad @@ -22,8 +21,8 @@ class FireDecision { judge_distance_ = yaml["judge_distance"].as(); } - bool ShouldFire( - predictor::GimbalCommand gimbal_command, const Eigen::Vector3d& valid_target_pos) { + bool ShouldFire(const double& gimbal_yaw, predictor::GimbalCommand gimbal_command, + const Eigen::Vector3d& valid_target_pos) { if (!auto_fire_) return false; const auto& tolerance = std::sqrt(valid_target_pos.x() * valid_target_pos.x() @@ -32,9 +31,9 @@ class FireDecision { ? second_tolerance_ : first_tolerance_; - if (std::abs(last_gimbal_command_.yaw - gimbal_yaw_) + if (std::abs(last_gimbal_command_.yaw - gimbal_yaw) < tolerance * 2 // 此时认为command突变不应该射击 - && std::abs(gimbal_yaw_ - last_gimbal_command_.yaw) < tolerance) { + && std::abs(gimbal_yaw - last_gimbal_command_.yaw) < tolerance) { last_gimbal_command_ = gimbal_command; return true; } @@ -46,8 +45,6 @@ class FireDecision { bool auto_fire_; predictor::GimbalCommand last_gimbal_command_; - double gimbal_yaw_; - double first_tolerance_ { 5 }; // 近距离射击容差，degree double second_tolerance_ { 2 }; // 远距离射击容差，degree double judge_distance_ { 3 }; // 距离判断阈值 diff --git a/src/tongji/predictor/kalman_filter/predict_model.hpp b/src/tongji/predictor/kalman_filter/predict_model.hpp index 03d7734..e811698 100644 --- a/src/tongji/predictor/kalman_filter/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -116,8 +116,8 @@ class EKFModel { }; - auto MatchArmor(const XVec& x, const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) const + auto MatchArmor(const XVec& x, const Eigen::Vector3d& armor_xyz_in_gimbal, + const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal) const ->const int { const auto& xyza_list = GetArmorXYZAList(x); @@ -139,8 +139,8 @@ class EKFModel { for (int i = 0; i < std::min(3, armor_num_); ++i) { const auto& xyza = xyza_i_list[i].first; auto ypd = util::math::xyz2ypd(xyza.head(3)); - double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_world(0) - xyza(3))) - + std::abs(util::math::clamp_pm_pi(armor_ypd_in_world(0) - ypd(0))); + double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - xyza(3))) + + std::abs(util::math::clamp_pm_pi(armor_ypd_in_gimbal(0) - ypd(0))); if (error < min_error) { min_error = error; @@ -204,14 +204,14 @@ class EKFModel { return H_armor_ypda * H_armor_xyza; } - auto R(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - const Eigen::Vector3d& armor_ypd_in_world, int id) const -> RMat const { + auto R(const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, + const Eigen::Vector3d& armor_ypd_in_gimbal, int id) const -> RMat const { // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; - auto center_yaw = std::atan2(armor_xyz_in_world(1), armor_xyz_in_world(0)); - auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world(0) - center_yaw); + auto center_yaw = std::atan2(armor_xyz_in_gimbal(1), armor_xyz_in_gimbal(0)); + auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - center_yaw); RDig R_dig(4); R_dig << 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, - log(std::abs(armor_ypd_in_world(2)) + 1) / 200 + 9e-2; + log(std::abs(armor_ypd_in_gimbal(2)) + 1) / 200 + 9e-2; // 测量过程噪声偏差的方差 return R_dig.asDiagonal(); diff --git a/src/tongji/predictor/live_target_manager/live_target.hpp b/src/tongji/predictor/live_target_manager/live_target.hpp index 58f0a20..2382504 100644 --- a/src/tongji/predictor/live_target_manager/live_target.hpp +++ b/src/tongji/predictor/live_target_manager/live_target.hpp @@ -19,8 +19,8 @@ class LiveTarget { using PredictorModel = EKFModel<11, 4>; using EKF = ExtendedKalmanFilter; - LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - const enumeration::CarIDFlag& car_id) + LiveTarget(const Eigen::Vector3d& armor_xyz_in_gimbal, + const Eigen::Vector3d& armor_ypr_in_gimbal, const enumeration::CarIDFlag& car_id) : last_see_time_stamp_(std::time(nullptr)) , model_(car_id) { // x vx y vy z vz a w r l h @@ -29,12 +29,12 @@ class LiveTarget { // l: r2 - r1 // h: z2 - z1 auto center_x = - armor_xyz_in_world[0] + model_.GetRadius() * std::cos(armor_ypr_in_world[0]); + armor_xyz_in_gimbal[0] + model_.GetRadius() * std::cos(armor_ypr_in_gimbal[0]); auto center_y = - armor_xyz_in_world[1] + model_.GetRadius() * std::sin(armor_ypr_in_world[0]); - auto center_z = armor_xyz_in_world[2]; + armor_xyz_in_gimbal[1] + model_.GetRadius() * std::sin(armor_ypr_in_gimbal[0]); + auto center_z = armor_xyz_in_gimbal[2]; - EKF::XVec x0 { center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, + EKF::XVec x0 { center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_gimbal[0], 0, model_.GetRadius(), 0, 0 }; EKF::PMat P0 = model_.GetP0Dig().asDiagonal(); @@ -62,15 +62,15 @@ class LiveTarget { return armors; } - void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { + void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_gimbal, + const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal) { // 装甲板匹配 - int id = - model_.MatchArmor(ekf_->x, armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); + int id = model_.MatchArmor( + ekf_->x, armor_xyz_in_gimbal, armor_ypr_in_gimbal, armor_ypd_in_gimbal); last_id_ = id; update_count_++; - Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id, dt); + Update_ypda(armor_xyz_in_gimbal, armor_ypr_in_gimbal, armor_ypd_in_gimbal, id, dt); last_see_time_stamp_ = std::time(nullptr); } @@ -93,15 +93,15 @@ class LiveTarget { return true; } // TODO:need to update correctly - void Update_ypda(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, + void Update_ypda(const Eigen::Vector3d& armor_xyz_in_gimbal, + const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal, const int& id, const double& dt) { // 观测jacobi auto H = model_.H(ekf_->x, id); - auto R = model_.R(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); + auto R = model_.R(armor_xyz_in_gimbal, armor_ypr_in_gimbal, armor_ypd_in_gimbal, id); - const Eigen::Vector3d& ypd = armor_ypd_in_world; - const Eigen::Vector3d& ypr = armor_ypr_in_world; + const Eigen::Vector3d& ypd = armor_ypd_in_gimbal; + const Eigen::Vector3d& ypr = armor_ypr_in_gimbal; // 获得观测量 EKF::ZVec z(4); diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp index 5a2642b..e8c867e 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -90,10 +90,10 @@ class LiveTargetManagerImpl { if (armors_list.empty()) return; const auto& armor = armors_list.front(); - const Eigen::Vector3d xyz_in_world = transform * armor.position; - const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX + const Eigen::Vector3d xyz_in_gimbal = transform * armor.position; + const Eigen::Vector3d ypr_in_gimbal = rotation_matrix.eulerAngles(2, 1, 0); // ZYX targets_map_[id] = - std::move(std::make_shared(xyz_in_world, ypr_in_world, id)); + std::move(std::make_shared(xyz_in_gimbal, ypr_in_gimbal, id)); } } @@ -109,10 +109,10 @@ class LiveTargetManagerImpl { if (armors_list.empty()) return; const auto& armor = armors_list.front(); - const Eigen::Vector3d xyz_in_world = transform * armor.position; - const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX + const Eigen::Vector3d xyz_in_gimbal = transform * armor.position; + const Eigen::Vector3d ypr_in_gimbal = rotation_matrix.eulerAngles(2, 1, 0); // ZYX targets_map_[tracking_id_]->Update(static_cast(now - last_update_timestamp_), - xyz_in_world, ypr_in_world, util::math::xyz2ypd(xyz_in_world)); + xyz_in_gimbal, ypr_in_gimbal, util::math::xyz2ypd(xyz_in_gimbal)); } std::unordered_map> targets_map_; diff --git a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp index 79857b3..f991480 100644 --- a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp +++ b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp @@ -46,15 +46,16 @@ class AimingSolver { // 预测目标在未来 dt时间后的位置 for (int i = 0; i < 10; ++i) { - double dt = time_delay + prev_fly_time; - const auto aim = SelectPredictedAim(snapshot, dt); - if (!aim.has_value()) return { false, 0, 0, { }, 0 }; // failed: no valid aim point + double dt = time_delay + prev_fly_time; + const auto aim_point = SelectPredictedAim(snapshot, dt); + if (!aim_point.has_value()) + return { false, 0, 0, { }, 0 }; // failed: no valid aim point - const auto traj = SolveTrajectory(aim->head(3), bullet_speed); + const auto traj = SolveTrajectory(aim_point->head<3>(), bullet_speed); if (!traj.has_value()) return { false, 0, 0, { }, 0 }; // failed: trajectory unsolvable if (i > 0 && std::abs(traj->fly_time - prev_fly_time) < 0.001) { - final_aim_point = *aim; + final_aim_point = *aim_point; final_trajectory = *traj; converged = true; break; @@ -63,7 +64,7 @@ class AimingSolver { } if (!converged) return { false, 0, 0, { }, 0 }; // failed: trajectory did not converge - const auto xyz = final_aim_point.head(3); + const auto xyz = final_aim_point.head<3>(); const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; const double pitch = -(final_trajectory.pitch + pitch_offset_); return { true, yaw, pitch, final_aim_point }; @@ -72,9 +73,11 @@ class AimingSolver { private: std::optional SelectPredictedAim( const std::shared_ptr& snapshot, const double& dt) const { - const auto& [selectable, aim_point] = aim_point_chooser_->ChooseAimArmor( + const auto& [selectable, aim_point_in_gimbal] = aim_point_chooser_->ChooseAimArmor( snapshot->Predict(dt), snapshot->GetPredictedXYZAList(dt), snapshot->GetID()); - return selectable ? std::optional { aim_point } : std::nullopt; + + if (!selectable) return std::nullopt; + return aim_point_in_gimbal; } std::optional SolveTrajectory( diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp index 363314e..f8a4fe3 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp @@ -34,7 +34,7 @@ class TargetSnapshot { // } // return armors; // } - + // TODO： auto GetPredictedXYZAList(const double& dt) -> std::vector const { return model_.GetArmorXYZAList(this->Predict(dt)); } diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 21b2e40..e8cba53 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -1,14 +1,21 @@ #include "solver.hpp" +#include #include + #include #include +#include +#include +#include #include "data/armor_camera_spacing.hpp" -#include "data/armor_gimbal_control_spacing.hpp" +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" #include "solved_armor.hpp" + #include "util/coordinate.hpp" #include "util/index.hpp" #include "util/math.hpp" @@ -16,11 +23,14 @@ namespace world_exe::tongji::solver { class SolverImpl { public: - SolverImpl(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, - Eigen::Vector3d t_camera2gimbal) - : R_camera2gimbal_(R_camera2gimbal) - , R_gimbal2world_(R_gimbal2world) - , t_camera2gimbal_(t_camera2gimbal) { } + SolverImpl(const std::string& config_path) { + const auto yaml = YAML::LoadFile(config_path); + auto R_gimbalcamera_data = yaml["R_gimbal2camera"].as>(); + auto t_gimbal2camera_data = yaml["t_gimbal2camera"].as>(); + Eigen::Matrix3d R_muzzle2camera_ = + Eigen::Matrix(R_gimbalcamera_data.data()); + t_gimbal2camera_ = Eigen::Matrix(t_gimbal2camera_data.data()); + } std::shared_ptr SolvePnp( std::shared_ptr armors_in_image) { @@ -31,15 +41,13 @@ class SolverImpl { const auto& armors = armors_in_image->GetArmors(armor_id); for (const auto& armor : armors) { auto solved_armor = Solve(armor); - if (solved_armor.has_value()) { - armor_plates.emplace_back(solved_armor.value()); - } + armor_plates.emplace_back(solved_armor); } } return std::make_shared(armor_plates); } - std::optional Solve( + world_exe::data::ArmorCameraSpacing Solve( const world_exe::data::ArmorImageSpacing& armor_in_image) const { const auto& object_points = armor_in_image.isLargeArmor ? parameters::Robomaster::LargeArmorObjectPointsOpencv @@ -59,78 +67,39 @@ class SolverImpl { Eigen::Matrix3d R_armor2camera; cv::cv2eigen(rmat, R_armor2camera); - Eigen::Quaterniond orientation_in_camera(R_armor2camera); - - world_exe::data::ArmorCameraSpacing armor_in_camera; - auto orientation_ros = util::coordinate::opencv2ros_orientation(orientation_in_camera); - orientation_ros.normalize(); - armor_in_camera.id = armor_in_image.id; - armor_in_camera.position = util::coordinate::opencv2ros_position(xyz_in_camera); - armor_in_camera.orientation = orientation_ros; - - if (armor_in_camera.position.norm() > MaxArmorDistance) { - return { }; - } + const auto armor_in_camera = + convert_to_ros_spacing(armor_in_image.id, xyz_in_camera, R_armor2camera); return armor_in_camera; } - world_exe::data::ArmorGimbalControlSpacing OptimizeYaw( - const world_exe::data::ArmorImageSpacing& armor_in_image, - const world_exe::data::ArmorCameraSpacing& armor_in_camera) const { - - Eigen::Vector3d armor_in_camera_ocv = - util::coordinate::ros2opencv_position(armor_in_camera.position); - Eigen::Vector3d armor_xyz_in_gimbal = - R_camera2gimbal_ * armor_in_camera_ocv + t_camera2gimbal_; - Eigen::Vector3d armor_xyz_in_world = R_gimbal2world_ * armor_xyz_in_gimbal; // why no t? - - Eigen::Quaterniond armor_orientation_in_camera_ocv = - util::coordinate::ros2opencv_orientation(armor_in_camera.orientation); - Eigen::Matrix3d R_armor2camera_ocv = armor_orientation_in_camera_ocv.toRotationMatrix(); - Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera_ocv; - Eigen::Matrix3d R_armor2world_initial = R_gimbal2world_ * R_armor2gimbal; - - Eigen::Vector3d armor_ypr_in_world = - util::math::matrix_to_euler(R_armor2world_initial, 2, 1, 0); - // double initial_yaw = armor_ypr_in_world[0]; - - Eigen::Vector3d gimbal_ypr = util::math::matrix_to_euler(R_gimbal2world_, 2, 1, 0); + auto OptimizeYaw(const data::ArmorImageSpacing& armor_in_image, const double& gimbal_yaw, + const double& armor_yaw_in_gimbal, const Eigen::Vector3d& armor_xyz_in_gimbal) const + -> const double { constexpr double SEARCH_RANGE = 140; // degree - auto yaw0 = util::math::clamp_pm_pi(gimbal_ypr[0] - SEARCH_RANGE / 2 * CV_PI / 180.0); - auto min_error = 1e10; - auto best_yaw = armor_ypr_in_world[0]; + const auto yaw0 = util::math::clamp_pm_pi(gimbal_yaw - SEARCH_RANGE / 2 * CV_PI / 180.0); + auto min_error = 1e10; + auto best_yaw = armor_yaw_in_gimbal; for (int i = 0; i < SEARCH_RANGE; i++) { double yaw = util::math::clamp_pm_pi(yaw0 + i * CV_PI / 180.0); auto error = ArmorReprojectionError( - armor_in_image, armor_xyz_in_world, yaw, (i - SEARCH_RANGE / 2) * CV_PI / 180.0); - + armor_in_image, armor_xyz_in_gimbal, yaw, (i - SEARCH_RANGE / 2) * CV_PI / 180.0); if (error < min_error) { min_error = error; best_yaw = yaw; } } - - armor_ypr_in_world[0] = best_yaw; - - Eigen::Matrix3d R_armor2world_optimized = util::math::euler_to_matrix(armor_ypr_in_world); - - Eigen::Quaterniond orientation_in_world(R_armor2world_optimized); - orientation_in_world.normalize(); - - world_exe::data::ArmorGimbalControlSpacing result; - result.id = armor_in_camera.id; - result.position = armor_xyz_in_world; - result.orientation = orientation_in_world; - return result; + return best_yaw; } - const std::time_t& GetTimeStamp() const { return time_stamp_; } + const std::time_t GetTimeStamp() const { + return time_stamp::TimeStamp(std::chrono::steady_clock::now()).GetTimeStamp(); + } private: - std::vector ReprojectArmor(const Eigen::Vector3d& xyz_in_world, const double& yaw, + std::vector ReprojectArmor(const Eigen::Vector3d& xyz_in_gimbal, const double& yaw, const bool& is_large, const enumeration::ArmorIdFlag& id) const { auto sin_yaw = std::sin(yaw); auto cos_yaw = std::cos(yaw); @@ -141,26 +110,24 @@ class SolverImpl { auto cos_pitch = std::cos(pitch); // clang-format off - const Eigen::Matrix3d R_armor2world { - {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch}, - {sin_yaw * cos_pitch, cos_yaw, sin_yaw * sin_pitch}, - { -sin_pitch, 0, cos_pitch} + const Eigen::Matrix3d R_armor2gimbal { + {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch}, + {sin_yaw * cos_pitch, cos_yaw, sin_yaw * sin_pitch}, + { -sin_pitch, 0, cos_pitch} }; // clang-format on // get R_armor2camera t_armor2camera - const Eigen::Vector3d& t_armor2world = xyz_in_world; - Eigen::Matrix3d R_armor2camera = - R_camera2gimbal_.transpose() * R_gimbal2world_.transpose() * R_armor2world; - Eigen::Vector3d t_armor2camera = R_camera2gimbal_.transpose() - * (R_gimbal2world_.transpose() * t_armor2world - t_camera2gimbal_); + Eigen::Matrix3d R_armor2camera = R_gimbal2camera_ * R_armor2gimbal; + Eigen::Vector3d t_armor2camera = (R_gimbal2camera_ * xyz_in_gimbal + t_gimbal2camera_); // get rvec tvec cv::Vec3d rvec; cv::Mat R_armor2camera_cv; - cv::eigen2cv(R_armor2camera, R_armor2camera_cv); + cv::eigen2cv(util::coordinate::ros2opencv_rotation(R_armor2camera), R_armor2camera_cv); cv::Rodrigues(R_armor2camera_cv, rvec); - cv::Vec3d tvec(t_armor2camera[0], t_armor2camera[1], t_armor2camera[2]); + const auto t_armor2camera_cv = util::coordinate::ros2opencv_position(t_armor2camera); + cv::Vec3d tvec(t_armor2camera_cv[0], t_armor2camera_cv[1], t_armor2camera_cv[2]); // reproject std::vector image_points; @@ -174,11 +141,10 @@ class SolverImpl { } double ArmorReprojectionError(const world_exe::data::ArmorImageSpacing& armor_in_image, - const Eigen::Vector3d& armor_xyz_in_world, const double& yaw, + const Eigen::Vector3d& armor_xyz_in_gimbal, const double& yaw, const double& inclined) const { - - auto image_points = - ReprojectArmor(armor_xyz_in_world, yaw, armor_in_image.isLargeArmor, armor_in_image.id); + const auto image_points = ReprojectArmor( + armor_xyz_in_gimbal, yaw, armor_in_image.isLargeArmor, armor_in_image.id); auto error = 0.0; for (int i = 0; i < 4; i++) error += cv::norm(armor_in_image.image_points[i] - image_points[i]); @@ -187,8 +153,8 @@ class SolverImpl { return error; } - double OupostReprojectionError(world_exe::data::ArmorImageSpacing armor_in_image, - Eigen::Vector3d armor_xyz_in_world, const double& pitch) const { + double OutpostReprojectionError(world_exe::data::ArmorImageSpacing armor_in_image, + Eigen::Vector3d armor_xyz_in_gimbal, const double& pitch) const { // solve const auto& object_points = (armor_in_image.isLargeArmor) @@ -201,21 +167,20 @@ class SolverImpl { parameters::HikCameraProfile::get_distortion_parameters(), rvec, tvec, false, cv::SOLVEPNP_IPPE); - Eigen::Vector3d xyz_in_camera; - cv::cv2eigen(tvec, xyz_in_camera); + Eigen::Vector3d xyz_in_camera_cv; + cv::cv2eigen(tvec, xyz_in_camera_cv); + auto xyz_in_camera = util::coordinate::opencv2ros_position(xyz_in_camera_cv); cv::Mat rmat; cv::Rodrigues(rvec, rmat); - Eigen::Matrix3d R_armor2camera; - cv::cv2eigen(rmat, R_armor2camera); - Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal_ * R_armor2camera; - Eigen::Matrix3d R_armor2world = R_gimbal2world_ * R_armor2gimbal; - auto armor_ypr_in_gimbal = util::math::matrix_to_euler(R_armor2gimbal, 2, 1, 0); - auto armor_ypr_in_world = util::math::matrix_to_euler(R_armor2world, 2, 1, 0); + Eigen::Matrix3d R_armor2camera_cv; + cv::cv2eigen(rmat, R_armor2camera_cv); + auto R_armor2camera = util::coordinate::opencv2ros_rotation(R_armor2camera_cv); - auto yaw = armor_ypr_in_world[0]; - auto xyz_in_world = armor_xyz_in_world; + Eigen::Matrix3d R_armor2gimbal = R_gimbal2camera_.transpose() * R_armor2camera; + auto armor_ypr_in_gimbal = util::math::matrix_to_euler(R_armor2gimbal, 2, 1, 0); + auto yaw = armor_ypr_in_gimbal[0]; auto sin_yaw = std::sin(yaw); auto cos_yaw = std::cos(yaw); @@ -223,26 +188,23 @@ class SolverImpl { auto cos_pitch = std::cos(pitch); // clang-format off - const Eigen::Matrix3d _R_armor2world { - {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch}, - {sin_yaw * cos_pitch, cos_yaw, sin_yaw * sin_pitch}, - { -sin_pitch, 0, cos_pitch} - }; + const Eigen::Matrix3d _R_armor2gimbal { + {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch}, + {sin_yaw * cos_pitch, cos_yaw, sin_yaw * sin_pitch}, + { -sin_pitch, 0, cos_pitch}}; // clang-format on // get R_armor2camera t_armor2camera - const Eigen::Vector3d& t_armor2world = xyz_in_world; - Eigen::Matrix3d _R_armor2camera = - R_camera2gimbal_.transpose() * R_gimbal2world_.transpose() * _R_armor2world; - Eigen::Vector3d t_armor2camera = R_camera2gimbal_.transpose() - * (R_gimbal2world_.transpose() * t_armor2world - t_camera2gimbal_); + Eigen::Matrix3d _R_armor2camera = R_gimbal2camera_ * _R_armor2gimbal; + Eigen::Vector3d t_armor2camera = R_gimbal2camera_ * armor_xyz_in_gimbal + t_gimbal2camera_; // get rvec tvec cv::Vec3d _rvec; - cv::Mat R_armor2camera_cv; - cv::eigen2cv(_R_armor2camera, R_armor2camera_cv); - cv::Rodrigues(R_armor2camera_cv, _rvec); - cv::Vec3d _tvec(t_armor2camera[0], t_armor2camera[1], t_armor2camera[2]); + cv::Mat _R_armor2camera_cv; + cv::eigen2cv(util::coordinate::ros2opencv_rotation(_R_armor2camera), _R_armor2camera_cv); + cv::Rodrigues(_R_armor2camera_cv, _rvec); + const auto t_armor2camera_cv = util::coordinate::ros2opencv_position(t_armor2camera); + cv::Vec3d _tvec(t_armor2camera_cv[0], t_armor2camera_cv[1], t_armor2camera_cv[2]); // reproject std::vector image_points; @@ -289,18 +251,27 @@ class SolverImpl { return cost; } -private: - Eigen::Matrix3d R_camera2gimbal_; - Eigen::Matrix3d R_gimbal2world_; - Eigen::Vector3d t_camera2gimbal_; - inline constexpr static const double MaxArmorDistance = 15.0; + data::ArmorCameraSpacing convert_to_ros_spacing(const enumeration::ArmorIdFlag& id, + const Eigen::Vector3d& xyz_in_opencv, const Eigen::Matrix3d& R_armor2camera_opencv) const { + world_exe::data::ArmorCameraSpacing armor_in_camera; + + armor_in_camera.position = util::coordinate::opencv2ros_position(xyz_in_opencv); + Eigen::Matrix3d R_in_ros = util::coordinate::opencv2ros_rotation(R_armor2camera_opencv); + Eigen::Quaterniond orientation_ros(R_in_ros); + orientation_ros.normalize(); + armor_in_camera.id = id; + armor_in_camera.orientation = orientation_ros; - std::time_t time_stamp_ { 0 }; + return armor_in_camera; + } + +private: + Eigen::Matrix3d R_gimbal2camera_; + Eigen::Vector3d t_gimbal2camera_; }; -Solver::Solver(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, - Eigen::Vector3d t_camera2gimbal) - : pimpl_(std::make_unique(R_camera2gimbal, R_gimbal2world, t_camera2gimbal)) { } +Solver::Solver(const std::string& config_path) + : pimpl_(std::make_unique(config_path)) { } Solver::~Solver() = default; const std::time_t Solver::GetTimeStamp() const { return pimpl_->GetTimeStamp(); } diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index 83118f3..f865aeb 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -7,8 +7,7 @@ namespace world_exe::tongji::solver { class SolverImpl; class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStamped { public: - explicit Solver(Eigen::Matrix3d R_camera2gimbal, Eigen::Matrix3d R_gimbal2world, - Eigen::Vector3d t_camera2gimbal); + explicit Solver(const std::string& config_path); ~Solver(); std::shared_ptr SolvePnp( diff --git a/src/util/coordinate.hpp b/src/util/coordinate.hpp index a0c6e96..4f632e0 100644 --- a/src/util/coordinate.hpp +++ b/src/util/coordinate.hpp @@ -16,10 +16,6 @@ static inline Eigen::Matrix3d opencv2ros_rotation(const Eigen::Matrix3d& rotatio } // clangd-format on -static inline Eigen::Quaterniond opencv2ros_orientation(const Eigen::Quaterniond& quat) { - return Eigen::Quaterniond(opencv2ros_rotation(quat.toRotationMatrix())); -} - static inline Eigen::Vector3d ros2opencv_position(const Eigen::Vector3d& position) { return Eigen::Vector3d(-position.y(), -position.z(), position.x()); } @@ -32,8 +28,4 @@ static inline Eigen::Matrix3d ros2opencv_rotation(const Eigen::Matrix3d& rotatio return t.transpose() * rotation * t; } -static inline Eigen::Quaterniond ros2opencv_orientation(const Eigen::Quaterniond& quat) { - return Eigen::Quaterniond(ros2opencv_rotation(quat.toRotationMatrix())); -} - } \ No newline at end of file From adea28efa97ce53cdcd33a66483e0dd232e4f4ce Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 27 Oct 2025 12:17:15 +0800 Subject: [PATCH 62/93] refactor(solver): extract common logic for better code reuse --- src/tongji/solver/reprojection_util.hpp | 156 ++++++++++++++++ src/tongji/solver/solver.cpp | 226 +++++------------------- src/tongji/solver/solver.hpp | 11 ++ 3 files changed, 214 insertions(+), 179 deletions(-) create mode 100644 src/tongji/solver/reprojection_util.hpp diff --git a/src/tongji/solver/reprojection_util.hpp b/src/tongji/solver/reprojection_util.hpp new file mode 100644 index 0000000..d083e5b --- /dev/null +++ b/src/tongji/solver/reprojection_util.hpp @@ -0,0 +1,156 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" +#include "parameters/profile.hpp" +#include "parameters/rm_parameters.hpp" +#include "util/coordinate.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::solver { + +class ReprojectionUtil { +public: + explicit ReprojectionUtil(const Eigen::Matrix3d& R_gimbal2camera, + const Eigen::Vector3d& t_gimbal2camera, const cv::Mat& camera_matrix, + const cv::Mat& distort_coeffs) + : R_gimbal2camera_(R_gimbal2camera) + , t_gimbal2camera_(t_gimbal2camera) + , camera_matrix_(camera_matrix) + , distort_coeffs_(distort_coeffs) { } + + std::vector ReprojectWithYpr(const Eigen::Vector3d& xyz_in_gimbal, + const double& yaw, const double& pitch, const bool& is_large) const { + + double sin_yaw = std::sin(yaw); + double cos_yaw = std::cos(yaw); + + double sin_pitch = std::sin(pitch); + double cos_pitch = std::cos(pitch); + + // clang-format off + const Eigen::Matrix3d R_armor2gimbal { + {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch}, + {sin_yaw * cos_pitch, cos_yaw, sin_yaw * sin_pitch}, + { -sin_pitch, 0, cos_pitch} + }; + // clang-format on + + return ReprojectWithR_gimbal(xyz_in_gimbal, R_armor2gimbal, is_large); + } + + double CalculateYawError(const world_exe::data::ArmorImageSpacing& armor_in_image, + const Eigen::Vector3d& armor_xyz_in_gimbal, const double& armor_yaw_in_gimbal, + const double& inclined) const { + auto pitch = + (armor_in_image.id == enumeration::ArmorIdFlag::Outpost) ? -15.0 * CV_PI / 180.0 : 15.0; + + auto image_points = ReprojectWithYpr( + armor_xyz_in_gimbal, armor_yaw_in_gimbal, pitch, armor_in_image.isLargeArmor); + + auto error = 0.0; + for (int i = 0; i < 4; i++) { + error += cv::norm(armor_in_image.image_points[i] - image_points[i]); + } + + // SJTU_cost + // auto cost = SJTU_cost(image_points, armor_in_image.image_points, inclined); + return error; + } + + double CalculatePitchError(const world_exe::data::ArmorImageSpacing& armor_in_image, + const double& armor_yaw_in_gimbal, const Eigen::Vector3d& armor_xyz_in_gimbal, + const double& pitch) const { + + auto image_points = ReprojectWithYpr( + armor_xyz_in_gimbal, armor_yaw_in_gimbal, pitch, armor_in_image.isLargeArmor); + + auto error = 0.0; + for (int i = 0; i < 4; i++) { + error += cv::norm(armor_in_image.image_points[i] - image_points[i]); + } + + return error; + } + +private: + auto TransformGimbalToCamera(const Eigen::Matrix3d& R_armor2gimbal, + const Eigen::Vector3d& xyz_in_gimbal, cv::Vec3d& rvec_out, cv::Vec3d& tvec_out) const + -> void { + + // R_A->C = R_G->C * R_A->G + Eigen::Matrix3d R_armor2camera = R_gimbal2camera_ * R_armor2gimbal; + // t_A->C = R_G->C * P_G + t_G->C + Eigen::Vector3d t_armor2camera = R_gimbal2camera_ * xyz_in_gimbal + t_gimbal2camera_; + + cv::Mat R_armor2camera_cv; + cv::eigen2cv(util::coordinate::ros2opencv_rotation(R_armor2camera), R_armor2camera_cv); + cv::Rodrigues(R_armor2camera_cv, rvec_out); + + const auto t_armor2camera_cv = util::coordinate::ros2opencv_position(t_armor2camera); + tvec_out = cv::Vec3d(t_armor2camera_cv[0], t_armor2camera_cv[1], t_armor2camera_cv[2]); + } + + std::vector ReprojectWithR_gimbal(const Eigen::Vector3d& armor_xyz_in_gimbal, + const Eigen::Matrix3d& R_armor2gimbal, const bool& is_large) const { + + cv::Vec3d rvec, tvec; + TransformGimbalToCamera(R_armor2gimbal, armor_xyz_in_gimbal, rvec, tvec); + + std::vector image_points; + const auto& object_points = (is_large) + ? parameters::Robomaster::LargeArmorObjectPointsOpencv + : parameters::Robomaster::NormalArmorObjectPointsOpencv; + cv::projectPoints(object_points, rvec, tvec, + parameters::HikCameraProfile::get_intrinsic_parameters(), + parameters::HikCameraProfile::get_distortion_parameters(), image_points); + return image_points; + } + + double SJTU_cost(const std::vector& cv_refs, + const std::vector& cv_pts, const double& inclined) const { + std::size_t size = cv_refs.size(); + std::vector refs; + std::vector pts; + for (std::size_t i = 0u; i < size; ++i) { + refs.emplace_back(cv_refs[i].x, cv_refs[i].y); + pts.emplace_back(cv_pts[i].x, cv_pts[i].y); + } + double cost = 0.; + for (std::size_t i = 0u; i < size; ++i) { + std::size_t p = (i + 1u) % size; + // i - p 构成线段。过程：先移动起点，再补长度，再旋转 + Eigen::Vector2d ref_d = refs[p] - refs[i]; // 标准 + Eigen::Vector2d pt_d = pts[p] - pts[i]; + // 长度差代价 + 起点差代价(1 / 2)（0 度左右应该抛弃) + double pixel_dis = // dis 是指方差平面内到原点的距离 + (0.5 * ((refs[i] - pts[i]).norm() + (refs[p] - pts[p]).norm()) + + std::fabs(ref_d.norm() - pt_d.norm())) + / ref_d.norm(); + double angular_dis = + ref_d.norm() * util::math::get_abs_angle(ref_d, pt_d) / ref_d.norm(); + // 平方可能是为了配合 sin 和 cos + // 弧度差代价（0 度左右占比应该大） + double cost_i = util::math::square(pixel_dis * std::sin(inclined)) + + util::math::square(angular_dis * std::cos(inclined)) + * 2.0; // DETECTOR_ERROR_PIXEL_BY_SLOPE + // 重投影像素误差越大，越相信斜率 + cost += std::sqrt(cost_i); + } + return cost; + } + + Eigen::Matrix3d R_gimbal2camera_; + Eigen::Vector3d t_gimbal2camera_; + const cv::Mat camera_matrix_; + const cv::Mat distort_coeffs_; +}; +} \ No newline at end of file diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index e8cba53..9a9cd50 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -2,11 +2,11 @@ #include #include +#include #include #include #include -#include #include #include "data/armor_camera_spacing.hpp" @@ -15,7 +15,7 @@ #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" #include "solved_armor.hpp" - +#include "tongji/solver/reprojection_util.hpp" #include "util/coordinate.hpp" #include "util/index.hpp" #include "util/math.hpp" @@ -30,62 +30,54 @@ class SolverImpl { Eigen::Matrix3d R_muzzle2camera_ = Eigen::Matrix(R_gimbalcamera_data.data()); t_gimbal2camera_ = Eigen::Matrix(t_gimbal2camera_data.data()); + + reprojection_util_ = std::make_unique(R_gimbal2camera_, t_gimbal2camera_, + parameters::HikCameraProfile::get_intrinsic_parameters(), + parameters::HikCameraProfile::get_distortion_parameters()); } - std::shared_ptr SolvePnp( + std::shared_ptr EstimateAllArmorPoses( std::shared_ptr armors_in_image) { std::vector armor_plates; for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); i++) { const auto& armor_id = util::enumeration::GetArmorIdFlag(i); const auto& armors = armors_in_image->GetArmors(armor_id); + for (const auto& armor : armors) { - auto solved_armor = Solve(armor); + auto solved_armor = EstimatePose(armor); armor_plates.emplace_back(solved_armor); } } return std::make_shared(armor_plates); } - world_exe::data::ArmorCameraSpacing Solve( + data::ArmorCameraSpacing EstimatePose( const world_exe::data::ArmorImageSpacing& armor_in_image) const { - const auto& object_points = armor_in_image.isLargeArmor - ? parameters::Robomaster::LargeArmorObjectPointsOpencv - : parameters::Robomaster::NormalArmorObjectPointsOpencv; - - cv::Vec3d rvec, tvec; - cv::solvePnP(object_points, armor_in_image.image_points, - parameters::HikCameraProfile::get_intrinsic_parameters(), - parameters::HikCameraProfile::get_distortion_parameters(), rvec, tvec, false, - cv::SOLVEPNP_IPPE); - - Eigen::Vector3d xyz_in_camera; - cv::cv2eigen(tvec, xyz_in_camera); + const auto result = EstimatePnp(armor_in_image); - cv::Mat rmat; - cv::Rodrigues(rvec, rmat); - Eigen::Matrix3d R_armor2camera; - cv::cv2eigen(rmat, R_armor2camera); - - const auto armor_in_camera = - convert_to_ros_spacing(armor_in_image.id, xyz_in_camera, R_armor2camera); - return armor_in_camera; + data::ArmorCameraSpacing pose; + pose.id = armor_in_image.id; + pose.orientation = Eigen::Quaterniond(result.R_armor2camera).normalized(); + pose.position = result.xyz_in_camera; + return pose; } - auto OptimizeYaw(const data::ArmorImageSpacing& armor_in_image, const double& gimbal_yaw, - const double& armor_yaw_in_gimbal, const Eigen::Vector3d& armor_xyz_in_gimbal) const - -> const double { - + auto OptimizeYawByReprojection(const data::ArmorImageSpacing& armor_in_image, + const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, + const double& initial_armor_yaw_in_gimbal) const -> const double { constexpr double SEARCH_RANGE = 140; // degree - const auto yaw0 = util::math::clamp_pm_pi(gimbal_yaw - SEARCH_RANGE / 2 * CV_PI / 180.0); auto min_error = 1e10; - auto best_yaw = armor_yaw_in_gimbal; + auto best_yaw = initial_armor_yaw_in_gimbal; + for (int i = 0; i < SEARCH_RANGE; i++) { double yaw = util::math::clamp_pm_pi(yaw0 + i * CV_PI / 180.0); - auto error = ArmorReprojectionError( + + auto error = reprojection_util_->CalculateYawError( armor_in_image, armor_xyz_in_gimbal, yaw, (i - SEARCH_RANGE / 2) * CV_PI / 180.0); + if (error < min_error) { min_error = error; best_yaw = yaw; @@ -99,175 +91,44 @@ class SolverImpl { } private: - std::vector ReprojectArmor(const Eigen::Vector3d& xyz_in_gimbal, const double& yaw, - const bool& is_large, const enumeration::ArmorIdFlag& id) const { - auto sin_yaw = std::sin(yaw); - auto cos_yaw = std::cos(yaw); - - auto pitch = (id == enumeration::ArmorIdFlag::Outpost) ? -15.0 * CV_PI / 180.0 - : 15.0 * CV_PI / 180.0; - auto sin_pitch = std::sin(pitch); - auto cos_pitch = std::cos(pitch); - - // clang-format off - const Eigen::Matrix3d R_armor2gimbal { - {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch}, - {sin_yaw * cos_pitch, cos_yaw, sin_yaw * sin_pitch}, - { -sin_pitch, 0, cos_pitch} - }; - // clang-format on - - // get R_armor2camera t_armor2camera - Eigen::Matrix3d R_armor2camera = R_gimbal2camera_ * R_armor2gimbal; - Eigen::Vector3d t_armor2camera = (R_gimbal2camera_ * xyz_in_gimbal + t_gimbal2camera_); - - // get rvec tvec - cv::Vec3d rvec; - cv::Mat R_armor2camera_cv; - cv::eigen2cv(util::coordinate::ros2opencv_rotation(R_armor2camera), R_armor2camera_cv); - cv::Rodrigues(R_armor2camera_cv, rvec); - const auto t_armor2camera_cv = util::coordinate::ros2opencv_position(t_armor2camera); - cv::Vec3d tvec(t_armor2camera_cv[0], t_armor2camera_cv[1], t_armor2camera_cv[2]); - - // reproject - std::vector image_points; - const auto& object_points = (is_large) + auto EstimatePnp(const world_exe::data::ArmorImageSpacing& armor_in_image) const + -> const PnPResultInGimbal { + const auto& object_points = armor_in_image.isLargeArmor ? parameters::Robomaster::LargeArmorObjectPointsOpencv : parameters::Robomaster::NormalArmorObjectPointsOpencv; - cv::projectPoints(object_points, rvec, tvec, - parameters::HikCameraProfile::get_intrinsic_parameters(), - parameters::HikCameraProfile::get_distortion_parameters(), image_points); - return image_points; - } - - double ArmorReprojectionError(const world_exe::data::ArmorImageSpacing& armor_in_image, - const Eigen::Vector3d& armor_xyz_in_gimbal, const double& yaw, - const double& inclined) const { - const auto image_points = ReprojectArmor( - armor_xyz_in_gimbal, yaw, armor_in_image.isLargeArmor, armor_in_image.id); - auto error = 0.0; - for (int i = 0; i < 4; i++) - error += cv::norm(armor_in_image.image_points[i] - image_points[i]); - // auto error = SJTU_cost(image_points, armor_in_image.image_points, inclined); - - return error; - } - - double OutpostReprojectionError(world_exe::data::ArmorImageSpacing armor_in_image, - Eigen::Vector3d armor_xyz_in_gimbal, const double& pitch) const { - - // solve - const auto& object_points = (armor_in_image.isLargeArmor) - ? world_exe::parameters::Robomaster::LargeArmorObjectPointsOpencv - : world_exe::parameters::Robomaster::NormalArmorObjectPointsOpencv; cv::Vec3d rvec, tvec; cv::solvePnP(object_points, armor_in_image.image_points, - world_exe::parameters::HikCameraProfile::get_intrinsic_parameters(), + parameters::HikCameraProfile::get_intrinsic_parameters(), parameters::HikCameraProfile::get_distortion_parameters(), rvec, tvec, false, cv::SOLVEPNP_IPPE); + // 1. P_C_cv -> P_C_ros (位置) Eigen::Vector3d xyz_in_camera_cv; cv::cv2eigen(tvec, xyz_in_camera_cv); - auto xyz_in_camera = util::coordinate::opencv2ros_position(xyz_in_camera_cv); + const auto xyz_in_camera = util::coordinate::opencv2ros_position(xyz_in_camera_cv); + // 2. R_A->C_cv -> R_A->C_ros (姿态) cv::Mat rmat; cv::Rodrigues(rvec, rmat); Eigen::Matrix3d R_armor2camera_cv; cv::cv2eigen(rmat, R_armor2camera_cv); - auto R_armor2camera = util::coordinate::opencv2ros_rotation(R_armor2camera_cv); - - Eigen::Matrix3d R_armor2gimbal = R_gimbal2camera_.transpose() * R_armor2camera; - auto armor_ypr_in_gimbal = util::math::matrix_to_euler(R_armor2gimbal, 2, 1, 0); - - auto yaw = armor_ypr_in_gimbal[0]; - auto sin_yaw = std::sin(yaw); - auto cos_yaw = std::cos(yaw); - - auto sin_pitch = std::sin(pitch); - auto cos_pitch = std::cos(pitch); - - // clang-format off - const Eigen::Matrix3d _R_armor2gimbal { - {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch}, - {sin_yaw * cos_pitch, cos_yaw, sin_yaw * sin_pitch}, - { -sin_pitch, 0, cos_pitch}}; - // clang-format on + const auto R_armor2camera = util::coordinate::opencv2ros_rotation(R_armor2camera_cv); - // get R_armor2camera t_armor2camera - Eigen::Matrix3d _R_armor2camera = R_gimbal2camera_ * _R_armor2gimbal; - Eigen::Vector3d t_armor2camera = R_gimbal2camera_ * armor_xyz_in_gimbal + t_gimbal2camera_; + Eigen::Matrix3d R_camera2gimbal = R_gimbal2camera_.transpose(); + Eigen::Vector3d t_camera2gimbal = -R_camera2gimbal * t_gimbal2camera_; - // get rvec tvec - cv::Vec3d _rvec; - cv::Mat _R_armor2camera_cv; - cv::eigen2cv(util::coordinate::ros2opencv_rotation(_R_armor2camera), _R_armor2camera_cv); - cv::Rodrigues(_R_armor2camera_cv, _rvec); - const auto t_armor2camera_cv = util::coordinate::ros2opencv_position(t_armor2camera); - cv::Vec3d _tvec(t_armor2camera_cv[0], t_armor2camera_cv[1], t_armor2camera_cv[2]); + Eigen::Vector3d xyz_in_gimbal = R_camera2gimbal * xyz_in_camera + t_camera2gimbal; + Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal * R_armor2camera; - // reproject - std::vector image_points; - cv::projectPoints(object_points, _rvec, _tvec, - parameters::HikCameraProfile::get_intrinsic_parameters(), - parameters::HikCameraProfile::get_distortion_parameters(), image_points); - - auto error = 0.0; - for (int i = 0; i < 4; i++) - error += cv::norm(armor_in_image.image_points[i] - image_points[i]); - return error; - } - - double SJTU_cost(const std::vector& cv_refs, - const std::vector& cv_pts, const double& inclined) const { - std::size_t size = cv_refs.size(); - std::vector refs; - std::vector pts; - for (std::size_t i = 0u; i < size; ++i) { - refs.emplace_back(cv_refs[i].x, cv_refs[i].y); - pts.emplace_back(cv_pts[i].x, cv_pts[i].y); - } - double cost = 0.; - for (std::size_t i = 0u; i < size; ++i) { - std::size_t p = (i + 1u) % size; - // i - p 构成线段。过程：先移动起点，再补长度，再旋转 - Eigen::Vector2d ref_d = refs[p] - refs[i]; // 标准 - Eigen::Vector2d pt_d = pts[p] - pts[i]; - // 长度差代价 + 起点差代价(1 / 2)（0 度左右应该抛弃) - double pixel_dis = // dis 是指方差平面内到原点的距离 - (0.5 * ((refs[i] - pts[i]).norm() + (refs[p] - pts[p]).norm()) - + std::fabs(ref_d.norm() - pt_d.norm())) - / ref_d.norm(); - double angular_dis = - ref_d.norm() * util::math::get_abs_angle(ref_d, pt_d) / ref_d.norm(); - // 平方可能是为了配合 sin 和 cos - // 弧度差代价（0 度左右占比应该大） - double cost_i = util::math::square(pixel_dis * std::sin(inclined)) - + util::math::square(angular_dis * std::cos(inclined)) - * 2.0; // DETECTOR_ERROR_PIXEL_BY_SLOPE - // 重投影像素误差越大，越相信斜率 - cost += std::sqrt(cost_i); - } - return cost; - } - - data::ArmorCameraSpacing convert_to_ros_spacing(const enumeration::ArmorIdFlag& id, - const Eigen::Vector3d& xyz_in_opencv, const Eigen::Matrix3d& R_armor2camera_opencv) const { - world_exe::data::ArmorCameraSpacing armor_in_camera; - - armor_in_camera.position = util::coordinate::opencv2ros_position(xyz_in_opencv); - Eigen::Matrix3d R_in_ros = util::coordinate::opencv2ros_rotation(R_armor2camera_opencv); - Eigen::Quaterniond orientation_ros(R_in_ros); - orientation_ros.normalize(); - armor_in_camera.id = id; - armor_in_camera.orientation = orientation_ros; - - return armor_in_camera; + return { xyz_in_gimbal, R_armor2gimbal, xyz_in_camera, R_armor2camera }; } private: Eigen::Matrix3d R_gimbal2camera_; Eigen::Vector3d t_gimbal2camera_; + + std::unique_ptr reprojection_util_; }; Solver::Solver(const std::string& config_path) @@ -278,7 +139,14 @@ const std::time_t Solver::GetTimeStamp() const { return pimpl_->GetTimeStamp(); std::shared_ptr Solver::SolvePnp( std::shared_ptr armors_in_image) { - return pimpl_->SolvePnp(armors_in_image); + return pimpl_->EstimateAllArmorPoses(armors_in_image); +} + +auto Solver::OptimizeYawByReprojection(const data::ArmorImageSpacing& armor_in_image, + const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, + const double& initial_armor_yaw_in_gimbal) const -> const double { + return OptimizeYawByReprojection( + armor_in_image, armor_xyz_in_gimbal, gimbal_yaw, initial_armor_yaw_in_gimbal); } } \ No newline at end of file diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index f865aeb..d4a1b92 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -5,6 +5,13 @@ namespace world_exe::tongji::solver { class SolverImpl; + +struct PnPResultInGimbal { + Eigen::Vector3d xyz_in_gimbal; + Eigen::Matrix3d R_armor2gimbal; + Eigen::Vector3d xyz_in_camera; + Eigen::Matrix3d R_armor2camera; +}; class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStamped { public: explicit Solver(const std::string& config_path); @@ -15,6 +22,10 @@ class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStam const std::time_t GetTimeStamp() const override; + auto OptimizeYawByReprojection(const data::ArmorImageSpacing& armor_in_image, + const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, + const double& initial_armor_yaw_in_gimbal) const -> const double; + Solver(const Solver&) = delete; Solver& operator=(const Solver&) = delete; Solver(Solver&&) noexcept = default; From e982df89c88eaaae1fbc0a72b91ecde28dd80680 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 27 Oct 2025 23:17:04 +0800 Subject: [PATCH 63/93] fix(pimpl): correct PIMPL implementation errors --- .../fire_controller/fire_controller.cpp | 11 +++---- .../fire_controller/fire_controller.hpp | 5 +-- src/tongji/identifier/identifier.cpp | 12 +++---- src/tongji/identifier/identifier.hpp | 6 ++-- .../live_target_manager.cpp | 33 +++++++------------ .../live_target_manager.hpp | 12 +++---- .../target_snapshot_manager.cpp | 17 +++++----- .../target_snapshot_manager.hpp | 6 ++-- src/tongji/solver/reprojection_util.hpp | 13 +++----- src/tongji/solver/solver.cpp | 10 +++--- src/tongji/solver/solver.hpp | 5 +-- src/tongji/state_machine/state_machine.cpp | 9 ++--- src/tongji/state_machine/state_machine.hpp | 5 +-- 13 files changed, 63 insertions(+), 81 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 754851b..c9ed459 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -23,9 +23,9 @@ using CarIDFlag = enumeration::CarIDFlag; using LiveTargetManager = predictor::LiveTargetManager; using TimeStamp = time_stamp::TimeStamp; -class FireControllerImpl { +class FireController::Impl { public: - FireControllerImpl(const std::string& config_path, + Impl(const std::string& config_path, std::shared_ptr state_machine, std::shared_ptr live_target_manager) : allowed_target_id_(CarIDFlag::None) @@ -63,7 +63,7 @@ class FireControllerImpl { std::dynamic_pointer_cast(snapshot_manager)->GetGimbalCommand(); auto fire_command = - fire_decision_->ShouldFire(gimbal_yaw_,gimbal_command, target_gimbal_spacing.position); + fire_decision_->ShouldFire(gimbal_yaw_, gimbal_command, target_gimbal_spacing.position); firable_ = fire_command; data::FireControl result; @@ -105,9 +105,8 @@ class FireControllerImpl { FireController::FireController(const std::string& config_path, std::shared_ptr state_machine, std::shared_ptr live_target_manager) - : pimpl_( - std::make_unique(config_path, state_machine, live_target_manager)) { } - + : pimpl_(std::make_unique(config_path, state_machine, live_target_manager)) { } +FireController::~FireController() = default; const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { return pimpl_->CalculateTarget(time_duration); } diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 8ef5630..76d41c1 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -10,12 +10,12 @@ namespace world_exe::tongji::fire_control { -class FireControllerImpl; class FireController final : public interfaces::IFireControl { public: FireController(const std::string& config_path, std::shared_ptr state_machine, std::shared_ptr live_target_manager); + ~FireController(); const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; @@ -30,7 +30,8 @@ class FireController final : public interfaces::IFireControl { FireController& operator=(FireController&&) noexcept = default; private: - std::unique_ptr pimpl_; + class Impl; + std::unique_ptr pimpl_; }; } \ No newline at end of file diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index a8f9faf..973234b 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -26,11 +26,11 @@ #include "util/stringifier.hpp" namespace world_exe::tongji::identifier { - -class IdentifierImpl { +// TODO:need to refact +class Identifier::Impl { public: - explicit IdentifierImpl(const std::string& config_path, const std::string& save_path, - const bool& debug, const bool& record) + explicit Impl(const std::string& config_path, const std::string& save_path, const bool& debug, + const bool& record) : classifier_(std::make_unique(config_path)) , save_path_(save_path) , debug_(debug) @@ -341,7 +341,7 @@ class IdentifierImpl { Identifier::Identifier(const std::string& config_path, const std::string& save_path, const bool& debug, const bool& record) - : pimpl_(std::make_unique(config_path, save_path, debug, record)) { } + : pimpl_(std::make_unique(config_path, save_path, debug, record)) { } Identifier::~Identifier() = default; const std::tuple, enumeration::CarIDFlag> @@ -350,4 +350,4 @@ Identifier::identify(const cv::Mat& input_image) { } void Identifier::SetTargetColor(Color target_color) { return pimpl_->SetTargetColor(target_color); } -} \ No newline at end of file +} diff --git a/src/tongji/identifier/identifier.hpp b/src/tongji/identifier/identifier.hpp index 8ca44ce..85dcc65 100644 --- a/src/tongji/identifier/identifier.hpp +++ b/src/tongji/identifier/identifier.hpp @@ -38,7 +38,6 @@ struct Lightbar { } }; -class IdentifierImpl; class Identifier final : public interfaces::IIdentifier { public: explicit Identifier(const std::string& config_path, const std::string& save_path, @@ -56,7 +55,8 @@ class Identifier final : public interfaces::IIdentifier { Identifier& operator=(Identifier&&) noexcept = default; private: - std::unique_ptr pimpl_; + class Impl; + std::unique_ptr pimpl_; }; -} \ No newline at end of file +} diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp index e8c867e..34892c4 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -5,8 +5,6 @@ #include #include -#include - #include "../in_gimbal_control_armor.hpp" #include "../target_snapshot_manager/target_snapshot_manager.hpp" #include "enum/armor_id.hpp" @@ -19,15 +17,13 @@ namespace world_exe::tongji::predictor { -class LiveTargetManagerImpl { +class LiveTargetManager::Impl { public: - LiveTargetManagerImpl( - const std::string& config_path, const double& time_delay, const double& timeout_sec) + Impl(const std::string& config_path, const double& timeout_sec) : targets_map_() , tracker_(std::make_unique()) , last_update_timestamp_(std::time(nullptr)) , tracking_id_(enumeration::CarIDFlag::None) - , time_delay_(time_delay) , config_path_(config_path) { } std::shared_ptr Predict( @@ -52,17 +48,15 @@ class LiveTargetManagerImpl { if (targets_map_.empty()) return nullptr; // TODO return std::make_shared( - config_path_, flag, targets_map_, last_update_timestamp_, bullet_speed_); + config_path_, flag, targets_map_, last_update_timestamp_); } void Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const std::time_t& now, - const double& bullet_speed) { + const std::shared_ptr& armors_in_image, const std::time_t& now) { UpdateTimeStamp(data->GetTimeStamped().GetTimeStamp()); UpdateTargetMap(data, now); UpdateTarget(data, armors_in_image, now); - UpdateBulletSpeed(bullet_speed); } auto GetAllowedTargetID() const -> enumeration::CarIDFlag const { @@ -73,7 +67,6 @@ class LiveTargetManagerImpl { } private: - void UpdateBulletSpeed(const double& bullet_speed) { bullet_speed_ = bullet_speed; } void UpdateTimeStamp(const time_t& time_stamp) { last_update_timestamp_ = time_stamp; } void UpdateTargetMap( std::shared_ptr data, const std::time_t& now) { @@ -89,7 +82,7 @@ class LiveTargetManagerImpl { const auto& armors_list = armors_interface->GetArmors(id); if (armors_list.empty()) return; - const auto& armor = armors_list.front(); + const auto& armor = armors_list.front(); const Eigen::Vector3d xyz_in_gimbal = transform * armor.position; const Eigen::Vector3d ypr_in_gimbal = rotation_matrix.eulerAngles(2, 1, 0); // ZYX targets_map_[id] = @@ -108,7 +101,7 @@ class LiveTargetManagerImpl { const auto& armors_list = armors_interface->GetArmors(tracking_id_); if (armors_list.empty()) return; - const auto& armor = armors_list.front(); + const auto& armor = armors_list.front(); const Eigen::Vector3d xyz_in_gimbal = transform * armor.position; const Eigen::Vector3d ypr_in_gimbal = rotation_matrix.eulerAngles(2, 1, 0); // ZYX targets_map_[tracking_id_]->Update(static_cast(now - last_update_timestamp_), @@ -120,14 +113,11 @@ class LiveTargetManagerImpl { std::time_t last_update_timestamp_; enumeration::CarIDFlag tracking_id_; - double bullet_speed_; - const double time_delay_; const std::string config_path_; }; -LiveTargetManager::LiveTargetManager( - const std::string& config_path, const double& time_delay, const double& timeout_sec) - : pimpl_(std::make_unique(config_path, time_delay, timeout_sec)) { } +LiveTargetManager::LiveTargetManager(const std::string& config_path, const double& timeout_sec) + : pimpl_(std::make_unique(config_path, timeout_sec)) { } LiveTargetManager::~LiveTargetManager() = default; std ::shared_ptr LiveTargetManager::Predict( @@ -140,11 +130,10 @@ std ::shared_ptr LiveTargetManager::GetPredictor( } void LiveTargetManager::Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const std::time_t& now, - const double& bullet_speed) { - return pimpl_->Update(data, armors_in_image, now, bullet_speed); + const std::shared_ptr& armors_in_image, const std::time_t& now) { + return pimpl_->Update(data, armors_in_image, now); } auto LiveTargetManager::GetAllowedTargetID() const -> enumeration::ArmorIdFlag const { return pimpl_->GetAllowedTargetID(); } -} \ No newline at end of file +} diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.hpp b/src/tongji/predictor/live_target_manager/live_target_manager.hpp index 039f725..2f43584 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.hpp @@ -9,11 +9,9 @@ namespace world_exe::tongji::predictor { -class LiveTargetManagerImpl; class LiveTargetManager final : public interfaces::ITargetPredictor { public: - LiveTargetManager( - const std::string& config_path, const double& time_delay, const double& timeout_sec=0.1); + LiveTargetManager(const std::string& config_path, const double& timeout_sec = 0.1); ~LiveTargetManager(); std ::shared_ptr Predict( @@ -22,8 +20,7 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { const enumeration ::ArmorIdFlag& id) const override; void Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const std::time_t& now, - const double& bullet_speed); + const std::shared_ptr& armors_in_image, const std::time_t& now); auto GetAllowedTargetID() const -> enumeration::ArmorIdFlag const; @@ -33,7 +30,8 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { LiveTargetManager& operator=(LiveTargetManager&&) noexcept = default; private: - std::unique_ptr pimpl_; + class Impl; + std::unique_ptr pimpl_; }; -} \ No newline at end of file +} diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp index 7483d22..389de31 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -14,17 +14,16 @@ namespace world_exe::tongji::predictor { -class TargetSnapshotManagerImpl { +class TargetSnapshotManager::Impl { public: - TargetSnapshotManagerImpl(const std::string& config_path, const enumeration::ArmorIdFlag& id, + Impl(const std::string& config_path, const enumeration::ArmorIdFlag& id, const std::unordered_map>& live_target_map, - const std::time_t& now, const double& bullet_speed) + const std::time_t& now) : aim_solver_(std::make_unique(config_path)) , snapshots_(BuildSnapshots(live_target_map)) , now_(now) , ids_(id) - , bullet_speed_(bullet_speed) , gimbal_command_({ std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN() }) { @@ -33,6 +32,7 @@ class TargetSnapshotManagerImpl { decision_speed_ = yaml["decision_speed"].as(); high_speed_delay_time_ = yaml["high_speed_delay_time"].as(); low_speed_delay_time_ = yaml["low_speed_delay_time"].as(); + bullet_speed_ = yaml["bullet_spped"].as(); } const enumeration::ArmorIdFlag& GetId() const { return ids_; } @@ -85,7 +85,7 @@ class TargetSnapshotManagerImpl { const std::unordered_map snapshots_; const std::time_t& now_; const enumeration::ArmorIdFlag ids_; - const double bullet_speed_; + double bullet_speed_; mutable GimbalCommand gimbal_command_; double decision_speed_; double high_speed_delay_time_; @@ -97,9 +97,8 @@ TargetSnapshotManager::TargetSnapshotManager(const std::string& config_path, const enumeration::ArmorIdFlag& id, const std::unordered_map>& live_target_map, - const std::time_t& now, const double& bullet_speed) - : pimpl_(std::make_unique( - config_path, id, live_target_map, now, bullet_speed)) { } + const std::time_t& now) + : pimpl_(std::make_unique(config_path, id, live_target_map, now)) { } TargetSnapshotManager::~TargetSnapshotManager() = default; const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } @@ -111,4 +110,4 @@ std ::shared_ptr TargetSnapshotManager::Predi auto TargetSnapshotManager::GetGimbalCommand() const -> GimbalCommand const { return pimpl_->GetGimbalCommand(); } -} \ No newline at end of file +} diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp index 3187731..4db47e6 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp @@ -8,7 +8,6 @@ #include "interfaces/predictor.hpp" namespace world_exe::tongji::predictor { -class TargetSnapshotManagerImpl; struct GimbalCommand { double yaw; double pitch; @@ -19,7 +18,7 @@ class TargetSnapshotManager final : public interfaces::IPredictor { TargetSnapshotManager(const std::string& config_path, const enumeration::ArmorIdFlag& id, const std::unordered_map>& live_target_map, - const std::time_t& now, const double& bullet_speed); + const std::time_t& now); ~TargetSnapshotManager(); const enumeration ::ArmorIdFlag& GetId() const override; @@ -34,6 +33,7 @@ class TargetSnapshotManager final : public interfaces::IPredictor { TargetSnapshotManager& operator=(TargetSnapshotManager&&) noexcept = default; private: - std::unique_ptr pimpl_; + class Impl; + std::unique_ptr pimpl_; }; } diff --git a/src/tongji/solver/reprojection_util.hpp b/src/tongji/solver/reprojection_util.hpp index d083e5b..b02c9c9 100644 --- a/src/tongji/solver/reprojection_util.hpp +++ b/src/tongji/solver/reprojection_util.hpp @@ -19,13 +19,10 @@ namespace world_exe::tongji::solver { class ReprojectionUtil { public: - explicit ReprojectionUtil(const Eigen::Matrix3d& R_gimbal2camera, - const Eigen::Vector3d& t_gimbal2camera, const cv::Mat& camera_matrix, - const cv::Mat& distort_coeffs) + explicit ReprojectionUtil( + const Eigen::Matrix3d& R_gimbal2camera, const Eigen::Vector3d& t_gimbal2camera) : R_gimbal2camera_(R_gimbal2camera) - , t_gimbal2camera_(t_gimbal2camera) - , camera_matrix_(camera_matrix) - , distort_coeffs_(distort_coeffs) { } + , t_gimbal2camera_(t_gimbal2camera) { } std::vector ReprojectWithYpr(const Eigen::Vector3d& xyz_in_gimbal, const double& yaw, const double& pitch, const bool& is_large) const { @@ -150,7 +147,5 @@ class ReprojectionUtil { Eigen::Matrix3d R_gimbal2camera_; Eigen::Vector3d t_gimbal2camera_; - const cv::Mat camera_matrix_; - const cv::Mat distort_coeffs_; }; -} \ No newline at end of file +} diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 9a9cd50..09183e2 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -21,9 +21,9 @@ #include "util/math.hpp" namespace world_exe::tongji::solver { -class SolverImpl { +class Solver::Impl { public: - SolverImpl(const std::string& config_path) { + Impl(const std::string& config_path) { const auto yaml = YAML::LoadFile(config_path); auto R_gimbalcamera_data = yaml["R_gimbal2camera"].as>(); auto t_gimbal2camera_data = yaml["t_gimbal2camera"].as>(); @@ -31,9 +31,7 @@ class SolverImpl { Eigen::Matrix(R_gimbalcamera_data.data()); t_gimbal2camera_ = Eigen::Matrix(t_gimbal2camera_data.data()); - reprojection_util_ = std::make_unique(R_gimbal2camera_, t_gimbal2camera_, - parameters::HikCameraProfile::get_intrinsic_parameters(), - parameters::HikCameraProfile::get_distortion_parameters()); + reprojection_util_ = std::make_unique(R_gimbal2camera_, t_gimbal2camera_); } std::shared_ptr EstimateAllArmorPoses( @@ -132,7 +130,7 @@ class SolverImpl { }; Solver::Solver(const std::string& config_path) - : pimpl_(std::make_unique(config_path)) { } + : pimpl_(std::make_unique(config_path)) { } Solver::~Solver() = default; const std::time_t Solver::GetTimeStamp() const { return pimpl_->GetTimeStamp(); } diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index d4a1b92..d7c6a44 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -4,7 +4,6 @@ #include "interfaces/time_stamped.hpp" namespace world_exe::tongji::solver { -class SolverImpl; struct PnPResultInGimbal { Eigen::Vector3d xyz_in_gimbal; @@ -32,7 +31,9 @@ class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStam Solver& operator=(Solver&&) noexcept = default; private: - std::unique_ptr pimpl_; + class Impl; + + std::unique_ptr pimpl_; }; } \ No newline at end of file diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index c6a70ea..dc570e4 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -8,14 +8,14 @@ namespace world_exe::tongji::state_machine { -class StateMachineImpl { +class StateMachine::Impl { public: - StateMachineImpl(std::shared_ptr live_target_manager) + Impl(std::shared_ptr live_target_manager) : live_target_manager_(std::move(live_target_manager)) { } const enumeration::CarIDFlag& GetAllowdToFires() const { return target_ids_; } - void Update() { + auto Update() -> void { auto live_target_manager = std::dynamic_pointer_cast(live_target_manager_); target_ids_ = live_target_manager->GetAllowedTargetID(); @@ -28,11 +28,12 @@ class StateMachineImpl { StateMachine::StateMachine( std::shared_ptr live_target_manager) - : pimpl_(std::make_unique(live_target_manager)) { } + : pimpl_(std::make_unique(live_target_manager)) { } StateMachine::~StateMachine() = default; const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return pimpl_->GetAllowdToFires(); } +auto StateMachine::Update() const -> void { return pimpl_->Update(); } } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index b5d1f73..47eb042 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -8,7 +8,6 @@ #include "interfaces/target_predictor.hpp" namespace world_exe::tongji::state_machine { -class StateMachineImpl; class StateMachine final : public interfaces::ICarState { public: StateMachine(); @@ -16,6 +15,7 @@ class StateMachine final : public interfaces::ICarState { const enumeration ::CarIDFlag& GetAllowdToFires() const override; StateMachine(std::shared_ptr live_target_manager); + auto Update() const -> void; StateMachine(const StateMachine&) = delete; StateMachine& operator=(const StateMachine&) = delete; @@ -23,6 +23,7 @@ class StateMachine final : public interfaces::ICarState { StateMachine& operator=(StateMachine&&) noexcept = default; private: - std::unique_ptr pimpl_; + class Impl; + std::unique_ptr pimpl_; }; } \ No newline at end of file From 2b7f99776d94c01896adee6546b60aa80eb7a3c0 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 28 Oct 2025 13:28:26 +0800 Subject: [PATCH 64/93] fix: resolve solver redundancies and predictor timestamp issues --- .../fire_controller/fire_controller.cpp | 20 ++-- .../live_target_manager/live_target.hpp | 1 + .../live_target_manager.cpp | 16 ++- .../live_target_manager.hpp | 3 +- .../predictor/live_target_manager/tracker.hpp | 3 +- .../target_snapshot_manager/aim_solver.hpp | 3 +- .../target_snapshot.hpp | 17 +-- .../target_snapshot_manager.cpp | 4 +- src/tongji/solver/reprojection_util.hpp | 103 ++++++------------ src/tongji/solver/solver.cpp | 59 +++++----- src/tongji/solver/solver.hpp | 8 +- 11 files changed, 99 insertions(+), 138 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index c9ed459..a0b79e0 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -25,8 +25,7 @@ using TimeStamp = time_stamp::TimeStamp; class FireController::Impl { public: - Impl(const std::string& config_path, - std::shared_ptr state_machine, + Impl(const std::string& config_path, std::shared_ptr state_machine, std::shared_ptr live_target_manager) : allowed_target_id_(CarIDFlag::None) , firable_(false) @@ -36,13 +35,12 @@ class FireController::Impl { auto yaml = YAML::LoadFile(config_path); control_delay_ = yaml["control_delay"].as(); - bullet_speed_ = yaml["bullet_speed"].as(); } - // TODO:std::time_t + // TODO:time_duration 没有使用 const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { - if (!identified_armors_ || !fire_decision_ || !state_machine_ || !live_target_manager_) + if (!fire_decision_ || !state_machine_ || !live_target_manager_) return { .fire_allowance = false }; auto converged_cars = state_machine_->GetAllowdToFires(); @@ -54,8 +52,10 @@ class FireController::Impl { .fire_allowance = false }; - auto armors_in_gimbal = snapshot_manager->Predictor(control_delay_); - allowed_target_id_ = state_machine_->GetAllowdToFires(); + // TODO:接口语义不明 + auto armors_in_gimbal = snapshot_manager->Predictor( + std::chrono::steady_clock::now().time_since_epoch().count()); + allowed_target_id_ = state_machine_->GetAllowdToFires(); auto target_gimbal_spacing = armors_in_gimbal->GetArmors(allowed_target_id_).front(); @@ -79,20 +79,14 @@ class FireController::Impl { } void Update(std::shared_ptr armors, const double& gimbal_yaw) { - UpdateIdentifiedArmor(armors); UpdateGimbalPosition(gimbal_yaw); } private: - void UpdateIdentifiedArmor(std::shared_ptr armors) { - identified_armors_ = armors; - } void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; double gimbal_yaw_; double control_delay_; - double bullet_speed_; - std::shared_ptr identified_armors_; mutable CarIDFlag allowed_target_id_; mutable double firable_; diff --git a/src/tongji/predictor/live_target_manager/live_target.hpp b/src/tongji/predictor/live_target_manager/live_target.hpp index 2382504..7e45e2b 100644 --- a/src/tongji/predictor/live_target_manager/live_target.hpp +++ b/src/tongji/predictor/live_target_manager/live_target.hpp @@ -92,6 +92,7 @@ class LiveTarget { // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_->x[8], ekf_->x[9]); return true; } + // TODO:need to update correctly void Update_ypda(const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal, diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp index 34892c4..13a0972 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -55,7 +55,7 @@ class LiveTargetManager::Impl { const std::shared_ptr& armors_in_image, const std::time_t& now) { UpdateTimeStamp(data->GetTimeStamped().GetTimeStamp()); - UpdateTargetMap(data, now); + UpdateTargetMap(data); UpdateTarget(data, armors_in_image, now); } @@ -68,8 +68,7 @@ class LiveTargetManager::Impl { private: void UpdateTimeStamp(const time_t& time_stamp) { last_update_timestamp_ = time_stamp; } - void UpdateTargetMap( - std::shared_ptr data, const std::time_t& now) { + void UpdateTargetMap(std::shared_ptr data) { const Eigen::Affine3d transform = data->GetTransform(); const Eigen::Matrix3d rotation_matrix = transform.linear(); const auto armors_interface = data->GetArmors(); @@ -85,18 +84,17 @@ class LiveTargetManager::Impl { const auto& armor = armors_list.front(); const Eigen::Vector3d xyz_in_gimbal = transform * armor.position; const Eigen::Vector3d ypr_in_gimbal = rotation_matrix.eulerAngles(2, 1, 0); // ZYX - targets_map_[id] = - std::move(std::make_shared(xyz_in_gimbal, ypr_in_gimbal, id)); + targets_map_[id] = std::make_shared(xyz_in_gimbal, ypr_in_gimbal, id); } } void UpdateTarget(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const std::time_t& now) { + const std::shared_ptr& armors_in_image, const double& dt) { const Eigen::Affine3d transform = data->GetTransform(); const Eigen::Matrix3d rotation_matrix = transform.linear(); const auto armors_interface = data->GetArmors(); - tracking_id_ = tracker_->SelectTrackingTargetID(armors_in_image, now); + tracking_id_ = tracker_->SelectTrackingTargetID(armors_in_image); const auto& armors_list = armors_interface->GetArmors(tracking_id_); if (armors_list.empty()) return; @@ -104,8 +102,8 @@ class LiveTargetManager::Impl { const auto& armor = armors_list.front(); const Eigen::Vector3d xyz_in_gimbal = transform * armor.position; const Eigen::Vector3d ypr_in_gimbal = rotation_matrix.eulerAngles(2, 1, 0); // ZYX - targets_map_[tracking_id_]->Update(static_cast(now - last_update_timestamp_), - xyz_in_gimbal, ypr_in_gimbal, util::math::xyz2ypd(xyz_in_gimbal)); + targets_map_[tracking_id_]->Update( + dt, xyz_in_gimbal, ypr_in_gimbal, util::math::xyz2ypd(xyz_in_gimbal)); } std::unordered_map> targets_map_; diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.hpp b/src/tongji/predictor/live_target_manager/live_target_manager.hpp index 2f43584..980e03a 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.hpp @@ -20,7 +20,8 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { const enumeration ::ArmorIdFlag& id) const override; void Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const std::time_t& now); + const std::shared_ptr& armors_in_image, + const std::time_t& time_stamp); auto GetAllowedTargetID() const -> enumeration::ArmorIdFlag const; diff --git a/src/tongji/predictor/live_target_manager/tracker.hpp b/src/tongji/predictor/live_target_manager/tracker.hpp index 6a37b20..d2f7a85 100644 --- a/src/tongji/predictor/live_target_manager/tracker.hpp +++ b/src/tongji/predictor/live_target_manager/tracker.hpp @@ -38,8 +38,7 @@ class Tracker final { ~Tracker() = default; - auto SelectTrackingTargetID(const std::shared_ptr& armors_in_image, - const std::time_t& now) noexcept -> enumeration::ArmorIdFlag const { + auto SelectTrackingTargetID(const std::shared_ptr& armors_in_image) noexcept -> enumeration::ArmorIdFlag const { CheckCameraOffline(); last_track_timestamp_ = std::chrono::steady_clock::now(); diff --git a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp index f991480..f65ac65 100644 --- a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp +++ b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp @@ -73,8 +73,9 @@ class AimingSolver { private: std::optional SelectPredictedAim( const std::shared_ptr& snapshot, const double& dt) const { + const auto& [selectable, aim_point_in_gimbal] = aim_point_chooser_->ChooseAimArmor( - snapshot->Predict(dt), snapshot->GetPredictedXYZAList(dt), snapshot->GetID()); + snapshot->GetPredictedX(dt), snapshot->GetPredictedXYZAList(dt), snapshot->GetID()); if (!selectable) return std::nullopt; return aim_point_in_gimbal; diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp index f8a4fe3..3c2e22e 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp @@ -36,18 +36,19 @@ class TargetSnapshot { // } // TODO： auto GetPredictedXYZAList(const double& dt) -> std::vector const { - return model_.GetArmorXYZAList(this->Predict(dt)); + const auto [x_n, P_n] = ekf_.PredictOnce(dt); + return model_.GetArmorXYZAList(x_n); } - auto GetTimeStamp() const { return time_stamp_; } - auto GetID() const { return model_.GetID(); } - auto GetEkfX() const { return ekf_.x; } - - auto Predict(const double& dt) -> Eigen::Vector const { - auto predicted_x = model_.f(ekf_.x, dt); - return predicted_x; + auto GetPredictedX(const double& dt) const -> const auto { + const auto& [x_n, P_n] = ekf_.PredictOnce(dt); + return x_n; } + auto GetTimeStamp() const -> const auto { return time_stamp_; } + auto GetID() const -> const auto { return model_.GetID(); } + auto GetEkfX() const { return ekf_.x; } + private: PredictorModel model_; ExtendedKalmanFilter ekf_; diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp index 389de31..ce40b0b 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -32,7 +32,7 @@ class TargetSnapshotManager::Impl { decision_speed_ = yaml["decision_speed"].as(); high_speed_delay_time_ = yaml["high_speed_delay_time"].as(); low_speed_delay_time_ = yaml["low_speed_delay_time"].as(); - bullet_speed_ = yaml["bullet_spped"].as(); + bullet_speed_ = yaml["bullet_speed"].as(); } const enumeration::ArmorIdFlag& GetId() const { return ids_; } @@ -47,7 +47,7 @@ class TargetSnapshotManager::Impl { auto ekf_x = snapshot.GetEkfX(); const auto delay_time = ekf_x[7] > decision_speed_ ? high_speed_delay_time_ : low_speed_delay_time_; - const auto dt = control_delay_time_ + delay_time; // TODO:add delta(now)? + const auto dt = control_delay_time_ + delay_time; auto aim_solution = aim_solver_->SolveAimSolution( std::make_unique(snapshot), bullet_speed_, dt); diff --git a/src/tongji/solver/reprojection_util.hpp b/src/tongji/solver/reprojection_util.hpp index b02c9c9..109163a 100644 --- a/src/tongji/solver/reprojection_util.hpp +++ b/src/tongji/solver/reprojection_util.hpp @@ -9,7 +9,6 @@ #include #include "data/armor_image_spaceing.hpp" -#include "enum/armor_id.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" #include "util/coordinate.hpp" @@ -19,39 +18,17 @@ namespace world_exe::tongji::solver { class ReprojectionUtil { public: - explicit ReprojectionUtil( - const Eigen::Matrix3d& R_gimbal2camera, const Eigen::Vector3d& t_gimbal2camera) - : R_gimbal2camera_(R_gimbal2camera) - , t_gimbal2camera_(t_gimbal2camera) { } + ReprojectionUtil() = default; + ~ReprojectionUtil() = default; - std::vector ReprojectWithYpr(const Eigen::Vector3d& xyz_in_gimbal, - const double& yaw, const double& pitch, const bool& is_large) const { + double CalculateReprojectionError(const Eigen::Matrix3d& R_camera2gimbal, + const Eigen::Vector3d& t_gimbal2camera, + const world_exe::data::ArmorImageSpacing& armor_in_image, + const Eigen::Vector3d& armor_xyz_in_gimbal, const double& armor_yaw, + const double& armor_pitch, const double& inclined) const { - double sin_yaw = std::sin(yaw); - double cos_yaw = std::cos(yaw); - - double sin_pitch = std::sin(pitch); - double cos_pitch = std::cos(pitch); - - // clang-format off - const Eigen::Matrix3d R_armor2gimbal { - {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch}, - {sin_yaw * cos_pitch, cos_yaw, sin_yaw * sin_pitch}, - { -sin_pitch, 0, cos_pitch} - }; - // clang-format on - - return ReprojectWithR_gimbal(xyz_in_gimbal, R_armor2gimbal, is_large); - } - - double CalculateYawError(const world_exe::data::ArmorImageSpacing& armor_in_image, - const Eigen::Vector3d& armor_xyz_in_gimbal, const double& armor_yaw_in_gimbal, - const double& inclined) const { - auto pitch = - (armor_in_image.id == enumeration::ArmorIdFlag::Outpost) ? -15.0 * CV_PI / 180.0 : 15.0; - - auto image_points = ReprojectWithYpr( - armor_xyz_in_gimbal, armor_yaw_in_gimbal, pitch, armor_in_image.isLargeArmor); + auto image_points = ReprojectArmor(R_camera2gimbal, t_gimbal2camera, armor_xyz_in_gimbal, + armor_yaw, armor_pitch, armor_in_image.isLargeArmor); auto error = 0.0; for (int i = 0; i < 4; i++) { @@ -63,44 +40,39 @@ class ReprojectionUtil { return error; } - double CalculatePitchError(const world_exe::data::ArmorImageSpacing& armor_in_image, - const double& armor_yaw_in_gimbal, const Eigen::Vector3d& armor_xyz_in_gimbal, - const double& pitch) const { - - auto image_points = ReprojectWithYpr( - armor_xyz_in_gimbal, armor_yaw_in_gimbal, pitch, armor_in_image.isLargeArmor); - - auto error = 0.0; - for (int i = 0; i < 4; i++) { - error += cv::norm(armor_in_image.image_points[i] - image_points[i]); - } - - return error; - } - private: - auto TransformGimbalToCamera(const Eigen::Matrix3d& R_armor2gimbal, - const Eigen::Vector3d& xyz_in_gimbal, cv::Vec3d& rvec_out, cv::Vec3d& tvec_out) const - -> void { + std::vector ReprojectArmor(const Eigen::Matrix3d& R_camera2gimbal, + const Eigen::Vector3d& t_gimbal2camera, const Eigen::Vector3d& armor_xyz_in_gimbal, + const double& armor_yaw, const double& armor_pitch, const bool& is_large) const { - // R_A->C = R_G->C * R_A->G - Eigen::Matrix3d R_armor2camera = R_gimbal2camera_ * R_armor2gimbal; - // t_A->C = R_G->C * P_G + t_G->C - Eigen::Vector3d t_armor2camera = R_gimbal2camera_ * xyz_in_gimbal + t_gimbal2camera_; + auto sin_yaw = std::sin(armor_yaw); + auto cos_yaw = std::cos(armor_yaw); + auto sin_pitch = std::sin(armor_pitch); + auto cos_pitch = std::cos(armor_pitch); - cv::Mat R_armor2camera_cv; - cv::eigen2cv(util::coordinate::ros2opencv_rotation(R_armor2camera), R_armor2camera_cv); - cv::Rodrigues(R_armor2camera_cv, rvec_out); + // clang-format off + const Eigen::Matrix3d R_armor2gimbal { + {cos_yaw * cos_pitch, -sin_yaw, cos_yaw * sin_pitch}, + {sin_yaw * cos_pitch, cos_yaw, sin_yaw * sin_pitch}, + { -sin_pitch, 0, cos_pitch} + }; + // clang-format on - const auto t_armor2camera_cv = util::coordinate::ros2opencv_position(t_armor2camera); - tvec_out = cv::Vec3d(t_armor2camera_cv[0], t_armor2camera_cv[1], t_armor2camera_cv[2]); - } + // get R_armor2camera t_armor2camera + const Eigen::Vector3d& t_armor2gimbal = armor_xyz_in_gimbal; + Eigen::Matrix3d R_armor2camera = R_camera2gimbal.transpose() * R_armor2gimbal; + Eigen::Matrix3d R_armor2camera_cv = util::coordinate::ros2opencv_rotation(R_armor2camera); - std::vector ReprojectWithR_gimbal(const Eigen::Vector3d& armor_xyz_in_gimbal, - const Eigen::Matrix3d& R_armor2gimbal, const bool& is_large) const { + Eigen::Vector3d t_armor2camera = + R_camera2gimbal.transpose() * (armor_xyz_in_gimbal) + t_gimbal2camera; + Eigen::Vector3d t_armor2camera_cv = util::coordinate::ros2opencv_position(t_armor2camera); - cv::Vec3d rvec, tvec; - TransformGimbalToCamera(R_armor2gimbal, armor_xyz_in_gimbal, rvec, tvec); + // get rvec tvec + cv::Vec3d rvec; + cv::Mat _R_armor2camera_cv; + cv::eigen2cv(R_armor2camera_cv, _R_armor2camera_cv); + cv::Rodrigues(_R_armor2camera_cv, rvec); + cv::Vec3d tvec(t_armor2camera_cv[0], t_armor2camera_cv[1], t_armor2camera_cv[2]); std::vector image_points; const auto& object_points = (is_large) @@ -144,8 +116,5 @@ class ReprojectionUtil { } return cost; } - - Eigen::Matrix3d R_gimbal2camera_; - Eigen::Vector3d t_gimbal2camera_; }; } diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 09183e2..7a81447 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -1,7 +1,9 @@ #include "solver.hpp" +#include #include #include +#include #include #include @@ -23,16 +25,7 @@ namespace world_exe::tongji::solver { class Solver::Impl { public: - Impl(const std::string& config_path) { - const auto yaml = YAML::LoadFile(config_path); - auto R_gimbalcamera_data = yaml["R_gimbal2camera"].as>(); - auto t_gimbal2camera_data = yaml["t_gimbal2camera"].as>(); - Eigen::Matrix3d R_muzzle2camera_ = - Eigen::Matrix(R_gimbalcamera_data.data()); - t_gimbal2camera_ = Eigen::Matrix(t_gimbal2camera_data.data()); - - reprojection_util_ = std::make_unique(R_gimbal2camera_, t_gimbal2camera_); - } + Impl() { } std::shared_ptr EstimateAllArmorPoses( std::shared_ptr armors_in_image) { @@ -52,16 +45,28 @@ class Solver::Impl { data::ArmorCameraSpacing EstimatePose( const world_exe::data::ArmorImageSpacing& armor_in_image) const { - const auto result = EstimatePnp(armor_in_image); + const auto& [xyz_in_camera, R_armor2camera] = EstimatePnp(armor_in_image); data::ArmorCameraSpacing pose; pose.id = armor_in_image.id; - pose.orientation = Eigen::Quaterniond(result.R_armor2camera).normalized(); - pose.position = result.xyz_in_camera; + pose.orientation = Eigen::Quaterniond(R_armor2camera).normalized(); + pose.position = xyz_in_camera; return pose; } - auto OptimizeYawByReprojection(const data::ArmorImageSpacing& armor_in_image, + auto SetCamera2Gimbal( + const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal) -> void { + R_camera2gimbal_ = R_camera2gimbal; + t_camera2gimbal_ = t_camera2gimbal; + } + + auto Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) -> const auto { + Eigen::Vector3d xyz_in_gimbal = + R_camera2gimbal_.transpose() * xyz_in_camera + t_camera2gimbal_; + return xyz_in_gimbal; + } + + auto CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, const double& initial_armor_yaw_in_gimbal) const -> const double { constexpr double SEARCH_RANGE = 140; // degree @@ -70,11 +75,15 @@ class Solver::Impl { auto min_error = 1e10; auto best_yaw = initial_armor_yaw_in_gimbal; + auto pitch = + (armor_in_image.id == enumeration::ArmorIdFlag::Outpost) ? -15.0 * CV_PI / 180.0 : 15.0; + for (int i = 0; i < SEARCH_RANGE; i++) { double yaw = util::math::clamp_pm_pi(yaw0 + i * CV_PI / 180.0); - auto error = reprojection_util_->CalculateYawError( - armor_in_image, armor_xyz_in_gimbal, yaw, (i - SEARCH_RANGE / 2) * CV_PI / 180.0); + auto error = reprojection_util_->CalculateReprojectionError(R_camera2gimbal_, + t_camera2gimbal_, armor_in_image, armor_xyz_in_gimbal, yaw, pitch, + (i - SEARCH_RANGE / 2) * CV_PI / 180.0); if (error < min_error) { min_error = error; @@ -90,7 +99,7 @@ class Solver::Impl { private: auto EstimatePnp(const world_exe::data::ArmorImageSpacing& armor_in_image) const - -> const PnPResultInGimbal { + -> const std::tuple { const auto& object_points = armor_in_image.isLargeArmor ? parameters::Robomaster::LargeArmorObjectPointsOpencv : parameters::Robomaster::NormalArmorObjectPointsOpencv; @@ -113,24 +122,18 @@ class Solver::Impl { cv::cv2eigen(rmat, R_armor2camera_cv); const auto R_armor2camera = util::coordinate::opencv2ros_rotation(R_armor2camera_cv); - Eigen::Matrix3d R_camera2gimbal = R_gimbal2camera_.transpose(); - Eigen::Vector3d t_camera2gimbal = -R_camera2gimbal * t_gimbal2camera_; - - Eigen::Vector3d xyz_in_gimbal = R_camera2gimbal * xyz_in_camera + t_camera2gimbal; - Eigen::Matrix3d R_armor2gimbal = R_camera2gimbal * R_armor2camera; - - return { xyz_in_gimbal, R_armor2gimbal, xyz_in_camera, R_armor2camera }; + return { xyz_in_camera, R_armor2camera }; } private: - Eigen::Matrix3d R_gimbal2camera_; - Eigen::Vector3d t_gimbal2camera_; + Eigen::Matrix3d R_camera2gimbal_; + Eigen::Vector3d t_camera2gimbal_; std::unique_ptr reprojection_util_; }; -Solver::Solver(const std::string& config_path) - : pimpl_(std::make_unique(config_path)) { } +Solver::Solver() + : pimpl_(std::make_unique()) { } Solver::~Solver() = default; const std::time_t Solver::GetTimeStamp() const { return pimpl_->GetTimeStamp(); } diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index d7c6a44..daab81c 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -5,15 +5,9 @@ namespace world_exe::tongji::solver { -struct PnPResultInGimbal { - Eigen::Vector3d xyz_in_gimbal; - Eigen::Matrix3d R_armor2gimbal; - Eigen::Vector3d xyz_in_camera; - Eigen::Matrix3d R_armor2camera; -}; class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStamped { public: - explicit Solver(const std::string& config_path); + explicit Solver(); ~Solver(); std::shared_ptr SolvePnp( From 1798e334cd2b922b4f7a13c242af597fbeef0e06 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 28 Oct 2025 22:45:58 +0800 Subject: [PATCH 65/93] WIP: initial implementation of auto_aim_system with design questions --- src/tongji/auto_aim_system.cpp | 111 ++++++++++++++++++ src/tongji/auto_aim_system.hpp | 18 +++ .../fire_controller/fire_controller.cpp | 30 +++-- .../fire_controller/fire_controller.hpp | 6 +- src/tongji/identifier/classifier.hpp | 4 +- src/tongji/identifier/identifier.cpp | 1 - .../live_target_manager.cpp | 34 ++++-- .../target_snapshot_manager.cpp | 21 ++++ src/tongji/solver/solver.cpp | 31 ++--- src/tongji/solver/solver.hpp | 5 +- src/tongji/state_machine/state_machine.cpp | 20 ++-- src/tongji/state_machine/state_machine.hpp | 1 - src/v1/auto_aim_system_v1.cpp | 2 +- 13 files changed, 233 insertions(+), 51 deletions(-) create mode 100644 src/tongji/auto_aim_system.cpp create mode 100644 src/tongji/auto_aim_system.hpp diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp new file mode 100644 index 0000000..b87c1d2 --- /dev/null +++ b/src/tongji/auto_aim_system.cpp @@ -0,0 +1,111 @@ +#include "auto_aim_system.hpp" + +#include +#include + +#include "../v1/sync/syncer.hpp" +#include "core/event_bus.hpp" +#include "parameters/params_system_v1.hpp" +#include "tongji/fire_controller/fire_controller.hpp" +#include "tongji/identifier/identifier.hpp" +#include "tongji/predictor/live_target_manager/live_target_manager.hpp" +#include "tongji/solver/solver.hpp" +#include "tongji/state_machine/state_machine.hpp" +namespace world_exe::tongji { +using namespace std::chrono; + +class Combined final : public interfaces::IPreDictorUpdatePackage, interfaces::ITimeStamped { +public: + virtual const std::time_t GetTimeStamp() const { + return data1_.camera_capture_begin_time_stamp; + }; + const world_exe::interfaces::ITimeStamped& GetTimeStamped() const { return *this; } + std::shared_ptr GetArmors() const { return data2_; }; + Eigen::Affine3d GetTransform() const { return data1_.camera_to_gimbal; }; + + // but why? + Combined(const data::CameraGimbalMuzzleSyncData& data1, + std::shared_ptr data2) + : data1_(data1) + , data2_(data2) { } + Combined() = delete; + ~Combined() = default; + +private: + const data::CameraGimbalMuzzleSyncData& data1_; + const std::shared_ptr data2_; +}; + +class AutoAimSystem::Impl { +public: + Impl(const bool& debug) + : debug(debug) + , config_path_("") + , save_path_("") { + identifier_ = std::make_unique(config_path_, save_path_); + pnp_solver_ = std::make_unique(); + live_target_manager_ = std::make_shared(config_path_); + state_machine_ = std::make_shared(); + fire_controller_ = std::make_unique( + config_path_, state_machine_, live_target_manager_); + time_stamp_ = std::chrono::steady_clock::now(); + syncer_ = std::make_unique(seconds(2), 6e-6); + + core::EventBus::Subscript(parameters::ParamsForSystemV1::raw_image_event, + [this](const auto& mat) { Solve(mat); }); + core::EventBus::Subscript( + parameters::ParamsForSystemV1::camera_capture_transforms, + [this](const auto& pkg) { SetTransfroms(pkg); }); + } + + auto Solve(const cv::Mat& raw) -> void { + const auto& [armors_in_image, flag] = identifier_->identify(raw); + if (flag == enumeration::ArmorIdFlag::None) return; + + const auto& [pack, check] = + syncer_->get_data(armors_in_image->GetTimeStamped().GetTimeStamp()); + if (!check) return; + + const auto R_camera2gimbal = pack.camera_to_gimbal.rotation(); + const auto t_camera2gimbal = pack.camera_to_gimbal.translation(); + + pnp_solver_->SetCamera2Gimbal(R_camera2gimbal, t_camera2gimbal); + const auto& armors_in_camera = pnp_solver_->SolvePnp(armors_in_image); + + auto combined = std::make_shared(pack, armors_in_camera); + + live_target_manager_->Update(combined, armors_in_image, combined->GetTimeStamp()); + + const auto target_id = state_machine_->GetAllowdToFires(); + + const auto gimbal_yaw = R_camera2gimbal.eulerAngles(2, 1, 0)[0]; + fire_controller_->UpdateGimbalPosition(gimbal_yaw); + + } + + void SetTransfroms(const data::CameraGimbalMuzzleSyncData& data) { syncer_->set_data(data); } + + data::FireControl GetControlCommand() { + fire_controller_->GetAttackCarId(); + return fire_controller_->CalculateTarget( + (std::chrono::steady_clock::now() - time_stamp_).count()); + } + +private: + bool debug; + const std::string config_path_; + const std::string save_path_; + + std::chrono::steady_clock::time_point time_stamp_; + std::unique_ptr identifier_; + std::unique_ptr pnp_solver_; + std::shared_ptr state_machine_; + std::shared_ptr live_target_manager_; + std::unique_ptr syncer_; + std::unique_ptr fire_controller_; +}; + +AutoAimSystem::AutoAimSystem(const bool& debug) + : pimpl_(std::make_unique(debug)) { } +AutoAimSystem::~AutoAimSystem() = default; +} diff --git a/src/tongji/auto_aim_system.hpp b/src/tongji/auto_aim_system.hpp new file mode 100644 index 0000000..daec9f4 --- /dev/null +++ b/src/tongji/auto_aim_system.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include +namespace world_exe::tongji { + +class AutoAimSystem final { +public: + explicit AutoAimSystem(const bool& debug); + ~AutoAimSystem(); + + AutoAimSystem(const AutoAimSystem&) = delete; + AutoAimSystem& operator=(const AutoAimSystem&) = delete; + +private: + class Impl; + std::unique_ptr pimpl_; +}; +} \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index a0b79e0..a3d9dfd 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -37,7 +37,21 @@ class FireController::Impl { control_delay_ = yaml["control_delay"].as(); } - // TODO:time_duration 没有使用 + // TODO:time_duration 没有使用，详见std::shared_ptr + // Predictor(conststd::time_t&time_stamp) const这个接口的注释 + + /* + 从这个接口：std::shared_ptr Predictor(const + std::time_t&time_stamp) const 已经可以得到std::shared_ptr了 + 接下来是要 据此得到最终的控制指令data ::FireControl + + 为了得到最终的控制指令，data ::FireControl，需要对GimbalCommand进行判断， + 当GimbalCommand不突变 且 云台位于合适位置时，返回有效指令 + + 因为在这个函数中，从std::shared_ptr中选出了开火的对象， + 需要保存用于作为这个接口const CarIDFlag GetAttackCarId() const的返回值，所以只好破坏const约束了 + */ + const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { if (!fire_decision_ || !state_machine_ || !live_target_manager_) @@ -52,7 +66,7 @@ class FireController::Impl { .fire_allowance = false }; - // TODO:接口语义不明 + // TODO:接口语义不明，此函数传入的参数有待修正 auto armors_in_gimbal = snapshot_manager->Predictor( std::chrono::steady_clock::now().time_since_epoch().count()); allowed_target_id_ = state_machine_->GetAllowdToFires(); @@ -73,18 +87,17 @@ class FireController::Impl { return result; } + /* + 感觉这个和状态机那边的GetAllowdToFires()有点重复了 + */ const CarIDFlag GetAttackCarId() const { if (firable_) return allowed_target_id_; return CarIDFlag::None; } - void Update(std::shared_ptr armors, const double& gimbal_yaw) { - UpdateGimbalPosition(gimbal_yaw); - } - -private: void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; +private: double gimbal_yaw_; double control_delay_; @@ -106,5 +119,8 @@ const data ::FireControl FireController::CalculateTarget(const std ::time_t& tim } const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } +void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { + return pimpl_->UpdateGimbalPosition(gimbal_yaw); +}; } \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 76d41c1..0b7eeb7 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -3,7 +3,6 @@ #include #include "../time_stamp/time_stamp.hpp" -#include "interfaces/armor_in_image.hpp" #include "interfaces/car_state.hpp" #include "interfaces/fire_controller.hpp" #include "interfaces/target_predictor.hpp" @@ -19,11 +18,10 @@ class FireController final : public interfaces::IFireControl { const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; - - void Update(std::shared_ptr armors, const double& gimbal_yaw); - time_stamp::TimeStamp GetTimeStamp() const; + void UpdateGimbalPosition(const double& gimbal_yaw); + FireController(const FireController&) = delete; FireController& operator=(const FireController&) = delete; FireController(FireController&&) noexcept = default; diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp index 7bcf8bd..ca8e919 100644 --- a/src/tongji/identifier/classifier.hpp +++ b/src/tongji/identifier/classifier.hpp @@ -12,6 +12,8 @@ #include "enum/armor_id.hpp" namespace world_exe::tongji::identifier { + +// TODO:早期代码，和深大模型不适配，需要TODOTODO class Classifier final { public: explicit Classifier(const std::string& config_path) { @@ -24,8 +26,6 @@ class Classifier final { auto ovmodel = core_.read_model(model_path); compiled_model_ = core_.compile_model( ovmodel, "AUTO", ov::hint::performance_mode(ov::hint::PerformanceMode::LATENCY)); - - // TODO:需要对模型做适配 } void Classify(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag& armor_id, diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index 973234b..2f716f0 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -26,7 +26,6 @@ #include "util/stringifier.hpp" namespace world_exe::tongji::identifier { -// TODO:need to refact class Identifier::Impl { public: explicit Impl(const std::string& config_path, const std::string& save_path, const bool& debug, diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp index 13a0972..2056754 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -26,6 +26,17 @@ class LiveTargetManager::Impl { , tracking_id_(enumeration::CarIDFlag::None) , config_path_(config_path) { } + /* + 不懂为什么要实现这个接口，不需要这个返回值 + std::shared_ptr，吧？ + + 原因是：卡尔曼滤波器 更改状态变量x的值 的操纵 + 写在了update函数中，这里的predict是没有副作用的预测， + 但是又没考虑飞行时间，理论上来说，误差更大了， + 需要得到的是考虑飞行时间得到的 std::shared_ptr + 所以目前认为它是多余的 + */ + std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { std::unordered_map> @@ -40,23 +51,30 @@ class LiveTargetManager::Impl { } return std::make_shared(result, time_stamp); - } // TODO: ** 目前 ** 我认为这个函数是多余的 + } + + /* + 猜测接口的意思是通过外界传入id来 获得对应的预测器（副本）， + 而从外界传入的id是从 const CarIDFlag GetAttackCarId() const这类接口中传入的， + 但是我具体的id已经存在了targets_map_中，无需从外部传入 + 哦，原来是这样，怪不得不知道传啥参数进去 + */ std::shared_ptr GetPredictor( const enumeration::ArmorIdFlag& flag) const { - if (targets_map_.empty()) return nullptr; // TODO + if (targets_map_.empty()) return nullptr; return std::make_shared( config_path_, flag, targets_map_, last_update_timestamp_); } void Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const std::time_t& now) { + const std::shared_ptr& armors_in_image, const double& dt) { UpdateTimeStamp(data->GetTimeStamped().GetTimeStamp()); UpdateTargetMap(data); - UpdateTarget(data, armors_in_image, now); + UpdateTarget(data, armors_in_image, dt); } auto GetAllowedTargetID() const -> enumeration::CarIDFlag const { @@ -70,7 +88,7 @@ class LiveTargetManager::Impl { void UpdateTimeStamp(const time_t& time_stamp) { last_update_timestamp_ = time_stamp; } void UpdateTargetMap(std::shared_ptr data) { const Eigen::Affine3d transform = data->GetTransform(); - const Eigen::Matrix3d rotation_matrix = transform.linear(); + const Eigen::Matrix3d rotation_matrix = transform.rotation(); const auto armors_interface = data->GetArmors(); targets_map_.clear(); @@ -131,7 +149,7 @@ void LiveTargetManager::Update(std::shared_ptr& armors_in_image, const std::time_t& now) { return pimpl_->Update(data, armors_in_image, now); } -auto LiveTargetManager::GetAllowedTargetID() const -> enumeration::ArmorIdFlag const { - return pimpl_->GetAllowedTargetID(); -} +// auto LiveTargetManager::GetAllowedTargetID() const -> enumeration::ArmorIdFlag const { +// return pimpl_->GetAllowedTargetID(); +// } } diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp index ce40b0b..2794f89 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -37,6 +37,23 @@ class TargetSnapshotManager::Impl { const enumeration::ArmorIdFlag& GetId() const { return ids_; } + /* + 通过aim_solver_->SolveAimSolution()来解算瞄准击打点 + aim_solution.aim_point，从而得到返回值std::shared_ptr + 所以此处进行飞行时间的迭代 + 但飞行时间的迭代需要考虑到时间延迟（图像传输这些等），这个参数在这个接口中体现： + data::FireControl CalculateTarget(const std::time_t& time_duration) + + 很显然为了实现这个接口，得到返回值std::shared_ptr， + 我需要进行飞行时间的迭代，但在这个接口中我没法把实际的时间延迟传入（我认为这个值是动态的，从上层调用它的地方传入） + v1的版本，传入的是 时间间隔（延迟），但这个接口的含义是传入 + 某个时间点返回装甲板信息，而非时间间隔 + + 如果是在data::FireControl CalculateTarget(const std::time_t& + time_duration)这里进行飞行时间的迭代，那么已经得到了data::FireControl，也就是最后的命令， + 那就不需要这个接口来得到中间量std::shared_ptr + + */ std::shared_ptr Predictor( const std::time_t& time_stamp) const { @@ -67,6 +84,10 @@ class TargetSnapshotManager::Impl { return std::make_shared(result, time_stamp); } + /* + 飞行时间的迭代得到一些有用的信息，比如说，云台控制指令GimbalCommand， + 需要保存一下，后续需要和当前云台yaw来分析这个指令有没有突变，但是有const约束，虽然好像问题不大 + */ auto GetGimbalCommand() const -> GimbalCommand const { return gimbal_command_; } private: diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 7a81447..308de1f 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -43,17 +43,6 @@ class Solver::Impl { return std::make_shared(armor_plates); } - data::ArmorCameraSpacing EstimatePose( - const world_exe::data::ArmorImageSpacing& armor_in_image) const { - const auto& [xyz_in_camera, R_armor2camera] = EstimatePnp(armor_in_image); - - data::ArmorCameraSpacing pose; - pose.id = armor_in_image.id; - pose.orientation = Eigen::Quaterniond(R_armor2camera).normalized(); - pose.position = xyz_in_camera; - return pose; - } - auto SetCamera2Gimbal( const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal) -> void { R_camera2gimbal_ = R_camera2gimbal; @@ -98,6 +87,17 @@ class Solver::Impl { } private: + data::ArmorCameraSpacing EstimatePose( + const world_exe::data::ArmorImageSpacing& armor_in_image) const { + const auto& [xyz_in_camera, R_armor2camera] = EstimatePnp(armor_in_image); + + data::ArmorCameraSpacing pose; + pose.id = armor_in_image.id; + pose.orientation = Eigen::Quaterniond(R_armor2camera).normalized(); + pose.position = xyz_in_camera; + return pose; + } + auto EstimatePnp(const world_exe::data::ArmorImageSpacing& armor_in_image) const -> const std::tuple { const auto& object_points = armor_in_image.isLargeArmor @@ -142,12 +142,15 @@ std::shared_ptr Solver::SolvePnp( std::shared_ptr armors_in_image) { return pimpl_->EstimateAllArmorPoses(armors_in_image); } +void Solver::SetCamera2Gimbal( + const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal) { + pimpl_->SetCamera2Gimbal(R_camera2gimbal, t_camera2gimbal); +} -auto Solver::OptimizeYawByReprojection(const data::ArmorImageSpacing& armor_in_image, +auto Solver::CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, const double& initial_armor_yaw_in_gimbal) const -> const double { - return OptimizeYawByReprojection( + return pimpl_->CalculateOptimizeYaw( armor_in_image, armor_xyz_in_gimbal, gimbal_yaw, initial_armor_yaw_in_gimbal); } - } \ No newline at end of file diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index daab81c..e52e4c4 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -12,10 +12,11 @@ class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStam std::shared_ptr SolvePnp( std::shared_ptr armor) override; - const std::time_t GetTimeStamp() const override; - auto OptimizeYawByReprojection(const data::ArmorImageSpacing& armor_in_image, + void SetCamera2Gimbal( + const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal); + auto CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, const double& initial_armor_yaw_in_gimbal) const -> const double; diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index dc570e4..c5f5309 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -11,29 +11,27 @@ namespace world_exe::tongji::state_machine { class StateMachine::Impl { public: Impl(std::shared_ptr live_target_manager) - : live_target_manager_(std::move(live_target_manager)) { } + : live_target_manager_(live_target_manager) + , target_ids_(enumeration::CarIDFlag::None) { } - const enumeration::CarIDFlag& GetAllowdToFires() const { return target_ids_; } - - auto Update() -> void { + const enumeration::CarIDFlag& GetAllowdToFires() const { auto live_target_manager = std::dynamic_pointer_cast(live_target_manager_); target_ids_ = live_target_manager->GetAllowedTargetID(); + + return target_ids_; } private: - std::shared_ptr live_target_manager_; - enumeration::CarIDFlag target_ids_; + std::shared_ptr live_target_manager_; + mutable enumeration::CarIDFlag target_ids_; }; -StateMachine::StateMachine( - std::shared_ptr live_target_manager) +StateMachine::StateMachine(std::shared_ptr live_target_manager) : pimpl_(std::make_unique(live_target_manager)) { } StateMachine::~StateMachine() = default; const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return pimpl_->GetAllowdToFires(); } - -auto StateMachine::Update() const -> void { return pimpl_->Update(); } -} +} \ No newline at end of file diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 47eb042..d178de2 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -15,7 +15,6 @@ class StateMachine final : public interfaces::ICarState { const enumeration ::CarIDFlag& GetAllowdToFires() const override; StateMachine(std::shared_ptr live_target_manager); - auto Update() const -> void; StateMachine(const StateMachine&) = delete; StateMachine& operator=(const StateMachine&) = delete; diff --git a/src/v1/auto_aim_system_v1.cpp b/src/v1/auto_aim_system_v1.cpp index 7a2849b..966ef19 100644 --- a/src/v1/auto_aim_system_v1.cpp +++ b/src/v1/auto_aim_system_v1.cpp @@ -35,7 +35,7 @@ using namespace std::chrono; class Combined final : public interfaces::IPreDictorUpdatePackage, interfaces::ITimeStamped{ public: - virtual const std::time_t& GetTimeStamp() const{ return data1_.camera_capture_begin_time_stamp; }; + virtual const std::time_t GetTimeStamp() const{ return data1_.camera_capture_begin_time_stamp; }; const world_exe::interfaces::ITimeStamped& GetTimeStamped() const {return *this;} std::shared_ptr GetArmors() const{return data2_;}; Eigen::Affine3d GetTransform() const {return data1_.camera_to_gimbal;}; From 10e685a5324b805013e10990364f6787978a1da7 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Tue, 28 Oct 2025 23:54:22 +0800 Subject: [PATCH 66/93] =?UTF-8?q?[V1]=20identifier=E8=AE=BE=E7=BD=AE?= =?UTF-8?q?=E5=9B=BE=E7=89=87=E6=97=B6=E9=97=B4=E6=88=B3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/v1/identifier/identifier.cpp | 10 ++++++++-- src/v1/identifier/identifier_armor.hpp | 7 +++++-- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/src/v1/identifier/identifier.cpp b/src/v1/identifier/identifier.cpp index be64c60..16daf46 100644 --- a/src/v1/identifier/identifier.cpp +++ b/src/v1/identifier/identifier.cpp @@ -16,6 +16,7 @@ #include "openvino/core/preprocess/pre_post_process.hpp" #include "openvino/runtime/core.hpp" +#include #include namespace world_exe::v1::identifier { @@ -74,10 +75,15 @@ class Identifier::Impl { */ std::tuple, enumeration::CarIDFlag> Identify( const cv::Mat& input_image) { + + time_t time_now = std::chrono::steady_clock::now().time_since_epoch().count(); + // 首先使用深度学习模型进行装甲板检测得到roi区域 const auto armor_infos = model_infer(input_image); // 然后进行灯条匹配验证 - return matchPlate(input_image, armor_infos); + auto [a, b] = matchPlate(input_image, armor_infos); + a->time_stamp_ = time_now; + return {a, b}; } /** @@ -228,7 +234,7 @@ class Identifier::Impl { , angle_(angle) { } }; - std::tuple, enumeration::CarIDFlag> matchPlate( + std::tuple, enumeration::CarIDFlag> matchPlate( const cv::Mat& img, const std::vector& armor_infos) { if (armor_infos.empty()) return {}; diff --git a/src/v1/identifier/identifier_armor.hpp b/src/v1/identifier/identifier_armor.hpp index 7bfbb69..2696658 100644 --- a/src/v1/identifier/identifier_armor.hpp +++ b/src/v1/identifier/identifier_armor.hpp @@ -3,12 +3,14 @@ #include "interfaces/armor_in_image.hpp" #include "interfaces/time_stamped.hpp" #include "util/index.hpp" +#include namespace world_exe::v1::identifier { class IdentifierArmor : public interfaces::IArmorInImage, public interfaces::ITimeStamped { public: IdentifierArmor() = default; - IdentifierArmor(const std::vector& armors) { + IdentifierArmor(const std::vector& armors) + { for (const auto armor : armors) armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); } @@ -29,8 +31,9 @@ class IdentifierArmor : public interfaces::IArmorInImage, public interfaces::ITi return armors_[util::enumeration::GetIndex(armor_id)]; } + std::time_t time_stamp_ {0}; + private: - std::time_t time_stamp_ { 0 }; std::array, 8> armors_; }; } \ No newline at end of file From 8aead8f9d88f29f7400ad6c74a78bc7851bb02d9 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Wed, 29 Oct 2025 00:02:26 +0800 Subject: [PATCH 67/93] =?UTF-8?q?[Tongji]=20PnpSolver=20=E4=BF=AE=E6=AD=A3?= =?UTF-8?q?=E8=BE=93=E5=87=BA=E7=9A=84=E6=97=B6=E9=97=B4=E6=88=B3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/tongji/solver/solved_armor.hpp | 5 ++--- src/tongji/solver/solver.cpp | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/tongji/solver/solved_armor.hpp b/src/tongji/solver/solved_armor.hpp index d44b45c..dfc298e 100644 --- a/src/tongji/solver/solved_armor.hpp +++ b/src/tongji/solver/solved_armor.hpp @@ -1,7 +1,6 @@ #pragma once #include -#include #include #include "interfaces/armor_in_camera.hpp" @@ -12,8 +11,8 @@ namespace world_exe::tongji::solver { class SolvedArmor final : public interfaces::IArmorInCamera { public: - explicit SolvedArmor(const std::vector& armors) - : time_stamp_(std::chrono::steady_clock::now()) { + explicit SolvedArmor(const std::vector& armors, time_t when_image_come) + : time_stamp_(when_image_come) { for (const auto& armor : armors) { armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); } diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 308de1f..ab56cdc 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -40,7 +40,7 @@ class Solver::Impl { armor_plates.emplace_back(solved_armor); } } - return std::make_shared(armor_plates); + return std::make_shared(armor_plates, armors_in_image->GetTimeStamped().GetTimeStamp()); } auto SetCamera2Gimbal( From 797750d7ecda25d8a17168a2da9e02a5fc9e553f Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Wed, 29 Oct 2025 00:29:33 +0800 Subject: [PATCH 68/93] =?UTF-8?q?[Tongji]=20TargetManager=20=E4=BF=AE?= =?UTF-8?q?=E5=A4=8D=E5=8D=A1=E5=B0=94=E6=9B=BC=E6=BB=A4=E6=B3=A2=E5=99=A8?= =?UTF-8?q?=E6=9B=B4=E6=96=B0=E6=89=80=E4=BC=A0=E5=85=A5=E6=97=B6=E9=97=B4?= =?UTF-8?q?=E7=9A=84=E9=94=99=E8=AF=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../predictor/live_target_manager/live_target_manager.cpp | 8 +++++--- .../predictor/live_target_manager/live_target_manager.hpp | 3 +-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp index 2056754..69b3263 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -68,7 +68,7 @@ class LiveTargetManager::Impl { return std::make_shared( config_path_, flag, targets_map_, last_update_timestamp_); } - + // 为何传递了一个time_t 给double void Update(std::shared_ptr data, const std::shared_ptr& armors_in_image, const double& dt) { @@ -145,9 +145,11 @@ std ::shared_ptr LiveTargetManager::GetPredictor( return pimpl_->GetPredictor(id); } +static constexpr double cast_nanosec_sec(const long nanosec) {return nanosec * 1e-9;} + void LiveTargetManager::Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const std::time_t& now) { - return pimpl_->Update(data, armors_in_image, now); + const std::shared_ptr& armors_in_image) { + return pimpl_->Update(data, armors_in_image, cast_nanosec_sec(data->GetTimeStamped().GetTimeStamp())); } // auto LiveTargetManager::GetAllowedTargetID() const -> enumeration::ArmorIdFlag const { // return pimpl_->GetAllowedTargetID(); diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.hpp b/src/tongji/predictor/live_target_manager/live_target_manager.hpp index 980e03a..9e90eba 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.hpp @@ -20,8 +20,7 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { const enumeration ::ArmorIdFlag& id) const override; void Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image, - const std::time_t& time_stamp); + const std::shared_ptr& armors_in_image); auto GetAllowedTargetID() const -> enumeration::ArmorIdFlag const; From 86c7c6126b2deca05bf0016626f94fffbb283008 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Wed, 29 Oct 2025 00:30:28 +0800 Subject: [PATCH 69/93] =?UTF-8?q?[Tongji]=20=E6=8A=8A=E5=86=85=E9=83=A8?= =?UTF-8?q?=E7=9A=84=E6=8C=87=E9=92=88=E8=BD=AC=E6=8D=A2=E5=8F=98=E4=B8=BA?= =?UTF-8?q?=E4=BC=A0=E5=85=A5=E5=8F=82=E6=95=B0=E7=9A=84=E7=BA=A6=E6=9D=9F?= =?UTF-8?q?=EF=BC=8C=E7=BB=B4=E6=8C=81=E7=8A=B6=E6=80=81=E6=9C=BA=E5=8E=9F?= =?UTF-8?q?=E5=A7=8B=E5=AE=9E=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/tongji/state_machine/state_machine.cpp | 13 +++++-------- src/tongji/state_machine/state_machine.hpp | 7 +++++-- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index c5f5309..19bfeb3 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -3,31 +3,28 @@ #include #include "enum/car_id.hpp" -#include "interfaces/target_predictor.hpp" #include "tongji/predictor/live_target_manager/live_target_manager.hpp" namespace world_exe::tongji::state_machine { class StateMachine::Impl { public: - Impl(std::shared_ptr live_target_manager) + Impl(std::shared_ptr live_target_manager) : live_target_manager_(live_target_manager) , target_ids_(enumeration::CarIDFlag::None) { } + /// but why? const enumeration::CarIDFlag& GetAllowdToFires() const { - auto live_target_manager = - std::dynamic_pointer_cast(live_target_manager_); - target_ids_ = live_target_manager->GetAllowedTargetID(); - + target_ids_ = live_target_manager_->GetAllowedTargetID(); return target_ids_; } private: - std::shared_ptr live_target_manager_; + std::shared_ptr live_target_manager_; mutable enumeration::CarIDFlag target_ids_; }; -StateMachine::StateMachine(std::shared_ptr live_target_manager) +StateMachine::StateMachine(std::shared_ptr live_target_manager) : pimpl_(std::make_unique(live_target_manager)) { } StateMachine::~StateMachine() = default; diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index d178de2..992b86e 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -5,7 +5,10 @@ #include "enum/car_id.hpp" #include "interfaces/car_state.hpp" -#include "interfaces/target_predictor.hpp" + +namespace world_exe::tongji::predictor { + class LiveTargetManager; +} namespace world_exe::tongji::state_machine { class StateMachine final : public interfaces::ICarState { @@ -14,7 +17,7 @@ class StateMachine final : public interfaces::ICarState { ~StateMachine(); const enumeration ::CarIDFlag& GetAllowdToFires() const override; - StateMachine(std::shared_ptr live_target_manager); + StateMachine(std::shared_ptr live_target_manager); StateMachine(const StateMachine&) = delete; StateMachine& operator=(const StateMachine&) = delete; From 83167b68be73da3a0dc7814f37191fa1f5d56733 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Wed, 29 Oct 2025 00:47:32 +0800 Subject: [PATCH 70/93] =?UTF-8?q?[Tongji]=20Firecontrol=20=E4=BF=AE?= =?UTF-8?q?=E5=A4=8D=E6=97=B6=E9=97=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fire_controller/fire_controller.cpp | 35 ++++++++++++------- src/v1/fire_controller/fire_controller.cpp | 1 - 2 files changed, 23 insertions(+), 13 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index a3d9dfd..7397b3d 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -27,7 +27,7 @@ class FireController::Impl { public: Impl(const std::string& config_path, std::shared_ptr state_machine, std::shared_ptr live_target_manager) - : allowed_target_id_(CarIDFlag::None) + : locked_target(CarIDFlag::None) , firable_(false) , fire_decision_(std::make_unique(config_path)) , state_machine_(state_machine) @@ -50,9 +50,11 @@ class FireController::Impl { 因为在这个函数中，从std::shared_ptr中选出了开火的对象， 需要保存用于作为这个接口const CarIDFlag GetAttackCarId() const的返回值，所以只好破坏const约束了 + + - rep: 也许改个名字就好了 */ - const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { + const data ::FireControl CalculateTarget(const std ::time_t& time_from_tracker_timepoint) const { if (!fire_decision_ || !state_machine_ || !live_target_manager_) return { .fire_allowance = false }; @@ -66,13 +68,21 @@ class FireController::Impl { .fire_allowance = false }; - // TODO:接口语义不明，此函数传入的参数有待修正 - auto armors_in_gimbal = snapshot_manager->Predictor( - std::chrono::steady_clock::now().time_since_epoch().count()); - allowed_target_id_ = state_machine_->GetAllowdToFires(); - - auto target_gimbal_spacing = armors_in_gimbal->GetArmors(allowed_target_id_).front(); - + // - TODO:接口语义不明，此函数传入的参数有待修正 + // - rep: 这里你需要算的不是程序执行时候的时间，而是预计的未来的某个时间的装甲板坐标 + // btw, 不计算飞行时间的话，获取这个predictor是为了什么， + // 为了击中未来的装甲板，需要使用某种算法去获取击中时候的装甲板位姿，然后反推云台位置 + // FIXME: 火控系统错误 + auto armors_in_gimbal = snapshot_manager->Predictor(time_from_tracker_timepoint + control_delay_); + auto lockable_target = state_machine_->GetAllowdToFires(); + + // 逻辑真的是选择所有可以击打的装甲板的第一个吗，这里有问题 + auto target_gimbal_spacing = armors_in_gimbal->GetArmors(lockable_target).front(); + + locked_target =target_gimbal_spacing.id; + + // FIXME: 不要偷偷的指针转换 + // 牵连太多，临时不好修 auto gimbal_command = std::dynamic_pointer_cast(snapshot_manager)->GetGimbalCommand(); @@ -88,10 +98,11 @@ class FireController::Impl { } /* - 感觉这个和状态机那边的GetAllowdToFires()有点重复了 + - 感觉这个和状态机那边的GetAllowdToFires()有点重复了 + - rep: 不重复的，锁定和可锁定的差别（好名字，用了） */ const CarIDFlag GetAttackCarId() const { - if (firable_) return allowed_target_id_; + if (firable_) return locked_target; return CarIDFlag::None; } @@ -101,7 +112,7 @@ class FireController::Impl { double gimbal_yaw_; double control_delay_; - mutable CarIDFlag allowed_target_id_; + mutable CarIDFlag locked_target; mutable double firable_; std::unique_ptr fire_decision_; diff --git a/src/v1/fire_controller/fire_controller.cpp b/src/v1/fire_controller/fire_controller.cpp index ba9f866..c618f1d 100644 --- a/src/v1/fire_controller/fire_controller.cpp +++ b/src/v1/fire_controller/fire_controller.cpp @@ -2,7 +2,6 @@ #include "./fire_controller.hpp" #include "enum/enum_tools.hpp" #include "interfaces/armor_in_gimbal_control.hpp" -#include "interfaces/car_state.hpp" #include "interfaces/predictor.hpp" #include "trajectory.hpp" #include From ea3ee969dd58c21cbdd1825b5cb4e6cabb575396 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Wed, 29 Oct 2025 00:48:17 +0800 Subject: [PATCH 71/93] =?UTF-8?q?[Tongji]=20=E4=BF=AE=E5=A4=8DSolve?= =?UTF-8?q?=E7=9A=84=E6=97=B6=E5=BA=8F=E9=94=99=E8=AF=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/extensions.json | 5 +++++ src/tongji/auto_aim_system.cpp | 24 ++++++++++++++++++------ 2 files changed, 23 insertions(+), 6 deletions(-) create mode 100644 .vscode/extensions.json diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..16e940b --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,5 @@ +{ + "recommendations": [ + "gruntfuggly.todo-tree" + ] +} \ No newline at end of file diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index b87c1d2..d8fd994 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -6,11 +6,13 @@ #include "../v1/sync/syncer.hpp" #include "core/event_bus.hpp" #include "parameters/params_system_v1.hpp" +#include "parameters/profile.hpp" #include "tongji/fire_controller/fire_controller.hpp" -#include "tongji/identifier/identifier.hpp" +// #include "tongji/identifier/identifier.hpp" #include "tongji/predictor/live_target_manager/live_target_manager.hpp" #include "tongji/solver/solver.hpp" #include "tongji/state_machine/state_machine.hpp" +#include "v1/identifier/identifier.hpp" namespace world_exe::tongji { using namespace std::chrono; @@ -42,7 +44,11 @@ class AutoAimSystem::Impl { : debug(debug) , config_path_("") , save_path_("") { - identifier_ = std::make_unique(config_path_, save_path_); + identifier_ = std::make_unique( + parameters::ParamsForSystemV1::szu_model_path(), + parameters::ParamsForSystemV1::device(), + parameters::HikCameraProfile::get_width(), + parameters::HikCameraProfile::get_height()); pnp_solver_ = std::make_unique(); live_target_manager_ = std::make_shared(config_path_); state_machine_ = std::make_shared(); @@ -62,8 +68,8 @@ class AutoAimSystem::Impl { const auto& [armors_in_image, flag] = identifier_->identify(raw); if (flag == enumeration::ArmorIdFlag::None) return; - const auto& [pack, check] = - syncer_->get_data(armors_in_image->GetTimeStamped().GetTimeStamp()); + // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分 + const auto& [pack, check] = syncer_->get_data(armors_in_image->GetTimeStamped().GetTimeStamp()); if (!check) return; const auto R_camera2gimbal = pack.camera_to_gimbal.rotation(); @@ -74,13 +80,19 @@ class AutoAimSystem::Impl { auto combined = std::make_shared(pack, armors_in_camera); - live_target_manager_->Update(combined, armors_in_image, combined->GetTimeStamp()); + live_target_manager_->Update(combined, armors_in_image); + + state_machine_ = std::make_shared(live_target_manager_); const auto target_id = state_machine_->GetAllowdToFires(); const auto gimbal_yaw = R_camera2gimbal.eulerAngles(2, 1, 0)[0]; fire_controller_->UpdateGimbalPosition(gimbal_yaw); + + /// 这里应该有一个线程进行稳定的输出之类的 + /// 轨迹规划器没有实现，先不管 + core::EventBus::Publish(parameters::ParamsForSystemV1::fire_control_event, GetControlCommand()); } void SetTransfroms(const data::CameraGimbalMuzzleSyncData& data) { syncer_->set_data(data); } @@ -97,7 +109,7 @@ class AutoAimSystem::Impl { const std::string save_path_; std::chrono::steady_clock::time_point time_stamp_; - std::unique_ptr identifier_; + std::unique_ptr identifier_; std::unique_ptr pnp_solver_; std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; From e383c646ea0c7a1f48014184abf237d6ff5760c2 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Wed, 29 Oct 2025 03:05:03 +0800 Subject: [PATCH 72/93] =?UTF-8?q?[Framework]:=20=E5=B0=86=E5=86=85?= =?UTF-8?q?=E5=AE=B9=E5=8D=95=E4=B8=80=E9=87=8D=E5=A4=8D=E7=9A=84=E6=8E=A5?= =?UTF-8?q?=E5=8F=A3=E6=8F=90=E5=8F=96=E4=B8=BADTO?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 时间戳: data::TimeStamp - 预测器更新包: data::PredictorPackage --- 自今日起 书同文 车同轨 --- CMakeLists.txt | 1 + include/data/fire_control.hpp | 4 +- include/data/predictor_update_package.hpp | 44 +++++++++++++++++++ include/data/sync_data.hpp | 3 +- include/data/time_stamped.hpp | 36 +++++++++++++++ include/interfaces/armor_in_camera.hpp | 6 ++- .../interfaces/armor_in_gimbal_control.hpp | 4 +- include/interfaces/armor_in_image.hpp | 4 +- include/interfaces/fire_controller.hpp | 3 +- include/interfaces/predictor.hpp | 3 +- .../interfaces/predictor_update_package.hpp | 36 --------------- include/interfaces/sync_block.hpp | 6 +-- include/interfaces/target_predictor.hpp | 3 +- include/interfaces/time_stamped.hpp | 14 ------ include/sync/hik_camera_syncdata.hpp | 27 ------------ include/sync/sync_data.hpp | 13 ------ .../identifier/armor_identifier_detail.cpp | 1 + .../identifier/armor_identifier_detail.hpp | 2 +- .../HZA/identifier/armor_image_detail.cpp | 3 +- .../HZA/identifier/armor_image_detail.hpp | 7 ++- .../HZA/identifier/time_stamped_detail.cpp | 8 ---- .../HZA/identifier/time_stamped_detail.hpp | 14 ------ src/tongji/auto_aim_system.cpp | 29 ++---------- .../fire_controller/fire_controller.cpp | 15 +++---- .../fire_controller/fire_controller.hpp | 6 +-- src/tongji/identifier/identified_armor.hpp | 7 ++- .../predictor/in_gimbal_control_armor.hpp | 10 ++--- .../live_target_manager/live_target.hpp | 14 +++--- .../live_target_manager.cpp | 36 +++++++-------- .../live_target_manager.hpp | 7 +-- .../target_snapshot.hpp | 3 +- .../target_snapshot_manager.cpp | 11 ++--- .../target_snapshot_manager.hpp | 5 ++- src/tongji/solver/solved_armor.hpp | 9 ++-- src/tongji/solver/solver.cpp | 9 ++-- src/tongji/solver/solver.hpp | 4 +- src/tongji/state_machine/state_machine.cpp | 4 +- src/tongji/time_stamp/time_stamp.hpp | 34 -------------- src/v1/auto_aim_system_v1.cpp | 31 +++---------- src/v1/fire_controller/fire_controller.cpp | 12 ++--- src/v1/fire_controller/fire_controller.hpp | 4 +- src/v1/identifier/identifier.cpp | 4 +- src/v1/identifier/identifier_armor.hpp | 10 ++--- src/v1/pnpsolver/armor_pnp_solver.cpp | 12 ++--- src/v1/pnpsolver/armor_pnp_solver.hpp | 8 ++-- src/v1/predictor/car/car_predictor.cpp | 26 +++++------ src/v1/predictor/car/car_predictor.hpp | 11 ++--- .../predict_armor_in_gimbal_control.cpp | 19 ++++---- .../predict_armor_in_gimbal_control.hpp | 12 ++--- src/v1/predictor/predict_time_stamp.hpp | 19 -------- src/v1/predictor/predictor_manager.cpp | 28 ++++++------ src/v1/predictor/predictor_manager.hpp | 7 +-- src/v1/sync/syncer.cpp | 22 +++++----- src/v1/sync/syncer.hpp | 6 +-- src/v1/workflow.puml | 6 +-- tests/CMakeLists.txt | 2 +- tests/Pnpsolver.cpp | 2 - tests/mocks/MockArmor2D.hpp | 11 +++-- tests/sync_test.cpp | 36 +++++++-------- tests/test_main.cpp | 4 +- 60 files changed, 305 insertions(+), 432 deletions(-) create mode 100644 include/data/predictor_update_package.hpp create mode 100644 include/data/time_stamped.hpp delete mode 100644 include/interfaces/predictor_update_package.hpp delete mode 100644 include/interfaces/time_stamped.hpp delete mode 100644 include/sync/hik_camera_syncdata.hpp delete mode 100644 include/sync/sync_data.hpp delete mode 100644 src/playground/HZA/identifier/time_stamped_detail.cpp delete mode 100644 src/playground/HZA/identifier/time_stamped_detail.hpp delete mode 100644 src/tongji/time_stamp/time_stamp.hpp delete mode 100644 src/v1/predictor/predict_time_stamp.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 145ffe0..8e80351 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,7 @@ target_link_libraries(alliance_auto_aim PUBLIC TBB::tbb openvino::runtime Ceres::ceres + yaml-cpp ) # 安装规则 diff --git a/include/data/fire_control.hpp b/include/data/fire_control.hpp index 567c618..110af04 100644 --- a/include/data/fire_control.hpp +++ b/include/data/fire_control.hpp @@ -1,12 +1,14 @@ #pragma once +#include "data/time_stamped.hpp" + #include #include #include namespace world_exe::data { struct FireControl { - time_t time_stamp; + data::TimeStamp time_stamp; /// 以ROS系，光轴方向为x, 上为z, /// 四个点为： 1->左上 2->右上 3->右下 4->左下, /// 点实际坐标为opencv系下的像素坐标 diff --git a/include/data/predictor_update_package.hpp b/include/data/predictor_update_package.hpp new file mode 100644 index 0000000..cae7c75 --- /dev/null +++ b/include/data/predictor_update_package.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include "data/sync_data.hpp" +#include "interfaces/armor_in_camera.hpp" + +#include "data/time_stamped.hpp" +#include + +namespace world_exe::data { + +struct PredictorUpdatePackage final{ +public: + + /** + * @brief 传感器数据获取时的时间戳 + */ + const data::TimeStamp& GetTimeStamp() const{ return data1_.camera_capture_begin_time_stamp; }; + + /** + * @brief 求解好的装甲板三维信息 + * + * @return std::shared_ptr + */ + std::shared_ptr GetArmors() const{return data2_;}; + + /** + * @brief 相机坐标系到世界坐标系的仿射变换 + * + * @return Eigen::Affine3d + */ + Eigen::Affine3d GetCameraToWorld() const {return data1_.camera_to_gimbal;}; + + // but why? + PredictorUpdatePackage(const data::CameraGimbalMuzzleSyncData& data1, std::shared_ptr data2) + : data1_(data1) + , data2_(data2){ + } + PredictorUpdatePackage() = delete; + ~PredictorUpdatePackage() = default; +private: + const data::CameraGimbalMuzzleSyncData& data1_; + const std::shared_ptr data2_; +}; +} \ No newline at end of file diff --git a/include/data/sync_data.hpp b/include/data/sync_data.hpp index 765bc4e..cc5a21d 100644 --- a/include/data/sync_data.hpp +++ b/include/data/sync_data.hpp @@ -1,10 +1,11 @@ #pragma once +#include "data/time_stamped.hpp" #include #include namespace world_exe::data { struct CameraGimbalMuzzleSyncData { - std::time_t camera_capture_begin_time_stamp; + data::TimeStamp camera_capture_begin_time_stamp; Eigen::Affine3d camera_to_gimbal; Eigen::Affine3d gimbal_to_muzzle; }; diff --git a/include/data/time_stamped.hpp b/include/data/time_stamped.hpp new file mode 100644 index 0000000..85d7e25 --- /dev/null +++ b/include/data/time_stamped.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include +#include +namespace world_exe::data { +/** + * @brief 时间戳，通常是SteadyClock + */ +struct TimeStamp { + +public: + + inline TimeStamp() : stamp_(){} + inline TimeStamp(const TimeStamp& stamp): stamp_(stamp.stamp_) {} + inline TimeStamp(const std::chrono::nanoseconds& stamp): stamp_(stamp) {} + inline TimeStamp(const std::chrono::seconds& stamp): stamp_(std::chrono::duration_cast(stamp)) {} + + inline constexpr double to_seconds() const{return stamp_.count() * 1e-9;} + inline constexpr double to_nanosec() const{return stamp_.count();} + + auto operator<=>(const TimeStamp& other) const noexcept = default; + inline TimeStamp operator- (const TimeStamp& other) const noexcept {return TimeStamp{stamp_ - other.stamp_};} + inline TimeStamp operator+ (const TimeStamp& other) const noexcept {return TimeStamp{stamp_ + other.stamp_};} + inline TimeStamp operator* (const double& ratio) const noexcept {return from_nanosec(stamp_.count() * ratio);} + inline TimeStamp operator/ (const double& ratio) const noexcept {return from_nanosec(stamp_.count() / ratio);} + + template + static inline TimeStamp from_seconds(const T time){return TimeStamp{std::chrono::nanoseconds(static_cast(time * 1e9))};} + template + static inline TimeStamp from_nanosec(const T time){return TimeStamp{std::chrono::nanoseconds(static_cast(time))};} + +private: + std::chrono::nanoseconds stamp_; + +}; +} \ No newline at end of file diff --git a/include/interfaces/armor_in_camera.hpp b/include/interfaces/armor_in_camera.hpp index d376a85..7083950 100644 --- a/include/interfaces/armor_in_camera.hpp +++ b/include/interfaces/armor_in_camera.hpp @@ -1,10 +1,12 @@ #pragma once #include "data/armor_camera_spacing.hpp" +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" -#include "interfaces/time_stamped.hpp" +#include "data/time_stamped.hpp" #include + namespace world_exe::interfaces { /** @@ -16,7 +18,7 @@ namespace world_exe::interfaces { class IArmorInCamera { public: /// 获取时间戳，标志其内容装甲板的准确时间点 - virtual const ITimeStamped& GetTimeStamped() const = 0; + virtual const data::TimeStamp& GetTimeStamp() const = 0; /// 获取某个车辆ID的装甲板集合 virtual const std::vector& GetArmors( diff --git a/include/interfaces/armor_in_gimbal_control.hpp b/include/interfaces/armor_in_gimbal_control.hpp index 757142c..182f44f 100644 --- a/include/interfaces/armor_in_gimbal_control.hpp +++ b/include/interfaces/armor_in_gimbal_control.hpp @@ -2,7 +2,7 @@ #include "data/armor_gimbal_control_spacing.hpp" #include "enum/armor_id.hpp" -#include "time_stamped.hpp" +#include "data/time_stamped.hpp" #include namespace world_exe::interfaces { @@ -17,7 +17,7 @@ class IArmorInGimbalControl { public: /// 获取时间戳，标志其内容装甲板的准确时间点 - virtual const ITimeStamped& GetTimeStamped() const = 0; + virtual const data::TimeStamp& GetTimeStamp() const = 0; /// 获取某个车辆ID的装甲板集合 virtual const std::vector& GetArmors( diff --git a/include/interfaces/armor_in_image.hpp b/include/interfaces/armor_in_image.hpp index 61f758e..92a60b9 100644 --- a/include/interfaces/armor_in_image.hpp +++ b/include/interfaces/armor_in_image.hpp @@ -2,7 +2,7 @@ #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" -#include "time_stamped.hpp" +#include "data/time_stamped.hpp" #include #include @@ -21,7 +21,7 @@ class IArmorInImage { public: /// 获取时间戳，标志其内容装甲板的准确时间点 - virtual const ITimeStamped& GetTimeStamped() const = 0; + virtual const data::TimeStamp& GetTimeStamp() const = 0; /** * @brief 获取某个车辆ID的装甲板集合 diff --git a/include/interfaces/fire_controller.hpp b/include/interfaces/fire_controller.hpp index 901dd68..4c5a3e2 100644 --- a/include/interfaces/fire_controller.hpp +++ b/include/interfaces/fire_controller.hpp @@ -2,6 +2,7 @@ #include "data/fire_control.hpp" #include "enum/car_id.hpp" +#include #include namespace world_exe::interfaces { @@ -17,7 +18,7 @@ class IFireControl { * @param time_duration 额外时间差 典型值：当前时刻到当前帧传感器传入内容的时间差 * @return const data::FireControl */ - virtual const data::FireControl CalculateTarget(const std::time_t& time_duration) const = 0; + virtual const data::FireControl CalculateTarget(const std::chrono::seconds& time_duration) const = 0; /** * @brief 获取当前火控系统锁定的车辆ID diff --git a/include/interfaces/predictor.hpp b/include/interfaces/predictor.hpp index 248ed8b..9824f66 100644 --- a/include/interfaces/predictor.hpp +++ b/include/interfaces/predictor.hpp @@ -1,6 +1,7 @@ #pragma once #include "armor_in_gimbal_control.hpp" +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include @@ -24,7 +25,7 @@ class IPredictor { * @return const IArmorInGimbalControl& */ virtual std::shared_ptr Predictor( - const std::time_t& time_stamp) const = 0; + const data::TimeStamp& time_stamp) const = 0; virtual ~IPredictor() = default; }; diff --git a/include/interfaces/predictor_update_package.hpp b/include/interfaces/predictor_update_package.hpp deleted file mode 100644 index 9d2e86b..0000000 --- a/include/interfaces/predictor_update_package.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#include "./armor_in_camera.hpp" - -#include "interfaces/time_stamped.hpp" -#include - -namespace world_exe::interfaces { - -/** - * @brief 给出滤波器需要的所有数据 - * @todo 应该使用结构体，但是先写着无所谓 - */ -class IPreDictorUpdatePackage { -public: - /** - * @brief 传感器数据获取时的时间戳 - */ - virtual const ITimeStamped& GetTimeStamped() const = 0; - /** - * @brief 求解好的装甲板三维信息 - * - * @return std::shared_ptr - */ - virtual std::shared_ptr GetArmors() const = 0; - - /** - * @brief 相机坐标系到世界坐标系的仿射变换 - * - * @return Eigen::Affine3d - */ - virtual Eigen::Affine3d GetTransform() const = 0; - - virtual ~IPreDictorUpdatePackage() = default; -}; -} \ No newline at end of file diff --git a/include/interfaces/sync_block.hpp b/include/interfaces/sync_block.hpp index 3f0a6a4..36b3612 100644 --- a/include/interfaces/sync_block.hpp +++ b/include/interfaces/sync_block.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include "data/time_stamped.hpp" namespace world_exe::interfaces { /** @@ -12,9 +12,9 @@ template class ISyncBlock { public: - virtual void set_data(const T& camera_data); + virtual void set_data(const T& camera_data) = 0; - virtual std::tuple get_data(time_t timestamp); + virtual std::tuple get_data(const data::TimeStamp& timestamp) = 0; virtual ~ISyncBlock() = default; }; diff --git a/include/interfaces/target_predictor.hpp b/include/interfaces/target_predictor.hpp index b7a3a44..c2bb915 100644 --- a/include/interfaces/target_predictor.hpp +++ b/include/interfaces/target_predictor.hpp @@ -1,6 +1,7 @@ #pragma once #include "armor_in_gimbal_control.hpp" +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" #include @@ -22,7 +23,7 @@ class ITargetPredictor { * @return std::shared_ptr */ virtual std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) = 0; + const enumeration::ArmorIdFlag& id, const data::TimeStamp& time_stamp) = 0; /** * @brief 按照传入的id生成IPredictor diff --git a/include/interfaces/time_stamped.hpp b/include/interfaces/time_stamped.hpp deleted file mode 100644 index c074e20..0000000 --- a/include/interfaces/time_stamped.hpp +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include -namespace world_exe::interfaces { -/** - * @brief 时间戳，通常是SteadyClock - */ -class ITimeStamped { -public: - virtual const std::time_t GetTimeStamp() const = 0; - - virtual ~ITimeStamped() = default; -}; -} \ No newline at end of file diff --git a/include/sync/hik_camera_syncdata.hpp b/include/sync/hik_camera_syncdata.hpp deleted file mode 100644 index c955ef3..0000000 --- a/include/sync/hik_camera_syncdata.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once -#include "opencv2/core/mat.hpp" - -#include "data/sync_data.hpp" -#include "sync/sync_data.hpp" - -namespace world_exe::sync { - -struct HikCameraSyncData final - : public world_exe::core::SyncData { -public: - std::tuple load() override { - return { mat_, data_ }; - }; - void store(cv::Mat arg1, world_exe::data::CameraGimbalMuzzleSyncData arg2) override { - arg1.copyTo(mat_); - std::memcpy((void*)&data_, (void*)&arg2, sizeof(data_)); - }; - explicit HikCameraSyncData(const cv::Mat& mat) - : mat_ { mat.clone() } - , data_ {} { } - -private: - cv::Mat mat_; - world_exe::data::CameraGimbalMuzzleSyncData data_; -}; -} // namespace world_exe::sync \ No newline at end of file diff --git a/include/sync/sync_data.hpp b/include/sync/sync_data.hpp deleted file mode 100644 index 9ba1ad6..0000000 --- a/include/sync/sync_data.hpp +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -#include - -namespace world_exe::core { - -template struct SyncData { - - virtual std::tuple load() = 0; - virtual void store(Args... args) = 0; - virtual ~SyncData() = default; -}; -} \ No newline at end of file diff --git a/src/playground/HZA/identifier/armor_identifier_detail.cpp b/src/playground/HZA/identifier/armor_identifier_detail.cpp index 4773d71..66e5c63 100644 --- a/src/playground/HZA/identifier/armor_identifier_detail.cpp +++ b/src/playground/HZA/identifier/armor_identifier_detail.cpp @@ -1,3 +1,4 @@ +#include "armor_image_detail.hpp" #include "armor_identifier_detail.hpp" #include #include diff --git a/src/playground/HZA/identifier/armor_identifier_detail.hpp b/src/playground/HZA/identifier/armor_identifier_detail.hpp index d5cc035..222df90 100644 --- a/src/playground/HZA/identifier/armor_identifier_detail.hpp +++ b/src/playground/HZA/identifier/armor_identifier_detail.hpp @@ -1,6 +1,6 @@ #pragma once #include "interfaces/identifier.hpp" -#include "armor_image_detail.hpp" + namespace world_exe::interfaces::detail { class ArmorIdentifier : public world_exe::interfaces::IIdentifier { diff --git a/src/playground/HZA/identifier/armor_image_detail.cpp b/src/playground/HZA/identifier/armor_image_detail.cpp index 6b02a55..a6198cd 100644 --- a/src/playground/HZA/identifier/armor_image_detail.cpp +++ b/src/playground/HZA/identifier/armor_image_detail.cpp @@ -15,8 +15,7 @@ const std::vector& ArmorInImage::GetArmors(const enumer return result; } -const ITimeStamped& ArmorInImage::GetTimeStamped() const { - time_stamped_.GetTimeStamp(); +const data::TimeStamp& ArmorInImage::GetTimeStamp() const { return time_stamped_; } } \ No newline at end of file diff --git a/src/playground/HZA/identifier/armor_image_detail.hpp b/src/playground/HZA/identifier/armor_image_detail.hpp index 83cb71a..4d569cf 100644 --- a/src/playground/HZA/identifier/armor_image_detail.hpp +++ b/src/playground/HZA/identifier/armor_image_detail.hpp @@ -1,7 +1,6 @@ #pragma once #include "interfaces/armor_in_image.hpp" -#include "interfaces/time_stamped.hpp" -#include "time_stamped_detail.hpp" +#include "data/time_stamped.hpp" namespace world_exe::interfaces::detail{ class ArmorInImage : public world_exe::interfaces::IArmorInImage { @@ -9,11 +8,11 @@ namespace world_exe::interfaces::detail{ ArmorInImage(const std::vector& armors_) : armors_(armors_) {} virtual ~ArmorInImage() = default; - const ITimeStamped& GetTimeStamped() const override; + const data::TimeStamp& GetTimeStamp() const override; const std::vector armors_; const std::vector& GetArmors(const enumeration::ArmorIdFlag& armor_id) const override; private: - TimeStamped time_stamped_; + data::TimeStamp time_stamped_{}; }; } \ No newline at end of file diff --git a/src/playground/HZA/identifier/time_stamped_detail.cpp b/src/playground/HZA/identifier/time_stamped_detail.cpp deleted file mode 100644 index 1ea13df..0000000 --- a/src/playground/HZA/identifier/time_stamped_detail.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include "time_stamped_detail.hpp" - -namespace world_exe::interfaces::detail { -const std::time_t TimeStamped::GetTimeStamp() const { - // 不是很清楚具体实现是什么 - return time_stamp_; -} -} \ No newline at end of file diff --git a/src/playground/HZA/identifier/time_stamped_detail.hpp b/src/playground/HZA/identifier/time_stamped_detail.hpp deleted file mode 100644 index 455d1f4..0000000 --- a/src/playground/HZA/identifier/time_stamped_detail.hpp +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once -#include "interfaces/time_stamped.hpp" - -namespace world_exe::interfaces::detail { -class TimeStamped : public world_exe::interfaces::ITimeStamped { -public: - TimeStamped() = default; - virtual ~TimeStamped() = default; - const std::time_t GetTimeStamp() const override; - -private: - std::time_t time_stamp_; -}; -} \ No newline at end of file diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index d8fd994..4d4f560 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -5,6 +5,7 @@ #include "../v1/sync/syncer.hpp" #include "core/event_bus.hpp" +#include "data/predictor_update_package.hpp" #include "parameters/params_system_v1.hpp" #include "parameters/profile.hpp" #include "tongji/fire_controller/fire_controller.hpp" @@ -16,27 +17,6 @@ namespace world_exe::tongji { using namespace std::chrono; -class Combined final : public interfaces::IPreDictorUpdatePackage, interfaces::ITimeStamped { -public: - virtual const std::time_t GetTimeStamp() const { - return data1_.camera_capture_begin_time_stamp; - }; - const world_exe::interfaces::ITimeStamped& GetTimeStamped() const { return *this; } - std::shared_ptr GetArmors() const { return data2_; }; - Eigen::Affine3d GetTransform() const { return data1_.camera_to_gimbal; }; - - // but why? - Combined(const data::CameraGimbalMuzzleSyncData& data1, - std::shared_ptr data2) - : data1_(data1) - , data2_(data2) { } - Combined() = delete; - ~Combined() = default; - -private: - const data::CameraGimbalMuzzleSyncData& data1_; - const std::shared_ptr data2_; -}; class AutoAimSystem::Impl { public: @@ -69,7 +49,7 @@ class AutoAimSystem::Impl { if (flag == enumeration::ArmorIdFlag::None) return; // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分 - const auto& [pack, check] = syncer_->get_data(armors_in_image->GetTimeStamped().GetTimeStamp()); + const auto& [pack, check] = syncer_->get_data(armors_in_image->GetTimeStamp()); if (!check) return; const auto R_camera2gimbal = pack.camera_to_gimbal.rotation(); @@ -78,7 +58,7 @@ class AutoAimSystem::Impl { pnp_solver_->SetCamera2Gimbal(R_camera2gimbal, t_camera2gimbal); const auto& armors_in_camera = pnp_solver_->SolvePnp(armors_in_image); - auto combined = std::make_shared(pack, armors_in_camera); + auto combined = std::make_shared(pack, armors_in_camera); live_target_manager_->Update(combined, armors_in_image); @@ -99,8 +79,7 @@ class AutoAimSystem::Impl { data::FireControl GetControlCommand() { fire_controller_->GetAttackCarId(); - return fire_controller_->CalculateTarget( - (std::chrono::steady_clock::now() - time_stamp_).count()); + return fire_controller_->CalculateTarget(std::chrono::duration_cast(std::chrono::steady_clock::now() - time_stamp_)); } private: diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 7397b3d..fc61e66 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -12,7 +12,6 @@ #include "data/fire_control.hpp" #include "fire_decision.hpp" #include "interfaces/target_predictor.hpp" -#include "tongji/time_stamp/time_stamp.hpp" namespace world_exe::tongji::fire_control { @@ -21,7 +20,7 @@ using StateMachine = state_machine::StateMachine; using IdentifiedArmor = identifier::IdentifiedArmor; using CarIDFlag = enumeration::CarIDFlag; using LiveTargetManager = predictor::LiveTargetManager; -using TimeStamp = time_stamp::TimeStamp; +using TimeStamp = data::TimeStamp; class FireController::Impl { public: @@ -34,7 +33,7 @@ class FireController::Impl { , live_target_manager_(live_target_manager) { auto yaml = YAML::LoadFile(config_path); - control_delay_ = yaml["control_delay"].as(); + control_delay_ = std::chrono::seconds{static_cast(yaml["control_delay"].as() * 1e9)}; } // TODO:time_duration 没有使用，详见std::shared_ptr @@ -54,7 +53,7 @@ class FireController::Impl { - rep: 也许改个名字就好了 */ - const data ::FireControl CalculateTarget(const std ::time_t& time_from_tracker_timepoint) const { + const data ::FireControl CalculateTarget(const std::chrono::seconds& time_from_tracker_timepoint) const { if (!fire_decision_ || !state_machine_ || !live_target_manager_) return { .fire_allowance = false }; @@ -63,7 +62,7 @@ class FireController::Impl { auto snapshot_manager = live_target_manager_->GetPredictor(converged_cars); if (!snapshot_manager) return data::FireControl { - .time_stamp = TimeStamp(std::chrono::steady_clock::now()).GetTimeStamp(), + .time_stamp = data::TimeStamp{time_from_tracker_timepoint}, .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), .fire_allowance = false }; @@ -93,7 +92,7 @@ class FireController::Impl { data::FireControl result; result.fire_allowance = fire_command; result.gimbal_dir << gimbal_command.yaw, gimbal_command.pitch, 0; - result.time_stamp = TimeStamp(std::chrono::steady_clock::now()).GetTimeStamp(); + result.time_stamp = data::TimeStamp{std::chrono::steady_clock::now().time_since_epoch()}; return result; } @@ -110,7 +109,7 @@ class FireController::Impl { private: double gimbal_yaw_; - double control_delay_; + std::chrono::seconds control_delay_; mutable CarIDFlag locked_target; mutable double firable_; @@ -125,7 +124,7 @@ FireController::FireController(const std::string& config_path, std::shared_ptr live_target_manager) : pimpl_(std::make_unique(config_path, state_machine, live_target_manager)) { } FireController::~FireController() = default; -const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { +const data ::FireControl FireController::CalculateTarget(const std::chrono::seconds& time_duration) const { return pimpl_->CalculateTarget(time_duration); } diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 0b7eeb7..0c948e2 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -1,8 +1,8 @@ #pragma once +#include #include -#include "../time_stamp/time_stamp.hpp" #include "interfaces/car_state.hpp" #include "interfaces/fire_controller.hpp" #include "interfaces/target_predictor.hpp" @@ -16,9 +16,9 @@ class FireController final : public interfaces::IFireControl { std::shared_ptr live_target_manager); ~FireController(); - const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; + const data ::FireControl CalculateTarget(const std::chrono::seconds& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; - time_stamp::TimeStamp GetTimeStamp() const; + data::TimeStamp GetTimeStamp() const; void UpdateGimbalPosition(const double& gimbal_yaw); diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp index 9a658f4..079c0c4 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -2,8 +2,7 @@ #include "enum/armor_id.hpp" #include "interfaces/armor_in_image.hpp" -#include "interfaces/time_stamped.hpp" -#include "tongji/time_stamp/time_stamp.hpp" +#include "data/time_stamped.hpp" #include "util/index.hpp" namespace world_exe::tongji::identifier { @@ -16,7 +15,7 @@ class IdentifiedArmor final : public interfaces::IArmorInImage { } } - const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } + const data::TimeStamp& GetTimeStamp() const override { return time_stamp_; } const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const override { @@ -28,7 +27,7 @@ class IdentifiedArmor final : public interfaces::IArmorInImage { } private: - time_stamp::TimeStamp time_stamp_ { std::chrono::steady_clock::now() }; + data::TimeStamp time_stamp_ { std::chrono::steady_clock::now().time_since_epoch() }; std::array, 8> armors_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index 9dbb878..b8d70ff 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -4,8 +4,8 @@ #include #include -#include "../time_stamp/time_stamp.hpp" #include "data/armor_gimbal_control_spacing.hpp" +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "interfaces/armor_in_gimbal_control.hpp" @@ -14,8 +14,8 @@ namespace world_exe::tongji::predictor { class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { public: InGimbalControlArmor(const std::unordered_map>& all_armors, - const std::time_t& time_stamp) + std::vector>& all_armors, + const data::TimeStamp& time_stamp) : armors_map_(std::move(all_armors)) , time_stamp_(time_stamp) { } @@ -25,10 +25,10 @@ class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { return it != armors_map_.end() ? it->second : empty; } - const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } + const data::TimeStamp& GetTimeStamp() const override { return time_stamp_; } private: - time_stamp::TimeStamp time_stamp_; + data::TimeStamp time_stamp_; std::unordered_map> armors_map_; static inline const std::vector empty={}; diff --git a/src/tongji/predictor/live_target_manager/live_target.hpp b/src/tongji/predictor/live_target_manager/live_target.hpp index 7e45e2b..37ddd00 100644 --- a/src/tongji/predictor/live_target_manager/live_target.hpp +++ b/src/tongji/predictor/live_target_manager/live_target.hpp @@ -6,10 +6,10 @@ #include -#include "../../time_stamp/time_stamp.hpp" #include "../kalman_filter/extended_kalman_filter.hpp" #include "../kalman_filter/predict_model.hpp" #include "data/armor_gimbal_control_spacing.hpp" +#include "data/time_stamped.hpp" #include "enum/car_id.hpp" namespace world_exe::tongji::predictor { @@ -21,7 +21,7 @@ class LiveTarget { LiveTarget(const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, const enumeration::CarIDFlag& car_id) - : last_see_time_stamp_(std::time(nullptr)) + : last_see_time_stamp_() , model_(car_id) { // x vx y vy z vz a w r l h // a: angle @@ -44,7 +44,7 @@ class LiveTarget { EKF::XVec GetEkfX() const { return ekf_->x; } EKF::PDig GetP0Dig() const { return model_.GetP0Dig(); } const PredictorModel& GetModel() const { return model_; } - time_stamp::TimeStamp LastSeen() const { return time_stamp::TimeStamp(last_see_time_stamp_); } + data::TimeStamp LastSeen() const { return data::TimeStamp(last_see_time_stamp_); } std::vector GetArmorGimbalControlSpacings() const { std::vector armors; @@ -71,8 +71,6 @@ class LiveTarget { update_count_++; Update_ypda(armor_xyz_in_gimbal, armor_ypr_in_gimbal, armor_ypd_in_gimbal, id, dt); - - last_see_time_stamp_ = std::time(nullptr); } bool IsConverged() const { @@ -119,9 +117,9 @@ class LiveTarget { } } - std::time_t last_see_time_stamp_; - PredictorModel model_; - std::optional ekf_; + std::chrono::nanoseconds last_see_time_stamp_; + PredictorModel model_; + std::optional ekf_; int last_id_ = -1; int update_count_ = 0; diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp index 69b3263..2b45fe8 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -7,9 +7,10 @@ #include "../in_gimbal_control_armor.hpp" #include "../target_snapshot_manager/target_snapshot_manager.hpp" +#include "data/predictor_update_package.hpp" +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" -#include "interfaces/predictor_update_package.hpp" #include "live_target.hpp" #include "tracker.hpp" #include "util/index.hpp" @@ -22,7 +23,7 @@ class LiveTargetManager::Impl { Impl(const std::string& config_path, const double& timeout_sec) : targets_map_() , tracker_(std::make_unique()) - , last_update_timestamp_(std::time(nullptr)) + , last_update_timestamp_() , tracking_id_(enumeration::CarIDFlag::None) , config_path_(config_path) { } @@ -38,7 +39,7 @@ class LiveTargetManager::Impl { */ std::shared_ptr Predict( - const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { + const enumeration::ArmorIdFlag& flag, const data::TimeStamp& time_stamp) { std::unordered_map> result; @@ -69,10 +70,10 @@ class LiveTargetManager::Impl { config_path_, flag, targets_map_, last_update_timestamp_); } // 为何传递了一个time_t 给double - void Update(std::shared_ptr data, + void Update(std::shared_ptr data, const std::shared_ptr& armors_in_image, const double& dt) { - UpdateTimeStamp(data->GetTimeStamped().GetTimeStamp()); + UpdateTimeStamp(data->GetTimeStamp()); UpdateTargetMap(data); UpdateTarget(data, armors_in_image, dt); } @@ -85,9 +86,9 @@ class LiveTargetManager::Impl { } private: - void UpdateTimeStamp(const time_t& time_stamp) { last_update_timestamp_ = time_stamp; } - void UpdateTargetMap(std::shared_ptr data) { - const Eigen::Affine3d transform = data->GetTransform(); + void UpdateTimeStamp(const data::TimeStamp& time_stamp) { last_update_timestamp_ = time_stamp; } + void UpdateTargetMap(std::shared_ptr data) { + const Eigen::Affine3d transform = data->GetCameraToWorld(); const Eigen::Matrix3d rotation_matrix = transform.rotation(); const auto armors_interface = data->GetArmors(); @@ -106,9 +107,9 @@ class LiveTargetManager::Impl { } } - void UpdateTarget(std::shared_ptr data, + void UpdateTarget(std::shared_ptr data, const std::shared_ptr& armors_in_image, const double& dt) { - const Eigen::Affine3d transform = data->GetTransform(); + const Eigen::Affine3d transform = data->GetCameraToWorld(); const Eigen::Matrix3d rotation_matrix = transform.linear(); const auto armors_interface = data->GetArmors(); @@ -126,7 +127,7 @@ class LiveTargetManager::Impl { std::unordered_map> targets_map_; std::unique_ptr tracker_; - std::time_t last_update_timestamp_; + data::TimeStamp last_update_timestamp_; enumeration::CarIDFlag tracking_id_; const std::string config_path_; @@ -137,7 +138,7 @@ LiveTargetManager::LiveTargetManager(const std::string& config_path, const doubl LiveTargetManager::~LiveTargetManager() = default; std ::shared_ptr LiveTargetManager::Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { + const enumeration ::ArmorIdFlag& id, const data::TimeStamp& time_stamp) { return pimpl_->Predict(id, time_stamp); } std ::shared_ptr LiveTargetManager::GetPredictor( @@ -145,13 +146,12 @@ std ::shared_ptr LiveTargetManager::GetPredictor( return pimpl_->GetPredictor(id); } -static constexpr double cast_nanosec_sec(const long nanosec) {return nanosec * 1e-9;} -void LiveTargetManager::Update(std::shared_ptr data, +void LiveTargetManager::Update(std::shared_ptr data, const std::shared_ptr& armors_in_image) { - return pimpl_->Update(data, armors_in_image, cast_nanosec_sec(data->GetTimeStamped().GetTimeStamp())); + return pimpl_->Update(data, armors_in_image, data->GetTimeStamp().to_seconds()); +} +auto LiveTargetManager::GetAllowedTargetID() const -> enumeration::ArmorIdFlag const { + return pimpl_->GetAllowedTargetID(); } -// auto LiveTargetManager::GetAllowedTargetID() const -> enumeration::ArmorIdFlag const { -// return pimpl_->GetAllowedTargetID(); -// } } diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.hpp b/src/tongji/predictor/live_target_manager/live_target_manager.hpp index 9e90eba..355b63e 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.hpp @@ -2,9 +2,10 @@ #include +#include "data/predictor_update_package.hpp" +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "interfaces/armor_in_image.hpp" -#include "interfaces/predictor_update_package.hpp" #include "interfaces/target_predictor.hpp" namespace world_exe::tongji::predictor { @@ -15,11 +16,11 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { ~LiveTargetManager(); std ::shared_ptr Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; + const enumeration ::ArmorIdFlag& id, const data::TimeStamp& time_stamp) override; std ::shared_ptr GetPredictor( const enumeration ::ArmorIdFlag& id) const override; - void Update(std::shared_ptr data, + void Update(std::shared_ptr data, const std::shared_ptr& armors_in_image); auto GetAllowedTargetID() const -> enumeration::ArmorIdFlag const; diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp index 3c2e22e..4d5935b 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp @@ -2,7 +2,6 @@ #include -#include "../../time_stamp/time_stamp.hpp" #include "../kalman_filter/extended_kalman_filter.hpp" #include "../kalman_filter/predict_model.hpp" #include "../live_target_manager/live_target.hpp" @@ -52,7 +51,7 @@ class TargetSnapshot { private: PredictorModel model_; ExtendedKalmanFilter ekf_; - time_stamp::TimeStamp time_stamp_; + data::TimeStamp time_stamp_; }; } diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp index 2794f89..3e3247a 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -10,6 +10,7 @@ #include "../live_target_manager/live_target.hpp" #include "aim_solver.hpp" #include "data/armor_gimbal_control_spacing.hpp" +#include "data/time_stamped.hpp" #include "enum/enum_tools.hpp" namespace world_exe::tongji::predictor { @@ -19,7 +20,7 @@ class TargetSnapshotManager::Impl { Impl(const std::string& config_path, const enumeration::ArmorIdFlag& id, const std::unordered_map>& live_target_map, - const std::time_t& now) + const data::TimeStamp& now) : aim_solver_(std::make_unique(config_path)) , snapshots_(BuildSnapshots(live_target_map)) , now_(now) @@ -55,7 +56,7 @@ class TargetSnapshotManager::Impl { */ std::shared_ptr Predictor( - const std::time_t& time_stamp) const { + const data::TimeStamp& time_stamp) const { std::unordered_map> result; @@ -104,7 +105,7 @@ class TargetSnapshotManager::Impl { std::unique_ptr aim_solver_; const std::unordered_map snapshots_; - const std::time_t& now_; + const data::TimeStamp& now_; const enumeration::ArmorIdFlag ids_; double bullet_speed_; mutable GimbalCommand gimbal_command_; @@ -118,13 +119,13 @@ TargetSnapshotManager::TargetSnapshotManager(const std::string& config_path, const enumeration::ArmorIdFlag& id, const std::unordered_map>& live_target_map, - const std::time_t& now) + const data::TimeStamp& now) : pimpl_(std::make_unique(config_path, id, live_target_map, now)) { } TargetSnapshotManager::~TargetSnapshotManager() = default; const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } std ::shared_ptr TargetSnapshotManager::Predictor( - const std ::time_t& time_stamp) const { + const data::TimeStamp& time_stamp) const { return pimpl_->Predictor(time_stamp); } diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp index 4db47e6..1ad478e 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp @@ -4,6 +4,7 @@ #include #include "../live_target_manager/live_target.hpp" +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" @@ -18,12 +19,12 @@ class TargetSnapshotManager final : public interfaces::IPredictor { TargetSnapshotManager(const std::string& config_path, const enumeration::ArmorIdFlag& id, const std::unordered_map>& live_target_map, - const std::time_t& now); + const data::TimeStamp& now); ~TargetSnapshotManager(); const enumeration ::ArmorIdFlag& GetId() const override; std ::shared_ptr Predictor( - const std ::time_t& time_stamp) const override; + const data::TimeStamp& time_stamp) const override; auto GetGimbalCommand() const -> GimbalCommand const; diff --git a/src/tongji/solver/solved_armor.hpp b/src/tongji/solver/solved_armor.hpp index dfc298e..7e6fe34 100644 --- a/src/tongji/solver/solved_armor.hpp +++ b/src/tongji/solver/solved_armor.hpp @@ -4,14 +4,13 @@ #include #include "interfaces/armor_in_camera.hpp" -#include "interfaces/time_stamped.hpp" -#include "tongji/time_stamp/time_stamp.hpp" +#include "data/time_stamped.hpp" #include "util/index.hpp" namespace world_exe::tongji::solver { class SolvedArmor final : public interfaces::IArmorInCamera { public: - explicit SolvedArmor(const std::vector& armors, time_t when_image_come) + explicit SolvedArmor(const std::vector& armors, data::TimeStamp when_image_come) : time_stamp_(when_image_come) { for (const auto& armor : armors) { armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); @@ -22,10 +21,10 @@ class SolvedArmor final : public interfaces::IArmorInCamera { const enumeration::ArmorIdFlag& armor_id) const override { return armors_[util::enumeration::GetIndex(armor_id)]; } - const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } + const data::TimeStamp& GetTimeStamp() const override { return time_stamp_; } private: std::array, 8> armors_; - time_stamp::TimeStamp time_stamp_; + data::TimeStamp time_stamp_; }; } diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index ab56cdc..3950a31 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -13,6 +13,7 @@ #include "data/armor_camera_spacing.hpp" #include "data/armor_image_spaceing.hpp" +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" @@ -40,7 +41,7 @@ class Solver::Impl { armor_plates.emplace_back(solved_armor); } } - return std::make_shared(armor_plates, armors_in_image->GetTimeStamped().GetTimeStamp()); + return std::make_shared(armor_plates, armors_in_image->GetTimeStamp()); } auto SetCamera2Gimbal( @@ -82,8 +83,8 @@ class Solver::Impl { return best_yaw; } - const std::time_t GetTimeStamp() const { - return time_stamp::TimeStamp(std::chrono::steady_clock::now()).GetTimeStamp(); + const data::TimeStamp GetTimeStamp() const { + return data::TimeStamp(std::chrono::steady_clock::now().time_since_epoch()); } private: @@ -136,8 +137,6 @@ Solver::Solver() : pimpl_(std::make_unique()) { } Solver::~Solver() = default; -const std::time_t Solver::GetTimeStamp() const { return pimpl_->GetTimeStamp(); } - std::shared_ptr Solver::SolvePnp( std::shared_ptr armors_in_image) { return pimpl_->EstimateAllArmorPoses(armors_in_image); diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index e52e4c4..442ca17 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -1,18 +1,16 @@ #pragma once #include "interfaces/pnp_solver.hpp" -#include "interfaces/time_stamped.hpp" namespace world_exe::tongji::solver { -class Solver final : public interfaces::IPnpSolver, public interfaces::ITimeStamped { +class Solver final : public interfaces::IPnpSolver{ public: explicit Solver(); ~Solver(); std::shared_ptr SolvePnp( std::shared_ptr armor) override; - const std::time_t GetTimeStamp() const override; void SetCamera2Gimbal( const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal); diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 19bfeb3..5bd855f 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -26,7 +26,9 @@ class StateMachine::Impl { StateMachine::StateMachine(std::shared_ptr live_target_manager) : pimpl_(std::make_unique(live_target_manager)) { } -StateMachine::~StateMachine() = default; + +StateMachine::~StateMachine() {}; +StateMachine::StateMachine() {}; const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return pimpl_->GetAllowdToFires(); diff --git a/src/tongji/time_stamp/time_stamp.hpp b/src/tongji/time_stamp/time_stamp.hpp deleted file mode 100644 index 454bac0..0000000 --- a/src/tongji/time_stamp/time_stamp.hpp +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "interfaces/time_stamped.hpp" - -namespace world_exe::tongji::time_stamp { -using namespace std::chrono; -class TimeStamp final : public interfaces::ITimeStamped { -public: - using StorageType = int64_t; - using ClockType = std::chrono::steady_clock; - using TimeDuration = nanoseconds; - - explicit TimeStamp(const steady_clock::time_point& tp) - : value_ns_(duration_cast(tp.time_since_epoch()).count()) { } - explicit TimeStamp(const StorageType& ns_count) - : value_ns_(ns_count) { } - - auto DeltaTime(const TimeStamp& other) const -> double { - TimeDuration diff_ns = TimeDuration(value_ns_ - other.value_ns_); - return std::chrono::duration(diff_ns).count(); - } - - const std::time_t GetTimeStamp() const override { - return std::bit_cast(value_ns_); - } - -private: - const StorageType value_ns_; -}; -} \ No newline at end of file diff --git a/src/v1/auto_aim_system_v1.cpp b/src/v1/auto_aim_system_v1.cpp index 966ef19..6f64e54 100644 --- a/src/v1/auto_aim_system_v1.cpp +++ b/src/v1/auto_aim_system_v1.cpp @@ -7,8 +7,7 @@ #include "identifier/identifier.hpp" #include "interfaces/armor_in_camera.hpp" #include "interfaces/armor_in_image.hpp" -#include "interfaces/predictor_update_package.hpp" -#include "interfaces/time_stamped.hpp" +#include "data/predictor_update_package.hpp" #include "parameters/params_system_v1.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" @@ -33,24 +32,6 @@ using namespace v1; using namespace parameters; using namespace std::chrono; -class Combined final : public interfaces::IPreDictorUpdatePackage, interfaces::ITimeStamped{ -public: - virtual const std::time_t GetTimeStamp() const{ return data1_.camera_capture_begin_time_stamp; }; - const world_exe::interfaces::ITimeStamped& GetTimeStamped() const {return *this;} - std::shared_ptr GetArmors() const{return data2_;}; - Eigen::Affine3d GetTransform() const {return data1_.camera_to_gimbal;}; - - // but why? - Combined(const data::CameraGimbalMuzzleSyncData& data1, std::shared_ptr data2) - : data1_(data1) - , data2_(data2){ - } - Combined() = delete; - ~Combined() = default; -private: - const data::CameraGimbalMuzzleSyncData& data1_; - const std::shared_ptr data2_; -}; class world_exe::v1::SystemV1::Impl{ public: @@ -94,16 +75,16 @@ class world_exe::v1::SystemV1::Impl{ const auto& solved = armor_pnp->SolvePnp(armors); - const auto& [pack, check] = sync->get_data(solved->GetTimeStamped().GetTimeStamp()); + const auto& [pack, check] = sync->get_data(solved->GetTimeStamp()); if(!check) [[unlikely]] return; time_point_ = std::chrono::steady_clock::now(); state_machine ->Update(flag); const auto& fire_targets = state_machine->GetAllowdToFires(); - auto combined = std::make_shared(pack, solved); + auto combined = std::make_shared(pack, solved); predictor ->Update(combined); - const auto& time = combined->GetTimeStamped().GetTimeStamp(); + const auto& time = combined->GetTimeStamp(); const auto& armor3d = predictor->Predict(fire_targets,time); fire_control ->set_armor(armor3d); @@ -124,7 +105,7 @@ class world_exe::v1::SystemV1::Impl{ core::EventBus::Publish>( parameters::ParamsForSystemV1::armors_in_camera_pnp_event, solved); - core::EventBus::Publish>( + core::EventBus::Publish>( parameters::ParamsForSystemV1::tracker_update_event, combined); core::EventBus::Publish( @@ -137,7 +118,7 @@ class world_exe::v1::SystemV1::Impl{ } data::FireControl control(){ - return fire_control->CalculateTarget((std::chrono::steady_clock::now() - time_point_).count()); + return fire_control->CalculateTarget(std::chrono::duration_cast(std::chrono::steady_clock::now() - time_point_)); } Impl(const Impl&) = delete; diff --git a/src/v1/fire_controller/fire_controller.cpp b/src/v1/fire_controller/fire_controller.cpp index c618f1d..de57210 100644 --- a/src/v1/fire_controller/fire_controller.cpp +++ b/src/v1/fire_controller/fire_controller.cpp @@ -1,9 +1,11 @@ #include "./fire_controller.hpp" +#include "data/time_stamped.hpp" #include "enum/enum_tools.hpp" #include "interfaces/armor_in_gimbal_control.hpp" #include "interfaces/predictor.hpp" #include "trajectory.hpp" +#include #include #include @@ -44,8 +46,8 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { predictor_ = predictor; }; - const world_exe::data::FireControl CalculateTarget(const std::time_t& time_duration) { - time_t fly_time = 0; + const world_exe::data::FireControl CalculateTarget(const std::chrono::seconds& time_duration) { + std::chrono::seconds fly_time{0}; const auto& pre1 = predictor_->Predictor(fly_time + time_duration + control_delay_); const auto& pre2 = pre1->GetArmors(predictor_->GetId()); double min_angular_dis = 1e9; @@ -71,13 +73,13 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { trajectory_solver::gravity_only(armors[index].position, velocity_begin_, gravity_); } - return { .time_stamp = fly_time + time_duration + control_delay_, .fire_allowance = true }; + return { .time_stamp = data::TimeStamp{std::chrono::nanoseconds(fly_time + time_duration + control_delay_)}, .fire_allowance = true }; } private: world_exe::enumeration::CarIDFlag tracing_ = enumeration::CarIDFlag::None; time_t time_predict_point_; - const time_t control_delay_; + const std::chrono::seconds control_delay_; const double velocity_begin_; const double gravity_; const world_exe::data::FireControl no_allow_ { .fire_allowance = false }; @@ -87,7 +89,7 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { const world_exe::data::FireControl // world_exe::v1::fire_control::TracingFireControl::CalculateTarget( - const std::time_t& time_duration) const { + const std::chrono::seconds& time_duration) const { return pimpl_->CalculateTarget(time_duration); } diff --git a/src/v1/fire_controller/fire_controller.hpp b/src/v1/fire_controller/fire_controller.hpp index d1e2910..171c0e6 100644 --- a/src/v1/fire_controller/fire_controller.hpp +++ b/src/v1/fire_controller/fire_controller.hpp @@ -1,14 +1,14 @@ #pragma once #include "data/fire_control.hpp" -#include "interfaces/car_state.hpp" #include "interfaces/fire_controller.hpp" #include "interfaces/predictor.hpp" +#include #include namespace world_exe::v1::fire_control { class TracingFireControl final : public interfaces::IFireControl { public: - const data::FireControl CalculateTarget(const std::time_t& time_duration) const override; + const data::FireControl CalculateTarget(const std::chrono::seconds& time_duration) const override; const enumeration::CarIDFlag GetAttackCarId() const override; void set_armor(const std::shared_ptr& armors); void SetPredictor(const std::shared_ptr& predictor); diff --git a/src/v1/identifier/identifier.cpp b/src/v1/identifier/identifier.cpp index 16daf46..c4f615a 100644 --- a/src/v1/identifier/identifier.cpp +++ b/src/v1/identifier/identifier.cpp @@ -76,13 +76,11 @@ class Identifier::Impl { std::tuple, enumeration::CarIDFlag> Identify( const cv::Mat& input_image) { - time_t time_now = std::chrono::steady_clock::now().time_since_epoch().count(); - // 首先使用深度学习模型进行装甲板检测得到roi区域 const auto armor_infos = model_infer(input_image); // 然后进行灯条匹配验证 auto [a, b] = matchPlate(input_image, armor_infos); - a->time_stamp_ = time_now; + a->time_stamp_ = std::chrono::steady_clock::now().time_since_epoch(); return {a, b}; } diff --git a/src/v1/identifier/identifier_armor.hpp b/src/v1/identifier/identifier_armor.hpp index 2696658..310adf5 100644 --- a/src/v1/identifier/identifier_armor.hpp +++ b/src/v1/identifier/identifier_armor.hpp @@ -1,12 +1,12 @@ #pragma once #include "interfaces/armor_in_image.hpp" -#include "interfaces/time_stamped.hpp" +#include "data/time_stamped.hpp" #include "util/index.hpp" #include namespace world_exe::v1::identifier { -class IdentifierArmor : public interfaces::IArmorInImage, public interfaces::ITimeStamped { +class IdentifierArmor final : public interfaces::IArmorInImage { public: IdentifierArmor() = default; IdentifierArmor(const std::vector& armors) @@ -15,9 +15,7 @@ class IdentifierArmor : public interfaces::IArmorInImage, public interfaces::ITi armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); } - const interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } - - const std::time_t GetTimeStamp() const override { return time_stamp_; }; + const data::TimeStamp& GetTimeStamp() const override { return time_stamp_; } void SetArmors(const std::vector& armors) { for (auto& armors : armors_) @@ -31,7 +29,7 @@ class IdentifierArmor : public interfaces::IArmorInImage, public interfaces::ITi return armors_[util::enumeration::GetIndex(armor_id)]; } - std::time_t time_stamp_ {0}; + data::TimeStamp time_stamp_ {}; private: std::array, 8> armors_; diff --git a/src/v1/pnpsolver/armor_pnp_solver.cpp b/src/v1/pnpsolver/armor_pnp_solver.cpp index 9feb630..3f82652 100644 --- a/src/v1/pnpsolver/armor_pnp_solver.cpp +++ b/src/v1/pnpsolver/armor_pnp_solver.cpp @@ -5,7 +5,7 @@ #include "armor_pnp_solver.hpp" #include "interfaces/armor_in_camera.hpp" -#include "interfaces/time_stamped.hpp" +#include "data/time_stamped.hpp" #include "parameters/profile.hpp" #include "util/index.hpp" @@ -55,17 +55,15 @@ class ArmorIPPEPnPSolver::Impl { const std::vector& NormalArmorObjectPoints_; }; -class ArmorInCamera final : public world_exe::interfaces::IArmorInCamera, - world_exe::interfaces::ITimeStamped { +class ArmorInCamera final : public world_exe::interfaces::IArmorInCamera { public: - const world_exe::interfaces::ITimeStamped& GetTimeStamped() const override { return *this; } + const world_exe::data::TimeStamp& GetTimeStamp() const override { return time_stampe; } const std::vector& GetArmors( const world_exe::enumeration::ArmorIdFlag& armor_id) const override { return armors[world_exe::util::enumeration::GetIndex(armor_id)]; } - const std::time_t GetTimeStamp() const override { return time_stampe; } - std::time_t time_stampe = 0; + world_exe::data::TimeStamp time_stampe{}; std::vector armors[static_cast(world_exe::enumeration::ArmorIdFlag::Count)]; @@ -86,8 +84,6 @@ std::shared_ptr ArmorIPPEPnPSolver::Solve return armors_; } -const std::time_t ArmorIPPEPnPSolver::GetTimeStamp() const { return time_point_; } - ArmorIPPEPnPSolver::ArmorIPPEPnPSolver(const std::vector& LargeArmorObjectPoints, const std::vector& NormalArmorObjectPoints) : pimpl_(std::make_unique(LargeArmorObjectPoints, NormalArmorObjectPoints)) { } diff --git a/src/v1/pnpsolver/armor_pnp_solver.hpp b/src/v1/pnpsolver/armor_pnp_solver.hpp index 2760ad6..c9d5d87 100644 --- a/src/v1/pnpsolver/armor_pnp_solver.hpp +++ b/src/v1/pnpsolver/armor_pnp_solver.hpp @@ -1,4 +1,5 @@ #pragma once +#include #include #include @@ -7,20 +8,19 @@ namespace world_exe::v1::pnpsolver { -class ArmorIPPEPnPSolver final : public interfaces::IPnpSolver, public interfaces::ITimeStamped { +class ArmorIPPEPnPSolver final : public interfaces::IPnpSolver { public: ArmorIPPEPnPSolver(const std::vector& LargeArmorObjectPointsOpencv, const std::vector& NormalArmorObjectPointsOpencv); ~ArmorIPPEPnPSolver(); - void set_time_point(const std::time_t& time_point); + void set_time_point(const std::chrono::nanoseconds& time_point); std::shared_ptr SolvePnp( std::shared_ptr armor) override; - const std::time_t GetTimeStamp() const override; private: class Impl; std::unique_ptr pimpl_; - std::time_t time_point_; + std::chrono::nanoseconds time_point_; }; } \ No newline at end of file diff --git a/src/v1/predictor/car/car_predictor.cpp b/src/v1/predictor/car/car_predictor.cpp index cc67c1f..43c674f 100644 --- a/src/v1/predictor/car/car_predictor.cpp +++ b/src/v1/predictor/car/car_predictor.cpp @@ -1,26 +1,24 @@ #include "car_predictor.hpp" #include "../predict_armor_in_gimbal_control.hpp" -#include "interfaces/car_state.hpp" -#include "interfaces/time_stamped.hpp" -#include "v1/predictor/predict_time_stamp.hpp" +#include "data/time_stamped.hpp" #include namespace world_exe::v1::predictor { class CarPredictor::Impl { public: Impl(const enumeration::CarIDFlag& id, const CarPredictEkf& ekf, - const interfaces::ITimeStamped& create_time_stamp) + const data::TimeStamp& create_time_stamp) : id_(id) , ekf_(ekf) { - create_time_stamp_.SetTimeStamp(create_time_stamp.GetTimeStamp()); + create_time_stamp_ = create_time_stamp; } const enumeration::ArmorIdFlag& GetId() const { return id_; } - std::shared_ptr Predictor(const std::time_t& time_stamp) { + std::shared_ptr Predictor(const data::TimeStamp& time_stamp) { auto ptr = std::make_shared(); ptr->SetWithSingleId(ekf_.get_predict_output_armor( - id_, (time_stamp - create_time_stamp_.GetTimeStamp()) / 1.e9), + id_, (time_stamp - create_time_stamp_).to_seconds()), time_stamp); return ptr; } @@ -29,22 +27,22 @@ class CarPredictor::Impl { inline void SetEkf(const CarPredictEkf& ekf) { ekf_ = ekf; } - inline void SetTimeStamp(const interfaces::ITimeStamped& time_stamp) { - create_time_stamp_.SetTimeStamp(time_stamp.GetTimeStamp()); + inline void SetTimeStamp(const data::TimeStamp& time_stamp) { + create_time_stamp_ = time_stamp; } private: enumeration::CarIDFlag id_; CarPredictEkf ekf_; - PredictTimeStamp create_time_stamp_; + data::TimeStamp create_time_stamp_; }; CarPredictor::CarPredictor() : pimpl_(std::make_unique( - enumeration::CarIDFlag::None, CarPredictEkf {}, PredictTimeStamp {})) { } + enumeration::CarIDFlag::None, CarPredictEkf {}, data::TimeStamp {})) { } CarPredictor::CarPredictor(const enumeration::CarIDFlag& id, const CarPredictEkf& ekf, - const interfaces::ITimeStamped& create_time_stamp) + const data::TimeStamp& create_time_stamp) : pimpl_(std::make_unique(id, ekf, create_time_stamp)) { } CarPredictor::~CarPredictor() = default; @@ -52,7 +50,7 @@ CarPredictor::~CarPredictor() = default; const enumeration::ArmorIdFlag& CarPredictor::GetId() const { return pimpl_->GetId(); } std::shared_ptr CarPredictor::Predictor( - const std::time_t& time_stamp) const { + const data::TimeStamp& time_stamp) const { return pimpl_->Predictor(time_stamp); } @@ -60,7 +58,7 @@ void CarPredictor::SetId(const enumeration::CarIDFlag& id) { return pimpl_->SetI void CarPredictor::SetEkf(const CarPredictEkf& ekf) { return pimpl_->SetEkf(ekf); } -void CarPredictor::SetTimeStamp(const interfaces::ITimeStamped& time_stamp) { +void CarPredictor::SetTimeStamp(const data::TimeStamp& time_stamp) { return pimpl_->SetTimeStamp(time_stamp); }; } \ No newline at end of file diff --git a/src/v1/predictor/car/car_predictor.hpp b/src/v1/predictor/car/car_predictor.hpp index acebd0b..9289164 100644 --- a/src/v1/predictor/car/car_predictor.hpp +++ b/src/v1/predictor/car/car_predictor.hpp @@ -1,24 +1,25 @@ #pragma once #include "car_predictor_ekf.hpp" -#include "interfaces/car_state.hpp" +#include "data/time_stamped.hpp" +#include "enum/car_id.hpp" #include "interfaces/predictor.hpp" namespace world_exe::v1::predictor { -class CarPredictor : public interfaces::IPredictor { +class CarPredictor final: public interfaces::IPredictor { public: CarPredictor(); ~CarPredictor(); CarPredictor(const enumeration::CarIDFlag& id, const CarPredictEkf& ekf, - const interfaces::ITimeStamped& create_time_stamp); + const data::TimeStamp& create_time_stamp); void SetId(const enumeration::CarIDFlag& id); void SetEkf(const CarPredictEkf& ekf); - void SetTimeStamp(const interfaces::ITimeStamped& time_stamp); + void SetTimeStamp(const data::TimeStamp& time_stamp); const enumeration::ArmorIdFlag& GetId() const override; std::shared_ptr Predictor( - const std::time_t& time_stamp) const override; + const data::TimeStamp& time_stamp) const override; private: class Impl; diff --git a/src/v1/predictor/predict_armor_in_gimbal_control.cpp b/src/v1/predictor/predict_armor_in_gimbal_control.cpp index 99969ba..f8e7eb5 100644 --- a/src/v1/predictor/predict_armor_in_gimbal_control.cpp +++ b/src/v1/predictor/predict_armor_in_gimbal_control.cpp @@ -1,5 +1,6 @@ #include "predict_armor_in_gimbal_control.hpp" +#include "data/time_stamped.hpp" #include "enum/enum_tools.hpp" #include "util/index.hpp" #include @@ -9,18 +10,18 @@ class PredictArmorInGimbalControl::Impl { public: Impl() = default; Impl(const std::array, 8>& armors, - predictor::PredictTimeStamp predict_time_stamp) + data::TimeStamp predict_time_stamp) : armors_(armors) , predict_time_stamp_(std::move(predict_time_stamp)) { } void Set(const std::array, 8>& armors, - const predictor::PredictTimeStamp& predict_time_stamp) { + const data::TimeStamp& predict_time_stamp) { armors_ = armors; predict_time_stamp_ = predict_time_stamp; } void SetWithSingleId(const std::vector& armors, - const predictor::PredictTimeStamp& predict_time_stamp) { + const data::TimeStamp& predict_time_stamp) { if (armors.empty()) return; armors_[util::enumeration::GetIndex(armors[0].id)] = armors; } @@ -37,30 +38,30 @@ class PredictArmorInGimbalControl::Impl { return output_armors_; } - const interfaces::ITimeStamped& GetTimeStamped() const { return predict_time_stamp_; } + const data::TimeStamp& GetTimeStamped() const { return predict_time_stamp_; } private: std::array, 8> armors_; std::vector output_armors_; - predictor::PredictTimeStamp predict_time_stamp_; + data::TimeStamp predict_time_stamp_; }; PredictArmorInGimbalControl::PredictArmorInGimbalControl() : pimpl_(std::make_unique()) { } PredictArmorInGimbalControl::PredictArmorInGimbalControl( const std::array, 8>& armors, - const predictor::PredictTimeStamp& predict_time_stamp) + const data::TimeStamp& predict_time_stamp) : pimpl_(std::make_unique(armors, predict_time_stamp)) { } void PredictArmorInGimbalControl::Set( const std::array, 8>& armors, - const predictor::PredictTimeStamp& predict_time_stamp) { + const data::TimeStamp& predict_time_stamp) { return pimpl_->Set(armors, predict_time_stamp); } void PredictArmorInGimbalControl::SetWithSingleId( const std::vector& armors, - const predictor::PredictTimeStamp& predict_time_stamp) { + const data::TimeStamp& predict_time_stamp) { return pimpl_->SetWithSingleId(armors, predict_time_stamp); } @@ -69,7 +70,7 @@ const std::vector& PredictArmorInGimbalControl: return pimpl_->GetArmors(armor_id); } -const interfaces::ITimeStamped& PredictArmorInGimbalControl::GetTimeStamped() const { +const data::TimeStamp& PredictArmorInGimbalControl::GetTimeStamp() const { return pimpl_->GetTimeStamped(); } diff --git a/src/v1/predictor/predict_armor_in_gimbal_control.hpp b/src/v1/predictor/predict_armor_in_gimbal_control.hpp index 3139755..8cf945a 100644 --- a/src/v1/predictor/predict_armor_in_gimbal_control.hpp +++ b/src/v1/predictor/predict_armor_in_gimbal_control.hpp @@ -1,27 +1,27 @@ #pragma once +#include "data/time_stamped.hpp" #include "interfaces/armor_in_gimbal_control.hpp" -#include "predict_time_stamp.hpp" namespace world_exe::v1::predictor { -class PredictArmorInGimbalControl : public world_exe::interfaces::IArmorInGimbalControl { +class PredictArmorInGimbalControl final : public world_exe::interfaces::IArmorInGimbalControl { public: PredictArmorInGimbalControl(); PredictArmorInGimbalControl( const std::array, 8>& armors, - const predictor::PredictTimeStamp& predict_time_stamp); + const data::TimeStamp& predict_time_stamp); ~PredictArmorInGimbalControl(); void Set(const std::array, 8>& armors, - const predictor::PredictTimeStamp& predict_time_stamp); + const data::TimeStamp& predict_time_stamp); void SetWithSingleId(const std::vector& armors, - const predictor::PredictTimeStamp& predict_time_stamp); + const data::TimeStamp& predict_time_stamp); const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const override; - const interfaces::ITimeStamped& GetTimeStamped() const override; + const data::TimeStamp& GetTimeStamp() const override; private: class Impl; diff --git a/src/v1/predictor/predict_time_stamp.hpp b/src/v1/predictor/predict_time_stamp.hpp deleted file mode 100644 index fe818c4..0000000 --- a/src/v1/predictor/predict_time_stamp.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include "interfaces/time_stamped.hpp" - -namespace world_exe::v1::predictor { -class PredictTimeStamp : public interfaces::ITimeStamped { -public: - PredictTimeStamp() = default; - PredictTimeStamp(const std::time_t& time_stamp) - : time_stamp_(time_stamp) { } - - inline void SetTimeStamp(const time_t& time_stamp) { time_stamp_ = time_stamp; } - - const std::time_t GetTimeStamp() const override { return time_stamp_; }; - -private: - std::time_t time_stamp_; -}; -} \ No newline at end of file diff --git a/src/v1/predictor/predictor_manager.cpp b/src/v1/predictor/predictor_manager.cpp index e964dd2..f4f2b65 100644 --- a/src/v1/predictor/predictor_manager.cpp +++ b/src/v1/predictor/predictor_manager.cpp @@ -1,9 +1,9 @@ #include "predictor_manager.hpp" #include "car/car_predictor.hpp" #include "car/car_predictor_ekf.hpp" +#include "data/time_stamped.hpp" #include "enum/enum_tools.hpp" #include "predict_armor_in_gimbal_control.hpp" -#include "predict_time_stamp.hpp" #include "util/index.hpp" #include @@ -11,11 +11,11 @@ namespace world_exe::v1::predictor { class PredictorManager::Impl { public: - inline void Update(const std::shared_ptr& data) { - const auto time_stamp = data->GetTimeStamped().GetTimeStamp(); - const auto dt = (time_stamp - last_update_time_stamp_.GetTimeStamp()); + inline void Update(const std::shared_ptr& data) { + const auto time_stamp = data->GetTimeStamp(); + const auto dt = (time_stamp - last_update_time_stamp_); - const auto transform = data->GetTransform(); + const auto transform = data->GetCameraToWorld(); const auto rotation_transform = Eigen::Quaterniond { transform.linear() }; for (int i = 0; i < 8; i++) { @@ -31,7 +31,7 @@ class PredictorManager::Impl { std::atan(tmp_armor.position.y() / tmp_armor.position.x()), -std::atan(tmp_armor.position.z() / tmp_armor.position.x()), tmp_armor.position.norm(); - predictors_[i].Update(input, {}, dt); + predictors_[i].Update(input, {}, dt.to_seconds()); } else if (armors.size() == 2) { // 当同时识别到两块装甲板时，优先更新近的那块，再更新远的 const auto armor0_yaw = util::math::get_yaw_from_quaternion(armors[0].orientation); @@ -47,7 +47,7 @@ class PredictorManager::Impl { std::atan(tmp_armor0.position.y() / tmp_armor0.position.x()), -std::atan(tmp_armor0.position.z() / tmp_armor0.position.x()), tmp_armor0.position.norm(); - predictors_[i].Update(input, {}, dt); + predictors_[i].Update(input, {}, dt.to_seconds()); // 同时识别到一辆车的两块装甲板时要调这个函数 predictors_[i].set_second_armor(); input << util::math::get_yaw_from_quaternion(tmp_armor1.orientation), @@ -60,7 +60,7 @@ class PredictorManager::Impl { std::atan(tmp_armor1.position.y() / tmp_armor1.position.x()), -std::atan(tmp_armor1.position.z() / tmp_armor1.position.x()), tmp_armor1.position.norm(); - predictors_[i].Update(input, {}, dt); + predictors_[i].Update(input, {}, dt.to_seconds()); // 同时识别到一辆车的两块装甲板时要调这个函数 predictors_[i].set_second_armor(); input << util::math::get_yaw_from_quaternion(tmp_armor0.orientation), @@ -72,12 +72,12 @@ class PredictorManager::Impl { } } - last_update_time_stamp_.SetTimeStamp(time_stamp); + last_update_time_stamp_ = time_stamp; } inline std::shared_ptr Predict( - const world_exe::enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { - const auto dt = (time_stamp - last_update_time_stamp_.GetTimeStamp()) / 1.e9; + const world_exe::enumeration::ArmorIdFlag& id, const data::TimeStamp& time_stamp) { + const auto dt = (time_stamp - last_update_time_stamp_).to_seconds(); uint32_t id_index = static_cast(enumeration::ArmorIdFlag::Hero); std::array, 8> armors; @@ -99,7 +99,7 @@ class PredictorManager::Impl { } private: - PredictTimeStamp last_update_time_stamp_ { 0 }; + data::TimeStamp last_update_time_stamp_ {}; std::array predictors_; Eigen::Affine3d transform_from_camera_to_gimbal_; @@ -112,11 +112,11 @@ PredictorManager::PredictorManager() PredictorManager::~PredictorManager() = default; std::shared_ptr PredictorManager::Predict( - const world_exe::enumeration::ArmorIdFlag& id, const std::time_t& time_stamp) { + const world_exe::enumeration::ArmorIdFlag& id, const data::TimeStamp& time_stamp) { return pimpl_->Predict(id, time_stamp); }; -void PredictorManager::Update(std::shared_ptr data) { +void PredictorManager::Update(std::shared_ptr data) { return pimpl_->Update(data); }; diff --git a/src/v1/predictor/predictor_manager.hpp b/src/v1/predictor/predictor_manager.hpp index 77b53f6..38bbb8f 100644 --- a/src/v1/predictor/predictor_manager.hpp +++ b/src/v1/predictor/predictor_manager.hpp @@ -1,6 +1,7 @@ #pragma once -#include "interfaces/predictor_update_package.hpp" +#include "data/time_stamped.hpp" +#include "data/predictor_update_package.hpp" #include "interfaces/target_predictor.hpp" namespace world_exe::v1::predictor { @@ -9,10 +10,10 @@ class PredictorManager final : public world_exe::interfaces::ITargetPredictor { PredictorManager(); ~PredictorManager(); - void Update(std::shared_ptr data); + void Update(std::shared_ptr data); virtual std::shared_ptr Predict( - const enumeration::ArmorIdFlag& id, const std::time_t& time_stamp); + const enumeration::ArmorIdFlag& id, const data::TimeStamp& time_stamp); std::shared_ptr GetPredictor(const enumeration::ArmorIdFlag& id) const; diff --git a/src/v1/sync/syncer.cpp b/src/v1/sync/syncer.cpp index 2197595..1277403 100644 --- a/src/v1/sync/syncer.cpp +++ b/src/v1/sync/syncer.cpp @@ -1,6 +1,7 @@ #include "v1/sync/syncer.hpp" #include "data/sync_data.hpp" +#include "data/time_stamped.hpp" #include #include #include @@ -14,29 +15,28 @@ struct Syncer::Impl { :time_to_hold_(time_to_hold) ,tolerable_ns_(tolerable_ns) {} - void set_data(const data::CameraGimbalMuzzleSyncData& armor_pnp) { + void set_data(const data::CameraGimbalMuzzleSyncData& data) { - data_queue_.emplace_back(armor_pnp); + data_queue_.emplace_back(data); - long now_time = - armor_pnp.camera_capture_begin_time_stamp - -std::chrono::duration_cast(time_to_hold_).count(); + auto now_time = + data.camera_capture_begin_time_stamp + - data::TimeStamp{time_to_hold_}; while(!data_queue_.empty() && data_queue_.front().camera_capture_begin_time_stamp < now_time) data_queue_.pop_front(); } - std::tuple get_data(const time_t timestamp) { - + std::tuple get_data(const data::TimeStamp& timestamp) { if (data_queue_.empty()) return {data::CameraGimbalMuzzleSyncData{}, false}; for(auto rit = data_queue_.rbegin(); rit != data_queue_.rend(); ++rit) - if (rit->camera_capture_begin_time_stamp <= timestamp) { - return {*rit, true}; - } + if (rit->camera_capture_begin_time_stamp <= timestamp) + return {*rit, true}; + return {data::CameraGimbalMuzzleSyncData{},false}; } @@ -58,7 +58,7 @@ Syncer::~Syncer() = default; void Syncer::set_data(const data::CameraGimbalMuzzleSyncData& armor_pnp) { pimpl_->set_data(armor_pnp); } -std::tuple Syncer::get_data(time_t timestamp) { +std::tuple Syncer::get_data(const data::TimeStamp& timestamp) { return pimpl_->get_data(timestamp); } diff --git a/src/v1/sync/syncer.hpp b/src/v1/sync/syncer.hpp index 330716e..189d4cb 100644 --- a/src/v1/sync/syncer.hpp +++ b/src/v1/sync/syncer.hpp @@ -1,7 +1,7 @@ #pragma once #include "data/sync_data.hpp" -#include "interfaces/predictor_update_package.hpp" +#include "data/predictor_update_package.hpp" #include #include #include @@ -15,11 +15,11 @@ class Syncer final : interfaces::ISyncBlock { void set_data(const data::CameraGimbalMuzzleSyncData& camera_data); - std::tuple get_data(time_t timestamp); + std::tuple get_data(const data::TimeStamp& timestamp); private: struct Impl; std::unique_ptr pimpl_; - std::shared_ptr last_; + std::shared_ptr last_; }; } \ No newline at end of file diff --git a/src/v1/workflow.puml b/src/v1/workflow.puml index fc67d42..5a95d64 100644 --- a/src/v1/workflow.puml +++ b/src/v1/workflow.puml @@ -46,10 +46,10 @@ car_tracing_event --> FireControl : enumeration::CarIDFlag car_tracing_event --> Tracker : enumeration::CarIDFlag Tracker --> tracker_current_armors_event : interfaces::IArmorInGimbalControl armors_in_camera_pnp_event --> SyncComponent : interfaces::IArmorInCamera -SyncComponent --> tracker_update_event : std::shared_ptr +SyncComponent --> tracker_update_event : std::shared_ptr camera_capture_transforms --> SyncComponent : data::CameraGimbalMuzzleSyncData -tracker_update_event --> FireControl : std::shared_ptr -tracker_update_event --> Tracker : std::shared_ptr +tracker_update_event --> FireControl : std::shared_ptr +tracker_update_event --> Tracker : std::shared_ptr tracker_current_armors_event --> FireControl : interfaces::IArmorInGimbalControl FireControl --> get_lastest_predictor_event : enumeration::CarIDFlag get_lastest_predictor_event --> Tracker : enumeration::CarIDFlag diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f2b9563..004859a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -15,7 +15,7 @@ target_compile_features(test_main PRIVATE cxx_std_20) include(FetchContent) FetchContent_Declare( googletest - URL https://github.com/google/googletest/archive/refs/heads/main.zip + SOURCE_DIR ${CMAKE_SOURCE_DIR}/thirdparty/googletest ) set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) diff --git a/tests/Pnpsolver.cpp b/tests/Pnpsolver.cpp index 2ebe8b9..74fb8d8 100644 --- a/tests/Pnpsolver.cpp +++ b/tests/Pnpsolver.cpp @@ -2,8 +2,6 @@ #include #include "mocks/MockArmor2D.hpp" #include "interfaces/pnp_solver.hpp" -#include "parameters/rm_parameters.hpp" -#include "mocks/MockHikCameraProfile.hpp" using world_exe::interfaces::IPnpSolver; using world_exe::tests::mock::MockArmorInImage; diff --git a/tests/mocks/MockArmor2D.hpp b/tests/mocks/MockArmor2D.hpp index 2cd1d63..87a8cb1 100644 --- a/tests/mocks/MockArmor2D.hpp +++ b/tests/mocks/MockArmor2D.hpp @@ -1,6 +1,6 @@ #pragma once #include "interfaces/armor_in_image.hpp" -#include "interfaces/time_stamped.hpp" +#include "data/time_stamped.hpp" #include #include #include @@ -8,10 +8,10 @@ #include namespace world_exe::tests::mock{ - class MockArmorInImage :public world_exe::interfaces::IArmorInImage,public world_exe::interfaces::ITimeStamped{ + class MockArmorInImage :public world_exe::interfaces::IArmorInImage{ public: std::vector armors; - std::time_t time_stamp = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + data::TimeStamp time_stamp = data::TimeStamp(std::chrono::system_clock::now().time_since_epoch()); MockArmorInImage(std::vector armors):armors(armors){} ~MockArmorInImage() = default; @@ -20,10 +20,9 @@ namespace world_exe::tests::mock{ { return armors; } - const world_exe::interfaces::ITimeStamped& GetTimeStamped() const override { - return *this; + const world_exe::data::TimeStamp& GetTimeStamp() const override { + return time_stamp; } - const std::time_t& GetTimeStamp() const override { return time_stamp; } //MockArmorInImage工厂函数 static std::shared_ptr createMockArmorInImage(){ //Mock装甲板生成 diff --git a/tests/sync_test.cpp b/tests/sync_test.cpp index 7e3652f..a893976 100644 --- a/tests/sync_test.cpp +++ b/tests/sync_test.cpp @@ -1,5 +1,6 @@ #include "data/sync_data.hpp" #include "v1/sync/syncer.hpp" +#include #include #include #include @@ -14,8 +15,8 @@ using ns_t = int64_t; // 纳秒 struct FrameB { data::CameraGimbalMuzzleSyncData data; // 纳秒 - FrameB(ns_t t) : data(t) { - data.camera_capture_begin_time_stamp = t; + FrameB(ns_t t) { + data.camera_capture_begin_time_stamp = data::TimeStamp::from_nanosec(t); } }; @@ -37,7 +38,7 @@ void sync_test_main() { const ns_t period_ns = static_cast(NS_PER_SEC / frequency + 0.5); const double total_seconds = 2.0; const ns_t total_ns = static_cast(total_seconds * NS_PER_SEC + 0.5); - const ns_t min_margin_to_next_b = 500'000; // 0.0005s + const data::TimeStamp min_margin_to_next_b = data::TimeStamp::from_nanosec(500'000); // 0.0005s const unsigned seed = 12345; std::mt19937 rng(seed); @@ -49,8 +50,8 @@ void sync_test_main() { std::vector generatedAs; // 生成 B 时间点（纳秒） - std::vector bTimes; - for (ns_t t = 0; t <= total_ns + period_ns; t += period_ns) bTimes.push_back(t); + std::vector bTimes; + for (ns_t t = 0; t <= total_ns + period_ns; t += period_ns) bTimes.push_back(data::TimeStamp::from_nanosec(t)); // 第一个 B data::CameraGimbalMuzzleSyncData firstB{}; @@ -58,26 +59,28 @@ void sync_test_main() { generatedBs.push_back(firstB); syncer.set_data(firstB); for (size_t i = 0; i + 1 < bTimes.size(); ++i) { - ns_t tb = bTimes[i]; - ns_t tbNext = bTimes[i + 1]; + auto tb = bTimes[i]; + auto tbNext = bTimes[i + 1]; data::CameraGimbalMuzzleSyncData b{}; b.camera_capture_begin_time_stamp = tbNext; + ASSERT_EQ(b.camera_capture_begin_time_stamp, tbNext) << "tbNext should be equal to camera_capture_begin_time_stamp"; generatedBs.push_back(b); syncer.set_data(b); int aCountThisInterval = aCountDist(rng); for (int k = 0; k < aCountThisInterval; ++k) { - ns_t windowStart = tb + 1; - ns_t windowEnd = std::max(windowStart, tbNext - min_margin_to_next_b); + auto windowStart = tb + data::TimeStamp::from_nanosec(1); + auto windowEnd = std::max(windowStart, tbNext - min_margin_to_next_b); if (windowEnd <= windowStart) continue; double u = unitDist(rng); - ns_t tA = windowStart + static_cast((windowEnd - windowStart) * u + 0.5); + data::TimeStamp tA = windowStart + ((windowEnd - windowStart) * u); data::CameraGimbalMuzzleSyncData a{}; a.camera_capture_begin_time_stamp = tA; generatedAs.push_back(a); + ASSERT_LT(tb, tA) << "ta should be larger then tb"; auto [matched, ok] = syncer.get_data(tA); ASSERT_TRUE(ok) << "get_data should succeed for A after at least one B"; ASSERT_LE(matched.camera_capture_begin_time_stamp, tA) << "matched B timestamp <= A timestamp"; @@ -91,7 +94,7 @@ void sync_test_main() { if (matchedIndex + 1 < generatedBs.size()) { const auto& nextB = generatedBs[matchedIndex + 1]; ASSERT_LT(tA, nextB.camera_capture_begin_time_stamp) << "A must occur before next B"; - ns_t gapToNextB = nextB.camera_capture_begin_time_stamp - tA; + auto gapToNextB = nextB.camera_capture_begin_time_stamp - tA; ASSERT_GE(gapToNextB, min_margin_to_next_b) << "A too close to next B"; } } @@ -105,14 +108,11 @@ void sync_test_main() { size_t limit = std::min(generatedAs.size(), 10); for (size_t i = 0; i < limit; ++i) { const auto& a = generatedAs[i]; - time_t tA_seconds = static_cast( - std::chrono::duration_cast(std::chrono::nanoseconds(a.camera_capture_begin_time_stamp)).count() - ); - auto [matched, ok] = syncer.get_data(tA_seconds); + auto [matched, ok] = syncer.get_data(a.camera_capture_begin_time_stamp); std::cout << "A#" << i - << " t(ns)=" << a.camera_capture_begin_time_stamp << " t(s)=" << ns_to_sec(a.camera_capture_begin_time_stamp) - << " -> matched B t(ns)=" << matched.camera_capture_begin_time_stamp - << " t(s)=" << ns_to_sec(matched.camera_capture_begin_time_stamp) << "\n"; + << " t(ns)=" << a.camera_capture_begin_time_stamp.to_nanosec() << " t(s)=" << a.camera_capture_begin_time_stamp.to_seconds() + << " -> matched B t(ns)=" << matched.camera_capture_begin_time_stamp.to_nanosec() + << " t(s)=" << matched.camera_capture_begin_time_stamp.to_seconds() << "\n"; } std::cout << "All assertions passed.\n"; diff --git a/tests/test_main.cpp b/tests/test_main.cpp index 4d4d345..74996f0 100644 --- a/tests/test_main.cpp +++ b/tests/test_main.cpp @@ -1,4 +1,3 @@ -#include "Pnpsolver.cpp" #include "v1/pnpsolver/armor_pnp_solver.hpp" #include #include @@ -6,6 +5,8 @@ #include #include #include +#include +#include "Pnpsolver.cpp" #include "sync_test.cpp" using namespace world_exe::v1::pnpsolver; @@ -14,6 +15,7 @@ using world_exe::parameters::Robomaster; using ::testing::Return; using ::testing::_; + TEST_P(PnpsolverTest,AbilityTest) { ArmorIPPEPnPSolver pnp_solver_test_v1(Robomaster::NormalArmorObjectPointsOpencv,Robomaster::LargeArmorObjectPointsOpencv); From af9f8af95a49d2e156424e53361652213582de89 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Fri, 31 Oct 2025 08:43:19 +0800 Subject: [PATCH 73/93] =?UTF-8?q?[V1]=20=E8=A1=A5=E5=85=85=E7=81=AB?= =?UTF-8?q?=E6=8E=A7=E7=9A=84=E9=A2=84=E6=B5=8B=E5=99=A8=E8=AE=BE=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/v1/auto_aim_system_v1.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/v1/auto_aim_system_v1.cpp b/src/v1/auto_aim_system_v1.cpp index 6f64e54..818b989 100644 --- a/src/v1/auto_aim_system_v1.cpp +++ b/src/v1/auto_aim_system_v1.cpp @@ -87,6 +87,7 @@ class world_exe::v1::SystemV1::Impl{ const auto& time = combined->GetTimeStamp(); const auto& armor3d = predictor->Predict(fire_targets,time); fire_control ->set_armor(armor3d); + fire_control ->SetPredictor(predictor->GetPredictor(fire_targets)); core::EventBus::Publish(ParamsForSystemV1::fire_control_event, control()); From a9d79d56927ec7d3828e503c551f365deb3053a3 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 1 Nov 2025 00:46:30 +0800 Subject: [PATCH 74/93] fix(fire_control): correct logical issues in fire controller --- src/tongji/auto_aim_system.cpp | 25 ++-- .../aim_point_chooser.hpp | 50 ++++---- src/tongji/fire_controller/aim_solver.hpp | 115 ++++++++++++++++++ .../fire_controller/fire_controller.cpp | 90 +++++--------- .../fire_controller/fire_controller.hpp | 6 +- src/tongji/fire_controller/fire_decision.hpp | 13 +- .../trajectory.hpp | 2 +- .../decider.cpp | 4 +- .../decider.hpp | 2 +- .../tracker.hpp | 32 +++-- .../live_target_manager/live_target.hpp | 55 +++++---- .../live_target_manager.cpp | 107 +++++----------- .../live_target_manager.hpp | 6 +- .../target_snapshot_manager/aim_solver.hpp | 96 --------------- .../target_snapshot.hpp | 35 +++--- .../target_snapshot_manager.cpp | 95 +++------------ .../target_snapshot_manager.hpp | 13 +- src/tongji/solver/solver.cpp | 2 +- src/tongji/solver/solver.hpp | 2 +- src/tongji/state_machine/state_machine.cpp | 36 +++--- src/tongji/state_machine/state_machine.hpp | 8 +- 21 files changed, 344 insertions(+), 450 deletions(-) rename src/tongji/{predictor/target_snapshot_manager => fire_controller}/aim_point_chooser.hpp (55%) create mode 100644 src/tongji/fire_controller/aim_solver.hpp rename src/tongji/{predictor/target_snapshot_manager => fire_controller}/trajectory.hpp (96%) rename src/tongji/{predictor/live_target_manager => identifier}/decider.cpp (96%) rename src/tongji/{predictor/live_target_manager => identifier}/decider.hpp (92%) rename src/tongji/{predictor/live_target_manager => identifier}/tracker.hpp (85%) delete mode 100644 src/tongji/predictor/target_snapshot_manager/aim_solver.hpp diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 4d4f560..ca5445e 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -17,18 +17,16 @@ namespace world_exe::tongji { using namespace std::chrono; - class AutoAimSystem::Impl { public: Impl(const bool& debug) : debug(debug) , config_path_("") , save_path_("") { - identifier_ = std::make_unique( - parameters::ParamsForSystemV1::szu_model_path(), - parameters::ParamsForSystemV1::device(), - parameters::HikCameraProfile::get_width(), - parameters::HikCameraProfile::get_height()); + identifier_ = std::make_unique( + parameters::ParamsForSystemV1::szu_model_path(), + parameters::ParamsForSystemV1::device(), parameters::HikCameraProfile::get_width(), + parameters::HikCameraProfile::get_height()); pnp_solver_ = std::make_unique(); live_target_manager_ = std::make_shared(config_path_); state_machine_ = std::make_shared(); @@ -47,6 +45,7 @@ class AutoAimSystem::Impl { auto Solve(const cv::Mat& raw) -> void { const auto& [armors_in_image, flag] = identifier_->identify(raw); if (flag == enumeration::ArmorIdFlag::None) return; + state_machine_->Update(armors_in_image); // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分 const auto& [pack, check] = syncer_->get_data(armors_in_image->GetTimeStamp()); @@ -60,9 +59,9 @@ class AutoAimSystem::Impl { auto combined = std::make_shared(pack, armors_in_camera); - live_target_manager_->Update(combined, armors_in_image); - - state_machine_ = std::make_shared(live_target_manager_); + live_target_manager_->Update(combined); + + state_machine_ = std::make_shared(); const auto target_id = state_machine_->GetAllowdToFires(); @@ -71,15 +70,17 @@ class AutoAimSystem::Impl { /// 这里应该有一个线程进行稳定的输出之类的 /// 轨迹规划器没有实现，先不管 - - core::EventBus::Publish(parameters::ParamsForSystemV1::fire_control_event, GetControlCommand()); + + core::EventBus::Publish( + parameters::ParamsForSystemV1::fire_control_event, GetControlCommand()); } void SetTransfroms(const data::CameraGimbalMuzzleSyncData& data) { syncer_->set_data(data); } data::FireControl GetControlCommand() { fire_controller_->GetAttackCarId(); - return fire_controller_->CalculateTarget(std::chrono::duration_cast(std::chrono::steady_clock::now() - time_stamp_)); + return fire_controller_->CalculateTarget( + std::chrono::duration_cast(std::chrono::steady_clock::now() - time_stamp_)); } private: diff --git a/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp similarity index 55% rename from src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp rename to src/tongji/fire_controller/aim_point_chooser.hpp index 6d46bee..51e2c71 100644 --- a/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -1,11 +1,12 @@ #pragma once +#include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" #include "util/math.hpp" #include -namespace world_exe::tongji::predictor { +namespace world_exe::tongji::fire_control { using CarIDFlag = enumeration::CarIDFlag; @@ -17,43 +18,43 @@ class AimPointChooser { leaving_angle_ = yaml["leaving_angle"].as() / 57.3; // degree to rad } - std::pair ChooseAimArmor(const Eigen::Vector& ekf_x, - const std::vector& xyza_list, const CarIDFlag& single_id) { - const auto armor_num = xyza_list.size(); + std::pair ChooseAimArmor( + const Eigen::Vector& ekf_x, + const std::vector armors, const CarIDFlag& single_id) { + const auto armor_num = armors.size(); int chosen_id = -1; // 整车旋转中心的球坐标yaw const auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); - // 如果delta_angle为0，则该装甲板中心和整车中心的连线在世界坐标系的xy平面过原点 - std::vector delta_angle_list; + std::vector> delta_angle_list; for (int i = 0; i < armor_num; i++) { - auto delta_angle = util::math::clamp_pm_pi(xyza_list[i][3] - center_yaw); - delta_angle_list.emplace_back(delta_angle); + auto delta_angle = util::math::clamp_pm_pi( + util::math::get_yaw_from_quaternion(armors[i].orientation) - center_yaw); + delta_angle_list.emplace_back(std::make_tuple(i, delta_angle)); } + std::sort(delta_angle_list.begin(), delta_angle_list.end(), + [](const auto& a, const auto& b) { return std::get<1>(a) > std::get<1>(b); }); // 不考虑小陀螺 if (std::abs(ekf_x[8]) <= 2 && single_id != CarIDFlag::Outpost) { // 选择在可射击范围内的装甲板 std::vector id_list; - for (int i = 0; i < armor_num; i++) { - if (std::abs(delta_angle_list[i]) > 60 / 57.3) continue; - id_list.push_back(i); + for (const auto& [id, delta_angle] : delta_angle_list) { + if (std::abs(delta_angle) > 60 / 57.3) continue; + id_list.emplace_back(id); } if (id_list.size() == 1) { chosen_id = id_list[0]; lock_id_ = -1; } else if (id_list.size() > 1) { - const int id0 = id_list[0], id1 = id_list[1]; // 未处于锁定模式时，选择delta_angle绝对值较小的装甲板，进入锁定模式 - if (lock_id_ != id0 && lock_id_ != id1) - lock_id_ = (std::abs(delta_angle_list[id0]) < std::abs(delta_angle_list[id1])) - ? id0 - : id1; + if (lock_id_ == -1) lock_id_ = id_list[0]; chosen_id = lock_id_; } else { - chosen_id = -1; + lock_id_ = -1; + chosen_id = lock_id_; } } else { // 小陀螺 @@ -61,23 +62,24 @@ class AimPointChooser { double leaving_angle = (single_id == CarIDFlag::Outpost) ? 30 / 57.3 : leaving_angle_; // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 - for (int i = 0; i < armor_num; i++) { - if (std::abs(delta_angle_list[i]) > coming_angle) continue; - if ((ekf_x[7] > 0 && delta_angle_list[i] < leaving_angle) - || (ekf_x[7] < 0 && delta_angle_list[i] > -leaving_angle)) { - chosen_id = i; + // + for (const auto& [id, delta_angle] : delta_angle_list) { + if (std::abs(delta_angle) > coming_angle) continue; + if ((ekf_x[7] > 0 && delta_angle < leaving_angle) + || (ekf_x[7] < 0 && delta_angle > -leaving_angle)) { + chosen_id = id; break; } } } if (chosen_id == -1) { - return { false, xyza_list[0] }; + return { false, armors[std::get<0>(delta_angle_list.front())] }; } return { true, - xyza_list[chosen_id], + armors[chosen_id], }; } diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp new file mode 100644 index 0000000..1aaa3c6 --- /dev/null +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -0,0 +1,115 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include "../predictor/target_snapshot_manager/target_snapshot.hpp" +#include "aim_point_chooser.hpp" +#include "interfaces/predictor.hpp" +#include "trajectory.hpp" + +namespace world_exe::tongji::fire_control { + +using TargetSnapshot = predictor::TargetSnapshot; + +struct AimSolution { + bool valid; + double yaw; + double pitch; + Eigen::Vector3d aim_point; // 最终瞄准点（世界坐标 + 装甲板yaw） + double horizon_distance = 0; // 无人机专有 +}; + +class AimingSolver { +public: + using PredictorModel = predictor::EKFModel<11, 4>; + using EKF = predictor::ExtendedKalmanFilter; + + AimingSolver(const std::string& config_path, const double& gravity = 9.7833) + : aim_point_chooser_(std::make_unique(config_path)) + , g_(gravity) { + + auto yaml = YAML::LoadFile(config_path); + yaw_offset_ = yaml["yaw_offset"].as() / 57.3; // degree to rad + pitch_offset_ = yaml["pitch_offset"].as() / 57.3; // degree to rad + bullet_speed_ = yaml["bullet_speed"].as(); + } + + AimSolution SolveAimSolution(std::shared_ptr snapshot, + data::TimeStamp time_stamp, const double& control_delay_s) { + + // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) + double prev_fly_time_s; + Eigen::Vector3d final_aim_point; + TrajectoryResult final_trajectory; + bool converged = false; + // HACK:不同击打点影响飞行时间的迭代，需要根据整车的状态（转速和坐标）来选择击打点，不得已将指针转换为派生类 + auto snapshot_derived = std::dynamic_pointer_cast(snapshot); + + // 预测目标在未来 dt时间后的位置 + for (int i = 0; i < 10; ++i) { + const auto& dt = control_delay_s + prev_fly_time_s; + const auto& armors = + snapshot->Predictor(time_stamp + data::TimeStamp::from_seconds(dt)); + + const auto& aim_point = SelectPredictedAim(snapshot_derived->GetPredictedX(dt), + armors->GetArmors(snapshot->GetId()), snapshot->GetId()); + if (!aim_point.has_value()) + return { false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), { }, + 0 }; // failed: no valid aim point + const auto traj = SolveTrajectory(aim_point.value(), bullet_speed_); + if (!traj.has_value()) + return { false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), { }, + 0 }; // failed: trajectory unsolvable + + if (i > 0 && std::abs(traj->fly_time - prev_fly_time_s) < 0.001) { + final_aim_point = *aim_point; + final_trajectory = *traj; + converged = true; + break; + } + prev_fly_time_s = traj->fly_time; + } + if (!converged) + return { false, std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), { }, + 0 }; // failed: trajectory did not converge + + const auto xyz = final_aim_point; + const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; + const double pitch = -(final_trajectory.pitch + pitch_offset_); + return { true, yaw, pitch, final_aim_point }; + } + +private: + std::optional SelectPredictedAim(const EKF::XVec& ekf_x, + const std::vector& armors, const CarIDFlag& id) const { + + const auto& [selectable, aim_point_in_gimbal] = + aim_point_chooser_->ChooseAimArmor(ekf_x, armors, id); + + if (!selectable) return std::nullopt; + return aim_point_in_gimbal.position; + } + + std::optional SolveTrajectory( + const Eigen::Vector3d& xyz, const double& bullet_speed) const { + double d = std::hypot(xyz.x(), xyz.y()); + auto result = TrajectorySolver::SolveTrajectory(bullet_speed, d, xyz.z(), g_); + return result.solvable ? std::optional { result } : std::nullopt; + } + + double yaw_offset_, pitch_offset_; + double bullet_speed_; + const double g_; + + std::unique_ptr aim_point_chooser_; +}; +} diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index fc61e66..72f1e8f 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -9,6 +9,7 @@ #include "../predictor/live_target_manager/live_target_manager.hpp" #include "../predictor/target_snapshot_manager/target_snapshot_manager.hpp" #include "../state_machine/state_machine.hpp" +#include "aim_solver.hpp" #include "data/fire_control.hpp" #include "fire_decision.hpp" #include "interfaces/target_predictor.hpp" @@ -28,78 +29,47 @@ class FireController::Impl { std::shared_ptr live_target_manager) : locked_target(CarIDFlag::None) , firable_(false) + , aiming_solver_(std::make_unique(config_path)) , fire_decision_(std::make_unique(config_path)) , state_machine_(state_machine) , live_target_manager_(live_target_manager) { - auto yaml = YAML::LoadFile(config_path); - control_delay_ = std::chrono::seconds{static_cast(yaml["control_delay"].as() * 1e9)}; + auto yaml = YAML::LoadFile(config_path); + control_delay_s_ = yaml["control_delay_s"].as(); } - // TODO:time_duration 没有使用，详见std::shared_ptr - // Predictor(conststd::time_t&time_stamp) const这个接口的注释 - - /* - 从这个接口：std::shared_ptr Predictor(const - std::time_t&time_stamp) const 已经可以得到std::shared_ptr了 - 接下来是要 据此得到最终的控制指令data ::FireControl - - 为了得到最终的控制指令，data ::FireControl，需要对GimbalCommand进行判断， - 当GimbalCommand不突变 且 云台位于合适位置时，返回有效指令 - - 因为在这个函数中，从std::shared_ptr中选出了开火的对象， - 需要保存用于作为这个接口const CarIDFlag GetAttackCarId() const的返回值，所以只好破坏const约束了 - - - rep: 也许改个名字就好了 - */ - - const data ::FireControl CalculateTarget(const std::chrono::seconds& time_from_tracker_timepoint) const { + const data ::FireControl CalculateTarget( + const std::chrono::seconds& time_from_tracker_timepoint) const { if (!fire_decision_ || !state_machine_ || !live_target_manager_) return { .fire_allowance = false }; - auto converged_cars = state_machine_->GetAllowdToFires(); - auto snapshot_manager = live_target_manager_->GetPredictor(converged_cars); + const auto& lockable_target = state_machine_->GetAllowdToFires(); + const auto& snapshot_manager = live_target_manager_->GetPredictor(lockable_target); if (!snapshot_manager) - return data::FireControl { - .time_stamp = data::TimeStamp{time_from_tracker_timepoint}, + return data::FireControl { .time_stamp = + data::TimeStamp { time_from_tracker_timepoint }, .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), - .fire_allowance = false - }; - - // - TODO:接口语义不明，此函数传入的参数有待修正 - // - rep: 这里你需要算的不是程序执行时候的时间，而是预计的未来的某个时间的装甲板坐标 - // btw, 不计算飞行时间的话，获取这个predictor是为了什么， - // 为了击中未来的装甲板，需要使用某种算法去获取击中时候的装甲板位姿，然后反推云台位置 - // FIXME: 火控系统错误 - auto armors_in_gimbal = snapshot_manager->Predictor(time_from_tracker_timepoint + control_delay_); - auto lockable_target = state_machine_->GetAllowdToFires(); - - // 逻辑真的是选择所有可以击打的装甲板的第一个吗，这里有问题 - auto target_gimbal_spacing = armors_in_gimbal->GetArmors(lockable_target).front(); - - locked_target =target_gimbal_spacing.id; - - // FIXME: 不要偷偷的指针转换 - // 牵连太多，临时不好修 - auto gimbal_command = - std::dynamic_pointer_cast(snapshot_manager)->GetGimbalCommand(); - - auto fire_command = - fire_decision_->ShouldFire(gimbal_yaw_, gimbal_command, target_gimbal_spacing.position); - firable_ = fire_command; + .fire_allowance = false }; + // const auto& armors_in_gimbal = snapshot_manager->Predictor(time_from_tracker_timepoint); + // TODO:这里不应该指针转换 + const auto& aim_solution = aiming_solver_->SolveAimSolution( + snapshot_manager, time_from_tracker_timepoint, control_delay_s_); + + const auto gimbal_command = GimbalCommand { aim_solution.yaw, aim_solution.pitch }; + const auto target_pos = Eigen::Vector3d { aim_solution.aim_point }; + auto fire_command = aim_solution.valid + ? fire_decision_->ShouldFire(gimbal_yaw_, gimbal_command, target_pos) + : false; + firable_ = fire_command; data::FireControl result; result.fire_allowance = fire_command; result.gimbal_dir << gimbal_command.yaw, gimbal_command.pitch, 0; - result.time_stamp = data::TimeStamp{std::chrono::steady_clock::now().time_since_epoch()}; + result.time_stamp = data::TimeStamp { time_from_tracker_timepoint }; return result; } - /* - - 感觉这个和状态机那边的GetAllowdToFires()有点重复了 - - rep: 不重复的，锁定和可锁定的差别（好名字，用了） - */ const CarIDFlag GetAttackCarId() const { if (firable_) return locked_target; return CarIDFlag::None; @@ -108,12 +78,14 @@ class FireController::Impl { void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; private: + double control_delay_s_; + double gimbal_yaw_; - std::chrono::seconds control_delay_; - mutable CarIDFlag locked_target; + CarIDFlag locked_target; mutable double firable_; + std::unique_ptr aiming_solver_; std::unique_ptr fire_decision_; std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; @@ -124,13 +96,15 @@ FireController::FireController(const std::string& config_path, std::shared_ptr live_target_manager) : pimpl_(std::make_unique(config_path, state_machine, live_target_manager)) { } FireController::~FireController() = default; -const data ::FireControl FireController::CalculateTarget(const std::chrono::seconds& time_duration) const { + +const data ::FireControl FireController::CalculateTarget( + const std::chrono::seconds& time_duration) const { return pimpl_->CalculateTarget(time_duration); } - const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } + void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { return pimpl_->UpdateGimbalPosition(gimbal_yaw); }; -} \ No newline at end of file +} diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 0c948e2..ffa7d15 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -16,9 +16,9 @@ class FireController final : public interfaces::IFireControl { std::shared_ptr live_target_manager); ~FireController(); - const data ::FireControl CalculateTarget(const std::chrono::seconds& time_duration) const override; + const data ::FireControl CalculateTarget( + const std::chrono::seconds& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; - data::TimeStamp GetTimeStamp() const; void UpdateGimbalPosition(const double& gimbal_yaw); @@ -32,4 +32,4 @@ class FireController final : public interfaces::IFireControl { std::unique_ptr pimpl_; }; -} \ No newline at end of file +} diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index 2c7f80f..39125c5 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -1,13 +1,16 @@ #pragma once +#include #include #include #include -#include "tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp" - namespace world_exe::tongji::fire_control { +struct GimbalCommand { + double yaw; + double pitch; +}; class FireDecision { public: @@ -21,7 +24,7 @@ class FireDecision { judge_distance_ = yaml["judge_distance"].as(); } - bool ShouldFire(const double& gimbal_yaw, predictor::GimbalCommand gimbal_command, + bool ShouldFire(const double& gimbal_yaw, GimbalCommand gimbal_command, const Eigen::Vector3d& valid_target_pos) { if (!auto_fire_) return false; @@ -43,11 +46,11 @@ class FireDecision { private: bool auto_fire_; - predictor::GimbalCommand last_gimbal_command_; + GimbalCommand last_gimbal_command_; double first_tolerance_ { 5 }; // 近距离射击容差，degree double second_tolerance_ { 2 }; // 远距离射击容差，degree double judge_distance_ { 3 }; // 距离判断阈值 }; -} \ No newline at end of file +} diff --git a/src/tongji/predictor/target_snapshot_manager/trajectory.hpp b/src/tongji/fire_controller/trajectory.hpp similarity index 96% rename from src/tongji/predictor/target_snapshot_manager/trajectory.hpp rename to src/tongji/fire_controller/trajectory.hpp index ff5adc1..2b9c5e1 100644 --- a/src/tongji/predictor/target_snapshot_manager/trajectory.hpp +++ b/src/tongji/fire_controller/trajectory.hpp @@ -1,7 +1,7 @@ #pragma once #include -namespace world_exe::tongji::predictor { +namespace world_exe::tongji::fire_control { struct TrajectoryResult { bool solvable = true; diff --git a/src/tongji/predictor/live_target_manager/decider.cpp b/src/tongji/identifier/decider.cpp similarity index 96% rename from src/tongji/predictor/live_target_manager/decider.cpp rename to src/tongji/identifier/decider.cpp index 4db8573..994bf39 100644 --- a/src/tongji/predictor/live_target_manager/decider.cpp +++ b/src/tongji/identifier/decider.cpp @@ -8,7 +8,7 @@ #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" -namespace world_exe::tongji::predictor { +namespace world_exe::tongji::identifier { class Decider::Impl { public: @@ -37,7 +37,7 @@ class Decider::Impl { } private: - using ArmorPriority = world_exe::tongji::predictor::ArmorPriority; + using ArmorPriority = world_exe::tongji::identifier::ArmorPriority; using ArmorId = world_exe::enumeration::ArmorIdFlag; using PriorityMap = std::unordered_map; diff --git a/src/tongji/predictor/live_target_manager/decider.hpp b/src/tongji/identifier/decider.hpp similarity index 92% rename from src/tongji/predictor/live_target_manager/decider.hpp rename to src/tongji/identifier/decider.hpp index 0a44730..4fef243 100644 --- a/src/tongji/predictor/live_target_manager/decider.hpp +++ b/src/tongji/identifier/decider.hpp @@ -8,7 +8,7 @@ #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" -namespace world_exe::tongji::predictor { +namespace world_exe::tongji::identifier { enum PriorityMode { MODE_ONE = 1, MODE_TWO }; diff --git a/src/tongji/predictor/live_target_manager/tracker.hpp b/src/tongji/identifier/tracker.hpp similarity index 85% rename from src/tongji/predictor/live_target_manager/tracker.hpp rename to src/tongji/identifier/tracker.hpp index d2f7a85..d5f43f0 100644 --- a/src/tongji/predictor/live_target_manager/tracker.hpp +++ b/src/tongji/identifier/tracker.hpp @@ -4,17 +4,16 @@ #include #include #include -#include #include -#include "../../identifier/armor_filter.hpp" -#include "../../identifier/identified_armor.hpp" -#include "../target_snapshot_manager/target_snapshot.hpp" -#include "../target_snapshot_manager/target_snapshot_manager.hpp" +#include "../predictor/target_snapshot_manager/target_snapshot.hpp" +#include "../predictor/target_snapshot_manager/target_snapshot_manager.hpp" +#include "armor_filter.hpp" #include "decider.hpp" #include "enum/armor_id.hpp" +#include "identified_armor.hpp" -namespace world_exe::tongji::predictor { +namespace world_exe::tongji::identifier { using namespace std::chrono_literals; enum class TrackState { @@ -38,12 +37,14 @@ class Tracker final { ~Tracker() = default; - auto SelectTrackingTargetID(const std::shared_ptr& armors_in_image) noexcept -> enumeration::ArmorIdFlag const { + auto SelectTrackingTargetID( + const std::shared_ptr& armors_in_image) noexcept + -> enumeration::ArmorIdFlag const { CheckCameraOffline(); last_track_timestamp_ = std::chrono::steady_clock::now(); auto filtered_ids = enumeration::ArmorIdFlag::None; - auto detected_ids = enumeration::ArmorIdFlag::None; + std::vector filtered_armors; for (uint32_t i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { auto id = static_cast( @@ -52,12 +53,9 @@ class Tracker final { if (armors_in_image->GetArmors(id).empty()) continue; // 图像中出现的装甲板 - auto armors = armors_in_image->GetArmors(id); - detected_ids = static_cast( - static_cast(detected_ids) | static_cast(id)); - + auto armors = armors_in_image->GetArmors(id); // 对从图像识别到的装甲板进行过滤 - filtered_armors = std::move(armor_filter_->FilterArmor(std::move(armors))); + filtered_armors = armor_filter_->FilterArmor(armors); if (!filtered_armors.empty()) { filtered_ids = static_cast(static_cast(filtered_ids) @@ -65,12 +63,15 @@ class Tracker final { } } - UpdateState(!(detected_ids == enumeration::ArmorIdFlag::None)); + UpdateState(!(filtered_ids == enumeration::ArmorIdFlag::None)); tracking_car_id_ = decider_->GetBestArmor(filtered_armors); return tracking_car_id_; } + TrackState GetState() const { return state_; } + +private: void UpdateState(bool found) { switch (state_) { case TrackState::Lost: { @@ -133,9 +134,6 @@ class Tracker final { } } - TrackState GetState() const { return state_; } - -private: void CheckCameraOffline() { if (state_ != TrackState::Lost && (std::chrono::steady_clock::now() - last_track_timestamp_) < timeout_sec_) diff --git a/src/tongji/predictor/live_target_manager/live_target.hpp b/src/tongji/predictor/live_target_manager/live_target.hpp index 37ddd00..0b1ca88 100644 --- a/src/tongji/predictor/live_target_manager/live_target.hpp +++ b/src/tongji/predictor/live_target_manager/live_target.hpp @@ -20,8 +20,9 @@ class LiveTarget { using EKF = ExtendedKalmanFilter; LiveTarget(const Eigen::Vector3d& armor_xyz_in_gimbal, - const Eigen::Vector3d& armor_ypr_in_gimbal, const enumeration::CarIDFlag& car_id) - : last_see_time_stamp_() + const Eigen::Vector3d& armor_ypr_in_gimbal, const enumeration::CarIDFlag& car_id, + const data::TimeStamp time_stamp) + : time_stamp_(time_stamp) , model_(car_id) { // x vx y vy z vz a w r l h // a: angle @@ -44,14 +45,15 @@ class LiveTarget { EKF::XVec GetEkfX() const { return ekf_->x; } EKF::PDig GetP0Dig() const { return model_.GetP0Dig(); } const PredictorModel& GetModel() const { return model_; } - data::TimeStamp LastSeen() const { return data::TimeStamp(last_see_time_stamp_); } + data::TimeStamp LastSeen() const { return time_stamp_; } - std::vector GetArmorGimbalControlSpacings() const { + std::vector GetPredictedArmorGimbalControlSpacings( + const data::TimeStamp& time_stamp) const { + const auto ekf_x = this->GetPredictedX((time_stamp - time_stamp_).to_seconds()); std::vector armors; for (int id = 0; id < model_.GetArmorNum(); id++) { - auto angle = - util::math::clamp_pm_pi(this->ekf_->x[6] + id * 2 * CV_PI / model_.GetArmorNum()); - auto xyz = model_.h_armor_xyz(this->ekf_->x, id); + auto angle = util::math::clamp_pm_pi(ekf_x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto xyz = model_.h_armor_xyz(ekf_x, id); data::ArmorGimbalControlSpacing armor; armor.id = model_.GetID(); @@ -62,27 +64,32 @@ class LiveTarget { return armors; } - void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_gimbal, + auto GetPredictedXYZAList(const double& dt) -> std::vector const { + const auto [x_n, P_n] = ekf_->PredictOnce(dt); + return model_.GetArmorXYZAList(x_n); + } + + auto GetPredictedX(const double& dt) const -> const EKF::XVec { + const auto& [x_n, P_n] = ekf_->PredictOnce(dt); + return x_n; + } + + void Update(const data::TimeStamp time_stamp, const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal) { + // 装甲板匹配 int id = model_.MatchArmor( ekf_->x, armor_xyz_in_gimbal, armor_ypr_in_gimbal, armor_ypd_in_gimbal); last_id_ = id; update_count_++; - Update_ypda(armor_xyz_in_gimbal, armor_ypr_in_gimbal, armor_ypd_in_gimbal, id, dt); - } + Update_ypda(armor_xyz_in_gimbal, armor_ypr_in_gimbal, armor_ypd_in_gimbal, id, + (time_stamp - time_stamp_).to_seconds()); - bool IsConverged() const { - if (!ekf_.has_value()) return false; - // 前哨站特殊判断 - const int required_count = (model_.GetID() == enumeration::CarIDFlag::Outpost) ? 10 : 3; - if (update_count_ < required_count || IsDivergened()) return false; - return true; + time_stamp_ = time_stamp; } -private: - bool IsDivergened() const { + bool IsConverged() const { auto r_ok = ekf_->x[8] > 0.05 && ekf_->x[8] < 0.5; auto l_ok = ekf_->x[8] + ekf_->x[9] > 0.05 && ekf_->x[8] + ekf_->x[9] < 0.5; @@ -90,8 +97,12 @@ class LiveTarget { // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_->x[8], ekf_->x[9]); return true; } + auto IsAppeared() -> bool { + const int required_count = (model_.GetID() == enumeration::CarIDFlag::Outpost) ? 10 : 3; + return update_count_ > required_count; + } - // TODO:need to update correctly +private: void Update_ypda(const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal, const int& id, const double& dt) { @@ -117,9 +128,9 @@ class LiveTarget { } } - std::chrono::nanoseconds last_see_time_stamp_; - PredictorModel model_; - std::optional ekf_; + data::TimeStamp time_stamp_; + PredictorModel model_; + std::optional ekf_; int last_id_ = -1; int update_count_ = 0; diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp index 2b45fe8..edff69c 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -12,7 +12,6 @@ #include "enum/armor_id.hpp" #include "enum/car_id.hpp" #include "live_target.hpp" -#include "tracker.hpp" #include "util/index.hpp" #include "util/math.hpp" @@ -22,22 +21,9 @@ class LiveTargetManager::Impl { public: Impl(const std::string& config_path, const double& timeout_sec) : targets_map_() - , tracker_(std::make_unique()) , last_update_timestamp_() - , tracking_id_(enumeration::CarIDFlag::None) , config_path_(config_path) { } - /* - 不懂为什么要实现这个接口，不需要这个返回值 - std::shared_ptr，吧？ - - 原因是：卡尔曼滤波器 更改状态变量x的值 的操纵 - 写在了update函数中，这里的predict是没有副作用的预测， - 但是又没考虑飞行时间，理论上来说，误差更大了， - 需要得到的是考虑飞行时间得到的 std::shared_ptr - 所以目前认为它是多余的 - */ - std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const data::TimeStamp& time_stamp) { std::unordered_map> @@ -46,7 +32,7 @@ class LiveTargetManager::Impl { for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { auto it = targets_map_.find(id); if (it != targets_map_.end() && it->second && it->second->IsConverged()) { - auto spacings = it->second->GetArmorGimbalControlSpacings(); + auto spacings = it->second->GetPredictedArmorGimbalControlSpacings(time_stamp); result[id] = spacings; } } @@ -54,81 +40,48 @@ class LiveTargetManager::Impl { return std::make_shared(result, time_stamp); } - /* - 猜测接口的意思是通过外界传入id来 获得对应的预测器（副本）， - 而从外界传入的id是从 const CarIDFlag GetAttackCarId() const这类接口中传入的， - 但是我具体的id已经存在了targets_map_中，无需从外部传入 - - 哦，原来是这样，怪不得不知道传啥参数进去 - */ std::shared_ptr GetPredictor( const enumeration::ArmorIdFlag& flag) const { if (targets_map_.empty()) return nullptr; return std::make_shared( - config_path_, flag, targets_map_, last_update_timestamp_); + config_path_, targets_map_, last_update_timestamp_); } - // 为何传递了一个time_t 给double - void Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const double& dt) { - UpdateTimeStamp(data->GetTimeStamp()); - UpdateTargetMap(data); - UpdateTarget(data, armors_in_image, dt); - } + void Update(std::shared_ptr data) { + last_update_timestamp_ = data->GetTimeStamp(); - auto GetAllowedTargetID() const -> enumeration::CarIDFlag const { - if (targets_map_.at(tracking_id_)->IsConverged()) { - return tracking_id_; - } - return enumeration::CarIDFlag::None; - } + const Eigen::Affine3d transform = data->GetCameraToWorld(); + const auto armors_interface = data->GetArmors(); -private: - void UpdateTimeStamp(const data::TimeStamp& time_stamp) { last_update_timestamp_ = time_stamp; } - void UpdateTargetMap(std::shared_ptr data) { - const Eigen::Affine3d transform = data->GetCameraToWorld(); - const Eigen::Matrix3d rotation_matrix = transform.rotation(); - const auto armors_interface = data->GetArmors(); - - targets_map_.clear(); - for (int i; i < static_cast(enumeration::CarIDFlag::Count); i++) { + for (int i = 0; i < 8; i++) { auto id = static_cast( static_cast(enumeration::CarIDFlag::Hero) << i); - const auto& armors_list = armors_interface->GetArmors(id); - if (armors_list.empty()) return; + const auto& armors = armors_interface->GetArmors(id); + if (armors.empty()) continue; + + for (const auto& armor : armors) { + if (!targets_map_.contains(armor.id)) { + targets_map_.try_emplace(armor.id, + std::make_unique(armor.position, + util::math::quaternion_to_euler(armor.orientation, 2, 1, 0), armor.id, + data->GetTimeStamp())); + } else { + targets_map_.at(armor.id)->Update(data->GetTimeStamp(), armor.position, + util::math::quaternion_to_euler(armor.orientation, 2, 1, 0), + util::math::xyz2ypd(armor.position)); + } + } - const auto& armor = armors_list.front(); - const Eigen::Vector3d xyz_in_gimbal = transform * armor.position; - const Eigen::Vector3d ypr_in_gimbal = rotation_matrix.eulerAngles(2, 1, 0); // ZYX - targets_map_[id] = std::make_shared(xyz_in_gimbal, ypr_in_gimbal, id); + std::erase_if(targets_map_, [](const auto& pair) { return pair.second->IsAppeared(); }); } } - void UpdateTarget(std::shared_ptr data, - const std::shared_ptr& armors_in_image, const double& dt) { - const Eigen::Affine3d transform = data->GetCameraToWorld(); - const Eigen::Matrix3d rotation_matrix = transform.linear(); - const auto armors_interface = data->GetArmors(); - - tracking_id_ = tracker_->SelectTrackingTargetID(armors_in_image); - - const auto& armors_list = armors_interface->GetArmors(tracking_id_); - if (armors_list.empty()) return; - - const auto& armor = armors_list.front(); - const Eigen::Vector3d xyz_in_gimbal = transform * armor.position; - const Eigen::Vector3d ypr_in_gimbal = rotation_matrix.eulerAngles(2, 1, 0); // ZYX - targets_map_[tracking_id_]->Update( - dt, xyz_in_gimbal, ypr_in_gimbal, util::math::xyz2ypd(xyz_in_gimbal)); - } - - std::unordered_map> targets_map_; - std::unique_ptr tracker_; +private: + std::unordered_map> targets_map_; data::TimeStamp last_update_timestamp_; - enumeration::CarIDFlag tracking_id_; const std::string config_path_; }; @@ -146,12 +99,8 @@ std ::shared_ptr LiveTargetManager::GetPredictor( return pimpl_->GetPredictor(id); } - -void LiveTargetManager::Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image) { - return pimpl_->Update(data, armors_in_image, data->GetTimeStamp().to_seconds()); -} -auto LiveTargetManager::GetAllowedTargetID() const -> enumeration::ArmorIdFlag const { - return pimpl_->GetAllowedTargetID(); +void LiveTargetManager::Update(std::shared_ptr data) { + return pimpl_->Update(data); } + } diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.hpp b/src/tongji/predictor/live_target_manager/live_target_manager.hpp index 355b63e..3b3597a 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.hpp @@ -5,7 +5,6 @@ #include "data/predictor_update_package.hpp" #include "data/time_stamped.hpp" #include "enum/armor_id.hpp" -#include "interfaces/armor_in_image.hpp" #include "interfaces/target_predictor.hpp" namespace world_exe::tongji::predictor { @@ -20,10 +19,7 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { std ::shared_ptr GetPredictor( const enumeration ::ArmorIdFlag& id) const override; - void Update(std::shared_ptr data, - const std::shared_ptr& armors_in_image); - - auto GetAllowedTargetID() const -> enumeration::ArmorIdFlag const; + void Update(std::shared_ptr data); LiveTargetManager(const LiveTargetManager&) = delete; LiveTargetManager& operator=(const LiveTargetManager&) = delete; diff --git a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp deleted file mode 100644 index f65ac65..0000000 --- a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp +++ /dev/null @@ -1,96 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include - -#include "aim_point_chooser.hpp" -#include "target_snapshot.hpp" -#include "trajectory.hpp" - -namespace world_exe::tongji::predictor { - -using TargetSnapshot = predictor::TargetSnapshot; - -struct AimSolution { - bool valid; - double yaw; - double pitch; - Eigen::Vector4d aim_point; // 最终瞄准点（世界坐标 + 装甲板yaw） - double horizon_distance = 0; // 无人机专有 -}; - -class AimingSolver { -public: - AimingSolver(const std::string& config_path, const double& gravity = 9.7833) - : aim_point_chooser_(std::make_unique(config_path)) - , g_(gravity) { - - auto yaml = YAML::LoadFile(config_path); - yaw_offset_ = yaml["yaw_offset"].as() / 57.3; // degree to rad - pitch_offset_ = yaml["pitch_offset"].as() / 57.3; // degree to rad - } - - AimSolution SolveAimSolution(const std::shared_ptr& snapshot, - const double& bullet_speed, const double& time_delay) { - if (!snapshot) return { false, 0, 0, { }, 0 }; - - // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) - double prev_fly_time = 0; - Eigen::Vector4d final_aim_point; - TrajectoryResult final_trajectory; - bool converged = false; - - // 预测目标在未来 dt时间后的位置 - for (int i = 0; i < 10; ++i) { - double dt = time_delay + prev_fly_time; - const auto aim_point = SelectPredictedAim(snapshot, dt); - if (!aim_point.has_value()) - return { false, 0, 0, { }, 0 }; // failed: no valid aim point - - const auto traj = SolveTrajectory(aim_point->head<3>(), bullet_speed); - if (!traj.has_value()) return { false, 0, 0, { }, 0 }; // failed: trajectory unsolvable - - if (i > 0 && std::abs(traj->fly_time - prev_fly_time) < 0.001) { - final_aim_point = *aim_point; - final_trajectory = *traj; - converged = true; - break; - } - prev_fly_time = traj->fly_time; - } - if (!converged) return { false, 0, 0, { }, 0 }; // failed: trajectory did not converge - - const auto xyz = final_aim_point.head<3>(); - const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; - const double pitch = -(final_trajectory.pitch + pitch_offset_); - return { true, yaw, pitch, final_aim_point }; - } - -private: - std::optional SelectPredictedAim( - const std::shared_ptr& snapshot, const double& dt) const { - - const auto& [selectable, aim_point_in_gimbal] = aim_point_chooser_->ChooseAimArmor( - snapshot->GetPredictedX(dt), snapshot->GetPredictedXYZAList(dt), snapshot->GetID()); - - if (!selectable) return std::nullopt; - return aim_point_in_gimbal; - } - - std::optional SolveTrajectory( - const Eigen::Vector3d& xyz, const double& bullet_speed) const { - double d = std::hypot(xyz.x(), xyz.y()); - auto result = TrajectorySolver::SolveTrajectory(bullet_speed, d, xyz.z(), g_); - return result.solvable ? std::optional { result } : std::nullopt; - } - - double yaw_offset_, pitch_offset_; - const double g_; - - std::unique_ptr aim_point_chooser_; -}; -} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp index 4d5935b..43274e5 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp @@ -18,28 +18,29 @@ class TargetSnapshot { , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_) , time_stamp_(target.LastSeen()) { } - // std::vector GetArmorGimbalControlSpacings() const { - // std::vector armors; - // for (int id = 0; id < model_.GetArmorNum(); id++) { - // auto angle = - // util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); - // auto xyz = model_.h_armor_xyz(this->ekf_.x, id); - - // data::ArmorGimbalControlSpacing armor; - // armor.id = model_.GetID(); - // armor.position = xyz; - // armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - // armors.emplace_back(std::move(armor)); - // } - // return armors; - // } - // TODO： + std::vector GetPredictedArmorGimbalControlSpacings( + const double& dt) const { + const auto ekf_x = this->GetPredictedX(dt); + std::vector armors; + for (int id = 0; id < model_.GetArmorNum(); id++) { + auto angle = util::math::clamp_pm_pi(ekf_x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto xyz = model_.h_armor_xyz(ekf_x, id); + + data::ArmorGimbalControlSpacing armor; + armor.id = model_.GetID(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armors.emplace_back(std::move(armor)); + } + return armors; + } + auto GetPredictedXYZAList(const double& dt) -> std::vector const { const auto [x_n, P_n] = ekf_.PredictOnce(dt); return model_.GetArmorXYZAList(x_n); } - auto GetPredictedX(const double& dt) const -> const auto { + auto GetPredictedX(const double& dt) const -> const EKF::XVec { const auto& [x_n, P_n] = ekf_.PredictOnce(dt); return x_n; } diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp index 3e3247a..b4f677d 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -8,92 +8,46 @@ #include "../in_gimbal_control_armor.hpp" #include "../live_target_manager/live_target.hpp" -#include "aim_solver.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "data/time_stamped.hpp" #include "enum/enum_tools.hpp" +#include "target_snapshot.hpp" namespace world_exe::tongji::predictor { class TargetSnapshotManager::Impl { public: - Impl(const std::string& config_path, const enumeration::ArmorIdFlag& id, - const std::unordered_map>& + Impl(const std::string& config_path, + const std::unordered_map>& live_target_map, - const data::TimeStamp& now) - : aim_solver_(std::make_unique(config_path)) - , snapshots_(BuildSnapshots(live_target_map)) - , now_(now) - , ids_(id) - , gimbal_command_({ std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN() }) { - - auto yaml = YAML::LoadFile(config_path); - - decision_speed_ = yaml["decision_speed"].as(); - high_speed_delay_time_ = yaml["high_speed_delay_time"].as(); - low_speed_delay_time_ = yaml["low_speed_delay_time"].as(); - bullet_speed_ = yaml["bullet_speed"].as(); + const data::TimeStamp& time_stamp) + : snapshots_(BuildSnapshots(live_target_map)) + , time_stamp_(time_stamp) { + for (const auto& [id, live_target] : live_target_map) { + ids_ = static_cast( + static_cast(id) | static_cast(ids_)); + } } const enumeration::ArmorIdFlag& GetId() const { return ids_; } - /* - 通过aim_solver_->SolveAimSolution()来解算瞄准击打点 - aim_solution.aim_point，从而得到返回值std::shared_ptr - 所以此处进行飞行时间的迭代 - 但飞行时间的迭代需要考虑到时间延迟（图像传输这些等），这个参数在这个接口中体现： - data::FireControl CalculateTarget(const std::time_t& time_duration) - - 很显然为了实现这个接口，得到返回值std::shared_ptr， - 我需要进行飞行时间的迭代，但在这个接口中我没法把实际的时间延迟传入（我认为这个值是动态的，从上层调用它的地方传入） - v1的版本，传入的是 时间间隔（延迟），但这个接口的含义是传入 - 某个时间点返回装甲板信息，而非时间间隔 - - 如果是在data::FireControl CalculateTarget(const std::time_t& - time_duration)这里进行飞行时间的迭代，那么已经得到了data::FireControl，也就是最后的命令， - 那就不需要这个接口来得到中间量std::shared_ptr - - */ std::shared_ptr Predictor( const data::TimeStamp& time_stamp) const { std::unordered_map> result; - // TODO:time_delay - for (const auto& [id, snapshot] : snapshots_) { - auto ekf_x = snapshot.GetEkfX(); - const auto delay_time = - ekf_x[7] > decision_speed_ ? high_speed_delay_time_ : low_speed_delay_time_; - const auto dt = control_delay_time_ + delay_time; - auto aim_solution = aim_solver_->SolveAimSolution( - std::make_unique(snapshot), bullet_speed_, dt); - - if (!aim_solution.valid) continue; - - auto target_pos = aim_solution.aim_point.head<3>(); - auto armor_yaw = aim_solution.aim_point[3]; + for (const auto& [id, snapshot] : snapshots_) { result.emplace(id, - std::vector { - data::ArmorGimbalControlSpacing { id, target_pos, - util::math::euler_to_quaternion(armor_yaw, 15. / 180. * CV_PI, 0) } }); - - gimbal_command_.yaw = aim_solution.yaw; - gimbal_command_.pitch = aim_solution.pitch; + snapshot.GetPredictedArmorGimbalControlSpacings( + (time_stamp - time_stamp_).to_seconds())); } return std::make_shared(result, time_stamp); } - /* - 飞行时间的迭代得到一些有用的信息，比如说，云台控制指令GimbalCommand， - 需要保存一下，后续需要和当前云台yaw来分析这个指令有没有突变，但是有const约束，虽然好像问题不大 - */ - auto GetGimbalCommand() const -> GimbalCommand const { return gimbal_command_; } - private: static std::unordered_map BuildSnapshots( - const std::unordered_map>& input) { + const std::unordered_map>& input) { std::unordered_map result; for (const auto& [id, target] : input) { if (target) { @@ -103,24 +57,17 @@ class TargetSnapshotManager::Impl { return result; } - std::unique_ptr aim_solver_; const std::unordered_map snapshots_; - const data::TimeStamp& now_; - const enumeration::ArmorIdFlag ids_; + const data::TimeStamp& time_stamp_; + enumeration::ArmorIdFlag ids_; double bullet_speed_; - mutable GimbalCommand gimbal_command_; - double decision_speed_; - double high_speed_delay_time_; - double low_speed_delay_time_; - double control_delay_time_; }; TargetSnapshotManager::TargetSnapshotManager(const std::string& config_path, - const enumeration::ArmorIdFlag& id, - const std::unordered_map>& + const std::unordered_map>& live_target_map, - const data::TimeStamp& now) - : pimpl_(std::make_unique(config_path, id, live_target_map, now)) { } + const data::TimeStamp& time_stamp) + : pimpl_(std::make_unique(config_path, live_target_map, time_stamp)) { } TargetSnapshotManager::~TargetSnapshotManager() = default; const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } @@ -128,8 +75,4 @@ std ::shared_ptr TargetSnapshotManager::Predi const data::TimeStamp& time_stamp) const { return pimpl_->Predictor(time_stamp); } - -auto TargetSnapshotManager::GetGimbalCommand() const -> GimbalCommand const { - return pimpl_->GetGimbalCommand(); -} } diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp index 1ad478e..5f2e24f 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp @@ -9,25 +9,18 @@ #include "interfaces/predictor.hpp" namespace world_exe::tongji::predictor { -struct GimbalCommand { - double yaw; - double pitch; -}; - class TargetSnapshotManager final : public interfaces::IPredictor { public: - TargetSnapshotManager(const std::string& config_path, const enumeration::ArmorIdFlag& id, - const std::unordered_map>& + TargetSnapshotManager(const std::string& config_path, + const std::unordered_map>& live_target_map, - const data::TimeStamp& now); + const data::TimeStamp& time_stamp); ~TargetSnapshotManager(); const enumeration ::ArmorIdFlag& GetId() const override; std ::shared_ptr Predictor( const data::TimeStamp& time_stamp) const override; - auto GetGimbalCommand() const -> GimbalCommand const; - TargetSnapshotManager(const TargetSnapshotManager&) = delete; TargetSnapshotManager& operator=(const TargetSnapshotManager&) = delete; TargetSnapshotManager(TargetSnapshotManager&&) noexcept = default; diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 3950a31..1790640 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -152,4 +152,4 @@ auto Solver::CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, return pimpl_->CalculateOptimizeYaw( armor_in_image, armor_xyz_in_gimbal, gimbal_yaw, initial_armor_yaw_in_gimbal); } -} \ No newline at end of file +} diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index 442ca17..0f286d0 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -10,7 +10,7 @@ class Solver final : public interfaces::IPnpSolver{ ~Solver(); std::shared_ptr SolvePnp( - std::shared_ptr armor) override; + std::shared_ptr armors) override; void SetCamera2Gimbal( const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal); diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 5bd855f..cbd1231 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -2,35 +2,37 @@ #include +#include "../identifier/tracker.hpp" #include "enum/car_id.hpp" -#include "tongji/predictor/live_target_manager/live_target_manager.hpp" namespace world_exe::tongji::state_machine { class StateMachine::Impl { public: - Impl(std::shared_ptr live_target_manager) - : live_target_manager_(live_target_manager) - , target_ids_(enumeration::CarIDFlag::None) { } - - /// but why? - const enumeration::CarIDFlag& GetAllowdToFires() const { - target_ids_ = live_target_manager_->GetAllowedTargetID(); - return target_ids_; + Impl() + : tracker_(std::make_unique()) + , target_id_(enumeration::CarIDFlag::None) { } + + const enumeration::CarIDFlag& GetAllowdToFires() const { return target_id_; } + + void Update(std::shared_ptr armors_in_image) { + target_id_ = tracker_->SelectTrackingTargetID(armors_in_image); } private: - std::shared_ptr live_target_manager_; - mutable enumeration::CarIDFlag target_ids_; + std::unique_ptr tracker_; + enumeration::CarIDFlag target_id_; }; -StateMachine::StateMachine(std::shared_ptr live_target_manager) - : pimpl_(std::make_unique(live_target_manager)) { } - -StateMachine::~StateMachine() {}; -StateMachine::StateMachine() {}; +StateMachine::StateMachine() + : pimpl_(std::make_unique()) { } +StateMachine::~StateMachine() { }; const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return pimpl_->GetAllowdToFires(); } -} \ No newline at end of file + +void StateMachine::Update(std::shared_ptr armors_in_image) { + return pimpl_->Update(armors_in_image); +} +} diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 992b86e..062decf 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -4,10 +4,11 @@ #include #include "enum/car_id.hpp" +#include "interfaces/armor_in_image.hpp" #include "interfaces/car_state.hpp" namespace world_exe::tongji::predictor { - class LiveTargetManager; +class LiveTargetManager; } namespace world_exe::tongji::state_machine { @@ -17,7 +18,8 @@ class StateMachine final : public interfaces::ICarState { ~StateMachine(); const enumeration ::CarIDFlag& GetAllowdToFires() const override; - StateMachine(std::shared_ptr live_target_manager); + + void Update(std::shared_ptr armors_in_image); StateMachine(const StateMachine&) = delete; StateMachine& operator=(const StateMachine&) = delete; @@ -28,4 +30,4 @@ class StateMachine final : public interfaces::ICarState { class Impl; std::unique_ptr pimpl_; }; -} \ No newline at end of file +} From b0705c5bd5caf105756d4f6dd51966db066ad35b Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 1 Nov 2025 02:27:08 +0800 Subject: [PATCH 75/93] chore(config): clean up redundant parameters and fix timestamp configuration --- configs/example.yaml | 23 +++++------ src/tongji/auto_aim_system.cpp | 9 ++--- src/tongji/identifier/tracker.hpp | 38 ++++++++----------- .../live_target_manager.cpp | 2 +- src/tongji/solver/solver.cpp | 11 +++++- src/tongji/solver/solver.hpp | 6 ++- src/tongji/state_machine/state_machine.cpp | 10 +++-- src/tongji/state_machine/state_machine.hpp | 3 +- 8 files changed, 54 insertions(+), 48 deletions(-) diff --git a/configs/example.yaml b/configs/example.yaml index dc92c3e..cdeb620 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -1,8 +1,8 @@ #####-----classfier parameters-----##### -classify_model: -model_image_width: -model_image_height: - +# classify_model: +# model_image_width: +# model_image_height: +# #####-----identifier parameters-----##### threshold: 150 max_angle_error: 45 #degree @@ -15,6 +15,11 @@ max_side_ratio: 1.5 min_confidence: 0.8 max_rectangular_error: 25 #degree +#####-----target_snapshot_manager parameters-----##### +decision_speed: 7 # rad/s +high_speed_delay_time: 0.050 # s +low_speed_delay_time: 0.015 # s + #####-----fire_decision parameters-----##### auto_fire: true first_tolerance: 100 # 近距离射击容差，degree @@ -22,16 +27,12 @@ second_tolerance: 100 # 远距离射击容差，degree judge_distance: 2 #距离判断阈值 #####-----fire_controller parameters-----##### +control_delay_s: 0.1 - -#####-----target_snapshot_manager parameters-----##### -decision_speed: 7 # rad/s -high_speed_delay_time: 0.050 # s -low_speed_delay_time: 0.015 # s - -#####-----live_target_manager parameters-----##### +#####-----aiming_solver parameters-----##### yaw_offset: 1.5 # degree pitch_offset: -0.5 # degree +bullet_speed: 26 #####-----aime_point_chooser parameters-----##### comming_angle: 60 # degree diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index ca5445e..d33c8ce 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -9,7 +9,6 @@ #include "parameters/params_system_v1.hpp" #include "parameters/profile.hpp" #include "tongji/fire_controller/fire_controller.hpp" -// #include "tongji/identifier/identifier.hpp" #include "tongji/predictor/live_target_manager/live_target_manager.hpp" #include "tongji/solver/solver.hpp" #include "tongji/state_machine/state_machine.hpp" @@ -21,8 +20,7 @@ class AutoAimSystem::Impl { public: Impl(const bool& debug) : debug(debug) - , config_path_("") - , save_path_("") { + , config_path_("../../configs/example.yaml") { identifier_ = std::make_unique( parameters::ParamsForSystemV1::szu_model_path(), parameters::ParamsForSystemV1::device(), parameters::HikCameraProfile::get_width(), @@ -45,7 +43,9 @@ class AutoAimSystem::Impl { auto Solve(const cv::Mat& raw) -> void { const auto& [armors_in_image, flag] = identifier_->identify(raw); if (flag == enumeration::ArmorIdFlag::None) return; - state_machine_->Update(armors_in_image); + state_machine_->Update(armors_in_image, + std::chrono::duration_cast( + std::chrono::steady_clock::now() - time_stamp_)); // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分 const auto& [pack, check] = syncer_->get_data(armors_in_image->GetTimeStamp()); @@ -86,7 +86,6 @@ class AutoAimSystem::Impl { private: bool debug; const std::string config_path_; - const std::string save_path_; std::chrono::steady_clock::time_point time_stamp_; std::unique_ptr identifier_; diff --git a/src/tongji/identifier/tracker.hpp b/src/tongji/identifier/tracker.hpp index d5f43f0..0373cbb 100644 --- a/src/tongji/identifier/tracker.hpp +++ b/src/tongji/identifier/tracker.hpp @@ -1,6 +1,5 @@ #pragma once -#include #include #include #include @@ -14,7 +13,6 @@ #include "identified_armor.hpp" namespace world_exe::tongji::identifier { -using namespace std::chrono_literals; enum class TrackState { Lost, // @@ -32,16 +30,15 @@ class Tracker final { public: Tracker() : armor_filter_(std::make_unique()) - , decider_(std::make_unique()) - , last_track_timestamp_(std::chrono::steady_clock::now()) { } + , decider_(std::make_unique()) { } ~Tracker() = default; - auto SelectTrackingTargetID( - const std::shared_ptr& armors_in_image) noexcept + auto SelectTrackingTargetID(const std::shared_ptr& armors_in_image, + const std::chrono::milliseconds& duration_from_last_update) noexcept + -> enumeration::ArmorIdFlag const { - CheckCameraOffline(); - last_track_timestamp_ = std::chrono::steady_clock::now(); + CheckCameraOffline(duration_from_last_update); auto filtered_ids = enumeration::ArmorIdFlag::None; @@ -134,9 +131,8 @@ class Tracker final { } } - void CheckCameraOffline() { - if (state_ != TrackState::Lost - && (std::chrono::steady_clock::now() - last_track_timestamp_) < timeout_sec_) + void CheckCameraOffline(const std::chrono::milliseconds duration_from_last_update) { + if (state_ != TrackState::Lost && (duration_from_last_update > timeout_sec_)) SetState(TrackState::Lost); } @@ -154,16 +150,14 @@ class Tracker final { std::unique_ptr armor_filter_; std::unique_ptr decider_; - int detect_count_ = 0; - int temp_lost_count_ = 0; - int max_temp_lost_count_ = 15; - const int min_detect_count_ = 5; - const int outpost_max_temp_lost_count_ = 75; - const int normal_max_temp_lost_count_ = max_temp_lost_count_; - const int max_switch_count_ = 200; - static constexpr auto timeout_sec_ = 100ms; - - std::chrono::steady_clock::time_point last_track_timestamp_; + int detect_count_ = 0; + int temp_lost_count_ = 0; + int max_temp_lost_count_ = 15; + const int min_detect_count_ = 5; + const int outpost_max_temp_lost_count_ = 75; + const int normal_max_temp_lost_count_ = max_temp_lost_count_; + const int max_switch_count_ = 200; + const std::chrono::milliseconds timeout_sec_ = std::chrono::milliseconds(100); }; -} \ No newline at end of file +} diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp index edff69c..dd51123 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -21,7 +21,7 @@ class LiveTargetManager::Impl { public: Impl(const std::string& config_path, const double& timeout_sec) : targets_map_() - , last_update_timestamp_() + , last_update_timestamp_(data::TimeStamp {}) , config_path_(config_path) { } std::shared_ptr Predict( diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 1790640..a9a91d4 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -26,7 +26,10 @@ namespace world_exe::tongji::solver { class Solver::Impl { public: - Impl() { } + explicit Impl() + : R_camera2gimbal_(Eigen::Matrix3d::Zero()) + , t_camera2gimbal_(Eigen::Vector3d::Zero()) + , reprojection_util_(std::make_unique()) { } std::shared_ptr EstimateAllArmorPoses( std::shared_ptr armors_in_image) { @@ -50,7 +53,7 @@ class Solver::Impl { t_camera2gimbal_ = t_camera2gimbal; } - auto Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) -> const auto { + auto Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> const auto { Eigen::Vector3d xyz_in_gimbal = R_camera2gimbal_.transpose() * xyz_in_camera + t_camera2gimbal_; return xyz_in_gimbal; @@ -152,4 +155,8 @@ auto Solver::CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, return pimpl_->CalculateOptimizeYaw( armor_in_image, armor_xyz_in_gimbal, gimbal_yaw, initial_armor_yaw_in_gimbal); } + +auto Solver::Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> const auto { + return pimpl_->Camera2Gimbal(xyz_in_camera); +} } diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index 0f286d0..5247343 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -4,7 +4,7 @@ namespace world_exe::tongji::solver { -class Solver final : public interfaces::IPnpSolver{ +class Solver final : public interfaces::IPnpSolver { public: explicit Solver(); ~Solver(); @@ -14,6 +14,8 @@ class Solver final : public interfaces::IPnpSolver{ void SetCamera2Gimbal( const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal); + + auto Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> const auto; auto CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, const double& initial_armor_yaw_in_gimbal) const -> const double; @@ -29,4 +31,4 @@ class Solver final : public interfaces::IPnpSolver{ std::unique_ptr pimpl_; }; -} \ No newline at end of file +} diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index cbd1231..284e855 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -15,8 +15,9 @@ class StateMachine::Impl { const enumeration::CarIDFlag& GetAllowdToFires() const { return target_id_; } - void Update(std::shared_ptr armors_in_image) { - target_id_ = tracker_->SelectTrackingTargetID(armors_in_image); + void Update(std::shared_ptr armors_in_image, + const std::chrono::milliseconds& duration_from_last_update) { + target_id_ = tracker_->SelectTrackingTargetID(armors_in_image, duration_from_last_update); } private: @@ -32,7 +33,8 @@ const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return pimpl_->GetAllowdToFires(); } -void StateMachine::Update(std::shared_ptr armors_in_image) { - return pimpl_->Update(armors_in_image); +void StateMachine::Update(std::shared_ptr armors_in_image, + const std::chrono::milliseconds& duration_from_last_update) { + return pimpl_->Update(armors_in_image, duration_from_last_update); } } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 062decf..0454c2a 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -19,7 +19,8 @@ class StateMachine final : public interfaces::ICarState { const enumeration ::CarIDFlag& GetAllowdToFires() const override; - void Update(std::shared_ptr armors_in_image); + void Update(std::shared_ptr armors_in_image, + const std::chrono::milliseconds& duration_from_last_update); StateMachine(const StateMachine&) = delete; StateMachine& operator=(const StateMachine&) = delete; From 3b880b5f54064319f89ed229613bae19e1227199 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 1 Nov 2025 04:43:45 +0800 Subject: [PATCH 76/93] chore(predictor): remove redundant Snapshot classes and related code --- src/tongji/auto_aim_system.cpp | 8 +- src/tongji/fire_controller/aim_solver.hpp | 9 +-- .../fire_controller/fire_controller.cpp | 14 ++-- src/tongji/identifier/tracker.hpp | 6 +- .../car_predictor.hpp} | 36 ++++++--- .../car_predictor_manager.cpp} | 55 +++++++------ .../car_predictor_manager.hpp} | 14 ++-- .../predictor/in_gimbal_control_armor.hpp | 20 ++--- .../target_snapshot.hpp | 58 -------------- .../target_snapshot_manager.cpp | 78 ------------------- .../target_snapshot_manager.hpp | 33 -------- src/tongji/state_machine/state_machine.hpp | 4 - 12 files changed, 88 insertions(+), 247 deletions(-) rename src/tongji/predictor/{live_target_manager/live_target.hpp => car_predictor/car_predictor.hpp} (82%) rename src/tongji/predictor/{live_target_manager/live_target_manager.cpp => car_predictor/car_predictor_manager.cpp} (62%) rename src/tongji/predictor/{live_target_manager/live_target_manager.hpp => car_predictor/car_predictor_manager.hpp} (56%) delete mode 100644 src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp delete mode 100644 src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp delete mode 100644 src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index d33c8ce..9789324 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -9,10 +9,11 @@ #include "parameters/params_system_v1.hpp" #include "parameters/profile.hpp" #include "tongji/fire_controller/fire_controller.hpp" -#include "tongji/predictor/live_target_manager/live_target_manager.hpp" +#include "tongji/predictor/car_predictor/car_predictor_manager.hpp" #include "tongji/solver/solver.hpp" #include "tongji/state_machine/state_machine.hpp" #include "v1/identifier/identifier.hpp" + namespace world_exe::tongji { using namespace std::chrono; @@ -21,12 +22,13 @@ class AutoAimSystem::Impl { Impl(const bool& debug) : debug(debug) , config_path_("../../configs/example.yaml") { + identifier_ = std::make_unique( parameters::ParamsForSystemV1::szu_model_path(), parameters::ParamsForSystemV1::device(), parameters::HikCameraProfile::get_width(), parameters::HikCameraProfile::get_height()); pnp_solver_ = std::make_unique(); - live_target_manager_ = std::make_shared(config_path_); + live_target_manager_ = std::make_shared(config_path_); state_machine_ = std::make_shared(); fire_controller_ = std::make_unique( config_path_, state_machine_, live_target_manager_); @@ -91,7 +93,7 @@ class AutoAimSystem::Impl { std::unique_ptr identifier_; std::unique_ptr pnp_solver_; std::shared_ptr state_machine_; - std::shared_ptr live_target_manager_; + std::shared_ptr live_target_manager_; std::unique_ptr syncer_; std::unique_ptr fire_controller_; }; diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index 1aaa3c6..cdeeb8f 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -8,15 +8,14 @@ #include #include -#include "../predictor/target_snapshot_manager/target_snapshot.hpp" +#include "../predictor/car_predictor/car_predictor.hpp" #include "aim_point_chooser.hpp" -#include "interfaces/predictor.hpp" +#include "tongji/predictor/kalman_filter/extended_kalman_filter.hpp" +#include "tongji/predictor/kalman_filter/predict_model.hpp" #include "trajectory.hpp" namespace world_exe::tongji::fire_control { -using TargetSnapshot = predictor::TargetSnapshot; - struct AimSolution { bool valid; double yaw; @@ -49,7 +48,7 @@ class AimingSolver { TrajectoryResult final_trajectory; bool converged = false; // HACK:不同击打点影响飞行时间的迭代，需要根据整车的状态（转速和坐标）来选择击打点，不得已将指针转换为派生类 - auto snapshot_derived = std::dynamic_pointer_cast(snapshot); + auto snapshot_derived = std::dynamic_pointer_cast(snapshot); // 预测目标在未来 dt时间后的位置 for (int i = 0; i < 10; ++i) { diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 72f1e8f..1e3a21e 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -6,22 +6,20 @@ #include #include "../identifier/identified_armor.hpp" -#include "../predictor/live_target_manager/live_target_manager.hpp" -#include "../predictor/target_snapshot_manager/target_snapshot_manager.hpp" #include "../state_machine/state_machine.hpp" #include "aim_solver.hpp" #include "data/fire_control.hpp" #include "fire_decision.hpp" #include "interfaces/target_predictor.hpp" +#include "tongji/predictor/car_predictor/car_predictor_manager.hpp" namespace world_exe::tongji::fire_control { -using TargetSnapshotManager = predictor::TargetSnapshotManager; -using StateMachine = state_machine::StateMachine; -using IdentifiedArmor = identifier::IdentifiedArmor; -using CarIDFlag = enumeration::CarIDFlag; -using LiveTargetManager = predictor::LiveTargetManager; -using TimeStamp = data::TimeStamp; +using StateMachine = state_machine::StateMachine; +using IdentifiedArmor = identifier::IdentifiedArmor; +using CarIDFlag = enumeration::CarIDFlag; +using CarPredictorManager = predictor ::CarPredictorManager; +using TimeStamp = data::TimeStamp; class FireController::Impl { public: diff --git a/src/tongji/identifier/tracker.hpp b/src/tongji/identifier/tracker.hpp index 0373cbb..aca3538 100644 --- a/src/tongji/identifier/tracker.hpp +++ b/src/tongji/identifier/tracker.hpp @@ -5,8 +5,6 @@ #include #include -#include "../predictor/target_snapshot_manager/target_snapshot.hpp" -#include "../predictor/target_snapshot_manager/target_snapshot_manager.hpp" #include "armor_filter.hpp" #include "decider.hpp" #include "enum/armor_id.hpp" @@ -23,9 +21,7 @@ enum class TrackState { }; class Tracker final { - using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; - using TargetSnapshot = world_exe::tongji::predictor::TargetSnapshot; - using ArmorInImage = world_exe::tongji::identifier::IdentifiedArmor; + using ArmorInImage = world_exe::tongji::identifier::IdentifiedArmor; public: Tracker() diff --git a/src/tongji/predictor/live_target_manager/live_target.hpp b/src/tongji/predictor/car_predictor/car_predictor.hpp similarity index 82% rename from src/tongji/predictor/live_target_manager/live_target.hpp rename to src/tongji/predictor/car_predictor/car_predictor.hpp index 0b1ca88..05136f7 100644 --- a/src/tongji/predictor/live_target_manager/live_target.hpp +++ b/src/tongji/predictor/car_predictor/car_predictor.hpp @@ -2,28 +2,38 @@ #include #include +#include #include #include +#include "../in_gimbal_control_armor.hpp" #include "../kalman_filter/extended_kalman_filter.hpp" #include "../kalman_filter/predict_model.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "data/time_stamped.hpp" #include "enum/car_id.hpp" +#include "interfaces/predictor.hpp" namespace world_exe::tongji::predictor { -class LiveTarget { +class CarPredictor final : public interfaces::IPredictor { public: using PredictorModel = EKFModel<11, 4>; using EKF = ExtendedKalmanFilter; - LiveTarget(const Eigen::Vector3d& armor_xyz_in_gimbal, + explicit CarPredictor( + const EKF& ekf, const PredictorModel& model, const data::TimeStamp& time_stamp) + : ekf_(ekf) + , model_(model) + , time_stamp_(time_stamp) { } + + explicit CarPredictor(const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, const enumeration::CarIDFlag& car_id, - const data::TimeStamp time_stamp) + const data::TimeStamp& time_stamp) : time_stamp_(time_stamp) - , model_(car_id) { + , model_(car_id) + , car_id_(car_id) { // x vx y vy z vz a w r l h // a: angle // w: angular velocity @@ -42,13 +52,10 @@ class LiveTarget { ekf_.emplace(x0, P0, model_); // 初始化滤波器（预测量、预测量协方差） } - EKF::XVec GetEkfX() const { return ekf_->x; } - EKF::PDig GetP0Dig() const { return model_.GetP0Dig(); } - const PredictorModel& GetModel() const { return model_; } - data::TimeStamp LastSeen() const { return time_stamp_; } + const enumeration ::ArmorIdFlag& GetId() const override { return car_id_; } - std::vector GetPredictedArmorGimbalControlSpacings( - const data::TimeStamp& time_stamp) const { + std ::shared_ptr Predictor( + const data ::TimeStamp& time_stamp) const override { const auto ekf_x = this->GetPredictedX((time_stamp - time_stamp_).to_seconds()); std::vector armors; for (int id = 0; id < model_.GetArmorNum(); id++) { @@ -61,9 +68,15 @@ class LiveTarget { armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); armors.emplace_back(std::move(armor)); } - return armors; + return std::make_shared(armors, time_stamp_); } + EKF::XVec GetEkfX() const { return ekf_->x; } + auto GetModel() const -> const PredictorModel { return model_; } + auto GetEkf() const -> const EKF { return ekf_.value(); } + + data::TimeStamp LastSeen() const { return time_stamp_; } + auto GetPredictedXYZAList(const double& dt) -> std::vector const { const auto [x_n, P_n] = ekf_->PredictOnce(dt); return model_.GetArmorXYZAList(x_n); @@ -131,6 +144,7 @@ class LiveTarget { data::TimeStamp time_stamp_; PredictorModel model_; std::optional ekf_; + enumeration::CarIDFlag car_id_; int last_id_ = -1; int update_count_ = 0; diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp similarity index 62% rename from src/tongji/predictor/live_target_manager/live_target_manager.cpp rename to src/tongji/predictor/car_predictor/car_predictor_manager.cpp index dd51123..1eaca95 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp @@ -1,4 +1,4 @@ -#include "live_target_manager.hpp" +#include "car_predictor_manager.hpp" #include #include @@ -6,34 +6,39 @@ #include #include "../in_gimbal_control_armor.hpp" -#include "../target_snapshot_manager/target_snapshot_manager.hpp" + +#include "car_predictor.hpp" #include "data/predictor_update_package.hpp" #include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" -#include "live_target.hpp" #include "util/index.hpp" #include "util/math.hpp" namespace world_exe::tongji::predictor { -class LiveTargetManager::Impl { +class CarPredictorManager::Impl { public: Impl(const std::string& config_path, const double& timeout_sec) : targets_map_() - , last_update_timestamp_(data::TimeStamp {}) + , last_update_timestamp_(data::TimeStamp { }) , config_path_(config_path) { } std::shared_ptr Predict( const enumeration::ArmorIdFlag& flag, const data::TimeStamp& time_stamp) { - std::unordered_map> - result; - - for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { - auto it = targets_map_.find(id); - if (it != targets_map_.end() && it->second && it->second->IsConverged()) { - auto spacings = it->second->GetPredictedArmorGimbalControlSpacings(time_stamp); - result[id] = spacings; + std::vector result; + + for (const auto& car_id : util::enumeration::ExpandArmorIdFlags(flag)) { + if (const auto it = targets_map_.find(car_id); it != targets_map_.end()) { + const auto& [id, predictor] = *it; + if (!predictor->IsConverged()) { + continue; + } + if (predictor->IsConverged()) { + auto spacings = predictor->Predictor(time_stamp); + result.insert(result.end(), spacings->GetArmors(id).begin(), + spacings->GetArmors(id).end()); + } } } @@ -42,11 +47,11 @@ class LiveTargetManager::Impl { std::shared_ptr GetPredictor( const enumeration::ArmorIdFlag& flag) const { - - if (targets_map_.empty()) return nullptr; - - return std::make_shared( - config_path_, targets_map_, last_update_timestamp_); + const auto& it = targets_map_.find(flag); + if (it == targets_map_.end()) return nullptr; + const auto& [id, predictor] = *it; + return std::make_shared( + predictor->GetEkf(), predictor->GetModel(), predictor->LastSeen()); } void Update(std::shared_ptr data) { @@ -65,7 +70,7 @@ class LiveTargetManager::Impl { for (const auto& armor : armors) { if (!targets_map_.contains(armor.id)) { targets_map_.try_emplace(armor.id, - std::make_unique(armor.position, + std::make_unique(armor.position, util::math::quaternion_to_euler(armor.orientation, 2, 1, 0), armor.id, data->GetTimeStamp())); } else { @@ -80,26 +85,26 @@ class LiveTargetManager::Impl { } private: - std::unordered_map> targets_map_; + std::unordered_map> targets_map_; data::TimeStamp last_update_timestamp_; const std::string config_path_; }; -LiveTargetManager::LiveTargetManager(const std::string& config_path, const double& timeout_sec) +CarPredictorManager::CarPredictorManager(const std::string& config_path, const double& timeout_sec) : pimpl_(std::make_unique(config_path, timeout_sec)) { } -LiveTargetManager::~LiveTargetManager() = default; +CarPredictorManager::~CarPredictorManager() = default; -std ::shared_ptr LiveTargetManager::Predict( +std ::shared_ptr CarPredictorManager::Predict( const enumeration ::ArmorIdFlag& id, const data::TimeStamp& time_stamp) { return pimpl_->Predict(id, time_stamp); } -std ::shared_ptr LiveTargetManager::GetPredictor( +std ::shared_ptr CarPredictorManager::GetPredictor( const enumeration ::ArmorIdFlag& id) const { return pimpl_->GetPredictor(id); } -void LiveTargetManager::Update(std::shared_ptr data) { +void CarPredictorManager::Update(std::shared_ptr data) { return pimpl_->Update(data); } diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.hpp b/src/tongji/predictor/car_predictor/car_predictor_manager.hpp similarity index 56% rename from src/tongji/predictor/live_target_manager/live_target_manager.hpp rename to src/tongji/predictor/car_predictor/car_predictor_manager.hpp index 3b3597a..d6190ba 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.hpp +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.hpp @@ -9,10 +9,10 @@ namespace world_exe::tongji::predictor { -class LiveTargetManager final : public interfaces::ITargetPredictor { +class CarPredictorManager final : public interfaces::ITargetPredictor { public: - LiveTargetManager(const std::string& config_path, const double& timeout_sec = 0.1); - ~LiveTargetManager(); + CarPredictorManager(const std::string& config_path, const double& timeout_sec = 0.1); + ~CarPredictorManager(); std ::shared_ptr Predict( const enumeration ::ArmorIdFlag& id, const data::TimeStamp& time_stamp) override; @@ -21,10 +21,10 @@ class LiveTargetManager final : public interfaces::ITargetPredictor { void Update(std::shared_ptr data); - LiveTargetManager(const LiveTargetManager&) = delete; - LiveTargetManager& operator=(const LiveTargetManager&) = delete; - LiveTargetManager(LiveTargetManager&&) noexcept = default; - LiveTargetManager& operator=(LiveTargetManager&&) noexcept = default; + CarPredictorManager(const CarPredictorManager&) = delete; + CarPredictorManager& operator=(const CarPredictorManager&) = delete; + CarPredictorManager(CarPredictorManager&&) noexcept = default; + CarPredictorManager& operator=(CarPredictorManager&&) noexcept = default; private: class Impl; diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index b8d70ff..6d7402f 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -1,7 +1,6 @@ #pragma once #include -#include #include #include "data/armor_gimbal_control_spacing.hpp" @@ -13,24 +12,25 @@ namespace world_exe::tongji::predictor { class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { public: - InGimbalControlArmor(const std::unordered_map>& all_armors, - const data::TimeStamp& time_stamp) - : armors_map_(std::move(all_armors)) + explicit InGimbalControlArmor(const std::vector& all_armors, + const data::TimeStamp& time_stamp) + : armors_(all_armors) , time_stamp_(time_stamp) { } const std ::vector& GetArmors( const enumeration ::ArmorIdFlag& armor_id) const override { - auto it = armors_map_.find(armor_id); - return it != armors_map_.end() ? it->second : empty; + return armors_; } const data::TimeStamp& GetTimeStamp() const override { return time_stamp_; } + InGimbalControlArmor(const InGimbalControlArmor&) = delete; + InGimbalControlArmor& operator=(const InGimbalControlArmor&) = delete; + InGimbalControlArmor(InGimbalControlArmor&&) noexcept = default; + InGimbalControlArmor& operator=(InGimbalControlArmor&&) noexcept = default; + private: data::TimeStamp time_stamp_; - std::unordered_map> - armors_map_; - static inline const std::vector empty={}; + std::vector armors_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp deleted file mode 100644 index 43274e5..0000000 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp +++ /dev/null @@ -1,58 +0,0 @@ -#pragma once - -#include - -#include "../kalman_filter/extended_kalman_filter.hpp" -#include "../kalman_filter/predict_model.hpp" -#include "../live_target_manager/live_target.hpp" - -namespace world_exe::tongji::predictor { - -class TargetSnapshot { -public: - using PredictorModel = EKFModel<11, 4>; - using EKF = ExtendedKalmanFilter; - - TargetSnapshot(const LiveTarget& target) - : model_(target.GetModel()) - , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_) - , time_stamp_(target.LastSeen()) { } - - std::vector GetPredictedArmorGimbalControlSpacings( - const double& dt) const { - const auto ekf_x = this->GetPredictedX(dt); - std::vector armors; - for (int id = 0; id < model_.GetArmorNum(); id++) { - auto angle = util::math::clamp_pm_pi(ekf_x[6] + id * 2 * CV_PI / model_.GetArmorNum()); - auto xyz = model_.h_armor_xyz(ekf_x, id); - - data::ArmorGimbalControlSpacing armor; - armor.id = model_.GetID(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - armors.emplace_back(std::move(armor)); - } - return armors; - } - - auto GetPredictedXYZAList(const double& dt) -> std::vector const { - const auto [x_n, P_n] = ekf_.PredictOnce(dt); - return model_.GetArmorXYZAList(x_n); - } - - auto GetPredictedX(const double& dt) const -> const EKF::XVec { - const auto& [x_n, P_n] = ekf_.PredictOnce(dt); - return x_n; - } - - auto GetTimeStamp() const -> const auto { return time_stamp_; } - auto GetID() const -> const auto { return model_.GetID(); } - auto GetEkfX() const { return ekf_.x; } - -private: - PredictorModel model_; - ExtendedKalmanFilter ekf_; - data::TimeStamp time_stamp_; -}; - -} diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp deleted file mode 100644 index b4f677d..0000000 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ /dev/null @@ -1,78 +0,0 @@ -#include "target_snapshot_manager.hpp" - -#include -#include -#include - -#include - -#include "../in_gimbal_control_armor.hpp" -#include "../live_target_manager/live_target.hpp" -#include "data/armor_gimbal_control_spacing.hpp" -#include "data/time_stamped.hpp" -#include "enum/enum_tools.hpp" -#include "target_snapshot.hpp" - -namespace world_exe::tongji::predictor { - -class TargetSnapshotManager::Impl { -public: - Impl(const std::string& config_path, - const std::unordered_map>& - live_target_map, - const data::TimeStamp& time_stamp) - : snapshots_(BuildSnapshots(live_target_map)) - , time_stamp_(time_stamp) { - for (const auto& [id, live_target] : live_target_map) { - ids_ = static_cast( - static_cast(id) | static_cast(ids_)); - } - } - - const enumeration::ArmorIdFlag& GetId() const { return ids_; } - - std::shared_ptr Predictor( - const data::TimeStamp& time_stamp) const { - - std::unordered_map> - result; - - for (const auto& [id, snapshot] : snapshots_) { - result.emplace(id, - snapshot.GetPredictedArmorGimbalControlSpacings( - (time_stamp - time_stamp_).to_seconds())); - } - return std::make_shared(result, time_stamp); - } - -private: - static std::unordered_map BuildSnapshots( - const std::unordered_map>& input) { - std::unordered_map result; - for (const auto& [id, target] : input) { - if (target) { - result.emplace(id, TargetSnapshot(*target)); - } - } - return result; - } - - const std::unordered_map snapshots_; - const data::TimeStamp& time_stamp_; - enumeration::ArmorIdFlag ids_; - double bullet_speed_; -}; - -TargetSnapshotManager::TargetSnapshotManager(const std::string& config_path, - const std::unordered_map>& - live_target_map, - const data::TimeStamp& time_stamp) - : pimpl_(std::make_unique(config_path, live_target_map, time_stamp)) { } -TargetSnapshotManager::~TargetSnapshotManager() = default; - -const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } -std ::shared_ptr TargetSnapshotManager::Predictor( - const data::TimeStamp& time_stamp) const { - return pimpl_->Predictor(time_stamp); -} -} diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp deleted file mode 100644 index 5f2e24f..0000000 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include -#include - -#include "../live_target_manager/live_target.hpp" -#include "data/time_stamped.hpp" -#include "enum/armor_id.hpp" -#include "interfaces/predictor.hpp" - -namespace world_exe::tongji::predictor { -class TargetSnapshotManager final : public interfaces::IPredictor { -public: - TargetSnapshotManager(const std::string& config_path, - const std::unordered_map>& - live_target_map, - const data::TimeStamp& time_stamp); - ~TargetSnapshotManager(); - - const enumeration ::ArmorIdFlag& GetId() const override; - std ::shared_ptr Predictor( - const data::TimeStamp& time_stamp) const override; - - TargetSnapshotManager(const TargetSnapshotManager&) = delete; - TargetSnapshotManager& operator=(const TargetSnapshotManager&) = delete; - TargetSnapshotManager(TargetSnapshotManager&&) noexcept = default; - TargetSnapshotManager& operator=(TargetSnapshotManager&&) noexcept = default; - -private: - class Impl; - std::unique_ptr pimpl_; -}; -} diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 0454c2a..14747b5 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -7,10 +7,6 @@ #include "interfaces/armor_in_image.hpp" #include "interfaces/car_state.hpp" -namespace world_exe::tongji::predictor { -class LiveTargetManager; -} - namespace world_exe::tongji::state_machine { class StateMachine final : public interfaces::ICarState { public: From 1f3bb9580ef8a0676e228b1155a6ee94e4fffa1a Mon Sep 17 00:00:00 2001 From: Alray Qiu <70491601+AlrayQiu@users.noreply.github.com> Date: Sat, 1 Nov 2025 12:19:09 +0800 Subject: [PATCH 77/93] =?UTF-8?q?[CI]=20=E6=B7=BB=E5=8A=A0=E5=AD=90?= =?UTF-8?q?=E4=BB=93=E5=BA=93recursive=20update?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Enable recursive submodule checkout in workflow. --- .github/workflows/test-single-platform.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/test-single-platform.yml b/.github/workflows/test-single-platform.yml index 6b570d7..840f0fe 100644 --- a/.github/workflows/test-single-platform.yml +++ b/.github/workflows/test-single-platform.yml @@ -12,7 +12,8 @@ jobs: steps: - name: Checkout code uses: actions/checkout@v4 - + with: + submodules: recursive - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 From a3f867911ad2ad5f4d0f4e88b82d0062d0005b10 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 1 Nov 2025 22:08:18 +0800 Subject: [PATCH 78/93] test(coordinate): verify coordinate transformation utilities --- tests/CMakeLists.txt | 11 +- tests/tongji_tests/utils/coordinate_test.cpp | 119 +++++++++++++++++++ 2 files changed, 126 insertions(+), 4 deletions(-) create mode 100644 tests/tongji_tests/utils/coordinate_test.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 004859a..21cb649 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -6,10 +6,13 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) # 包含主项目头文件 include_directories(../include) -add_executable(test_main test_main.cpp) +file(GLOB_RECURSE TONGJI_TEST_SOURCES "./tongji_tests/*.cpp") + +add_executable(all_tests + ${TONGJI_TEST_SOURCES}) # 确保测试使用 C++20 -target_compile_features(test_main PRIVATE cxx_std_20) +target_compile_features(all_tests PRIVATE cxx_std_20) # GoogleTest 配置 include(FetchContent) @@ -21,11 +24,11 @@ FetchContent_Declare( set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) FetchContent_MakeAvailable(googletest) -target_link_libraries(test_main +target_link_libraries(all_tests GTest::gtest_main GTest::gmock_main alliance_auto_aim ) include(GoogleTest) -gtest_discover_tests(test_main) \ No newline at end of file +gtest_discover_tests(all_tests) diff --git a/tests/tongji_tests/utils/coordinate_test.cpp b/tests/tongji_tests/utils/coordinate_test.cpp new file mode 100644 index 0000000..ea0139f --- /dev/null +++ b/tests/tongji_tests/utils/coordinate_test.cpp @@ -0,0 +1,119 @@ +#include "util/coordinate.hpp" + +#include "gtest/gtest.h" +#include + +namespace coord = world_exe::util::coordinate; + +TEST(CoordinateTest, Opencv2RosPosition_SimpleAxis) { + // 1. 准备 (Arrange) - 在 OpenCV 坐标系下，沿着每个轴单位移动 + Eigen::Vector3d opencv_pos_x(1.0, 0.0, 0.0); // 沿 X 轴 (右) + Eigen::Vector3d opencv_pos_y(0.0, 1.0, 0.0); // 沿 Y 轴 (下) + Eigen::Vector3d opencv_pos_z(0.0, 0.0, 1.0); // 沿 Z 轴 (前) + + // 预期结果 (ROS 坐标系: x->前, y->左, z->上) + // (x, y, z) -> (z, -x, -y) + Eigen::Vector3d ros_expected_x(0.0, -1.0, 0.0); // (0, -1, 0) + Eigen::Vector3d ros_expected_y(0.0, 0.0, -1.0); // (0, 0, -1) + Eigen::Vector3d ros_expected_z(1.0, 0.0, 0.0); // (1, 0, 0) + + // 2. 执行 (Act) + Eigen::Vector3d ros_actual_x = coord::opencv2ros_position(opencv_pos_x); + Eigen::Vector3d ros_actual_y = coord::opencv2ros_position(opencv_pos_y); + Eigen::Vector3d ros_actual_z = coord::opencv2ros_position(opencv_pos_z); + + // 3. 断言 (Assert) + // 使用 ASSERT_TRUE 结合 Eigen 的 isApprox() 方法进行浮点数向量的精确比较 + ASSERT_TRUE(ros_actual_x.isApprox(ros_expected_x)) << "X-axis conversion failed."; + ASSERT_TRUE(ros_actual_y.isApprox(ros_expected_y)) << "Y-axis conversion failed."; + ASSERT_TRUE(ros_actual_z.isApprox(ros_expected_z)) << "Z-axis conversion failed."; +} + +TEST(CoordinateTest, Ros2OpencvPosition_SimpleAxis) { + // 1. 准备 (Arrange) - ROS 坐标系下的单位向量 + Eigen::Vector3d ros_pos_x(1.0, 0.0, 0.0); // 沿 X 轴 (前) + Eigen::Vector3d ros_pos_y(0.0, 1.0, 0.0); // 沿 Y 轴 (左) + Eigen::Vector3d ros_pos_z(0.0, 0.0, 1.0); // 沿 Z 轴 (上) + + // 预期结果 (OpenCV 坐标系: x->右, y->下, z->前) + // (-y, -z, x) + Eigen::Vector3d opencv_expected_x(0.0, 0.0, 1.0); // (0, 0, 1) + Eigen::Vector3d opencv_expected_y(-1.0, 0.0, 0.0); // (-1, 0, 0) + Eigen::Vector3d opencv_expected_z(0.0, -1.0, 0.0); // (0, -1, 0) + + // 2. 执行 (Act) + Eigen::Vector3d opencv_actual_x = coord::ros2opencv_position(ros_pos_x); + Eigen::Vector3d opencv_actual_y = coord::ros2opencv_position(ros_pos_y); + Eigen::Vector3d opencv_actual_z = coord::ros2opencv_position(ros_pos_z); + + // 3. 断言 (Assert) + ASSERT_TRUE(opencv_actual_x.isApprox(opencv_expected_x)); + ASSERT_TRUE(opencv_actual_y.isApprox(opencv_expected_y)); + ASSERT_TRUE(opencv_actual_z.isApprox(opencv_expected_z)); +} + +// **关键交叉测试 (Round Trip Test):** 确保来回转换后结果不变 +TEST(CoordinateTest, Position_RoundTrip) { + // 1. 准备：一个复杂的随机向量 + Eigen::Vector3d original_pos(1.23, -4.56, 7.89); + + // 2. 执行：OpenCV -> ROS -> OpenCV + Eigen::Vector3d ros_pos = coord::opencv2ros_position(original_pos); + Eigen::Vector3d final_pos = coord::ros2opencv_position(ros_pos); + + // 3. 断言：最终位置应该近似等于原始位置 + ASSERT_TRUE(final_pos.isApprox(original_pos)) + << "Round trip conversion failed. Original: " << original_pos.transpose() + << ", Final: " << final_pos.transpose(); +} + +TEST(CoordinateTest, Opencv2RosRotation_Identity) { + // 1. 准备：单位旋转矩阵 (无旋转) + Eigen::Matrix3d identity_rot = Eigen::Matrix3d::Identity(); + + // 2. 执行 + Eigen::Matrix3d ros_rot = coord::opencv2ros_rotation(identity_rot); + + // 3. 断言：转换一个单位矩阵，结果应该是一个单位矩阵 + ASSERT_TRUE(ros_rot.isApprox(identity_rot)) << "Identity rotation failed."; +} + +TEST(CoordinateTest, Opencv2RosRotation_KnownRotation) { + // 1. 准备：绕 OpenCV 坐标系 Z 轴旋转 90 度 (Z-axis 绕前方向) + // OpenCV 坐标系：X(右), Y(下), Z(前) + // 绕 Z 轴旋转 90 度 (右转 90) => X->Y, Y->-X + double angle = M_PI / 2.0; + Eigen::Matrix3d rot_opencv; + rot_opencv << cos(angle), -sin(angle), 0, sin(angle), cos(angle), 0, 0, 0, 1; + + // 2. 执行 + Eigen::Matrix3d ros_rot = coord::opencv2ros_rotation(rot_opencv); + + // 3. 预期结果：验证一个点。 + // 在 OpenCV 中 (1, 0, 0) 旋转后变成 (0, 1, 0)。 + // 转换到 ROS 坐标系后: + // (1, 0, 0) -> (0, -1, 0) (ROS X) + // (0, 1, 0) -> (0, 0, -1) (ROS Y) + // ROS 旋转矩阵应该把 (0, -1, 0) 变成 (0, 0, -1)。 + + Eigen::Vector3d ros_vector_in(0.0, -1.0, 0.0); // 相当于 OpenCV 的 X 轴 + Eigen::Vector3d ros_vector_out = ros_rot * ros_vector_in; + + Eigen::Vector3d ros_expected_out(0.0, 0.0, -1.0); // 相当于 OpenCV 的 Y 轴 + + // 4. 断言 + ASSERT_TRUE(ros_vector_out.isApprox(ros_expected_out)) << "Known rotation test failed."; +} + +TEST(CoordinateTest, Rotation_RoundTrip) { + // 1. 准备：一个绕随机轴旋转的复杂旋转矩阵 + Eigen::AngleAxisd aa(0.5, Eigen::Vector3d(0.1, 0.5, -0.3).normalized()); + Eigen::Matrix3d original_rot = aa.toRotationMatrix(); + + // 2. 执行：OpenCV -> ROS -> OpenCV + Eigen::Matrix3d ros_rot = coord::opencv2ros_rotation(original_rot); + Eigen::Matrix3d final_rot = coord::ros2opencv_rotation(ros_rot); // 使用 ros2opencv_rotation + + // 3. 断言：最终矩阵应该近似等于原始矩阵 + ASSERT_TRUE(final_rot.isApprox(original_rot, 1e-6)) << "Rotation Round trip conversion failed."; +} From 0477bbf8b38199d01d3893262fbf30250143c60d Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 2 Nov 2025 01:22:45 +0800 Subject: [PATCH 79/93] test(math): verify mathematical utilities functionality --- src/util/math_tongji.hpp | 125 +++++++++++++++++++ tests/tongji_tests/utils/math_test.cpp | 165 +++++++++++++++++++++++++ 2 files changed, 290 insertions(+) create mode 100644 src/util/math_tongji.hpp create mode 100644 tests/tongji_tests/utils/math_test.cpp diff --git a/src/util/math_tongji.hpp b/src/util/math_tongji.hpp new file mode 100644 index 0000000..fa2c7c1 --- /dev/null +++ b/src/util/math_tongji.hpp @@ -0,0 +1,125 @@ +#pragma once + +#include + +namespace world_exe::util::math { + +// [−π,π] 注意边界值 +static constexpr double clamp_pm_pi(auto angle) { + return std::remainder(angle, 2.0 * std::numbers::pi); +} +// zyx order +static inline Eigen::Vector3d get_ypr_from_quaternion(const Eigen::Quaterniond& quaternion) { + const Eigen::Matrix3d R = quaternion.toRotationMatrix(); + return R.eulerAngles(2, 1, 0); +} + +/** + * @brief 将笛卡尔坐标点 (x, y) 绕原点旋转。 + * delta_angle > 0 时，执行逆时针旋转。 + * @param x 点的 x 坐标。 + * @param y 点的 y 坐标。 + * @param delta_angle 旋转角度 (弧度)。 + * @return std::pair 旋转后的新坐标 (x', y')。 + */ +static inline std::pair rotate_point_ccw(double x, double y, double delta_angle) { + const double cos_d = std::cos(delta_angle); + const double sin_d = std::sin(delta_angle); + + const double x_new = x * cos_d - y * sin_d; // x' = x*cos(d) - y*sin(d) + const double y_new = x * sin_d + y * cos_d; // y' = x*sin(d) + y*cos(d) + + return { x_new, y_new }; +} + +// zyx order +static Eigen::Matrix3d euler_to_matrix(const Eigen::Vector3d& ypr) { + Eigen::AngleAxisd rollAngle(ypr[2], Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd pitchAngle(ypr[1], Eigen::Vector3d::UnitY()); + Eigen::AngleAxisd yawAngle(ypr[0], Eigen::Vector3d::UnitZ()); + Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle; + + return q.toRotationMatrix(); +} + +static inline Eigen::Quaterniond euler_to_quaternion( + double yaw_rad, double pitch_rad, double roll_rad) { + return Eigen::AngleAxisd(yaw_rad, Eigen::Vector3d::UnitZ()) + * Eigen::AngleAxisd(pitch_rad, Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(roll_rad, Eigen::Vector3d::UnitX()); +} + +static inline Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& ypr) { + return Eigen::AngleAxisd(ypr[0], Eigen::Vector3d::UnitZ()) + * Eigen::AngleAxisd(ypr[1], Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(ypr[2], Eigen::Vector3d::UnitX()); +} +// zyx order +static Eigen::Vector3d quaternion_to_euler(Eigen::Quaterniond q) { + const Eigen::Matrix3d R = q.toRotationMatrix(); + return R.eulerAngles(2, 1, 0); +} + +static Eigen::Vector3d matrix_to_euler(Eigen::Matrix3d R) { return R.eulerAngles(2, 1, 0); } + +static inline Eigen::Vector3d xyz2ypd(const Eigen::Vector3d& xyz) { + const double x = xyz[0]; + const double y = xyz[1]; + const double z = xyz[2]; + + const double r_xy = std::hypot(x, y); + + const double distance = std::hypot(r_xy, z); + const double yaw = std::atan2(y, x); + const double pitch = std::atan2(z, r_xy); + + return { yaw, pitch, distance }; +} + +static Eigen::Matrix xyz2ypd_jacobian(const Eigen::Vector3d& xyz) { + const auto x = xyz[0], y = xyz[1], z = xyz[2]; + + const double r_xy_sq = x * x + y * y; // r_xy^2 + const double r_xy = std::sqrt(r_xy_sq); // r_xy + const double D_sq = r_xy_sq + z * z; // D^2 (总距离平方) + const double D = std::sqrt(D_sq); // D (总距离) + + if (r_xy_sq < 1e-12) { + return Eigen::Matrix3d::Zero(); + } + + // I. Yaw (行 0): J[0, *] + const double r_xy_sq_inv = 1.0 / r_xy_sq; + const double dyaw_dx = -y * r_xy_sq_inv; + const double dyaw_dy = x * r_xy_sq_inv; + const double dyaw_dz = 0.0; + + // II. Pitch (行 1): J[1, *] + // 简化后的公式使用 D_sq 和 r_xy + const double D_sq_inv = 1.0 / D_sq; + const double r_xy_inv = 1.0 / r_xy; + const double factor = z * D_sq_inv * r_xy_inv; // z / (D^2 * r_xy) + + const double dpitch_dx = -x * factor; + const double dpitch_dy = -y * factor; + // dpitch_dz = r_xy / D^2 + const double dpitch_dz = r_xy * D_sq_inv; + + // III. Distance (行 2): J[2, *] + // dD/dx = x/D + const double D_inv = 1.0 / D; + const double ddistance_dx = x * D_inv; + const double ddistance_dy = y * D_inv; + const double ddistance_dz = z * D_inv; + + Eigen::Matrix3d J; + // clang-format off + J << dyaw_dx , dyaw_dy , dyaw_dz , + dpitch_dx , dpitch_dy , dpitch_dz , + ddistance_dx, ddistance_dy, ddistance_dz; + // clang-format on + + return J; +} + +} // namespace rmcs_auto_aim::util::math diff --git a/tests/tongji_tests/utils/math_test.cpp b/tests/tongji_tests/utils/math_test.cpp new file mode 100644 index 0000000..2ac1cfc --- /dev/null +++ b/tests/tongji_tests/utils/math_test.cpp @@ -0,0 +1,165 @@ + +#include "util/math_tongji.hpp" + +#include +#include + +#include "gtest/gtest.h" +#include + +namespace math = world_exe::util::math; + +constexpr double TOLERANCE = 1e-6; + +// --- 1. 角度钳位测试 (clamp_pm_pi) --- +// 目标：确保将角度标准化到 [−π, π] 范围内。std::remainder(angle, 2*PI) 保证了结果在 [-PI, PI] + +TEST(MathUtilsTest, ClampPi_NormalRange) { + // 1. 正常范围内的角度 + EXPECT_NEAR(math::clamp_pm_pi(M_PI / 2.0), M_PI / 2.0, TOLERANCE); + EXPECT_NEAR(math::clamp_pm_pi(-M_PI / 2.0), -M_PI / 2.0, TOLERANCE); +} + +TEST(MathUtilsTest, ClampPi_BoundaryValues) { + // 容忍度常量 + constexpr double TOLERANCE = 1e-6; + + // --- 1. 测试 PI + eps -> -PI + eps 的行为 --- + // 确保大于 PI 的值被正确拉回负半轴。 + EXPECT_NEAR(math::clamp_pm_pi(std::numbers::pi + 1e-7), -std::numbers::pi + 1e-7, TOLERANCE) + << "Failed for PI + epsilon. Expected: -PI + epsilon."; + + // --- 2. 奇数倍 PI 测试 (绝对值断言) --- + // 目标：确保结果的绝对值是 PI，因为 std::remainder(x, 2PI) 不保证符号。 + + // 测试 3*PI: 结果必须是 PI 或 -PI + double result_3pi = math::clamp_pm_pi(3.0 * std::numbers::pi); + EXPECT_NEAR(std::abs(result_3pi), std::numbers::pi, TOLERANCE) + << "Failed for 3*PI. Expected absolute value PI, got: " << result_3pi; + + // 测试 -3*PI: 结果必须是 PI 或 -PI + double result_neg_3pi = math::clamp_pm_pi(-3.0 * std::numbers::pi); + EXPECT_NEAR(std::abs(result_neg_3pi), std::numbers::pi, TOLERANCE) + << "Failed for -3*PI. Expected absolute value PI, got: " << result_neg_3pi; + + // --- 3. 严格的 PI 边界 --- + // 测试 PI 本身。由于 std::remainder(PI, 2PI) 可能为 PI 或 -PI，我们只断言绝对值。 + double result_pi = math::clamp_pm_pi(std::numbers::pi); + EXPECT_NEAR(std::abs(result_pi), std::numbers::pi, TOLERANCE) + << "Failed for PI. Expected absolute value PI, got: " << result_pi; +} + +// --- 2. 坐标旋转测试 (rotate_point_ccw) --- +// 目标：确保点 (x, y) 绕原点正确地执行逆时针 (CCW) 旋转。 + +TEST(MathTransformTest, RotatePointCCW_ZeroRotation) { + // 1. 零旋转 + auto [x_new, y_new] = math::rotate_point_ccw(10.0, 5.0, 0.0); + EXPECT_NEAR(x_new, 10.0, TOLERANCE); + EXPECT_NEAR(y_new, 5.0, TOLERANCE); +} + +TEST(MathTransformTest, RotatePointCCW_90Degrees) { + // 2. 逆时针旋转 90 度 (PI/2)： (1, 0) -> (0, 1) + auto [x1, y1] = math::rotate_point_ccw(1.0, 0.0, M_PI / 2.0); + EXPECT_NEAR(x1, 0.0, TOLERANCE); + EXPECT_NEAR(y1, 1.0, TOLERANCE); + + // (0, 1) -> (-1, 0) + auto [x2, y2] = math::rotate_point_ccw(0.0, 1.0, M_PI / 2.0); + EXPECT_NEAR(x2, -1.0, TOLERANCE); + EXPECT_NEAR(y2, 0.0, TOLERANCE); +} + +TEST(MathTransformTest, RotatePointCCW_RoundTrip) { + // 3. 来回旋转：旋转 delta，再旋转 -delta，应回到原点 + double x = 3.0, y = 4.0; + double delta_angle = M_PI / 6.0; // 30度 + + auto [x_rot, y_rot] = math::rotate_point_ccw(x, y, delta_angle); + auto [x_final, y_final] = math::rotate_point_ccw(x_rot, y_rot, -delta_angle); + + EXPECT_NEAR(x_final, x, TOLERANCE); + EXPECT_NEAR(y_final, y, TOLERANCE); +} + +// --- 3. 欧拉角/四元数/矩阵互转 (Round Trip Tests) --- +// 目标：确保所有 ZYX 顺序转换函数是互逆的。 + +TEST(MathConversionTest, EulerToQuaternionAndBack_RoundTrip) { + // 1. Euler -> Quaternion -> Euler (使用统一的 ZYX 顺序) + Eigen::Vector3d original_ypr(0.5, -0.4, 0.3); // YAW, PITCH, ROLL + + Eigen::Quaterniond q = math::euler_to_quaternion(original_ypr); + Eigen::Vector3d final_ypr = math::quaternion_to_euler(q); + + // 两个函数都使用了 Eigen 的 ZYX 接口，结果应该匹配 + ASSERT_TRUE(final_ypr.isApprox(original_ypr, TOLERANCE)) + << "E->Q->E failed." + << "\nOriginal YPR: " << original_ypr.transpose() + << "\nFinal YPR: " << final_ypr.transpose(); +} + +TEST(MathConversionTest, EulerToMatrixAndBack_RoundTrip) { + // 2. Euler -> Matrix -> Euler + Eigen::Vector3d original_ypr(0.8, -0.1, 0.4); + + Eigen::Matrix3d R = math::euler_to_matrix(original_ypr); + Eigen::Vector3d final_ypr = math::matrix_to_euler(R); + + ASSERT_TRUE(final_ypr.isApprox(original_ypr, TOLERANCE)) + << "E->M->E failed." + << "\nOriginal YPR: " << original_ypr.transpose() + << "\nFinal YPR: " << final_ypr.transpose(); +} + +TEST(MathConversionTest, QuaternionToEuler_DirectAccess) { + // 3. get_ypr_from_quaternion 测试 + Eigen::Vector3d expected_ypr(1.0, 0.5, -0.3); + Eigen::Quaterniond q = math::euler_to_quaternion(expected_ypr); + Eigen::Vector3d actual_ypr = math::get_ypr_from_quaternion(q); + + ASSERT_TRUE(actual_ypr.isApprox(expected_ypr, TOLERANCE)) << "get_ypr_from_quaternion failed."; +} + +// --- 4. 坐标转换测试 (xyz2ypd, xyz2ypd_jacobian) --- + +TEST(MathConversionTest, XYZ2YPD_KnownPoint) { + // 1. 沿 X 轴 (前方) + Eigen::Vector3d xyz(10.0, 0.0, 0.0); + Eigen::Vector3d ypd = math::xyz2ypd(xyz); // 预期 Yaw=0, Pitch=0, Dist=10 + + Eigen::Vector3d expected_ypd(0.0, 0.0, 10.0); + + ASSERT_TRUE(ypd.isApprox(expected_ypd, TOLERANCE)) + << "Expected YPD: " << expected_ypd.transpose() << "\nActual YPD: " << ypd.transpose(); +} + +TEST(MathConversionTest, XYZ2YPD_Jacobian_KnownPoint) { + // 2. 雅可比矩阵测试 - 检查一个已知点 (1, 0, 0) + Eigen::Vector3d xyz(1.0, 0.0, 0.0); + Eigen::Matrix3d J = math::xyz2ypd_jacobian(xyz); + + // 预期结果 (x=1, y=0, z=0) + // J[0, *] (Yaw): dyaw/dx=0, dyaw/dy=1, dyaw/dz=0 + // J[1, *] (Pitch): dpitch/dx=0, dpitch/dy=0, dpitch/dz=1 + // J[2, *] (Dist): dD/dx=1, dD/dy=0, dD/dz=0 + Eigen::Matrix3d expected_J; + expected_J << 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0; + + ASSERT_TRUE(J.isApprox(expected_J, TOLERANCE)) << "Jacobian for (1,0,0) failed." + << "\nExpected:\n" + << expected_J << "\nActual:\n" + << J; +} + +TEST(MathConversionTest, Jacobian_Singularity) { + // 3. 奇点测试：(0, 0, z) 处，r_xy_sq < 1e-12 应该返回零矩阵 + Eigen::Vector3d xyz_singularity(0.0, 0.0, 5.0); + Eigen::Matrix3d J = math::xyz2ypd_jacobian(xyz_singularity); + + ASSERT_TRUE(J.isApprox(Eigen::Matrix3d::Zero(), TOLERANCE)) << "Jacobian failed at singularity " + "(0, 0, z)." + << "\nActual:\n" + << J; +} From 6382a71b89a63916163ea4d53d9ae47957377254 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 2 Nov 2025 03:03:06 +0800 Subject: [PATCH 80/93] feat(architecture): separate tongji-dev logic from v1 and complete core utilities - Split codebase into tongji-dev and v1 distinct branches - Complete math utility module with comprehensive test coverage - Finish coordinate transformation module with full validation --- src/tongji/auto_aim_system.cpp | 8 +- .../fire_controller/aim_point_chooser.hpp | 6 +- .../fire_controller/aim_solver.hpp | 6 +- .../fire_controller/fire_controller.cpp | 6 +- .../fire_controller/fire_controller.hpp | 0 .../fire_controller/fire_decision.hpp | 1 - .../fire_controller/trajectory.hpp | 0 .../{ => modules}/identifier/armor_filter.hpp | 0 .../{ => modules}/identifier/classifier.hpp | 0 .../{ => modules}/identifier/decider.cpp | 0 .../{ => modules}/identifier/decider.hpp | 0 .../identifier/identified_armor.hpp | 0 .../{ => modules}/identifier/identifier.cpp | 1 - .../{ => modules}/identifier/identifier.hpp | 0 .../{ => modules}/identifier/tracker.hpp | 0 .../predictor/car_predictor/car_predictor.hpp | 11 +- .../car_predictor/car_predictor_manager.cpp | 8 +- .../car_predictor/car_predictor_manager.hpp | 0 .../predictor/in_gimbal_control_armor.hpp | 0 .../kalman_filter/extended_kalman_filter.hpp | 0 .../predictor/kalman_filter/predict_model.hpp | 42 ++--- .../solver/reprojection_util.hpp | 10 +- .../{ => modules}/solver/solved_armor.hpp | 0 src/tongji/{ => modules}/solver/solver.cpp | 4 +- src/tongji/{ => modules}/solver/solver.hpp | 0 .../state_machine/state_machine.cpp | 0 .../state_machine/state_machine.hpp | 0 src/{util => tongji/utils}/coordinate.hpp | 0 .../math_tongji.hpp => tongji/utils/math.hpp} | 11 +- src/util/math.hpp | 160 ++---------------- tests/tongji_tests/utils/coordinate_test.cpp | 2 +- tests/tongji_tests/utils/math_test.cpp | 4 +- 32 files changed, 76 insertions(+), 204 deletions(-) rename src/tongji/{ => modules}/fire_controller/aim_point_chooser.hpp (94%) rename src/tongji/{ => modules}/fire_controller/aim_solver.hpp (95%) rename src/tongji/{ => modules}/fire_controller/fire_controller.cpp (95%) rename src/tongji/{ => modules}/fire_controller/fire_controller.hpp (100%) rename src/tongji/{ => modules}/fire_controller/fire_decision.hpp (98%) rename src/tongji/{ => modules}/fire_controller/trajectory.hpp (100%) rename src/tongji/{ => modules}/identifier/armor_filter.hpp (100%) rename src/tongji/{ => modules}/identifier/classifier.hpp (100%) rename src/tongji/{ => modules}/identifier/decider.cpp (100%) rename src/tongji/{ => modules}/identifier/decider.hpp (100%) rename src/tongji/{ => modules}/identifier/identified_armor.hpp (100%) rename src/tongji/{ => modules}/identifier/identifier.cpp (99%) rename src/tongji/{ => modules}/identifier/identifier.hpp (100%) rename src/tongji/{ => modules}/identifier/tracker.hpp (100%) rename src/tongji/{ => modules}/predictor/car_predictor/car_predictor.hpp (92%) rename src/tongji/{ => modules}/predictor/car_predictor/car_predictor_manager.cpp (93%) rename src/tongji/{ => modules}/predictor/car_predictor/car_predictor_manager.hpp (100%) rename src/tongji/{ => modules}/predictor/in_gimbal_control_armor.hpp (100%) rename src/tongji/{ => modules}/predictor/kalman_filter/extended_kalman_filter.hpp (100%) rename src/tongji/{ => modules}/predictor/kalman_filter/predict_model.hpp (87%) rename src/tongji/{ => modules}/solver/reprojection_util.hpp (93%) rename src/tongji/{ => modules}/solver/solved_armor.hpp (100%) rename src/tongji/{ => modules}/solver/solver.cpp (98%) rename src/tongji/{ => modules}/solver/solver.hpp (100%) rename src/tongji/{ => modules}/state_machine/state_machine.cpp (100%) rename src/tongji/{ => modules}/state_machine/state_machine.hpp (100%) rename src/{util => tongji/utils}/coordinate.hpp (100%) rename src/{util/math_tongji.hpp => tongji/utils/math.hpp} (92%) diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 9789324..7692b31 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -6,12 +6,12 @@ #include "../v1/sync/syncer.hpp" #include "core/event_bus.hpp" #include "data/predictor_update_package.hpp" +#include "modules/fire_controller/fire_controller.hpp" +#include "modules/predictor/car_predictor/car_predictor_manager.hpp" +#include "modules/solver/solver.hpp" +#include "modules/state_machine/state_machine.hpp" #include "parameters/params_system_v1.hpp" #include "parameters/profile.hpp" -#include "tongji/fire_controller/fire_controller.hpp" -#include "tongji/predictor/car_predictor/car_predictor_manager.hpp" -#include "tongji/solver/solver.hpp" -#include "tongji/state_machine/state_machine.hpp" #include "v1/identifier/identifier.hpp" namespace world_exe::tongji { diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/modules/fire_controller/aim_point_chooser.hpp similarity index 94% rename from src/tongji/fire_controller/aim_point_chooser.hpp rename to src/tongji/modules/fire_controller/aim_point_chooser.hpp index 51e2c71..8dd834b 100644 --- a/src/tongji/fire_controller/aim_point_chooser.hpp +++ b/src/tongji/modules/fire_controller/aim_point_chooser.hpp @@ -2,7 +2,7 @@ #include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" -#include "util/math.hpp" +#include "tongji/utils/math.hpp" #include @@ -29,8 +29,8 @@ class AimPointChooser { std::vector> delta_angle_list; for (int i = 0; i < armor_num; i++) { - auto delta_angle = util::math::clamp_pm_pi( - util::math::get_yaw_from_quaternion(armors[i].orientation) - center_yaw); + const auto ypr = utils::math::get_ypr_from_quaternion(armors[i].orientation); + auto delta_angle = utils::math::clamp_pm_pi(ypr[0] - center_yaw); delta_angle_list.emplace_back(std::make_tuple(i, delta_angle)); } std::sort(delta_angle_list.begin(), delta_angle_list.end(), diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/modules/fire_controller/aim_solver.hpp similarity index 95% rename from src/tongji/fire_controller/aim_solver.hpp rename to src/tongji/modules/fire_controller/aim_solver.hpp index cdeeb8f..25ebc6a 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/modules/fire_controller/aim_solver.hpp @@ -8,10 +8,10 @@ #include #include -#include "../predictor/car_predictor/car_predictor.hpp" #include "aim_point_chooser.hpp" -#include "tongji/predictor/kalman_filter/extended_kalman_filter.hpp" -#include "tongji/predictor/kalman_filter/predict_model.hpp" +#include "tongji/modules/predictor/car_predictor/car_predictor.hpp" +#include "tongji/modules/predictor/kalman_filter/extended_kalman_filter.hpp" +#include "tongji/modules/predictor/kalman_filter/predict_model.hpp" #include "trajectory.hpp" namespace world_exe::tongji::fire_control { diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/modules/fire_controller/fire_controller.cpp similarity index 95% rename from src/tongji/fire_controller/fire_controller.cpp rename to src/tongji/modules/fire_controller/fire_controller.cpp index 1e3a21e..ce925eb 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/modules/fire_controller/fire_controller.cpp @@ -5,13 +5,13 @@ #include -#include "../identifier/identified_armor.hpp" -#include "../state_machine/state_machine.hpp" #include "aim_solver.hpp" #include "data/fire_control.hpp" #include "fire_decision.hpp" #include "interfaces/target_predictor.hpp" -#include "tongji/predictor/car_predictor/car_predictor_manager.hpp" +#include "tongji/modules/identifier/identified_armor.hpp" +#include "tongji/modules/predictor/car_predictor/car_predictor_manager.hpp" +#include "tongji/modules/state_machine/state_machine.hpp" namespace world_exe::tongji::fire_control { diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/modules/fire_controller/fire_controller.hpp similarity index 100% rename from src/tongji/fire_controller/fire_controller.hpp rename to src/tongji/modules/fire_controller/fire_controller.hpp diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/modules/fire_controller/fire_decision.hpp similarity index 98% rename from src/tongji/fire_controller/fire_decision.hpp rename to src/tongji/modules/fire_controller/fire_decision.hpp index 39125c5..619500e 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/modules/fire_controller/fire_decision.hpp @@ -2,7 +2,6 @@ #include #include -#include #include diff --git a/src/tongji/fire_controller/trajectory.hpp b/src/tongji/modules/fire_controller/trajectory.hpp similarity index 100% rename from src/tongji/fire_controller/trajectory.hpp rename to src/tongji/modules/fire_controller/trajectory.hpp diff --git a/src/tongji/identifier/armor_filter.hpp b/src/tongji/modules/identifier/armor_filter.hpp similarity index 100% rename from src/tongji/identifier/armor_filter.hpp rename to src/tongji/modules/identifier/armor_filter.hpp diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/modules/identifier/classifier.hpp similarity index 100% rename from src/tongji/identifier/classifier.hpp rename to src/tongji/modules/identifier/classifier.hpp diff --git a/src/tongji/identifier/decider.cpp b/src/tongji/modules/identifier/decider.cpp similarity index 100% rename from src/tongji/identifier/decider.cpp rename to src/tongji/modules/identifier/decider.cpp diff --git a/src/tongji/identifier/decider.hpp b/src/tongji/modules/identifier/decider.hpp similarity index 100% rename from src/tongji/identifier/decider.hpp rename to src/tongji/modules/identifier/decider.hpp diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/modules/identifier/identified_armor.hpp similarity index 100% rename from src/tongji/identifier/identified_armor.hpp rename to src/tongji/modules/identifier/identified_armor.hpp diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/modules/identifier/identifier.cpp similarity index 99% rename from src/tongji/identifier/identifier.cpp rename to src/tongji/modules/identifier/identifier.cpp index 2f716f0..bf03a41 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/modules/identifier/identifier.cpp @@ -2,7 +2,6 @@ #include #include -#include #include #include #include diff --git a/src/tongji/identifier/identifier.hpp b/src/tongji/modules/identifier/identifier.hpp similarity index 100% rename from src/tongji/identifier/identifier.hpp rename to src/tongji/modules/identifier/identifier.hpp diff --git a/src/tongji/identifier/tracker.hpp b/src/tongji/modules/identifier/tracker.hpp similarity index 100% rename from src/tongji/identifier/tracker.hpp rename to src/tongji/modules/identifier/tracker.hpp diff --git a/src/tongji/predictor/car_predictor/car_predictor.hpp b/src/tongji/modules/predictor/car_predictor/car_predictor.hpp similarity index 92% rename from src/tongji/predictor/car_predictor/car_predictor.hpp rename to src/tongji/modules/predictor/car_predictor/car_predictor.hpp index 05136f7..0ba2230 100644 --- a/src/tongji/predictor/car_predictor/car_predictor.hpp +++ b/src/tongji/modules/predictor/car_predictor/car_predictor.hpp @@ -1,19 +1,18 @@ #pragma once -#include #include #include #include #include -#include "../in_gimbal_control_armor.hpp" -#include "../kalman_filter/extended_kalman_filter.hpp" -#include "../kalman_filter/predict_model.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "data/time_stamped.hpp" #include "enum/car_id.hpp" #include "interfaces/predictor.hpp" +#include "tongji/modules/predictor/in_gimbal_control_armor.hpp" +#include "tongji/modules/predictor/kalman_filter/extended_kalman_filter.hpp" +#include "tongji/modules/predictor/kalman_filter/predict_model.hpp" namespace world_exe::tongji::predictor { @@ -59,13 +58,13 @@ class CarPredictor final : public interfaces::IPredictor { const auto ekf_x = this->GetPredictedX((time_stamp - time_stamp_).to_seconds()); std::vector armors; for (int id = 0; id < model_.GetArmorNum(); id++) { - auto angle = util::math::clamp_pm_pi(ekf_x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto angle = utils::math::clamp_pm_pi(ekf_x[6] + id * 2 * CV_PI / model_.GetArmorNum()); auto xyz = model_.h_armor_xyz(ekf_x, id); data::ArmorGimbalControlSpacing armor; armor.id = model_.GetID(); armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armor.orientation = utils::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); armors.emplace_back(std::move(armor)); } return std::make_shared(armors, time_stamp_); diff --git a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp b/src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp similarity index 93% rename from src/tongji/predictor/car_predictor/car_predictor_manager.cpp rename to src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp index 1eaca95..458dde0 100644 --- a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp +++ b/src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp @@ -12,8 +12,8 @@ #include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" +#include "tongji/utils/math.hpp" #include "util/index.hpp" -#include "util/math.hpp" namespace world_exe::tongji::predictor { @@ -71,12 +71,12 @@ class CarPredictorManager::Impl { if (!targets_map_.contains(armor.id)) { targets_map_.try_emplace(armor.id, std::make_unique(armor.position, - util::math::quaternion_to_euler(armor.orientation, 2, 1, 0), armor.id, + utils::math::quaternion_to_euler(armor.orientation), armor.id, data->GetTimeStamp())); } else { targets_map_.at(armor.id)->Update(data->GetTimeStamp(), armor.position, - util::math::quaternion_to_euler(armor.orientation, 2, 1, 0), - util::math::xyz2ypd(armor.position)); + utils::math::quaternion_to_euler(armor.orientation), + utils::math::xyz2ypd(armor.position)); } } diff --git a/src/tongji/predictor/car_predictor/car_predictor_manager.hpp b/src/tongji/modules/predictor/car_predictor/car_predictor_manager.hpp similarity index 100% rename from src/tongji/predictor/car_predictor/car_predictor_manager.hpp rename to src/tongji/modules/predictor/car_predictor/car_predictor_manager.hpp diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/modules/predictor/in_gimbal_control_armor.hpp similarity index 100% rename from src/tongji/predictor/in_gimbal_control_armor.hpp rename to src/tongji/modules/predictor/in_gimbal_control_armor.hpp diff --git a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp b/src/tongji/modules/predictor/kalman_filter/extended_kalman_filter.hpp similarity index 100% rename from src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp rename to src/tongji/modules/predictor/kalman_filter/extended_kalman_filter.hpp diff --git a/src/tongji/predictor/kalman_filter/predict_model.hpp b/src/tongji/modules/predictor/kalman_filter/predict_model.hpp similarity index 87% rename from src/tongji/predictor/kalman_filter/predict_model.hpp rename to src/tongji/modules/predictor/kalman_filter/predict_model.hpp index e811698..412cc94 100644 --- a/src/tongji/predictor/kalman_filter/predict_model.hpp +++ b/src/tongji/modules/predictor/kalman_filter/predict_model.hpp @@ -7,7 +7,7 @@ #include #include "enum/car_id.hpp" -#include "util/math.hpp" +#include "tongji/utils/math.hpp" namespace world_exe::tongji::predictor { template @@ -75,15 +75,15 @@ class EKFModel { // 防止夹角求和出现异常值 constexpr auto x_add(const XVec& a, const XVec& b) const -> const auto { XVec c = a + b; - c(6) = util ::math::clamp_pm_pi(c(6)); + c(6) = utils::math::clamp_pm_pi(c(6)); return c; } constexpr auto z_substract(const ZVec& a, const ZVec& b) const -> const auto { auto c = a - b; - c(0) = util::math::clamp_pm_pi(c(0)); - c(1) = util::math::clamp_pm_pi(c(1)); - c(3) = util::math::clamp_pm_pi(c(3)); + c(0) = utils::math::clamp_pm_pi(c(0)); + c(1) = utils::math::clamp_pm_pi(c(1)); + c(3) = utils::math::clamp_pm_pi(c(3)); return c; } @@ -111,7 +111,7 @@ class EKFModel { // 防止夹角求和出现异常值 auto f(const XVec& x, const double& dt)const ->const auto{ XVec x_prior = this->A(dt) * x; - x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); + x_prior(6) = utils::math::clamp_pm_pi(x_prior(6)); return x_prior; }; @@ -128,8 +128,8 @@ class EKFModel { } std::sort(xyza_i_list.begin(), xyza_i_list.end(), [](const auto& a, const auto& b) { - auto ypd1 = util::math::xyz2ypd(a.first.head(3)); - auto ypd2 = util::math::xyz2ypd(b.first.head(3)); + auto ypd1 = utils::math::xyz2ypd(a.first.head(3)); + auto ypd2 = utils::math::xyz2ypd(b.first.head(3)); return ypd1(2) < ypd2(2); }); @@ -138,9 +138,9 @@ class EKFModel { for (int i = 0; i < std::min(3, armor_num_); ++i) { const auto& xyza = xyza_i_list[i].first; - auto ypd = util::math::xyz2ypd(xyza.head(3)); - double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - xyza(3))) - + std::abs(util::math::clamp_pm_pi(armor_ypd_in_gimbal(0) - ypd(0))); + auto ypd = utils::math::xyz2ypd(xyza.head(3)); + double error = std::abs(utils::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - xyza(3))) + + std::abs(utils::math::clamp_pm_pi(armor_ypd_in_gimbal(0) - ypd(0))); if (error < min_error) { min_error = error; @@ -152,7 +152,7 @@ class EKFModel { // 计算出装甲板中心的坐标（考虑长短轴） auto h_armor_xyz(const XVec& x, int id) const ->const auto { - auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); + auto angle = utils::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); auto r = (use_l_h) ? x(8) + x(9) : x(8); @@ -164,7 +164,7 @@ class EKFModel { } constexpr auto H(const XVec& x, int id) const->HMat const { - auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); + auto angle = utils::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto cos_angle=std::cos(angle); auto sin_angle=std::sin(angle); @@ -192,7 +192,7 @@ class EKFModel { // clang-format on auto armor_xyz = h_armor_xyz(x, id); - auto H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); + auto H_armor_ypd = utils::math::xyz2ypd_jacobian(armor_xyz); // clang-format off Eigen::MatrixH_armor_ypda; H_armor_ypda<< @@ -208,7 +208,7 @@ class EKFModel { const Eigen::Vector3d& armor_ypd_in_gimbal, int id) const -> RMat const { // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; auto center_yaw = std::atan2(armor_xyz_in_gimbal(1), armor_xyz_in_gimbal(0)); - auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - center_yaw); + auto delta_angle = utils::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - center_yaw); RDig R_dig(4); R_dig << 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, log(std::abs(armor_ypd_in_gimbal(2)) + 1) / 200 + 9e-2; @@ -254,17 +254,17 @@ class EKFModel { constexpr auto h(const XVec& x, const int& id) const -> const auto { auto xyz = this->h_armor_xyz(x, id); - auto ypd = util::math::xyz2ypd(xyz); - auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); + auto ypd = utils::math::xyz2ypd(xyz); + auto angle = utils::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; } // 防止夹角求差出现异常值 constexpr auto z_subtract(const ZVec& a, const ZVec& b) const -> const auto { ZVec c = a - b; - c(0) = util::math::clamp_pm_pi(c(0)); - c(1) = util::math::clamp_pm_pi(c(1)); - c(3) = util::math::clamp_pm_pi(c(3)); + c(0) = utils::math::clamp_pm_pi(c(0)); + c(1) = utils::math::clamp_pm_pi(c(1)); + c(3) = utils::math::clamp_pm_pi(c(3)); return c; }; @@ -272,7 +272,7 @@ class EKFModel { std::vector _armor_xyza_list; for (int i = 0; i < armor_num_; i++) { - auto angle = util::math::clamp_pm_pi(ekf_x(6) + i * 2 * CV_PI / armor_num_); + auto angle = utils::math::clamp_pm_pi(ekf_x(6) + i * 2 * CV_PI / armor_num_); auto xyz = h_armor_xyz(ekf_x, i); _armor_xyza_list.push_back({ xyz(0), xyz(1), xyz(2), angle }); } diff --git a/src/tongji/solver/reprojection_util.hpp b/src/tongji/modules/solver/reprojection_util.hpp similarity index 93% rename from src/tongji/solver/reprojection_util.hpp rename to src/tongji/modules/solver/reprojection_util.hpp index 109163a..2cd6638 100644 --- a/src/tongji/solver/reprojection_util.hpp +++ b/src/tongji/modules/solver/reprojection_util.hpp @@ -11,8 +11,8 @@ #include "data/armor_image_spaceing.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" -#include "util/coordinate.hpp" -#include "util/math.hpp" +#include "tongji/utils/coordinate.hpp" +#include "tongji/utils/math.hpp" namespace world_exe::tongji::solver { @@ -105,11 +105,11 @@ class ReprojectionUtil { + std::fabs(ref_d.norm() - pt_d.norm())) / ref_d.norm(); double angular_dis = - ref_d.norm() * util::math::get_abs_angle(ref_d, pt_d) / ref_d.norm(); + ref_d.norm() * utils::math::get_abs_angle(ref_d, pt_d) / ref_d.norm(); // 平方可能是为了配合 sin 和 cos // 弧度差代价（0 度左右占比应该大） - double cost_i = util::math::square(pixel_dis * std::sin(inclined)) - + util::math::square(angular_dis * std::cos(inclined)) + double cost_i = utils::math::square(pixel_dis * std::sin(inclined)) + + utils::math::square(angular_dis * std::cos(inclined)) * 2.0; // DETECTOR_ERROR_PIXEL_BY_SLOPE // 重投影像素误差越大，越相信斜率 cost += std::sqrt(cost_i); diff --git a/src/tongji/solver/solved_armor.hpp b/src/tongji/modules/solver/solved_armor.hpp similarity index 100% rename from src/tongji/solver/solved_armor.hpp rename to src/tongji/modules/solver/solved_armor.hpp diff --git a/src/tongji/solver/solver.cpp b/src/tongji/modules/solver/solver.cpp similarity index 98% rename from src/tongji/solver/solver.cpp rename to src/tongji/modules/solver/solver.cpp index a9a91d4..957282e 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/modules/solver/solver.cpp @@ -11,6 +11,7 @@ #include #include +#include "../solver/reprojection_util.hpp" #include "data/armor_camera_spacing.hpp" #include "data/armor_image_spaceing.hpp" #include "data/time_stamped.hpp" @@ -18,8 +19,7 @@ #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" #include "solved_armor.hpp" -#include "tongji/solver/reprojection_util.hpp" -#include "util/coordinate.hpp" +#include "tongji/utils/coordinate.hpp" #include "util/index.hpp" #include "util/math.hpp" namespace world_exe::tongji::solver { diff --git a/src/tongji/solver/solver.hpp b/src/tongji/modules/solver/solver.hpp similarity index 100% rename from src/tongji/solver/solver.hpp rename to src/tongji/modules/solver/solver.hpp diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/modules/state_machine/state_machine.cpp similarity index 100% rename from src/tongji/state_machine/state_machine.cpp rename to src/tongji/modules/state_machine/state_machine.cpp diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/modules/state_machine/state_machine.hpp similarity index 100% rename from src/tongji/state_machine/state_machine.hpp rename to src/tongji/modules/state_machine/state_machine.hpp diff --git a/src/util/coordinate.hpp b/src/tongji/utils/coordinate.hpp similarity index 100% rename from src/util/coordinate.hpp rename to src/tongji/utils/coordinate.hpp diff --git a/src/util/math_tongji.hpp b/src/tongji/utils/math.hpp similarity index 92% rename from src/util/math_tongji.hpp rename to src/tongji/utils/math.hpp index fa2c7c1..5001675 100644 --- a/src/util/math_tongji.hpp +++ b/src/tongji/utils/math.hpp @@ -2,7 +2,7 @@ #include -namespace world_exe::util::math { +namespace world_exe::tongji::utils::math { // [−π,π] 注意边界值 static constexpr double clamp_pm_pi(auto angle) { @@ -122,4 +122,13 @@ static Eigen::Matrix xyz2ypd_jacobian(const Eigen::Vector3d& xyz) return J; } +template T square(T const& a) { return a * a; }; + +static inline double get_abs_angle(const Eigen::Vector2d& vec1, const Eigen::Vector2d& vec2) { + if (vec1.norm() == 0. || vec2.norm() == 0.) { + return 0.; + } + return std::acos(vec1.dot(vec2) / (vec1.norm() * vec2.norm())); //(0~pi) +} + } // namespace rmcs_auto_aim::util::math diff --git a/src/util/math.hpp b/src/util/math.hpp index 6d91a7e..65b7b8b 100644 --- a/src/util/math.hpp +++ b/src/util/math.hpp @@ -3,23 +3,6 @@ #include namespace world_exe::util::math { -static constexpr double ratio(const auto& point) { return atan2(point.y, point.x); } -static constexpr double clamp_pm_pi(auto&& angle) { - while (angle >= std::numbers::pi) - angle -= std::numbers::pi; - while (angle <= -std::numbers::pi) - angle += std::numbers::pi; - - return angle; -} -static constexpr double clamp_pm_tau(auto&& angle) { - while (angle >= 2 * std::numbers::pi) - angle -= 2 * std::numbers::pi; - while (angle <= -2 * std::numbers::pi) - angle += 2 * std::numbers::pi; - - return angle; -} static inline double get_yaw_from_quaternion(const Eigen::Quaterniond& quaternion) { @@ -63,27 +46,6 @@ static inline std::tuple remap( return { distance * std::cos(distance_angle), distance * std::sin(distance_angle) }; } -// zyx order -static Eigen::Matrix3d euler_to_matrix(const Eigen::Vector3d& ypr) { - double roll = ypr[2]; - double pitch = ypr[1]; - double yaw = ypr[0]; - double cos_yaw = cos(yaw); - double sin_yaw = sin(yaw); - double cos_pitch = cos(pitch); - double sin_pitch = sin(pitch); - double cos_roll = cos(roll); - double sin_roll = sin(roll); - // clang-format off - Eigen::Matrix3d R{ - {cos_yaw * cos_pitch, cos_yaw * sin_pitch * sin_roll - sin_yaw * cos_roll, cos_yaw * sin_pitch * cos_roll + sin_yaw * sin_roll}, - {sin_yaw * cos_pitch, sin_yaw * sin_pitch * sin_roll + cos_yaw * cos_roll, sin_yaw * sin_pitch * cos_roll - cos_yaw * sin_roll}, - { -sin_pitch, cos_pitch * sin_roll, cos_pitch * cos_roll} - }; - // clang-format on - return R; -} - static inline Eigen::Quaterniond euler_to_quaternion( const double& yaw_rad, const double& pitch_rad, const double& roll_rad) { Eigen::AngleAxisd rollAngle(roll_rad, Eigen::Vector3d::UnitX()); @@ -94,81 +56,6 @@ static inline Eigen::Quaterniond euler_to_quaternion( return q; } -static Eigen::Vector3d quaternion_to_euler( - Eigen::Quaterniond q, int axis0, int axis1, int axis2, bool extrinsic = false) { - if (!extrinsic) std::swap(axis0, axis2); - - auto i = axis0, j = axis1, k = axis2; - auto is_proper = (i == k); - if (is_proper) k = 3 - i - j; - auto sign = (i - j) * (j - k) * (k - i) / 2; - - double a, b, c, d; - Eigen::Vector4d xyzw = q.coeffs(); - if (is_proper) { - a = xyzw[3]; - b = xyzw[i]; - c = xyzw[j]; - d = xyzw[k] * sign; - } else { - a = xyzw[3] - xyzw[j]; - b = xyzw[i] + xyzw[k] * sign; - c = xyzw[j] + xyzw[3]; - d = xyzw[k] * sign - xyzw[i]; - } - - Eigen::Vector3d eulers; - auto n2 = a * a + b * b + c * c + d * d; - eulers[1] = std::acos(2 * (a * a + b * b) / n2 - 1); - - auto half_sum = std::atan2(b, a); - auto half_diff = std::atan2(-d, c); - - auto eps = 1e-7; - auto safe1 = std::abs(eulers[1]) >= eps; - auto safe2 = std::abs(eulers[1] - std::numbers::pi) >= eps; - auto safe = safe1 && safe2; - if (safe) { - eulers[0] = half_sum + half_diff; - eulers[2] = half_sum - half_diff; - } else { - if (!extrinsic) { - eulers[0] = 0; - if (!safe1) eulers[2] = 2 * half_sum; - if (!safe2) eulers[2] = -2 * half_diff; - } else { - eulers[2] = 0; - if (!safe1) eulers[0] = 2 * half_sum; - if (!safe2) eulers[0] = 2 * half_diff; - } - } - - for (int i = 0; i < 3; i++) - eulers[i] = clamp_pm_pi(eulers[i]); - - if (!is_proper) { - eulers[2] *= sign; - eulers[1] -= std::numbers::pi / 2; - } - - if (!extrinsic) std::swap(eulers[0], eulers[2]); - - return eulers; -} - -static Eigen::Vector3d matrix_to_euler( - Eigen::Matrix3d R, int axis0, int axis1, int axis2, bool extrinsic = false) { - Eigen::Quaterniond q(R); - return quaternion_to_euler(q, axis0, axis1, axis2, extrinsic); -} - -static inline double get_abs_angle(const Eigen::Vector2d& vec1, const Eigen::Vector2d& vec2) { - if (vec1.norm() == 0. || vec2.norm() == 0.) { - return 0.; - } - return std::acos(vec1.dot(vec2) / (vec1.norm() * vec2.norm())); //(0~pi) -} - static inline double get_angle_err_rad_from_quaternion( const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2) { double yaw1 = get_yaw_from_quaternion(q1); @@ -180,7 +67,6 @@ static inline double get_angle_err_rad_from_quaternion( if (yaw_err > std::numbers::pi) yaw_err = 2 * std::numbers::pi - yaw_err; return yaw_err; } - static inline double get_distance_err_rad_from_vector3d( const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) { double d1 = v1.norm(); @@ -189,41 +75,21 @@ static inline double get_distance_err_rad_from_vector3d( return derr; } +static constexpr double ratio(const auto& point) { return atan2(point.y, point.x); } +static constexpr double clamp_pm_pi(auto&& angle) { + while (angle >= std::numbers::pi) + angle -= std::numbers::pi; + while (angle <= -std::numbers::pi) + angle += std::numbers::pi; -static inline Eigen::Vector3d xyz2ypd(const Eigen::Vector3d& xyz) { - auto x = xyz[0], y = xyz[1], z = xyz[2]; - auto yaw = std::atan2(y, x); - auto pitch = std::atan2(z, std::sqrt(x * x + y * y)); - auto distance = std::sqrt(x * x + y * y + z * z); - return { yaw, pitch, distance }; + return angle; } +static constexpr double clamp_pm_tau(auto&& angle) { + while (angle >= 2 * std::numbers::pi) + angle -= 2 * std::numbers::pi; + while (angle <= -2 * std::numbers::pi) + angle += 2 * std::numbers::pi; -static Eigen::Matrix xyz2ypd_jacobian(const Eigen::Vector3d& xyz) { - auto x = xyz[0], y = xyz[1], z = xyz[2]; - - auto dyaw_dx = -y / (x * x + y * y); - auto dyaw_dy = x / (x * x + y * y); - auto dyaw_dz = 0.0; - - auto dpitch_dx = -(x * z) / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 1.5)); - auto dpitch_dy = -(y * z) / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 1.5)); - auto dpitch_dz = 1 / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 0.5)); - - auto ddistance_dx = x / std::pow((x * x + y * y + z * z), 0.5); - auto ddistance_dy = y / std::pow((x * x + y * y + z * z), 0.5); - auto ddistance_dz = z / std::pow((x * x + y * y + z * z), 0.5); - - // clang-format off - Eigen::Matrix J{ - {dyaw_dx, dyaw_dy, dyaw_dz}, - {dpitch_dx, dpitch_dy, dpitch_dz}, - {ddistance_dx, ddistance_dy, ddistance_dz} - }; - // clang-format on - - return J; + return angle; } - -template T square(T const& a) { return a * a; }; - } // namespace rmcs_auto_aim::util::math \ No newline at end of file diff --git a/tests/tongji_tests/utils/coordinate_test.cpp b/tests/tongji_tests/utils/coordinate_test.cpp index ea0139f..df1e8d4 100644 --- a/tests/tongji_tests/utils/coordinate_test.cpp +++ b/tests/tongji_tests/utils/coordinate_test.cpp @@ -1,4 +1,4 @@ -#include "util/coordinate.hpp" +#include "tongji/utils/coordinate.hpp" #include "gtest/gtest.h" #include diff --git a/tests/tongji_tests/utils/math_test.cpp b/tests/tongji_tests/utils/math_test.cpp index 2ac1cfc..5c43d8d 100644 --- a/tests/tongji_tests/utils/math_test.cpp +++ b/tests/tongji_tests/utils/math_test.cpp @@ -1,5 +1,5 @@ -#include "util/math_tongji.hpp" +#include "tongji/utils/math.hpp" #include #include @@ -7,7 +7,7 @@ #include "gtest/gtest.h" #include -namespace math = world_exe::util::math; +namespace math = world_exe::tongji::utils::math; constexpr double TOLERANCE = 1e-6; From c26fbac51e06d4574d8283388c48118bba7c9dea Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sun, 2 Nov 2025 03:34:27 +0800 Subject: [PATCH 81/93] refactor(architecture): extract shared utilities from t v1 mo --- .../modules/identifier/armor_filter.hpp | 4 +- .../modules/identifier/identified_armor.hpp | 10 ++-- src/tongji/modules/identifier/identifier.cpp | 12 ++--- .../car_predictor/car_predictor_manager.cpp | 4 +- .../modules/solver/reprojection_util.hpp | 4 +- src/tongji/modules/solver/solver.cpp | 4 +- src/tongji/utils/coordinate.hpp | 2 +- src/tongji/utils/index.hpp | 49 +++++++++++++++++++ src/{util => tongji/utils}/logger.cpp | 2 +- src/{util => tongji/utils}/logger.hpp | 2 +- src/{util => tongji/utils}/stringifier.hpp | 2 +- src/util/index.hpp | 14 +----- tests/tongji_tests/utils/coordinate_test.cpp | 2 +- 13 files changed, 74 insertions(+), 37 deletions(-) create mode 100644 src/tongji/utils/index.hpp rename src/{util => tongji/utils}/logger.cpp (95%) rename src/{util => tongji/utils}/logger.hpp (70%) rename src/{util => tongji/utils}/stringifier.hpp (95%) diff --git a/src/tongji/modules/identifier/armor_filter.hpp b/src/tongji/modules/identifier/armor_filter.hpp index 21699e0..89a590b 100644 --- a/src/tongji/modules/identifier/armor_filter.hpp +++ b/src/tongji/modules/identifier/armor_filter.hpp @@ -7,7 +7,7 @@ #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" -#include "util/index.hpp" +#include "tongji/utils/index.hpp" namespace world_exe::tongji::identifier { @@ -34,7 +34,7 @@ class ArmorFilter { void Update(enumeration::CarIDFlag ids) { invincible_armor_.clear(); - for (auto id : util::enumeration::ExpandArmorIdFlags(ids)) { + for (auto id : tongji::utils::index::ExpandArmorIdFlags(ids)) { invincible_armor_.emplace(std::move(id)); } } diff --git a/src/tongji/modules/identifier/identified_armor.hpp b/src/tongji/modules/identifier/identified_armor.hpp index 079c0c4..d9d97b0 100644 --- a/src/tongji/modules/identifier/identified_armor.hpp +++ b/src/tongji/modules/identifier/identified_armor.hpp @@ -1,9 +1,9 @@ #pragma once +#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "interfaces/armor_in_image.hpp" -#include "data/time_stamped.hpp" -#include "util/index.hpp" +#include "tongji/utils/index.hpp" namespace world_exe::tongji::identifier { @@ -11,7 +11,7 @@ class IdentifiedArmor final : public interfaces::IArmorInImage { public: explicit IdentifiedArmor(const std::vector& armors) { for (const auto& armor : armors) { - armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); + armors_[utils::index::GetIndex(armor.id)].emplace_back(armor); } } @@ -19,7 +19,7 @@ class IdentifiedArmor final : public interfaces::IArmorInImage { const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const override { - return armors_[util::enumeration::GetIndex(armor_id)]; + return armors_[utils::index::GetIndex(armor_id)]; } static IdentifiedArmor DecorateIArmorInImage(const interfaces::IArmorInImage& armor) { @@ -30,4 +30,4 @@ class IdentifiedArmor final : public interfaces::IArmorInImage { data::TimeStamp time_stamp_ { std::chrono::steady_clock::now().time_since_epoch() }; std::array, 8> armors_; }; -} \ No newline at end of file +} diff --git a/src/tongji/modules/identifier/identifier.cpp b/src/tongji/modules/identifier/identifier.cpp index bf03a41..fafdb1c 100644 --- a/src/tongji/modules/identifier/identifier.cpp +++ b/src/tongji/modules/identifier/identifier.cpp @@ -21,8 +21,8 @@ #include "enum/armor_id.hpp" #include "identified_armor.hpp" #include "interfaces/armor_in_image.hpp" -#include "util/logger.hpp" -#include "util/stringifier.hpp" +#include "tongji/utils/logger.hpp" +#include "tongji/utils/stringifier.hpp" namespace world_exe::tongji::identifier { class Identifier::Impl { @@ -196,7 +196,7 @@ class Identifier::Impl { void Save(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag armor_id) const { auto file_name = fmt::format("{:%Y-%m-%d_%H-%M-%S}", std::chrono::system_clock::now()); auto img_path = fmt::format( - "{}/{}_{}.jpg", save_path_, util::stringifier::ToString(armor_id), file_name); + "{}/{}_{}.jpg", save_path_, utils::stringifier::ToString(armor_id), file_name); cv::imwrite(img_path, armor_pattern); } @@ -297,7 +297,7 @@ class Identifier::Impl { // 出现 5号 则显示 debug 信息。但不过滤。 if (armor_id == enumeration::ArmorIdFlag::InfantryV && debug_) - world_exe::util::logger::logger()->debug("See pattern 5 : InfantryV "); + utils::logger::logger()->debug("See pattern 5 : InfantryV "); return name_ok && confidence_ok; } @@ -311,8 +311,8 @@ class Identifier::Impl { // 保存异常的图案，用于分类器的迭代 if (!name_ok && debug_) { - util::logger::logger()->debug( - "see strange armor: {}", util::stringifier::ToString(armor_id)); + utils::logger::logger()->debug( + "see strange armor: {}", utils::stringifier::ToString(armor_id)); Save(armor_pattern, armor_id); } return name_ok; diff --git a/src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp b/src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp index 458dde0..cd15f4a 100644 --- a/src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp +++ b/src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp @@ -12,8 +12,8 @@ #include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" +#include "tongji/utils/index.hpp" #include "tongji/utils/math.hpp" -#include "util/index.hpp" namespace world_exe::tongji::predictor { @@ -28,7 +28,7 @@ class CarPredictorManager::Impl { const enumeration::ArmorIdFlag& flag, const data::TimeStamp& time_stamp) { std::vector result; - for (const auto& car_id : util::enumeration::ExpandArmorIdFlags(flag)) { + for (const auto& car_id : utils::index::ExpandArmorIdFlags(flag)) { if (const auto it = targets_map_.find(car_id); it != targets_map_.end()) { const auto& [id, predictor] = *it; if (!predictor->IsConverged()) { diff --git a/src/tongji/modules/solver/reprojection_util.hpp b/src/tongji/modules/solver/reprojection_util.hpp index 2cd6638..1bb0592 100644 --- a/src/tongji/modules/solver/reprojection_util.hpp +++ b/src/tongji/modules/solver/reprojection_util.hpp @@ -61,11 +61,11 @@ class ReprojectionUtil { // get R_armor2camera t_armor2camera const Eigen::Vector3d& t_armor2gimbal = armor_xyz_in_gimbal; Eigen::Matrix3d R_armor2camera = R_camera2gimbal.transpose() * R_armor2gimbal; - Eigen::Matrix3d R_armor2camera_cv = util::coordinate::ros2opencv_rotation(R_armor2camera); + Eigen::Matrix3d R_armor2camera_cv = utils::coordinate::ros2opencv_rotation(R_armor2camera); Eigen::Vector3d t_armor2camera = R_camera2gimbal.transpose() * (armor_xyz_in_gimbal) + t_gimbal2camera; - Eigen::Vector3d t_armor2camera_cv = util::coordinate::ros2opencv_position(t_armor2camera); + Eigen::Vector3d t_armor2camera_cv = utils::coordinate::ros2opencv_position(t_armor2camera); // get rvec tvec cv::Vec3d rvec; diff --git a/src/tongji/modules/solver/solver.cpp b/src/tongji/modules/solver/solver.cpp index 957282e..6ffb8fa 100644 --- a/src/tongji/modules/solver/solver.cpp +++ b/src/tongji/modules/solver/solver.cpp @@ -117,14 +117,14 @@ class Solver::Impl { // 1. P_C_cv -> P_C_ros (位置) Eigen::Vector3d xyz_in_camera_cv; cv::cv2eigen(tvec, xyz_in_camera_cv); - const auto xyz_in_camera = util::coordinate::opencv2ros_position(xyz_in_camera_cv); + const auto xyz_in_camera = utils::coordinate::opencv2ros_position(xyz_in_camera_cv); // 2. R_A->C_cv -> R_A->C_ros (姿态) cv::Mat rmat; cv::Rodrigues(rvec, rmat); Eigen::Matrix3d R_armor2camera_cv; cv::cv2eigen(rmat, R_armor2camera_cv); - const auto R_armor2camera = util::coordinate::opencv2ros_rotation(R_armor2camera_cv); + const auto R_armor2camera = utils::coordinate::opencv2ros_rotation(R_armor2camera_cv); return { xyz_in_camera, R_armor2camera }; } diff --git a/src/tongji/utils/coordinate.hpp b/src/tongji/utils/coordinate.hpp index 4f632e0..77015ac 100644 --- a/src/tongji/utils/coordinate.hpp +++ b/src/tongji/utils/coordinate.hpp @@ -2,7 +2,7 @@ #include -namespace world_exe::util::coordinate { +namespace world_exe::tongji::utils::coordinate { static inline Eigen::Vector3d opencv2ros_position(const Eigen::Vector3d& position) { return Eigen::Vector3d(position.z(), -position.x(), -position.y()); diff --git a/src/tongji/utils/index.hpp b/src/tongji/utils/index.hpp new file mode 100644 index 0000000..c328ebb --- /dev/null +++ b/src/tongji/utils/index.hpp @@ -0,0 +1,49 @@ +#pragma once +#define BACKWARD_HAS_DW 1 + +#include "enum/armor_id.hpp" +#include +#include + +namespace world_exe::tongji::utils::index { + +static std::vector ExpandArmorIdFlags( + ::world_exe::enumeration::ArmorIdFlag flags) { + std::vector<::world_exe::enumeration::ArmorIdFlag> result; + for (int i = 0; i < static_cast(::world_exe::enumeration::ArmorIdFlag::Count); ++i) { + auto single = static_cast<::world_exe::enumeration::ArmorIdFlag>( + static_cast(::world_exe::enumeration::ArmorIdFlag::Hero) << i); + if ((static_cast(flags) & static_cast(single)) != 0) { + result.push_back(single); + } + } + return result; +} + +static inline int GetIndex(const world_exe::enumeration::ArmorIdFlag& flag) { + const auto value = static_cast(flag); + if (value != 0 && (value & (value - 1)) == 0) return std::countr_zero(value); + else { + + // auto st = backward::StackTrace(); + // st.load_here(4); + // backward::Printer ptr; + // ptr.object = false; + // ptr.color_mode = backward::ColorMode::always; + // ptr.address = false; + // ptr.snippet = false; + // ptr.print(st, stderr); + throw std::runtime_error("Invalid ArmorIdFlag value: " + std::to_string(value)); + } +} + +static const world_exe::enumeration::ArmorIdFlag GetArmorIdFlag(int index) { + if (index >= 0 && index < static_cast(world_exe::enumeration::ArmorIdFlag::Count)) { + uint32_t value = 1U << index; + return static_cast(value); + } else { + throw std::out_of_range("Invalid index for ArmorIdFlag conversion."); + } +} + +} \ No newline at end of file diff --git a/src/util/logger.cpp b/src/tongji/utils/logger.cpp similarity index 95% rename from src/util/logger.cpp rename to src/tongji/utils/logger.cpp index 43e45e2..f0ce339 100644 --- a/src/util/logger.cpp +++ b/src/tongji/utils/logger.cpp @@ -7,7 +7,7 @@ #include #include -namespace world_exe::util::logger { +namespace world_exe::tongji::utils::logger { std::shared_ptr logger_ = nullptr; void set_logger() { diff --git a/src/util/logger.hpp b/src/tongji/utils/logger.hpp similarity index 70% rename from src/util/logger.hpp rename to src/tongji/utils/logger.hpp index 801db97..24706ec 100644 --- a/src/util/logger.hpp +++ b/src/tongji/utils/logger.hpp @@ -2,7 +2,7 @@ #include -namespace world_exe::util::logger { +namespace world_exe::tongji::utils::logger { std::shared_ptr logger(); } // namespace logger diff --git a/src/util/stringifier.hpp b/src/tongji/utils/stringifier.hpp similarity index 95% rename from src/util/stringifier.hpp rename to src/tongji/utils/stringifier.hpp index 0fe4974..6916248 100644 --- a/src/util/stringifier.hpp +++ b/src/tongji/utils/stringifier.hpp @@ -4,7 +4,7 @@ #include "enum/armor_id.hpp" -namespace world_exe::util::stringifier { +namespace world_exe::tongji::utils::stringifier { static inline std::string ToString(const world_exe::enumeration::ArmorIdFlag& id) { switch (id) { diff --git a/src/util/index.hpp b/src/util/index.hpp index 7a300a8..6e8bdef 100644 --- a/src/util/index.hpp +++ b/src/util/index.hpp @@ -1,6 +1,5 @@ #pragma once -#include #define BACKWARD_HAS_DW 1 // #include "backward.hpp" #include "enum/armor_id.hpp" @@ -33,17 +32,6 @@ static const world_exe::enumeration::ArmorIdFlag GetArmorIdFlag(int index) { } } -static std::vector<::world_exe::enumeration::ArmorIdFlag> ExpandArmorIdFlags( - ::world_exe::enumeration::ArmorIdFlag flags) { - std::vector<::world_exe::enumeration::ArmorIdFlag> result; - for (int i = 0; i < static_cast(::world_exe::enumeration::ArmorIdFlag::Count); ++i) { - auto single = static_cast<::world_exe::enumeration::ArmorIdFlag>( - static_cast(::world_exe::enumeration::ArmorIdFlag::Hero) << i); - if ((static_cast(flags) & static_cast(single)) != 0) { - result.push_back(single); - } - } - return result; -} + } \ No newline at end of file diff --git a/tests/tongji_tests/utils/coordinate_test.cpp b/tests/tongji_tests/utils/coordinate_test.cpp index df1e8d4..371cdc6 100644 --- a/tests/tongji_tests/utils/coordinate_test.cpp +++ b/tests/tongji_tests/utils/coordinate_test.cpp @@ -3,7 +3,7 @@ #include "gtest/gtest.h" #include -namespace coord = world_exe::util::coordinate; +namespace coord = world_exe::tongji::utils::coordinate; TEST(CoordinateTest, Opencv2RosPosition_SimpleAxis) { // 1. 准备 (Arrange) - 在 OpenCV 坐标系下，沿着每个轴单位移动 From 372885d2b0d572f1bfc34852db8400e3c2f1faef Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 3 Nov 2025 05:03:14 +0800 Subject: [PATCH 82/93] feat(planning): add tiny_mpc library for trajectory planning --- .../modules/planner/tinympc/CMakeLists.txt | 43 ++ src/tongji/modules/planner/tinympc/admm.cpp | 391 ++++++++++++++ src/tongji/modules/planner/tinympc/admm.hpp | 37 ++ .../modules/planner/tinympc/codegen.cpp | 447 ++++++++++++++++ .../modules/planner/tinympc/codegen.hpp | 23 + src/tongji/modules/planner/tinympc/error.hpp | 29 ++ .../modules/planner/tinympc/rho_benchmark.cpp | 250 +++++++++ .../modules/planner/tinympc/rho_benchmark.hpp | 95 ++++ .../modules/planner/tinympc/tiny_api.cpp | 476 ++++++++++++++++++ .../modules/planner/tinympc/tiny_api.hpp | 62 +++ .../planner/tinympc/tiny_api_constants.hpp | 14 + src/tongji/modules/planner/tinympc/types.hpp | 204 ++++++++ 12 files changed, 2071 insertions(+) create mode 100755 src/tongji/modules/planner/tinympc/CMakeLists.txt create mode 100755 src/tongji/modules/planner/tinympc/admm.cpp create mode 100755 src/tongji/modules/planner/tinympc/admm.hpp create mode 100755 src/tongji/modules/planner/tinympc/codegen.cpp create mode 100755 src/tongji/modules/planner/tinympc/codegen.hpp create mode 100644 src/tongji/modules/planner/tinympc/error.hpp create mode 100644 src/tongji/modules/planner/tinympc/rho_benchmark.cpp create mode 100644 src/tongji/modules/planner/tinympc/rho_benchmark.hpp create mode 100755 src/tongji/modules/planner/tinympc/tiny_api.cpp create mode 100755 src/tongji/modules/planner/tinympc/tiny_api.hpp create mode 100644 src/tongji/modules/planner/tinympc/tiny_api_constants.hpp create mode 100755 src/tongji/modules/planner/tinympc/types.hpp diff --git a/src/tongji/modules/planner/tinympc/CMakeLists.txt b/src/tongji/modules/planner/tinympc/CMakeLists.txt new file mode 100755 index 0000000..b8654af --- /dev/null +++ b/src/tongji/modules/planner/tinympc/CMakeLists.txt @@ -0,0 +1,43 @@ +add_library(tinympcstatic STATIC + admm.cpp + tiny_api.cpp + codegen.cpp + rho_benchmark.cpp +) + +set_property(TARGET tinympcstatic PROPERTY POSITION_INDEPENDENT_CODE ON) + +# target_link_libraries(tinympcstatic PUBLIC Eigen) +target_include_directories(tinympcstatic PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/..) +target_include_directories(tinympcstatic PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../../include/Eigen) + + + +if(USING_CODEGEN) # Defined in top-level CMakeLists.txt + +# Files that are needed for embedded code generation +list( APPEND EMBEDDED_FILES + "${CMAKE_CURRENT_SOURCE_DIR}/admm.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/admm.hpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tiny_api.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tiny_api.hpp" + "${CMAKE_CURRENT_SOURCE_DIR}/types.hpp" + "${CMAKE_CURRENT_SOURCE_DIR}/tiny_api_constants.hpp" ) + + +foreach( f ${EMBEDDED_FILES} ) + get_filename_component( fname ${f} NAME ) + + set( dest_file "${EMBEDDED_BUILD_TINYMPC_DIR}/${fname}" ) + list( APPEND EMBEDDED_BUILD_TINYMPC_FILES "${dest_file}" ) + + add_custom_command(OUTPUT ${dest_file} + COMMAND ${CMAKE_COMMAND} -E copy "${f}" "${dest_file}" + DEPENDS ${f} + COMMENT "Copying ${fname}") +endforeach() + +add_custom_target( copy_codegen_tinympc_files DEPENDS ${EMBEDDED_BUILD_TINYMPC_FILES} ) +add_dependencies( copy_codegen_files copy_codegen_tinympc_files ) + +endif(USING_CODEGEN) diff --git a/src/tongji/modules/planner/tinympc/admm.cpp b/src/tongji/modules/planner/tinympc/admm.cpp new file mode 100755 index 0000000..7051a2b --- /dev/null +++ b/src/tongji/modules/planner/tinympc/admm.cpp @@ -0,0 +1,391 @@ +#include + +#include "admm.hpp" +#include "rho_benchmark.hpp" + +#define DEBUG_MODULE "TINYALG" + +extern "C" { + +/** + * Update linear terms from Riccati backward pass +*/ +void backward_pass_grad(TinySolver *solver) +{ + for (int i = solver->work->N - 2; i >= 0; i--) + { + (solver->work->d.col(i)).noalias() = solver->cache->Quu_inv * (solver->work->Bdyn.transpose() * solver->work->p.col(i + 1) + solver->work->r.col(i) + solver->cache->BPf); + (solver->work->p.col(i)).noalias() = solver->work->q.col(i) + solver->cache->AmBKt.lazyProduct(solver->work->p.col(i + 1)) - (solver->cache->Kinf.transpose()).lazyProduct(solver->work->r.col(i)) + solver->cache->APf; + } +} + +/** + * Use LQR feedback policy to roll out trajectory +*/ +void forward_pass(TinySolver *solver) +{ + for (int i = 0; i < solver->work->N - 1; i++) + { + (solver->work->u.col(i)).noalias() = -solver->cache->Kinf.lazyProduct(solver->work->x.col(i)) - solver->work->d.col(i); + (solver->work->x.col(i + 1)).noalias() = solver->work->Adyn.lazyProduct(solver->work->x.col(i)) + solver->work->Bdyn.lazyProduct(solver->work->u.col(i)) + solver->work->fdyn; + } +} + +/** + * Project a vector s onto the second order cone defined by mu + * @param s, mu + * @return projection onto cone if s is outside cone. Return s if s is inside cone. +*/ +tinyVector project_soc(tinyVector s, float mu) { + tinytype u0 = s(Eigen::placeholders::last) * mu; + tinyVector u1 = s.head(s.rows()-1); + float a = u1.norm(); + tinyVector cone_origin(s.rows()); + cone_origin.setZero(); + + if (a <= -u0) { // below cone + return cone_origin; + } + else if (a <= u0) { // in cone + return s; + } + else if (a >= abs(u0)) { // outside cone + Matrix u2(u1.size() + 1); + u2 << u1, a/mu; + return 0.5 * (1 + u0/a) * u2; + } + else { + return cone_origin; + } +} + +/** + * Project a vector z onto a hyperplane defined by a^T z = b + * Implements equation (21): ΠH(z) = z - (⟨z, a⟩ − b)/||a||² * a + * @param z Vector to project + * @param a Normal vector of the hyperplane + * @param b Offset of the hyperplane + * @return Projection of z onto the hyperplane + */ +tinyVector project_hyperplane(const tinyVector& z, const tinyVector& a, tinytype b) { + tinytype dist = (a.dot(z) - b) / a.squaredNorm(); + return z - dist * a; +} + +/** + * Project slack (auxiliary) variables into their feasible domain, defined by + * projection functions related to each constraint + * TODO: pass in meta information with each constraint assigning it to a + * projection function +*/ +void update_slack(TinySolver *solver) +{ + + // Update bound constraint slack variables for state + solver->work->vnew = solver->work->x + solver->work->g; + + // Update bound constraint slack variables for input + solver->work->znew = solver->work->u + solver->work->y; + + // Box constraints on state + if (solver->settings->en_state_bound) { + solver->work->vnew = solver->work->x_max.cwiseMin(solver->work->x_min.cwiseMax(solver->work->vnew)); + } + + // Box constraints on input + if (solver->settings->en_input_bound) { + solver->work->znew = solver->work->u_max.cwiseMin(solver->work->u_min.cwiseMax(solver->work->znew)); + } + + + // Update second order cone slack variables for state + if (solver->settings->en_state_soc && solver->work->numStateCones > 0) { + solver->work->vcnew = solver->work->x + solver->work->gc; + } + + // Update second order cone slack variables for input + if (solver->settings->en_input_soc && solver->work->numInputCones > 0) { + solver->work->zcnew = solver->work->u + solver->work->yc; + } + + // Cone constraints on state + if (solver->settings->en_state_soc) { + for (int i=0; iwork->N; i++) { + for (int k=0; kwork->numStateCones; k++) { + int start = solver->work->Acx(k); + int num_xs = solver->work->qcx(k); + tinytype mu = solver->work->cx(k); + tinyVector col = solver->work->vcnew.block(start, i, num_xs, 1); + solver->work->vcnew.block(start, i, num_xs, 1) = project_soc(col, mu); + } + } + } + + // Cone constraints on input + if (solver->settings->en_input_soc) { + for (int i=0; iwork->N-1; i++) { + for (int k=0; kwork->numInputCones; k++) { + int start = solver->work->Acu(k); + int num_us = solver->work->qcu(k); + tinytype mu = solver->work->cu(k); + tinyVector col = solver->work->zcnew.block(start, i, num_us, 1); + solver->work->zcnew.block(start, i, num_us, 1) = project_soc(col, mu); + } + } + } + + // Update linear constraint slack variables for state + if (solver->settings->en_state_linear) { + solver->work->vlnew = solver->work->x + solver->work->gl; + } + + // Update linear constraint slack variables for input + if (solver->settings->en_input_linear) { + solver->work->zlnew = solver->work->u + solver->work->yl; + } + + // Linear constraints on state + if (solver->settings->en_state_linear) { + for (int i=0; iwork->N; i++) { + for (int k=0; kwork->numStateLinear; k++) { + tinyVector a = solver->work->Alin_x.row(k); + tinytype b = solver->work->blin_x(k); + tinytype constraint_value = a.dot(solver->work->vlnew.col(i)); + if (constraint_value > b) { // Only project if constraint is violated + solver->work->vlnew.col(i) = project_hyperplane(solver->work->vlnew.col(i), a, b); + } + } + } + } + + // Linear constraints on input + if (solver->settings->en_input_linear) { + for (int i=0; iwork->N-1; i++) { + for (int k=0; kwork->numInputLinear; k++) { + tinyVector a = solver->work->Alin_u.row(k); + tinytype b = solver->work->blin_u(k); + tinytype constraint_value = a.dot(solver->work->zlnew.col(i)); + if (constraint_value > b) { // Only project if constraint is violated + solver->work->zlnew.col(i) = project_hyperplane(solver->work->zlnew.col(i), a, b); + } + } + } + } + +} + +/** + * Update next iteration of dual variables by performing the augmented + * lagrangian multiplier update +*/ +void update_dual(TinySolver *solver) +{ + // Update bound constraint dual variables for state + solver->work->g = solver->work->g + solver->work->x - solver->work->vnew; + + // Update bound constraint dual variables for input + solver->work->y = solver->work->y + solver->work->u - solver->work->znew; + + // Update second order cone dual variables for state + if (solver->settings->en_state_soc && solver->work->numStateCones > 0) { + solver->work->gc = solver->work->gc + solver->work->x - solver->work->vcnew; + } + + // Update second order cone dual variables for input + if (solver->settings->en_input_soc && solver->work->numInputCones > 0) { + solver->work->yc = solver->work->yc + solver->work->u - solver->work->zcnew; + } + + // Update linear constraint dual variables for state + if (solver->settings->en_state_linear) { + solver->work->gl = solver->work->gl + solver->work->x - solver->work->vlnew; + } + + // Update linear constraint dual variables for input + if (solver->settings->en_input_linear) { + solver->work->yl = solver->work->yl + solver->work->u - solver->work->zlnew; + } +} + +/** + * Update linear control cost terms in the Riccati feedback using the changing + * slack and dual variables from ADMM +*/ +void update_linear_cost(TinySolver *solver) +{ + + // Update state cost terms + solver->work->q = -(solver->work->Xref.array().colwise() * solver->work->Q.array()); + (solver->work->q).noalias() -= solver->cache->rho * (solver->work->vnew - solver->work->g); + if (solver->settings->en_state_soc && solver->work->numStateCones > 0) { + (solver->work->q).noalias() -= solver->cache->rho * (solver->work->vcnew - solver->work->gc); + } + if (solver->settings->en_state_linear) { + (solver->work->q).noalias() -= solver->cache->rho * (solver->work->vlnew - solver->work->gl); + } + + // Update input cost terms + solver->work->r = -(solver->work->Uref.array().colwise() * solver->work->R.array()); + (solver->work->r).noalias() -= solver->cache->rho * (solver->work->znew - solver->work->y); + if (solver->settings->en_input_soc && solver->work->numInputCones > 0) { + (solver->work->r).noalias() -= solver->cache->rho * (solver->work->zcnew - solver->work->yc); + } + if (solver->settings->en_input_linear) { + (solver->work->r).noalias() -= solver->cache->rho * (solver->work->zlnew - solver->work->yl); + } + + // Update terminal cost + solver->work->p.col(solver->work->N - 1) = -(solver->work->Xref.col(solver->work->N - 1).transpose().lazyProduct(solver->cache->Pinf)); + (solver->work->p.col(solver->work->N - 1)).noalias() -= solver->cache->rho * (solver->work->vnew.col(solver->work->N - 1) - solver->work->g.col(solver->work->N - 1)); + + if (solver->settings->en_state_soc && solver->work->numStateCones > 0) { + solver->work->p.col(solver->work->N - 1) -= solver->cache->rho * (solver->work->vcnew.col(solver->work->N - 1) - solver->work->gc.col(solver->work->N - 1)); + } + if (solver->settings->en_state_linear) { + solver->work->p.col(solver->work->N - 1) -= solver->cache->rho * (solver->work->vlnew.col(solver->work->N - 1) - solver->work->gl.col(solver->work->N - 1)); + } +} + +/** + * Check for termination condition by evaluating whether the largest absolute + * primal and dual residuals for states and inputs are below threhold. +*/ +bool termination_condition(TinySolver *solver) +{ + if (solver->work->iter % solver->settings->check_termination == 0) + { + solver->work->primal_residual_state = (solver->work->x - solver->work->vnew).cwiseAbs().maxCoeff(); + solver->work->dual_residual_state = ((solver->work->v - solver->work->vnew).cwiseAbs().maxCoeff()) * solver->cache->rho; + solver->work->primal_residual_input = (solver->work->u - solver->work->znew).cwiseAbs().maxCoeff(); + solver->work->dual_residual_input = ((solver->work->z - solver->work->znew).cwiseAbs().maxCoeff()) * solver->cache->rho; + + if (solver->work->primal_residual_state < solver->settings->abs_pri_tol && + solver->work->primal_residual_input < solver->settings->abs_pri_tol && + solver->work->dual_residual_state < solver->settings->abs_dua_tol && + solver->work->dual_residual_input < solver->settings->abs_dua_tol) + { + return true; + } + } + return false; +} + + +int solve(TinySolver *solver) +{ + // Initialize variables + solver->solution->solved = 0; + solver->solution->iter = 0; + solver->work->status = 11; // TINY_UNSOLVED + solver->work->iter = 0; + + // Setup for adaptive rho + RhoAdapter adapter; + adapter.rho_min = solver->settings->adaptive_rho_min; + adapter.rho_max = solver->settings->adaptive_rho_max; + adapter.clip = solver->settings->adaptive_rho_enable_clipping; + + RhoBenchmarkResult rho_result; + + // Store previous values for residuals + tinyMatrix v_prev = solver->work->vnew; + tinyMatrix z_prev = solver->work->znew; + + // Initialize SOC slack variables if needed + if (solver->settings->en_state_soc && solver->work->numStateCones > 0) { + solver->work->vcnew = solver->work->x; + } + + if (solver->settings->en_input_soc && solver->work->numInputCones > 0) { + solver->work->zcnew = solver->work->u; + } + + // Initialize linear constraint slack variables if needed + if (solver->settings->en_state_linear) { + solver->work->vlnew = solver->work->x; + } + + if (solver->settings->en_input_linear) { + solver->work->zlnew = solver->work->u; + } + + for (int i = 0; i < solver->settings->max_iter; i++) + { + // Solve linear system with Riccati and roll out to get new trajectory + backward_pass_grad(solver); + + forward_pass(solver); + + // Project slack variables into feasible domain + update_slack(solver); + + // Compute next iteration of dual variables + update_dual(solver); + + // Update linear control cost terms using reference trajectory, duals, and slack variables + update_linear_cost(solver); + + solver->work->iter += 1; + + // Handle adaptive rho if enabled + if (solver->settings->adaptive_rho) { + // Calculate residuals for adaptive rho + tinytype pri_res_input = (solver->work->u - solver->work->znew).cwiseAbs().maxCoeff(); + tinytype pri_res_state = (solver->work->x - solver->work->vnew).cwiseAbs().maxCoeff(); + tinytype dua_res_input = solver->cache->rho * (solver->work->znew - z_prev).cwiseAbs().maxCoeff(); + tinytype dua_res_state = solver->cache->rho * (solver->work->vnew - v_prev).cwiseAbs().maxCoeff(); + + // Update rho every 5 iterations + if (i > 0 && i % 5 == 0) { + benchmark_rho_adaptation( + &adapter, + solver->work->x, + solver->work->u, + solver->work->vnew, + solver->work->znew, + solver->work->g, + solver->work->y, + solver->cache, + solver->work, + solver->work->N, + &rho_result + ); + + // Update matrices using Taylor expansion + update_matrices_with_derivatives(solver->cache, rho_result.final_rho); + } + } + + // Store previous values for next iteration + z_prev = solver->work->znew; + v_prev = solver->work->vnew; + + // Check for whether cost is minimized by calculating residuals + if (termination_condition(solver)) { + solver->work->status = 1; // TINY_SOLVED + + // Save solution + solver->solution->iter = solver->work->iter; + solver->solution->solved = 1; + solver->solution->x = solver->work->vnew; + solver->solution->u = solver->work->znew; + + // std::cout << "Solver converged in " << solver->work->iter << " iterations" << std::endl; + + return 0; + } + + // Save previous slack variables + solver->work->v = solver->work->vnew; + solver->work->z = solver->work->znew; + + } + + solver->solution->iter = solver->work->iter; + solver->solution->solved = 0; + solver->solution->x = solver->work->vnew; + solver->solution->u = solver->work->znew; + return 1; +} + +} /* extern "C" */ diff --git a/src/tongji/modules/planner/tinympc/admm.hpp b/src/tongji/modules/planner/tinympc/admm.hpp new file mode 100755 index 0000000..f07536d --- /dev/null +++ b/src/tongji/modules/planner/tinympc/admm.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include "types.hpp" + +#ifdef __cplusplus +extern "C" { +#endif + +int solve(TinySolver *solver); + +void update_primal(TinySolver *solver); +void backward_pass_grad(TinySolver *solver); +void forward_pass(TinySolver *solver); +void update_slack(TinySolver *solver); +void update_dual(TinySolver *solver); +void update_linear_cost(TinySolver *solver); +bool termination_condition(TinySolver *solver); + +/** + * Project a vector s onto the second order cone defined by mu + * @param s, mu + * @return projection onto cone if s is outside cone. Return s if s is inside cone. +*/ +tinyVector project_soc(tinyVector s, float mu); + +/** + * Project a vector z onto a hyperplane defined by a^T z = b + * Implements equation (21): ΠH(z) = z - (⟨z, a⟩ − b)/||a||² * a + * @param z Vector to project + * @param a Normal vector of the hyperplane + * @param b Offset of the hyperplane + * @return Projection of z onto the hyperplane + */ +tinyVector project_hyperplane(const tinyVector& z, const tinyVector& a, tinytype b); +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/codegen.cpp b/src/tongji/modules/planner/tinympc/codegen.cpp new file mode 100755 index 0000000..cdc91f3 --- /dev/null +++ b/src/tongji/modules/planner/tinympc/codegen.cpp @@ -0,0 +1,447 @@ +#include +#include +#include +#include +#include +#include +#include +//#include +#include "error.hpp" + +#include +#include + +// #include "types.hpp" +#include "codegen.hpp" + +#ifdef __MINGW32__ +#include +inline int mkdir(const char *pathname, int flags) { + return _mkdir(pathname); +} +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +/* Define the maximum allowed length of the path (directory + filename + extension) */ +#define PATH_LENGTH 2048 + +using namespace Eigen; + +static void print_matrix(FILE *f, MatrixXd mat, int num_elements) +{ + // Check if matrix is uninitialized or too small + if (mat.size() == 0 || mat.size() < num_elements) { + // Print zeros for all elements + for (int i = 0; i < num_elements; i++) { + fprintf(f, "(tinytype)0.0000000000000000"); + if (i < num_elements - 1) + fprintf(f, ","); + } + return; + } + + // Matrix is properly initialized and has enough elements + for (int i = 0; i < num_elements; i++) { + fprintf(f, "(tinytype)%.16f", mat.reshaped()[i]); + if (i < num_elements - 1) + fprintf(f, ","); + } +} + + +static void create_directory(const char* dir, int verbose) { + // Attempt to create directory + if (mkdir(dir, S_IRWXU|S_IRWXG|S_IROTH)) { + if (errno == EEXIST) { // Skip if directory already exists + if (verbose) + std::cout << dir << " already exists, skipping." << std::endl; + } else { + ERROR_MSG(EXIT_FAILURE, "Failed to create directory %s", dir); + } + } +} + +// TODO: Make this fail if tiny_setup has not already been called +int tiny_codegen(TinySolver* solver, const char* output_dir, int verbose) { + if (!solver) { + std::cout << "Error in tiny_codegen: solver is nullptr" << std::endl; + return 1; + } + int status = 0; + status |= codegen_create_directories(output_dir, verbose); + status |= codegen_data_header(output_dir, verbose); + status |= codegen_data_source(solver, output_dir, verbose); + status |= codegen_example(output_dir, verbose); + + return status; +} + +int tiny_codegen_with_sensitivity(TinySolver* solver, const char* output_dir, + tinyMatrix* dK, tinyMatrix* dP, + tinyMatrix* dC1, tinyMatrix* dC2, int verbose) { + if (!solver) { + std::cout << "Error in tiny_codegen_with_sensitivity: solver is nullptr" << std::endl; + return 1; + } + + // Only store sensitivity matrices if adaptive rho is enabled + if (solver->settings->adaptive_rho) { + // Store the sensitivity matrices in the solver's cache + solver->cache->dKinf_drho = *dK; + solver->cache->dPinf_drho = *dP; + solver->cache->dC1_drho = *dC1; + solver->cache->dC2_drho = *dC2; + } + + // Call the regular codegen function which will now include the sensitivity matrices if adaptive_rho is enabled + return tiny_codegen(solver, output_dir, verbose); +} + +// Create code generation folder structure in whichever directory the executable calling tiny_codegen was called +int codegen_create_directories(const char* output_dir, int verbose) { + + // Create output folder (root folder for code generation) + create_directory(output_dir, verbose); + + // Create src folder + char src_dir[PATH_LENGTH]; + sprintf(src_dir, "%s/src/", output_dir); + create_directory(src_dir, verbose); + + // Create tinympc folder + char tinympc_dir[PATH_LENGTH]; + sprintf(tinympc_dir, "%s/tinympc/", output_dir); + create_directory(tinympc_dir, verbose); + + // // Create include folder + // char inc_dir[PATH_LENGTH]; + // sprintf(inc_dir, "%s/include/", output_dir); + // create_directory(inc_dir, verbose); + + return EXIT_SUCCESS; +} + +// Create inc/tiny_data.hpp file +int codegen_data_header(const char* output_dir, int verbose) { + char data_hpp_fname[PATH_LENGTH]; + FILE *data_hpp_f; + + sprintf(data_hpp_fname, "%s/tinympc/tiny_data.hpp", output_dir); + + // Open data header file + data_hpp_f = fopen(data_hpp_fname, "w+"); + if (data_hpp_f == NULL) + ERROR_MSG(EXIT_FAILURE, "Failed to open file %s", data_hpp_fname); + + // Preamble + time_t start_time; + time(&start_time); + fprintf(data_hpp_f, "/*\n"); + fprintf(data_hpp_f, " * This file was autogenerated by TinyMPC on %s", ctime(&start_time)); + fprintf(data_hpp_f, " */\n\n"); + + fprintf(data_hpp_f, "#pragma once\n\n"); + + fprintf(data_hpp_f, "#include \"types.hpp\"\n\n"); + + fprintf(data_hpp_f, "#ifdef __cplusplus\n"); + fprintf(data_hpp_f, "extern \"C\" {\n"); + fprintf(data_hpp_f, "#endif\n\n"); + + fprintf(data_hpp_f, "extern TinySolver tiny_solver;\n\n"); + + fprintf(data_hpp_f, "#ifdef __cplusplus\n"); + fprintf(data_hpp_f, "}\n"); + fprintf(data_hpp_f, "#endif\n"); + + // Close codegen data header file + fclose(data_hpp_f); + + if (verbose) { + printf("Data header generated in %s\n", data_hpp_fname); + } + return 0; +} + +// Create src/tiny_data.cpp file +int codegen_data_source(TinySolver* solver, const char* output_dir, int verbose) { + char data_cpp_fname[PATH_LENGTH]; + FILE *data_cpp_f; + + int nx = solver->work->nx; + int nu = solver->work->nu; + int N = solver->work->N; + + sprintf(data_cpp_fname, "%s/src/tiny_data.cpp", output_dir); + + // Open data source file + data_cpp_f = fopen(data_cpp_fname, "w+"); + if (data_cpp_f == NULL) + ERROR_MSG(EXIT_FAILURE, "Failed to open file %s", data_cpp_fname); + + // Preamble + time_t start_time; + time(&start_time); + fprintf(data_cpp_f, "/*\n"); + fprintf(data_cpp_f, " * This file was autogenerated by TinyMPC on %s", ctime(&start_time)); + fprintf(data_cpp_f, " */\n\n"); + + // Open extern C + fprintf(data_cpp_f, "#include \"tinympc/tiny_data.hpp\"\n\n"); + fprintf(data_cpp_f, "#ifdef __cplusplus\n"); + fprintf(data_cpp_f, "extern \"C\" {\n"); + fprintf(data_cpp_f, "#endif\n\n"); + + // Solution + fprintf(data_cpp_f, "/* Solution */\n"); + fprintf(data_cpp_f, "TinySolution solution = {\n"); + + fprintf(data_cpp_f, "\t%d,\t\t// iter\n", solver->solution->iter); + fprintf(data_cpp_f, "\t%d,\t\t// solved\n", solver->solution->solved); + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); + print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); + fprintf(data_cpp_f, ").finished(),\t// x\n"); // x solution + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); + print_matrix(data_cpp_f, MatrixXd::Zero(nu, N-1), nu * (N-1)); + fprintf(data_cpp_f, ").finished(),\t// x\n"); // u solution + + fprintf(data_cpp_f, "};\n\n"); + + // Cache + fprintf(data_cpp_f, "/* Matrices that must be recomputed with changes in time step, rho */\n"); + fprintf(data_cpp_f, "TinyCache cache = {\n"); + + fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// rho (step size/penalty)\n", solver->cache->rho); + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, nx); + print_matrix(data_cpp_f, solver->cache->Kinf, nu * nx); + fprintf(data_cpp_f, ").finished(),\t// Kinf\n"); // Kinf + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); + print_matrix(data_cpp_f, solver->cache->Pinf, nx * nx); + fprintf(data_cpp_f, ").finished(),\t// Pinf\n"); // Pinf + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, nu); + print_matrix(data_cpp_f, solver->cache->Quu_inv, nu * nu); + fprintf(data_cpp_f, ").finished(),\t// Quu_inv\n"); // Quu_inv + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); + print_matrix(data_cpp_f, solver->cache->AmBKt, nx * nx); + fprintf(data_cpp_f, ").finished(),\t// AmBKt\n"); // AmBKt + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); + print_matrix(data_cpp_f, solver->cache->C1, nx * nx); + fprintf(data_cpp_f, ").finished(),\t// C1\n"); // C1 + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); + print_matrix(data_cpp_f, solver->cache->C2, nx * nx); + fprintf(data_cpp_f, ").finished()"); // C2, no comma if no sensitivity matrices + + // Only print sensitivity matrices if adaptive rho is enabled + if (solver->settings->adaptive_rho) { + fprintf(data_cpp_f, ",\t// C2\n"); // Add comma and comment for C2 if we have more matrices + + // Add sensitivity matrices within the struct initialization + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, nx); + print_matrix(data_cpp_f, solver->cache->dKinf_drho, nu * nx); + fprintf(data_cpp_f, ").finished(),\t// dKinf_drho\n"); // dKinf_drho + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); + print_matrix(data_cpp_f, solver->cache->dPinf_drho, nx * nx); + fprintf(data_cpp_f, ").finished(),\t// dPinf_drho\n"); // dPinf_drho + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); + print_matrix(data_cpp_f, solver->cache->dC1_drho, nx * nx); + fprintf(data_cpp_f, ").finished(),\t// dC1_drho\n"); // dC1_drho + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); + print_matrix(data_cpp_f, solver->cache->dC2_drho, nx * nx); + fprintf(data_cpp_f, ").finished()\t// dC2_drho\n"); // dC2_drho + } else { + fprintf(data_cpp_f, "\t// C2\n"); // Just add comment for C2 + } + + fprintf(data_cpp_f, "};\n\n"); + + // Settings + fprintf(data_cpp_f, "/* User settings */\n"); + fprintf(data_cpp_f, "TinySettings settings = {\n"); + + fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// primal tolerance\n", solver->settings->abs_pri_tol); + fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// dual tolerance\n", solver->settings->abs_dua_tol); + fprintf(data_cpp_f, "\t%d,\t\t// max iterations\n", solver->settings->max_iter); + fprintf(data_cpp_f, "\t%d,\t\t// iterations per termination check\n", solver->settings->check_termination); + fprintf(data_cpp_f, "\t%d,\t\t// enable state constraints\n", solver->settings->en_state_bound); + fprintf(data_cpp_f, "\t%d\t\t// enable input constraints\n", solver->settings->en_input_bound); + + fprintf(data_cpp_f, "};\n\n"); + + // Workspace + fprintf(data_cpp_f, "/* Problem variables */\n"); + fprintf(data_cpp_f, "TinyWorkspace work = {\n"); + + fprintf(data_cpp_f, "\t%d,\t// Number of states\n", nx); + fprintf(data_cpp_f, "\t%d,\t// Number of control inputs\n", nu); + fprintf(data_cpp_f, "\t%d,\t// Number of knotpoints in the horizon\n", N); + + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); + print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); + fprintf(data_cpp_f, ").finished(),\t// x\n"); // x + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); + print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); + fprintf(data_cpp_f, ").finished(),\t// u\n"); // u + + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); + print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); + fprintf(data_cpp_f, ").finished(),\t// q\n"); // q + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); + print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); + fprintf(data_cpp_f, ").finished(),\t// r\n"); // r + + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); + print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); + fprintf(data_cpp_f, ").finished(),\t// p\n"); // p + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); + print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); + fprintf(data_cpp_f, ").finished(),\t// d\n"); // d + + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); + print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); + fprintf(data_cpp_f, ").finished(),\t// v\n"); // v + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); + print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); + fprintf(data_cpp_f, ").finished(),\t// vnew\n"); // vnew + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); + print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); + fprintf(data_cpp_f, ").finished(),\t// z\n"); // z + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); + print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); + fprintf(data_cpp_f, ").finished(),\t// znew\n"); // znew + + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); + print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); + fprintf(data_cpp_f, ").finished(),\t// g\n"); // g + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); + print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); + fprintf(data_cpp_f, ").finished(),\t// y\n"); // y + + fprintf(data_cpp_f, "\t(tinyVector(%d) << ", nx); + print_matrix(data_cpp_f, solver->work->Q, nx); + fprintf(data_cpp_f, ").finished(),\t// Q\n"); // Q + fprintf(data_cpp_f, "\t(tinyVector(%d) << ", nu); + print_matrix(data_cpp_f, solver->work->R, nu); + fprintf(data_cpp_f, ").finished(),\t// R\n"); // R + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); + print_matrix(data_cpp_f, solver->work->Adyn, nx * nx); + fprintf(data_cpp_f, ").finished(),\t// Adyn\n"); // Adyn + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nu); + print_matrix(data_cpp_f, solver->work->Bdyn, nx * nu); + fprintf(data_cpp_f, ").finished(),\t// Bdyn\n"); // Bdyn + + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); + print_matrix(data_cpp_f, solver->work->x_min, nx * N); + fprintf(data_cpp_f, ").finished(),\t// x_min\n"); // x_min + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); + print_matrix(data_cpp_f, solver->work->x_max, nx * N); + fprintf(data_cpp_f, ").finished(),\t// x_max\n"); // x_max + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); + print_matrix(data_cpp_f, solver->work->u_min, nu * (N-1)); + fprintf(data_cpp_f, ").finished(),\t// u_min\n"); // u_min + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); + print_matrix(data_cpp_f, solver->work->u_max, nu * (N-1)); + fprintf(data_cpp_f, ").finished(),\t// u_max\n"); // u_max + + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); + print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); + fprintf(data_cpp_f, ").finished(),\t// Xref\n"); // Xref + fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); + print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); + fprintf(data_cpp_f, ").finished(),\t// Uref\n"); // Uref + + fprintf(data_cpp_f, "\t(tinyVector(%d) << ", nu); + print_matrix(data_cpp_f, MatrixXd::Zero(nu, 1), nu); + fprintf(data_cpp_f, ").finished(),\t// Qu\n"); // Qu + + fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// state primal residual\n", 0.0); + fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// input primal residual\n", 0.0); + fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// state dual residual\n", 0.0); + fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// input dual residual\n", 0.0); + fprintf(data_cpp_f, "\t%d,\t// solve status\n", 0); + fprintf(data_cpp_f, "\t%d,\t// solve iteration\n", 0); + + fprintf(data_cpp_f, "};\n\n"); + + // Write solver struct definition to workspace file + fprintf(data_cpp_f, "TinySolver tiny_solver = {&solution, &settings, &cache, &work};\n\n"); + + // Close extern C + fprintf(data_cpp_f, "#ifdef __cplusplus\n"); + fprintf(data_cpp_f, "}\n"); + fprintf(data_cpp_f, "#endif\n\n"); + + // Close codegen data file + fclose(data_cpp_f); + if (verbose) { + printf("Data generated in %s\n", data_cpp_fname); + } + return 0; +} + +int codegen_example(const char* output_dir, int verbose) { + char example_cpp_fname[PATH_LENGTH]; + FILE *example_cpp_f; + + sprintf(example_cpp_fname, "%s/src/tiny_main.cpp", output_dir); + + // Open example file + example_cpp_f = fopen(example_cpp_fname, "w+"); + if (example_cpp_f == NULL) + ERROR_MSG(EXIT_FAILURE, "Failed to open file %s", example_cpp_fname); + + // Preamble + time_t start_time; + time(&start_time); + fprintf(example_cpp_f, "/*\n"); + fprintf(example_cpp_f, " * This file was autogenerated by TinyMPC on %s", ctime(&start_time)); + fprintf(example_cpp_f, " */\n\n"); + + fprintf(example_cpp_f, "#include \n\n"); + + fprintf(example_cpp_f, "#include \n"); + fprintf(example_cpp_f, "#include \n\n"); + + fprintf(example_cpp_f, "using namespace Eigen;\n"); + fprintf(example_cpp_f, "IOFormat TinyFmt(4, 0, \", \", \"\\n\", \"[\", \"]\");\n\n"); + + fprintf(example_cpp_f, "#ifdef __cplusplus\n"); + fprintf(example_cpp_f, "extern \"C\" {\n"); + fprintf(example_cpp_f, "#endif\n\n"); + + fprintf(example_cpp_f, "int main()\n"); + fprintf(example_cpp_f, "{\n"); + fprintf(example_cpp_f, "\tint exitflag = 1;\n"); + fprintf(example_cpp_f, "\t// Double check some data\n"); + fprintf(example_cpp_f, "\tstd::cout << \"rho: \" << tiny_solver.cache->rho << std::endl;\n"); + fprintf(example_cpp_f, "\tstd::cout << \"\\nmax iters: \" << tiny_solver.settings->max_iter << std::endl;\n"); + fprintf(example_cpp_f, "\tstd::cout << \"\\nState transition matrix:\\n\" << tiny_solver.work->Adyn.format(TinyFmt) << std::endl;\n"); + fprintf(example_cpp_f, "\tstd::cout << \"\\nInput/control matrix:\\n\" << tiny_solver.work->Bdyn.format(TinyFmt) << std::endl;\n\n"); + + fprintf(example_cpp_f, "\t// Visit https://tinympc.org/ to see how to set the initial condition and update the reference trajectory.\n\n"); + + fprintf(example_cpp_f, "\tstd::cout << \"\\nSolving...\\n\" << std::endl;\n\n"); + fprintf(example_cpp_f, "\texitflag = tiny_solve(&tiny_solver);\n\n"); + fprintf(example_cpp_f, "\tif (exitflag == 0) printf(\"Hooray! Solved with no error!\\n\");\n"); + fprintf(example_cpp_f, "\telse printf(\"Oops! Something went wrong!\\n\");\n"); + + fprintf(example_cpp_f, "\treturn 0;\n"); + fprintf(example_cpp_f, "}\n\n"); + + fprintf(example_cpp_f, "#ifdef __cplusplus\n"); + fprintf(example_cpp_f, "} /* extern \"C\" */\n"); + fprintf(example_cpp_f, "#endif\n"); + + // Close codegen example main file + fclose(example_cpp_f); + if (verbose) { + printf("Example tinympc main generated in %s\n", example_cpp_fname); + } + return 0; +} + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/codegen.hpp b/src/tongji/modules/planner/tinympc/codegen.hpp new file mode 100755 index 0000000..4495d30 --- /dev/null +++ b/src/tongji/modules/planner/tinympc/codegen.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include "types.hpp" + +#ifdef __cplusplus +extern "C" { +#endif + + int tiny_codegen(TinySolver* solver, const char* output_dir, int verbose); + + int tiny_codegen_with_sensitivity(TinySolver* solver, const char* output_dir, + tinyMatrix* dK, tinyMatrix* dP, + tinyMatrix* dC1, tinyMatrix* dC2, int verbose); + + int codegen_create_directories(const char* output_dir, int verbose); + int codegen_data_header(const char* output_dir, int verbose); + int codegen_data_source(TinySolver* solver, const char* output_dir, int verbose); + int codegen_example(const char* output_dir, int verbose); + + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/error.hpp b/src/tongji/modules/planner/tinympc/error.hpp new file mode 100644 index 0000000..713c1ce --- /dev/null +++ b/src/tongji/modules/planner/tinympc/error.hpp @@ -0,0 +1,29 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include + +// #if defined(__linux__) || defined(__unix__)// Check if Linux +// #include +// #define ERROR_MSG(exit_code, format, ...) error(exit_code, errno, format, ##__VA_ARGS__) + +// #elif defined(__APPLE__) || defined(__MACH__) // Check if macOS +#define ERROR_MSG(exit_code, format, ...) \ + { \ + fprintf(stderr, format ": %s\n", ##__VA_ARGS__, strerror(errno)); \ + exit(exit_code); \ + } + +// #else +// #error "Unsupported operating system" +// #endif + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/rho_benchmark.cpp b/src/tongji/modules/planner/tinympc/rho_benchmark.cpp new file mode 100644 index 0000000..f10848d --- /dev/null +++ b/src/tongji/modules/planner/tinympc/rho_benchmark.cpp @@ -0,0 +1,250 @@ +#include "rho_benchmark.hpp" +#include +#include +#include +#ifdef ARDUINO +#include +#else +// For non-Arduino platforms +uint32_t micros() { + return 0; // Replace with appropriate timing function +} +#endif + +void initialize_format_matrices(RhoAdapter* adapter, int nx, int nu, int N) { + // Calculate dimensions + int x_decision_size = nx * N + nu * (N-1); + int constraint_rows = (nx + nu) * (N-1); + + // Pre-allocate matrices + adapter->A_matrix = tinyMatrix::Zero(constraint_rows, x_decision_size); + adapter->z_vector = tinyMatrix::Zero(constraint_rows, 1); + adapter->y_vector = tinyMatrix::Zero(constraint_rows, 1); + adapter->x_decision = tinyMatrix::Zero(x_decision_size, 1); + + // Pre-compute P matrix structure + adapter->P_matrix = tinyMatrix::Zero(x_decision_size, x_decision_size); + adapter->q_vector = tinyMatrix::Zero(x_decision_size, 1); + + // Pre-allocate residual computation matrices + adapter->Ax_vector = tinyMatrix::Zero(constraint_rows, 1); + adapter->r_prim_vector = tinyMatrix::Zero(constraint_rows, 1); + adapter->r_dual_vector = tinyMatrix::Zero(x_decision_size, 1); + adapter->Px_vector = tinyMatrix::Zero(x_decision_size, 1); + adapter->ATy_vector = tinyMatrix::Zero(x_decision_size, 1); + + // Store dimensions + adapter->format_nx = nx; + adapter->format_nu = nu; + adapter->format_N = N; + + adapter->matrices_initialized = true; +} + +void format_matrices( + RhoAdapter* adapter, + const tinyMatrix& x_prev, + const tinyMatrix& u_prev, + const tinyMatrix& v_prev, + const tinyMatrix& z_prev, + const tinyMatrix& g_prev, + const tinyMatrix& y_prev, + TinyCache* cache, + TinyWorkspace* work, + int N +) { + if (!adapter->matrices_initialized) { + initialize_format_matrices(adapter, x_prev.rows(), u_prev.rows(), N); + } + + int nx = adapter->format_nx; + int nu = adapter->format_nu; + + // Fill x_decision + int x_idx = 0; + for (int i = 0; i < N; i++) { + adapter->x_decision.block(x_idx, 0, nx, 1) = x_prev.col(i); + x_idx += nx; + if (i < N-1) { + adapter->x_decision.block(x_idx, 0, nu, 1) = u_prev.col(i); + x_idx += nu; + } + } + + // Clear A matrix for reuse + adapter->A_matrix.setZero(); + + // Fill A matrix with dynamics and input constraints + for (int i = 0; i < N-1; i++) { + // Input constraints + int row_start = i * nu; + int col_start = i * (nx+nu) + nx; + adapter->A_matrix.block(row_start, col_start, nu, nu) = tinyMatrix::Identity(nu, nu); + + // Dynamics constraints + row_start = (N-1) * nu + i * nx; + col_start = i * (nx+nu); + adapter->A_matrix.block(row_start, col_start, nx, nx) = work->Adyn; + adapter->A_matrix.block(row_start, col_start+nx, nx, nu) = work->Bdyn; + + int next_state_idx = col_start + nx + nu; + if (next_state_idx < adapter->A_matrix.cols()) { + adapter->A_matrix.block(row_start, next_state_idx, nx, nx) = -tinyMatrix::Identity(nx, nx); + } + } + + // Fill z and y vectors + for (int i = 0; i < N-1; i++) { + adapter->z_vector.block(i*nu, 0, nu, 1) = z_prev.col(i); + adapter->z_vector.block((N-1)*nu+i*nx, 0, nx, 1) = v_prev.col(i+1); + + adapter->y_vector.block(i*nu, 0, nu, 1) = y_prev.col(i); + adapter->y_vector.block((N-1)*nu+i*nx, 0, nx, 1) = g_prev.col(i+1); + } + + // Build P matrix (cost matrix) + adapter->P_matrix.setZero(); + + // Fill diagonal blocks + x_idx = 0; + for (int i = 0; i < N; i++) { + // State cost + if (i == N-1) { + adapter->P_matrix.block(x_idx, x_idx, nx, nx) = cache->Pinf; + } else { + adapter->P_matrix.block(x_idx, x_idx, nx, nx) = work->Q.asDiagonal(); + } + x_idx += nx; + + // Input cost + if (i < N-1) { + adapter->P_matrix.block(x_idx, x_idx, nu, nu) = work->R.asDiagonal(); + x_idx += nu; + } + } + + // Create q vector (linear cost vector) + x_idx = 0; + for (int i = 0; i < N; i++) { + // For simplicity, we'll use zero reference for now + // In a real implementation, you'd use your reference trajectory + tinyMatrix x_ref = tinyMatrix::Zero(nx, 1); + tinyMatrix delta_x = x_prev.col(i) - x_ref; + adapter->q_vector.block(x_idx, 0, nx, 1) = work->Q.asDiagonal() * delta_x; + x_idx += nx; + + if (i < N-1) { + // For simplicity, we'll use zero reference for now + tinyMatrix u_ref = tinyMatrix::Zero(nu, 1); + tinyMatrix delta_u = u_prev.col(i) - u_ref; + adapter->q_vector.block(x_idx, 0, nu, 1) = work->R.asDiagonal() * delta_u; + x_idx += nu; + } + } +} + +void compute_residuals( + RhoAdapter* adapter, + tinytype* pri_res, + tinytype* dual_res, + tinytype* pri_norm, + tinytype* dual_norm +) { + // Compute Ax + adapter->Ax_vector = adapter->A_matrix * adapter->x_decision; + + // Compute primal residual + adapter->r_prim_vector = adapter->Ax_vector - adapter->z_vector; + *pri_res = adapter->r_prim_vector.cwiseAbs().maxCoeff(); + *pri_norm = std::max(adapter->Ax_vector.cwiseAbs().maxCoeff(), adapter->z_vector.cwiseAbs().maxCoeff()); + + // Compute dual residual components + adapter->Px_vector = adapter->P_matrix * adapter->x_decision; + adapter->ATy_vector = adapter->A_matrix.transpose() * adapter->y_vector; + + // Compute full dual residual + adapter->r_dual_vector = adapter->Px_vector + adapter->q_vector + adapter->ATy_vector; + *dual_res = adapter->r_dual_vector.cwiseAbs().maxCoeff(); + + // Compute normalization + *dual_norm = std::max(std::max(adapter->Px_vector.cwiseAbs().maxCoeff(), + adapter->ATy_vector.cwiseAbs().maxCoeff()), + adapter->q_vector.cwiseAbs().maxCoeff()); +} + +tinytype predict_rho( + RhoAdapter* adapter, + tinytype pri_res, + tinytype dual_res, + tinytype pri_norm, + tinytype dual_norm, + tinytype current_rho +) { + const tinytype eps = 1e-10; + + tinytype normalized_pri = pri_res / (pri_norm + eps); + tinytype normalized_dual = dual_res / (dual_norm + eps); + + tinytype ratio = normalized_pri / (normalized_dual + eps); + + tinytype new_rho = current_rho * std::sqrt(ratio); + + if (adapter->clip) { + new_rho = std::min(std::max(new_rho, adapter->rho_min), adapter->rho_max); + } + + return new_rho; +} + +void update_matrices_with_derivatives(TinyCache* cache, tinytype new_rho) { + tinytype delta_rho = new_rho - cache->rho; + + + + cache->Kinf = cache->Kinf + delta_rho * cache->dKinf_drho; + cache->Pinf = cache->Pinf + delta_rho * cache->dPinf_drho; + cache->C1 = cache->C1 + delta_rho * cache->dC1_drho; + cache->C2 = cache->C2 + delta_rho * cache->dC2_drho; + + cache->rho = new_rho; + + +} + +void benchmark_rho_adaptation( + RhoAdapter* adapter, + const tinyMatrix& x_prev, + const tinyMatrix& u_prev, + const tinyMatrix& v_prev, + const tinyMatrix& z_prev, + const tinyMatrix& g_prev, + const tinyMatrix& y_prev, + TinyCache* cache, + TinyWorkspace* work, + int N, + RhoBenchmarkResult* result +) { + uint32_t start_time = micros(); + + // Format matrices + format_matrices(adapter, x_prev, u_prev, v_prev, z_prev, g_prev, y_prev, cache, work, N); + + // Compute residuals + tinytype pri_res, dual_res, pri_norm, dual_norm; + compute_residuals(adapter, &pri_res, &dual_res, &pri_norm, &dual_norm); + + // Predict new rho + tinytype new_rho = predict_rho(adapter, pri_res, dual_res, pri_norm, dual_norm, cache->rho); + + // Update matrices + update_matrices_with_derivatives(cache, new_rho); + + // Store results + result->time_us = micros() - start_time; + result->initial_rho = cache->rho; + result->final_rho = new_rho; + result->pri_res = pri_res; + result->dual_res = dual_res; + result->pri_norm = pri_norm; + result->dual_norm = dual_norm; +} \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/rho_benchmark.hpp b/src/tongji/modules/planner/tinympc/rho_benchmark.hpp new file mode 100644 index 0000000..7f69230 --- /dev/null +++ b/src/tongji/modules/planner/tinympc/rho_benchmark.hpp @@ -0,0 +1,95 @@ +#pragma once +#include +#include "types.hpp" + +struct RhoAdapter { + tinytype rho_min; + tinytype rho_max; + bool clip; + bool matrices_initialized; + + // Pre-allocated matrices for formatting + tinyMatrix A_matrix; + tinyMatrix z_vector; + tinyMatrix y_vector; + tinyMatrix x_decision; + tinyMatrix P_matrix; + tinyMatrix q_vector; + + // Pre-allocated matrices for residual computation + tinyMatrix Ax_vector; + tinyMatrix r_prim_vector; + tinyMatrix r_dual_vector; + tinyMatrix Px_vector; + tinyMatrix ATy_vector; + + // Dimensions + int format_nx; + int format_nu; + int format_N; +}; + +struct RhoBenchmarkResult { + uint32_t time_us; + tinytype initial_rho; + tinytype final_rho; + tinytype pri_res; + tinytype dual_res; + tinytype pri_norm; + tinytype dual_norm; +}; + +// Initialize matrices for formatting +void initialize_format_matrices(RhoAdapter* adapter, int nx, int nu, int N); + +// Format matrices for residual computation +void format_matrices( + RhoAdapter* adapter, + const tinyMatrix& x_prev, + const tinyMatrix& u_prev, + const tinyMatrix& v_prev, + const tinyMatrix& z_prev, + const tinyMatrix& g_prev, + const tinyMatrix& y_prev, + TinyCache* cache, + TinyWorkspace* work, + int N +); + +// Compute residuals +void compute_residuals( + RhoAdapter* adapter, + tinytype* pri_res, + tinytype* dual_res, + tinytype* pri_norm, + tinytype* dual_norm +); + +// Predict new rho value +tinytype predict_rho( + RhoAdapter* adapter, + tinytype pri_res, + tinytype dual_res, + tinytype pri_norm, + tinytype dual_norm, + tinytype current_rho +); + +// Update matrices using derivatives +void update_matrices_with_derivatives(TinyCache* cache, tinytype new_rho); + + +// Main benchmark function +void benchmark_rho_adaptation( + RhoAdapter* adapter, + const tinyMatrix& x_prev, + const tinyMatrix& u_prev, + const tinyMatrix& v_prev, + const tinyMatrix& z_prev, + const tinyMatrix& g_prev, + const tinyMatrix& y_prev, + TinyCache* cache, + TinyWorkspace* work, + int N, + RhoBenchmarkResult* result +); \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/tiny_api.cpp b/src/tongji/modules/planner/tinympc/tiny_api.cpp new file mode 100755 index 0000000..d05f40f --- /dev/null +++ b/src/tongji/modules/planner/tinympc/tiny_api.cpp @@ -0,0 +1,476 @@ +#include "tiny_api.hpp" +#include "tiny_api_constants.hpp" + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +using namespace Eigen; +IOFormat TinyApiFmt(4, 0, ", ", "\n", "[", "]"); + +static int check_dimension(std::string matrix_name, std::string rows_or_columns, int actual, int expected) { + if (actual != expected) { + std::cout << matrix_name << " has " << actual << " " << rows_or_columns << ". Expected " << expected << "." << std::endl; + return 1; + } + return 0; +} + +int tiny_setup(TinySolver** solverp, + tinyMatrix Adyn, tinyMatrix Bdyn, tinyMatrix fdyn, tinyMatrix Q, tinyMatrix R, + tinytype rho, int nx, int nu, int N, int verbose) { + + TinySolution *solution = new TinySolution(); + TinyCache *cache = new TinyCache(); + TinySettings *settings = new TinySettings(); + TinyWorkspace *work = new TinyWorkspace(); + TinySolver *solver = new TinySolver(); + + solver->solution = solution; + solver->cache = cache; + solver->settings = settings; + solver->work = work; + + *solverp = solver; + + // Initialize solution + solution->iter = 0; + solution->solved = 0; + solution->x = tinyMatrix::Zero(nx, N); + solution->u = tinyMatrix::Zero(nu, N-1); + + // Initialize settings + tiny_set_default_settings(settings); + + // Initialize workspace + work->nx = nx; + work->nu = nu; + work->N = N; + + // Make sure arguments are the correct shapes + int status = 0; + status |= check_dimension("State transition matrix (A)", "rows", Adyn.rows(), nx); + status |= check_dimension("State transition matrix (A)", "columns", Adyn.cols(), nx); + status |= check_dimension("Input matrix (B)", "rows", Bdyn.rows(), nx); + status |= check_dimension("Input matrix (B)", "columns", Bdyn.cols(), nu); + status |= check_dimension("Affine vector (f)", "rows", fdyn.rows(), nx); + status |= check_dimension("Affine vector (f)", "columns", fdyn.cols(), 1); + status |= check_dimension("State stage cost (Q)", "rows", Q.rows(), nx); + status |= check_dimension("State stage cost (Q)", "columns", Q.cols(), nx); + status |= check_dimension("State input cost (R)", "rows", R.rows(), nu); + status |= check_dimension("State input cost (R)", "columns", R.cols(), nu); + if (status) { + return status; + } + + work->x = tinyMatrix::Zero(nx, N); + work->u = tinyMatrix::Zero(nu, N-1); + + work->q = tinyMatrix::Zero(nx, N); + work->r = tinyMatrix::Zero(nu, N-1); + + work->p = tinyMatrix::Zero(nx, N); + work->d = tinyMatrix::Zero(nu, N-1); + + // Bound constraint slack variables + work->v = tinyMatrix::Zero(nx, N); + work->vnew = tinyMatrix::Zero(nx, N); + work->z = tinyMatrix::Zero(nu, N-1); + work->znew = tinyMatrix::Zero(nu, N-1); + + // Bound constraint dual variables + work->g = tinyMatrix::Zero(nx, N); + work->y = tinyMatrix::Zero(nu, N-1); + + // Cone constraint slack variables + work->vc = tinyMatrix::Zero(nx, N); + work->vcnew = tinyMatrix::Zero(nx, N); + work->zc = tinyMatrix::Zero(nu, N-1); + work->zcnew = tinyMatrix::Zero(nu, N-1); + + // Cone constraint dual variables + work->gc = tinyMatrix::Zero(nx, N); + work->yc = tinyMatrix::Zero(nu, N-1); + + // Linear constraint slack variables + work->vl = tinyMatrix::Zero(nx, N); + work->vlnew = tinyMatrix::Zero(nx, N); + work->zl = tinyMatrix::Zero(nu, N-1); + work->zlnew = tinyMatrix::Zero(nu, N-1); + + // Linear constraint dual variables + work->gl = tinyMatrix::Zero(nx, N); + work->yl = tinyMatrix::Zero(nu, N-1); + + work->Q = (Q + rho * tinyMatrix::Identity(nx, nx)).diagonal(); + work->R = (R + rho * tinyMatrix::Identity(nu, nu)).diagonal(); + work->Adyn = Adyn; // State transition matrix + work->Bdyn = Bdyn; // Input matrix + work->fdyn = fdyn; // Affine offset vector + + work->Xref = tinyMatrix::Zero(nx, N); + work->Uref = tinyMatrix::Zero(nu, N-1); + + work->Qu = tinyVector::Zero(nu); + + work->primal_residual_state = 0; + work->primal_residual_input = 0; + work->dual_residual_state = 0; + work->dual_residual_input = 0; + work->status = 0; + work->iter = 0; + + // Initialize cache + status = tiny_precompute_and_set_cache(cache, Adyn, Bdyn, fdyn, work->Q.asDiagonal(), work->R.asDiagonal(), nx, nu, rho, verbose); + if (status) { + return status; + } + + // Initialize sensitivity matrices for adaptive rho + if (solver->settings->adaptive_rho) { + tiny_initialize_sensitivity_matrices(solver); + } + + return 0; +} + +int tiny_set_bound_constraints(TinySolver* solver, + tinyMatrix x_min, tinyMatrix x_max, + tinyMatrix u_min, tinyMatrix u_max) { + if (!solver) { + std::cout << "Error in tiny_set_bound_constraints: solver is nullptr" << std::endl; + return 1; + } + + // Make sure all bound constraint matrix sizes are self-consistent + int status = 0; + status |= check_dimension("Lower state bounds (x_min)", "rows", x_min.rows(), solver->work->nx); + status |= check_dimension("Lower state bounds (x_min)", "cols", x_min.cols(), solver->work->N); + status |= check_dimension("Lower state bounds (x_max)", "rows", x_max.rows(), solver->work->nx); + status |= check_dimension("Lower state bounds (x_max)", "cols", x_max.cols(), solver->work->N); + status |= check_dimension("Lower input bounds (u_min)", "rows", u_min.rows(), solver->work->nu); + status |= check_dimension("Lower input bounds (u_min)", "cols", u_min.cols(), solver->work->N-1); + status |= check_dimension("Lower input bounds (u_max)", "rows", u_max.rows(), solver->work->nu); + status |= check_dimension("Lower input bounds (u_max)", "cols", u_max.cols(), solver->work->N-1); + + solver->work->x_min = x_min; + solver->work->x_max = x_max; + solver->work->u_min = u_min; + solver->work->u_max = u_max; + + return 0; +} + +int tiny_set_cone_constraints(TinySolver* solver, + VectorXi Acx, VectorXi qcx, tinyVector cx, + VectorXi Acu, VectorXi qcu, tinyVector cu) { + if (!solver) { + std::cout << "Error in tiny_set_cone_constraints: solver is nullptr" << std::endl; + return 1; + } + + // Make sure all cone constraint vector sizes are self-consistent + int num_state_cones = Acx.rows(); + int num_input_cones = Acu.rows(); + int status = 0; + status |= check_dimension("Cone state size (qcx)", "rows", qcx.rows(), num_state_cones); + status |= check_dimension("Cone mu value for state (cx)", "rows", cx.rows(), num_state_cones); + status |= check_dimension("Cone input size (qcu)", "rows", qcu.rows(), num_input_cones); + status |= check_dimension("Cone mu value for input (cu)", "rows", cu.rows(), num_input_cones); + if (status) { + return status; + } + + solver->work->numStateCones = num_state_cones; + solver->work->numInputCones = num_input_cones; + + solver->work->Acx = Acx; + solver->work->qcx = qcx; + solver->work->cx = cx; + + solver->work->Acu = Acu; + solver->work->qcu = qcu; + solver->work->cu = cu; + + return 0; +} + +int tiny_set_linear_constraints(TinySolver* solver, + tinyMatrix Alin_x, tinyVector blin_x, + tinyMatrix Alin_u, tinyVector blin_u) { + if (!solver) { + std::cout << "Error in tiny_set_linear_constraints: solver is nullptr" << std::endl; + return 1; + } + + // Make sure all linear constraint matrix sizes are self-consistent + int num_state_linear = Alin_x.rows(); + int num_input_linear = Alin_u.rows(); + int status = 0; + + // Check state constraint dimensions + if (num_state_linear > 0) { + status |= check_dimension("State linear constraint matrix (Alin_x)", "rows", Alin_x.rows(), num_state_linear); + status |= check_dimension("State linear constraint matrix (Alin_x)", "columns", Alin_x.cols(), solver->work->nx); + status |= check_dimension("State linear constraint vector (blin_x)", "rows", blin_x.rows(), num_state_linear); + status |= check_dimension("State linear constraint vector (blin_x)", "columns", blin_x.cols(), 1); + } + + // Check input constraint dimensions + if (num_input_linear > 0) { + status |= check_dimension("Input linear constraint matrix (Alin_u)", "rows", Alin_u.rows(), num_input_linear); + status |= check_dimension("Input linear constraint matrix (Alin_u)", "columns", Alin_u.cols(), solver->work->nu); + status |= check_dimension("Input linear constraint vector (blin_u)", "rows", blin_u.rows(), num_input_linear); + status |= check_dimension("Input linear constraint vector (blin_u)", "columns", blin_u.cols(), 1); + } + + if (status) { + return status; + } + + solver->work->numStateLinear = num_state_linear; + solver->work->numInputLinear = num_input_linear; + + solver->work->Alin_x = Alin_x; + solver->work->blin_x = blin_x; + solver->work->Alin_u = Alin_u; + solver->work->blin_u = blin_u; + + return 0; +} + +int tiny_precompute_and_set_cache(TinyCache *cache, + tinyMatrix Adyn, tinyMatrix Bdyn, tinyMatrix fdyn, tinyMatrix Q, tinyMatrix R, + int nx, int nu, tinytype rho, int verbose) { + + if (!cache) { + std::cout << "Error in tiny_precompute_and_set_cache: cache is nullptr" << std::endl; + return 1; + } + + // Update by adding rho * identity matrix to Q, R + tinyMatrix Q1 = Q + rho * tinyMatrix::Identity(nx, nx); + tinyMatrix R1 = R + rho * tinyMatrix::Identity(nu, nu); + + // Printing + if (verbose) { + std::cout << "A = " << Adyn.format(TinyApiFmt) << std::endl; + std::cout << "B = " << Bdyn.format(TinyApiFmt) << std::endl; + std::cout << "Q = " << Q1.format(TinyApiFmt) << std::endl; + std::cout << "R = " << R1.format(TinyApiFmt) << std::endl; + std::cout << "rho = " << rho << std::endl; + } + + // Riccati recursion to get Kinf, Pinf + tinyMatrix Ktp1 = tinyMatrix::Zero(nu, nx); + tinyMatrix Ptp1 = rho * tinyMatrix::Ones(nx, 1).array().matrix().asDiagonal(); + tinyMatrix Kinf = tinyMatrix::Zero(nu, nx); + tinyMatrix Pinf = tinyMatrix::Zero(nx, nx); + + for (int i = 0; i < 1000; i++) + { + Kinf = (R1 + Bdyn.transpose() * Ptp1 * Bdyn).inverse() * Bdyn.transpose() * Ptp1 * Adyn; + Pinf = Q1 + Adyn.transpose() * Ptp1 * (Adyn - Bdyn * Kinf); + // if Kinf converges, break + if ((Kinf - Ktp1).cwiseAbs().maxCoeff() < 1e-5) + { + if (verbose) { + std::cout << "Kinf converged after " << i + 1 << " iterations" << std::endl; + } + break; + } + Ktp1 = Kinf; + Ptp1 = Pinf; + } + + // Compute cached matrices + tinyMatrix Quu_inv = (R1 + Bdyn.transpose() * Pinf * Bdyn).inverse(); + tinyMatrix AmBKt = (Adyn - Bdyn * Kinf).transpose(); + + // Precomputation for affine term + tinyVector APf = AmBKt*Pinf*fdyn; + tinyVector BPf = Bdyn.transpose()*Pinf*fdyn; + + if (verbose) { + std::cout << "Kinf = " << Kinf.format(TinyApiFmt) << std::endl; + std::cout << "Pinf = " << Pinf.format(TinyApiFmt) << std::endl; + std::cout << "Quu_inv = " << Quu_inv.format(TinyApiFmt) << std::endl; + std::cout << "AmBKt = " << AmBKt.format(TinyApiFmt) << std::endl; + std::cout << "APf = " << APf.format(TinyApiFmt) << std::endl; + std::cout << "BPf = " << BPf.format(TinyApiFmt) << std::endl; + + std::cout << "\nPrecomputation finished!\n" << std::endl; + } + + cache->rho = rho; + cache->Kinf = Kinf; + cache->Pinf = Pinf; + cache->Quu_inv = Quu_inv; + cache->AmBKt = AmBKt; + cache->C1 = Quu_inv; + cache->C2 = AmBKt; + cache->APf = APf; + cache->BPf = BPf; + + return 0; // return success +} + + +int tiny_solve(TinySolver* solver) { + return solve(solver); +} + +int tiny_update_settings(TinySettings* settings, tinytype abs_pri_tol, tinytype abs_dua_tol, + int max_iter, int check_termination, + int en_state_bound, int en_input_bound, + int en_state_soc, int en_input_soc, + int en_state_linear, int en_input_linear) { + if (!settings) { + std::cout << "Error in tiny_update_settings: settings is nullptr" << std::endl; + return 1; + } + settings->abs_pri_tol = abs_pri_tol; + settings->abs_dua_tol = abs_dua_tol; + settings->max_iter = max_iter; + settings->check_termination = check_termination; + settings->en_state_bound = en_state_bound; + settings->en_input_bound = en_input_bound; + settings->en_state_soc = en_state_soc; + settings->en_input_soc = en_input_soc; + settings->en_state_linear = en_state_linear; + settings->en_input_linear = en_input_linear; + return 0; +} + +int tiny_set_default_settings(TinySettings* settings) { + if (!settings) { + std::cout << "Error in tiny_set_default_settings: settings is nullptr" << std::endl; + return 1; + } + settings->abs_pri_tol = TINY_DEFAULT_ABS_PRI_TOL; + settings->abs_dua_tol = TINY_DEFAULT_ABS_DUA_TOL; + settings->max_iter = TINY_DEFAULT_MAX_ITER; + settings->check_termination = TINY_DEFAULT_CHECK_TERMINATION; + + // Turn off constraints until they are set by tiny_set_bound_constraints or tiny_set_cone_constraints + settings->en_state_bound = TINY_DEFAULT_EN_STATE_BOUND; + settings->en_input_bound = TINY_DEFAULT_EN_INPUT_BOUND; + settings->en_state_soc = TINY_DEFAULT_EN_STATE_SOC; + settings->en_input_soc = TINY_DEFAULT_EN_INPUT_SOC; + settings->en_state_linear = TINY_DEFAULT_EN_STATE_LINEAR; + settings->en_input_linear = TINY_DEFAULT_EN_INPUT_LINEAR; + + // Initialize adaptive rho settings + // NOTE : Adaptive rho currently supports only quadrotor system + settings->adaptive_rho = 0; // Disabled by default + settings->adaptive_rho_min = 1.0; + settings->adaptive_rho_max = 100.0; + settings->adaptive_rho_enable_clipping = 1; + + return 0; +} + +int tiny_set_x0(TinySolver* solver, tinyVector x0) { + if (!solver) { + std::cout << "Error in tiny_set_x0: solver is nullptr" << std::endl; + return 1; + } + if (x0.rows() != solver->work->nx) { + perror("Error in tiny_set_x0: x0 is not the correct length"); + } + solver->work->x.col(0) = x0; + return 0; +} + +int tiny_set_x_ref(TinySolver* solver, tinyMatrix x_ref) { + if (!solver) { + std::cout << "Error in tiny_set_x_ref: solver is nullptr" << std::endl; + return 1; + } + int status = 0; + status |= check_dimension("State reference trajectory (x_ref)", "rows", x_ref.rows(), solver->work->nx); + status |= check_dimension("State reference trajectory (x_ref)", "columns", x_ref.cols(), solver->work->N); + solver->work->Xref = x_ref; + return 0; +} + +int tiny_set_u_ref(TinySolver* solver, tinyMatrix u_ref) { + if (!solver) { + std::cout << "Error in tiny_set_u_ref: solver is nullptr" << std::endl; + return 1; + } + int status = 0; + status |= check_dimension("Control/input reference trajectory (u_ref)", "rows", u_ref.rows(), solver->work->nu); + status |= check_dimension("Control/input reference trajectory (u_ref)", "columns",u_ref.cols(), solver->work->N-1); + solver->work->Uref = u_ref; + return 0; +} + +void tiny_initialize_sensitivity_matrices(TinySolver *solver) { + + int nu = solver->work->nu; + int nx = solver->work->nx; + // Initialize matrices with zeros + solver->cache->dKinf_drho = tinyMatrix::Zero(nu, nx); + solver->cache->dPinf_drho = tinyMatrix::Zero(nx, nx); + solver->cache->dC1_drho = tinyMatrix::Zero(nu, nu); + solver->cache->dC2_drho = tinyMatrix::Zero(nx, nx); + + const float dKinf_drho[4][12] = { + { 0.0001, -0.0001, -0.0025, 0.0003, 0.0007, 0.0050, 0.0001, -0.0001, -0.0008, 0.0000, 0.0001, 0.0008}, + { -0.0001, -0.0000, -0.0025, -0.0001, -0.0006, -0.0050, -0.0001, 0.0000, -0.0008, -0.0000, -0.0001, -0.0008}, + { 0.0000, 0.0000, -0.0025, 0.0001, 0.0004, 0.0050, 0.0000, 0.0000, -0.0008, 0.0000, 0.0000, 0.0008}, + { -0.0000, 0.0001, -0.0025, -0.0003, -0.0004, -0.0050, -0.0000, 0.0001, -0.0008, -0.0000, -0.0000, -0.0008} + }; + + const float dPinf_drho[12][12] = { + { 0.0494, -0.0045, -0.0000, 0.0110, 0.1300, -0.0283, 0.0280, -0.0026, -0.0000, 0.0004, 0.0070, -0.0094}, + { -0.0045, 0.0491, 0.0000, -0.1320, -0.0111, 0.0114, -0.0026, 0.0279, 0.0000, -0.0076, -0.0004, 0.0038}, + { -0.0000, 0.0000, 2.4450, 0.0000, -0.0000, -0.0000, -0.0000, 0.0000, 1.2593, 0.0000, 0.0000, 0.0000}, + { 0.0110, -0.1320, 0.0000, 0.3913, 0.0592, 0.3108, 0.0080, -0.0776, 0.0000, 0.0254, 0.0068, 0.0750}, + { 0.1300, -0.0111, -0.0000, 0.0592, 0.4420, 0.7771, 0.0797, -0.0081, -0.0000, 0.0068, 0.0350, 0.1875}, + { -0.0283, 0.0114, -0.0000, 0.3108, 0.7771, 10.0441, 0.0272, -0.0109, 0.0000, 0.0655, 0.1639, 2.6362}, + { 0.0280, -0.0026, -0.0000, 0.0080, 0.0797, 0.0272, 0.0163, -0.0016, -0.0000, 0.0005, 0.0047, 0.0032}, + { -0.0026, 0.0279, 0.0000, -0.0776, -0.0081, -0.0109, -0.0016, 0.0161, 0.0000, -0.0046, -0.0005, -0.0013}, + { -0.0000, 0.0000, 1.2593, 0.0000, -0.0000, 0.0000, -0.0000, 0.0000, 0.9232, 0.0000, 0.0000, 0.0000}, + { 0.0004, -0.0076, 0.0000, 0.0254, 0.0068, 0.0655, 0.0005, -0.0046, 0.0000, 0.0022, 0.0017, 0.0244}, + { 0.0070, -0.0004, 0.0000, 0.0068, 0.0350, 0.1639, 0.0047, -0.0005, 0.0000, 0.0017, 0.0054, 0.0610}, + { -0.0094, 0.0038, 0.0000, 0.0750, 0.1875, 2.6362, 0.0032, -0.0013, 0.0000, 0.0244, 0.0610, 0.9869} + }; + + const float dC1_drho[4][4] = { + { -0.0000, 0.0000, -0.0000, 0.0000}, + { 0.0000, -0.0000, 0.0000, -0.0000}, + { -0.0000, 0.0000, -0.0000, 0.0000}, + { 0.0000, -0.0000, 0.0000, -0.0000} + }; + + const float dC2_drho[12][12] = { + { 0.0000, -0.0000, 0.0000, 0.0000, 0.0000, -0.0000, 0.0000, -0.0000, 0.0000, 0.0000, 0.0000, -0.0000}, + { -0.0000, 0.0000, 0.0000, -0.0000, -0.0000, 0.0000, -0.0000, 0.0000, 0.0000, -0.0000, -0.0000, 0.0000}, + { -0.0000, 0.0000, 0.0001, 0.0000, -0.0000, -0.0000, -0.0000, 0.0000, 0.0000, 0.0000, -0.0000, -0.0000}, + { 0.0000, -0.0000, -0.0000, 0.0001, 0.0000, -0.0000, 0.0000, -0.0000, -0.0000, 0.0000, 0.0000, -0.0000}, + { 0.0000, -0.0000, -0.0000, 0.0000, 0.0001, -0.0000, 0.0000, -0.0000, -0.0000, 0.0000, 0.0000, -0.0000}, + { -0.0000, 0.0000, -0.0000, -0.0000, 0.0000, 0.0001, -0.0000, 0.0000, -0.0000, 0.0000, 0.0000, 0.0000}, + { 0.0000, -0.0000, 0.0000, 0.0000, 0.0000, -0.0000, 0.0000, -0.0000, 0.0000, 0.0000, 0.0000, -0.0000}, + { -0.0000, 0.0000, 0.0000, -0.0000, -0.0000, 0.0000, -0.0000, 0.0000, 0.0000, -0.0000, -0.0000, 0.0000}, + { -0.0000, 0.0000, 0.0021, 0.0000, -0.0000, -0.0000, -0.0000, 0.0000, 0.0006, 0.0000, -0.0000, -0.0000}, + { 0.0002, -0.0027, -0.0000, 0.0068, 0.0005, -0.0005, 0.0001, -0.0015, -0.0000, 0.0004, 0.0000, -0.0001}, + { 0.0027, -0.0002, 0.0000, 0.0005, 0.0066, -0.0011, 0.0015, -0.0001, 0.0000, 0.0000, 0.0004, -0.0002}, + { -0.0001, 0.0001, 0.0000, -0.0000, 0.0000, 0.0041, -0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0006} + }; + + + + // Map arrays to Eigen matrices + solver->cache->dKinf_drho = Map>(dKinf_drho[0]).cast(); + solver->cache->dPinf_drho = Map>(dPinf_drho[0]).cast(); + solver->cache->dC1_drho = Map>(dC1_drho[0]).cast(); + solver->cache->dC2_drho = Map>(dC2_drho[0]).cast(); +} + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/tiny_api.hpp b/src/tongji/modules/planner/tinympc/tiny_api.hpp new file mode 100755 index 0000000..5d9ce04 --- /dev/null +++ b/src/tongji/modules/planner/tinympc/tiny_api.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include +#include "admm.hpp" + +#ifdef __cplusplus +extern "C" { +#endif + +int tiny_setup(TinySolver** solverp, + tinyMatrix Adyn, tinyMatrix Bdyn, tinyMatrix fdyn, tinyMatrix Q, tinyMatrix R, + tinytype rho, int nx, int nu, int N, int verbose); +int tiny_set_bound_constraints(TinySolver* solver, + tinyMatrix x_min, tinyMatrix x_max, + tinyMatrix u_min, tinyMatrix u_max); +int tiny_set_cone_constraints(TinySolver* solver, + VectorXi Acu, VectorXi qcu, tinyVector cu, + VectorXi Acx, VectorXi qcx, tinyVector cx); +int tiny_set_linear_constraints(TinySolver* solver, + tinyMatrix Alin_x, tinyVector blin_x, + tinyMatrix Alin_u, tinyVector blin_u); +int tiny_precompute_and_set_cache(TinyCache *cache, + tinyMatrix Adyn, tinyMatrix Bdyn, tinyMatrix fdyn, tinyMatrix Q, tinyMatrix R, + int nx, int nu, tinytype rho, int verbose); + +void compute_sensitivity_matrices(TinyCache *cache, + tinyMatrix Adyn, tinyMatrix Bdyn, tinyMatrix Q, tinyMatrix R, + int nx, int nu, tinytype rho, int verbose); + +int tiny_update_matrices_with_derivatives(TinyCache *cache, tinytype delta_rho); +int tiny_solve(TinySolver *solver); + +int tiny_update_settings(TinySettings* settings, + tinytype abs_pri_tol, tinytype abs_dua_tol, + int max_iter, int check_termination, + int en_state_bound, int en_input_bound, + int en_state_soc, int en_input_soc, + int en_state_linear, int en_input_linear); +int tiny_set_default_settings(TinySettings* settings); + +int tiny_set_x0(TinySolver* solver, tinyVector x0); +int tiny_set_x_ref(TinySolver* solver, tinyMatrix x_ref); +int tiny_set_u_ref(TinySolver* solver, tinyMatrix u_ref); + +/** + * Initialize sensitivity matrices for adaptive rho + * + * @param solver Pointer to solver + */ +void tiny_initialize_sensitivity_matrices(TinySolver *solver); + +int tiny_setup_state_soc_constraints(TinySolver *solver, + tinyVector Acx, tinyVector qcx, tinyVector cx, + int numStateCones); + +int tiny_setup_input_soc_constraints(TinySolver *solver, + tinyVector Acu, tinyVector qcu, tinyVector cu, + int numInputCones); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/tiny_api_constants.hpp b/src/tongji/modules/planner/tinympc/tiny_api_constants.hpp new file mode 100644 index 0000000..d99e96a --- /dev/null +++ b/src/tongji/modules/planner/tinympc/tiny_api_constants.hpp @@ -0,0 +1,14 @@ +#pragma once + + +// Default settings +#define TINY_DEFAULT_ABS_PRI_TOL (1e-03) +#define TINY_DEFAULT_ABS_DUA_TOL (1e-03) +#define TINY_DEFAULT_MAX_ITER (1000) +#define TINY_DEFAULT_CHECK_TERMINATION (1) +#define TINY_DEFAULT_EN_STATE_BOUND (1) +#define TINY_DEFAULT_EN_INPUT_BOUND (1) +#define TINY_DEFAULT_EN_STATE_SOC (0) +#define TINY_DEFAULT_EN_INPUT_SOC (0) +#define TINY_DEFAULT_EN_STATE_LINEAR (0) +#define TINY_DEFAULT_EN_INPUT_LINEAR (0) diff --git a/src/tongji/modules/planner/tinympc/types.hpp b/src/tongji/modules/planner/tinympc/types.hpp new file mode 100755 index 0000000..409023a --- /dev/null +++ b/src/tongji/modules/planner/tinympc/types.hpp @@ -0,0 +1,204 @@ +#pragma once + +#include +// #include +// #include + +using namespace Eigen; + +#ifdef __cplusplus +extern "C" { +#endif + +typedef double tinytype; // should be double if you want to generate code +typedef Matrix tinyMatrix; +typedef Matrix tinyVector; + +// typedef Matrix tiny_VectorNx; +// typedef Matrix tiny_VectorNu; +// typedef Matrix tiny_MatrixNxNx; +// typedef Matrix tiny_MatrixNxNu; +// typedef Matrix tiny_MatrixNuNx; +// typedef Matrix tiny_MatrixNuNu; + +// typedef Matrix tiny_MatrixNxNh; // Nu x Nh +// typedef Matrix tiny_MatrixNuNhm1; // Nu x Nh-1 + +/** + * Solution + */ +typedef struct { + int iter; + int solved; + tinyMatrix x; // nx x N + tinyMatrix u; // nu x N-1 +} TinySolution; + + +/** +* Matrices that must be recomputed with changes in time step, rho +*/ +typedef struct { + tinytype rho; + tinyMatrix Kinf; // nu x nx + tinyMatrix Pinf; // nx x nx + tinyMatrix Quu_inv; // nu x nu + tinyMatrix AmBKt; // nx x nx + tinyVector APf; // nx x 1 + tinyVector BPf; // nu x 1 + tinyMatrix C1; // From adaptive rho + tinyMatrix C2; // From adaptive rho + + // Sensitivity matrices for adaptive rho + tinyMatrix dKinf_drho; + tinyMatrix dPinf_drho; + tinyMatrix dC1_drho; + tinyMatrix dC2_drho; +} TinyCache; +/** +* User settings +*/ +typedef struct { + tinytype abs_pri_tol; + tinytype abs_dua_tol; + int max_iter; + int check_termination; + int en_state_bound; + int en_input_bound; + int en_state_soc; + int en_input_soc; + int en_state_linear; + int en_input_linear; + + // Add adaptive rho parameters + int adaptive_rho; // Enable/disable adaptive rho (1/0) + tinytype adaptive_rho_min; // Minimum value for rho + tinytype adaptive_rho_max; // Maximum value for rho + int adaptive_rho_enable_clipping; // Enable/disable clipping of rho (1/0) +} TinySettings; + + +/** + * Problem variables + */ +typedef struct { + int nx; // Number of states + int nu; // Number of control inputs + int N; // Number of knotpoints in the horizon + + // State and input + tinyMatrix x; // nx x N + tinyMatrix u; // nu x N-1 + + // Linear control cost terms + tinyMatrix q; // nx x N + tinyMatrix r; // nu x N-1 + + // Linear Riccati backward pass terms + tinyMatrix p; // nx x N + tinyMatrix d; // nu x N-1 + + // Bound constraint variables + // Slack variables + tinyMatrix v; // nx x N + tinyMatrix vnew; // nx x N + tinyMatrix z; // nu x N-1 + tinyMatrix znew; // nu x N-1 + + // Dual variables + tinyMatrix g; // nx x N + tinyMatrix y; // nu x N-1 + + // State and input bounds + tinyMatrix x_min; // nx x N + tinyMatrix x_max; // nx x N + tinyMatrix u_min; // nu x N-1 + tinyMatrix u_max; // nu x N-1 + + // Cone constraint variables + // Variables to keep track of general cone information + int numStateCones; // Number of cone constraints on states at each time step + int numInputCones; // Number of cone constraints on inputs at each time step + tinyVector cx; // One coefficient for each state cone + tinyVector cu; // One coefficient for each input cone + VectorXi Acx; // Start indices for each state cone + VectorXi Acu; // Start indices for each input cone + VectorXi qcx; // Dimension for each state cone + VectorXi qcu; // Dimension for each input cone + + // Slack variables + tinyMatrix vc; // nx x N + tinyMatrix vcnew; // nx x N + tinyMatrix zc; // nu x N-1 + tinyMatrix zcnew; // nu x N-1 + + // Dual variables + tinyMatrix gc; // nx x N + tinyMatrix yc; // nu x N-1 + + // Linear constraint variables + // Variables to keep track of general linear constraint information + int numStateLinear; // Number of linear constraints on states at each time step + int numInputLinear; // Number of linear constraints on inputs at each time step + + // Constraint matrices and vectors + tinyMatrix Alin_x; // Normal vectors for state linear constraints (numStateLinear x nx) + tinyVector blin_x; // Offset values for state linear constraints (numStateLinear x 1) + tinyMatrix Alin_u; // Normal vectors for input linear constraints (numInputLinear x nu) + tinyVector blin_u; // Offset values for input linear constraints (numInputLinear x 1) + + // Slack variables for linear constraints + tinyMatrix vl; // nx x N + tinyMatrix vlnew; // nx x N + tinyMatrix zl; // nu x N-1 + tinyMatrix zlnew; // nu x N-1 + + // Dual variables for linear constraints + tinyMatrix gl; // nx x N + tinyMatrix yl; // nu x N-1 + + + + // Q, R, A, B, f given by user + tinyVector Q; // nx x 1 + tinyVector R; // nu x 1 + tinyMatrix Adyn; // nx x nx (state transition matrix) + tinyMatrix Bdyn; // nx x nu (control matrix) + tinyVector fdyn; // nx x 1 (affine vector) + + // Reference trajectory to track for one horizon + tinyMatrix Xref; // nx x N + tinyMatrix Uref; // nu x N-1 + + // Temporaries + tinyVector Qu; // nu x 1 + + + + // Variables for keeping track of solve status + tinytype primal_residual_state; + tinytype primal_residual_input; + tinytype dual_residual_state; + tinytype dual_residual_input; + int status; + int iter; +} TinyWorkspace; + +/** + * Main TinyMPC solver structure that holds all information. + */ +typedef struct { + TinySolution *solution; // Solution + TinySettings *settings; // Problem settings + TinyCache *cache; // Problem cache + TinyWorkspace *work; // Solver workspace +} TinySolver; + + +// Add at the top with other definitions +#define BENCH_NX 12 +#define BENCH_NU 4 + +#ifdef __cplusplus +} +#endif From 276ac5e8fbd15aeef97219137af66e820a3e937e Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 3 Nov 2025 08:16:20 +0800 Subject: [PATCH 83/93] feat(planning): implement trajectory planner module --- configs/example.yaml | 10 ++ src/tongji/modules/planner/planner.cpp | 121 +++++++++++++++++++++++++ src/tongji/modules/planner/planner.hpp | 45 +++++++++ 3 files changed, 176 insertions(+) create mode 100644 src/tongji/modules/planner/planner.cpp create mode 100644 src/tongji/modules/planner/planner.hpp diff --git a/configs/example.yaml b/configs/example.yaml index cdeb620..b3b5462 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -42,3 +42,13 @@ leaving_angle: 20 # degree R_muzzle2camera: t_muzzle2camera: +#####-----planner parameters-----##### +fire_thresh: 0.003 + +max_yaw_acc: 50 +Q_yaw: [9e6, 0] +R_yaw: [1] + +max_pitch_acc: 100 +Q_pitch: [9e6,0] +R_pitch: [1] diff --git a/src/tongji/modules/planner/planner.cpp b/src/tongji/modules/planner/planner.cpp new file mode 100644 index 0000000..37d246a --- /dev/null +++ b/src/tongji/modules/planner/planner.cpp @@ -0,0 +1,121 @@ +#include "planner.hpp" + +#include + +#include +#include + +#include "tinympc/tiny_api.hpp" +#include "tongji/utils/math.hpp" +namespace world_exe::tongji::planner { + +class Planner::Impl { +public: + Impl(const std::string& config_path) { + auto yaml = YAML::LoadFile(config_path); + fire_thresh_ = yaml["fire_thresh"].as(); + SetYawSolver(config_path); + SetPitchSolver(config_path); + } + ~Impl() = default; + + auto Plan(double yaw0) -> const struct Plan { + // Solve yaw + Eigen::Vector2d x0; + x0 << trajectory_(0, 0), trajectory_(1, 0); + tiny_set_x0(yaw_solver_, x0); + + yaw_solver_->work->Xref = + trajectory_.block(0, 0, 2, HORIZON); + tiny_solve(yaw_solver_); + + // Solve pitch + x0 << trajectory_(2, 0), trajectory_(3, 0); + tiny_set_x0(pitch_solver_, x0); + + pitch_solver_->work->Xref = trajectory_.block(2, 0, 2, HORIZON); + tiny_solve(pitch_solver_); + + struct Plan plan; + plan.control = true; + plan.yaw = utils::math::clamp_pm_pi(yaw_solver_->work->x(0, HALF_HORIZON) + yaw0); + plan.yaw_vel = yaw_solver_->work->x(1, HALF_HORIZON); + plan.yaw_acc = yaw_solver_->work->u(0, HALF_HORIZON); + + plan.pitch = pitch_solver_->work->x(0, HALF_HORIZON); + plan.pitch_vel = pitch_solver_->work->x(1, HALF_HORIZON); + plan.pitch_acc = pitch_solver_->work->u(0, HALF_HORIZON); + + plan.fire = std::hypot(trajectory_(0, HALF_HORIZON + shoot_offset_) + - yaw_solver_->work->x(0, HALF_HORIZON + shoot_offset_), + trajectory_(2, HALF_HORIZON + shoot_offset_) + - pitch_solver_->work->x(0, HALF_HORIZON + shoot_offset_)) + < fire_thresh_; + return plan; + } + + auto + SetTrajectory(const Trajectory trajectory) -> void { + trajectory_ = std::move(trajectory); + } + +private: + auto SetYawSolver(const std::string& config_path) -> void { + auto yaml = YAML::LoadFile(config_path); + auto max_yaw_acc = yaml["max_yaw_acc"].as(); + auto Q_yaw = yaml["Q_yaw"].as>(); + auto R_yaw = yaml["R_yaw"].as>(); + + Eigen::Matrix A { { 1, DT }, { 0, 1 } }; + Eigen::Matrix B { { 0 }, { DT } }; + Eigen::Vector2d f { { 0, 0 } }; + Eigen::Matrix Q(Q_yaw.data()); + Eigen::Matrix R(R_yaw.data()); + tiny_setup(&yaw_solver_, A, B, f, Q.asDiagonal(), R.asDiagonal(), 1.0, 2, 1, HORIZON, 0); + + yaw_solver_->settings->max_iter = 10; + } + + auto SetPitchSolver(const std::string& config_path) -> void { + auto yaml = YAML::LoadFile(config_path); + auto max_pitch_acc = yaml["max_pitch_acc"].as(); + auto Q_pitch = yaml["Q_pitch"].as>(); + auto R_pitch = yaml["R_pitch"].as>(); + + Eigen::Matrix A { { 1, DT }, { 0, 1 } }; + Eigen::Matrix B { { 0 }, { DT } }; + Eigen::Vector2d f { 0, 0 }; + Eigen::Matrix Q(Q_pitch.data()); + Eigen::Matrix R(R_pitch.data()); + tiny_setup(&pitch_solver_, A, B, f, Q.asDiagonal(), R.asDiagonal(), 1.0, 2, 1, HORIZON, 0); + + auto x_min = Eigen::MatrixXd::Constant(2, HORIZON, -1e17); + auto x_max = Eigen::MatrixXd::Constant(2, HORIZON, 1e17); + auto u_min = Eigen::MatrixXd::Constant(1, HORIZON - 1, -max_pitch_acc); + auto u_max = Eigen::MatrixXd::Constant(1, HORIZON - 1, max_pitch_acc); + tiny_set_bound_constraints(pitch_solver_, x_min, x_max, u_min, u_max); + + pitch_solver_->settings->max_iter = 10; + } + + Trajectory trajectory_; + int shoot_offset_ = 2; + + double fire_thresh_; + TinySolver* yaw_solver_; + TinySolver* pitch_solver_; +}; + +Planner::Planner(const std::string& config_path) + : pimpl_(std::make_unique(config_path)) { } +Planner::~Planner() = default; + +auto Planner::SetTrajectory(const Trajectory trajectory) -> void { + return pimpl_->SetTrajectory(trajectory); +} + +auto Planner::Plan(double yaw0) -> const struct Plan { + return pimpl_->Plan(yaw0); +} + +} diff --git a/src/tongji/modules/planner/planner.hpp b/src/tongji/modules/planner/planner.hpp new file mode 100644 index 0000000..073dfdd --- /dev/null +++ b/src/tongji/modules/planner/planner.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include + +#include + +namespace world_exe::tongji::planner { + +constexpr double DT = 0.01; +constexpr int HALF_HORIZON = 50; +constexpr int HORIZON = HALF_HORIZON * 2; + +using Trajectory = Eigen::Matrix; // delta_yaw, yaw_vel, delta_pitch, pitch_vel + +struct Plan { + bool control; + bool fire; + float target_yaw; + float target_pitch; + float yaw; + float yaw_vel; + float yaw_acc; + float pitch; + float pitch_vel; + float pitch_acc; +}; + +class Planner final { +public: + explicit Planner(const std::string& config_path); + ~Planner(); + + auto Plan(const double yaw0) -> const Plan; + auto SetTrajectory(const Trajectory trajectory) -> void; + + Planner(const Planner&) = delete; + Planner& operator=(const Planner&) = delete; + Planner(Planner&&) noexcept = default; + Planner& operator=(Planner&&) noexcept = default; + +private: + class Impl; + std::unique_ptr pimpl_; +}; +} From f1609a8e5be48094cbbf0bc982699894d7cd80ad Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 6 Nov 2025 11:08:23 +0800 Subject: [PATCH 84/93] Revert "feat(planning): implement trajectory planner module" This reverts commit 276ac5e8fbd15aeef97219137af66e820a3e937e. --- configs/example.yaml | 10 -- src/tongji/modules/planner/planner.cpp | 121 ------------------------- src/tongji/modules/planner/planner.hpp | 45 --------- 3 files changed, 176 deletions(-) delete mode 100644 src/tongji/modules/planner/planner.cpp delete mode 100644 src/tongji/modules/planner/planner.hpp diff --git a/configs/example.yaml b/configs/example.yaml index b3b5462..cdeb620 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -42,13 +42,3 @@ leaving_angle: 20 # degree R_muzzle2camera: t_muzzle2camera: -#####-----planner parameters-----##### -fire_thresh: 0.003 - -max_yaw_acc: 50 -Q_yaw: [9e6, 0] -R_yaw: [1] - -max_pitch_acc: 100 -Q_pitch: [9e6,0] -R_pitch: [1] diff --git a/src/tongji/modules/planner/planner.cpp b/src/tongji/modules/planner/planner.cpp deleted file mode 100644 index 37d246a..0000000 --- a/src/tongji/modules/planner/planner.cpp +++ /dev/null @@ -1,121 +0,0 @@ -#include "planner.hpp" - -#include - -#include -#include - -#include "tinympc/tiny_api.hpp" -#include "tongji/utils/math.hpp" -namespace world_exe::tongji::planner { - -class Planner::Impl { -public: - Impl(const std::string& config_path) { - auto yaml = YAML::LoadFile(config_path); - fire_thresh_ = yaml["fire_thresh"].as(); - SetYawSolver(config_path); - SetPitchSolver(config_path); - } - ~Impl() = default; - - auto Plan(double yaw0) -> const struct Plan { - // Solve yaw - Eigen::Vector2d x0; - x0 << trajectory_(0, 0), trajectory_(1, 0); - tiny_set_x0(yaw_solver_, x0); - - yaw_solver_->work->Xref = - trajectory_.block(0, 0, 2, HORIZON); - tiny_solve(yaw_solver_); - - // Solve pitch - x0 << trajectory_(2, 0), trajectory_(3, 0); - tiny_set_x0(pitch_solver_, x0); - - pitch_solver_->work->Xref = trajectory_.block(2, 0, 2, HORIZON); - tiny_solve(pitch_solver_); - - struct Plan plan; - plan.control = true; - plan.yaw = utils::math::clamp_pm_pi(yaw_solver_->work->x(0, HALF_HORIZON) + yaw0); - plan.yaw_vel = yaw_solver_->work->x(1, HALF_HORIZON); - plan.yaw_acc = yaw_solver_->work->u(0, HALF_HORIZON); - - plan.pitch = pitch_solver_->work->x(0, HALF_HORIZON); - plan.pitch_vel = pitch_solver_->work->x(1, HALF_HORIZON); - plan.pitch_acc = pitch_solver_->work->u(0, HALF_HORIZON); - - plan.fire = std::hypot(trajectory_(0, HALF_HORIZON + shoot_offset_) - - yaw_solver_->work->x(0, HALF_HORIZON + shoot_offset_), - trajectory_(2, HALF_HORIZON + shoot_offset_) - - pitch_solver_->work->x(0, HALF_HORIZON + shoot_offset_)) - < fire_thresh_; - return plan; - } - - auto - SetTrajectory(const Trajectory trajectory) -> void { - trajectory_ = std::move(trajectory); - } - -private: - auto SetYawSolver(const std::string& config_path) -> void { - auto yaml = YAML::LoadFile(config_path); - auto max_yaw_acc = yaml["max_yaw_acc"].as(); - auto Q_yaw = yaml["Q_yaw"].as>(); - auto R_yaw = yaml["R_yaw"].as>(); - - Eigen::Matrix A { { 1, DT }, { 0, 1 } }; - Eigen::Matrix B { { 0 }, { DT } }; - Eigen::Vector2d f { { 0, 0 } }; - Eigen::Matrix Q(Q_yaw.data()); - Eigen::Matrix R(R_yaw.data()); - tiny_setup(&yaw_solver_, A, B, f, Q.asDiagonal(), R.asDiagonal(), 1.0, 2, 1, HORIZON, 0); - - yaw_solver_->settings->max_iter = 10; - } - - auto SetPitchSolver(const std::string& config_path) -> void { - auto yaml = YAML::LoadFile(config_path); - auto max_pitch_acc = yaml["max_pitch_acc"].as(); - auto Q_pitch = yaml["Q_pitch"].as>(); - auto R_pitch = yaml["R_pitch"].as>(); - - Eigen::Matrix A { { 1, DT }, { 0, 1 } }; - Eigen::Matrix B { { 0 }, { DT } }; - Eigen::Vector2d f { 0, 0 }; - Eigen::Matrix Q(Q_pitch.data()); - Eigen::Matrix R(R_pitch.data()); - tiny_setup(&pitch_solver_, A, B, f, Q.asDiagonal(), R.asDiagonal(), 1.0, 2, 1, HORIZON, 0); - - auto x_min = Eigen::MatrixXd::Constant(2, HORIZON, -1e17); - auto x_max = Eigen::MatrixXd::Constant(2, HORIZON, 1e17); - auto u_min = Eigen::MatrixXd::Constant(1, HORIZON - 1, -max_pitch_acc); - auto u_max = Eigen::MatrixXd::Constant(1, HORIZON - 1, max_pitch_acc); - tiny_set_bound_constraints(pitch_solver_, x_min, x_max, u_min, u_max); - - pitch_solver_->settings->max_iter = 10; - } - - Trajectory trajectory_; - int shoot_offset_ = 2; - - double fire_thresh_; - TinySolver* yaw_solver_; - TinySolver* pitch_solver_; -}; - -Planner::Planner(const std::string& config_path) - : pimpl_(std::make_unique(config_path)) { } -Planner::~Planner() = default; - -auto Planner::SetTrajectory(const Trajectory trajectory) -> void { - return pimpl_->SetTrajectory(trajectory); -} - -auto Planner::Plan(double yaw0) -> const struct Plan { - return pimpl_->Plan(yaw0); -} - -} diff --git a/src/tongji/modules/planner/planner.hpp b/src/tongji/modules/planner/planner.hpp deleted file mode 100644 index 073dfdd..0000000 --- a/src/tongji/modules/planner/planner.hpp +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#include - -#include - -namespace world_exe::tongji::planner { - -constexpr double DT = 0.01; -constexpr int HALF_HORIZON = 50; -constexpr int HORIZON = HALF_HORIZON * 2; - -using Trajectory = Eigen::Matrix; // delta_yaw, yaw_vel, delta_pitch, pitch_vel - -struct Plan { - bool control; - bool fire; - float target_yaw; - float target_pitch; - float yaw; - float yaw_vel; - float yaw_acc; - float pitch; - float pitch_vel; - float pitch_acc; -}; - -class Planner final { -public: - explicit Planner(const std::string& config_path); - ~Planner(); - - auto Plan(const double yaw0) -> const Plan; - auto SetTrajectory(const Trajectory trajectory) -> void; - - Planner(const Planner&) = delete; - Planner& operator=(const Planner&) = delete; - Planner(Planner&&) noexcept = default; - Planner& operator=(Planner&&) noexcept = default; - -private: - class Impl; - std::unique_ptr pimpl_; -}; -} From 73ac1735141d449f1dc80d4f23e34c55fb938cc7 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 6 Nov 2025 11:10:20 +0800 Subject: [PATCH 85/93] Revert "feat(planning): add tiny_mpc library for trajectory planning" This reverts commit 372885d2b0d572f1bfc34852db8400e3c2f1faef. --- .../modules/planner/tinympc/CMakeLists.txt | 43 -- src/tongji/modules/planner/tinympc/admm.cpp | 391 -------------- src/tongji/modules/planner/tinympc/admm.hpp | 37 -- .../modules/planner/tinympc/codegen.cpp | 447 ---------------- .../modules/planner/tinympc/codegen.hpp | 23 - src/tongji/modules/planner/tinympc/error.hpp | 29 -- .../modules/planner/tinympc/rho_benchmark.cpp | 250 --------- .../modules/planner/tinympc/rho_benchmark.hpp | 95 ---- .../modules/planner/tinympc/tiny_api.cpp | 476 ------------------ .../modules/planner/tinympc/tiny_api.hpp | 62 --- .../planner/tinympc/tiny_api_constants.hpp | 14 - src/tongji/modules/planner/tinympc/types.hpp | 204 -------- 12 files changed, 2071 deletions(-) delete mode 100755 src/tongji/modules/planner/tinympc/CMakeLists.txt delete mode 100755 src/tongji/modules/planner/tinympc/admm.cpp delete mode 100755 src/tongji/modules/planner/tinympc/admm.hpp delete mode 100755 src/tongji/modules/planner/tinympc/codegen.cpp delete mode 100755 src/tongji/modules/planner/tinympc/codegen.hpp delete mode 100644 src/tongji/modules/planner/tinympc/error.hpp delete mode 100644 src/tongji/modules/planner/tinympc/rho_benchmark.cpp delete mode 100644 src/tongji/modules/planner/tinympc/rho_benchmark.hpp delete mode 100755 src/tongji/modules/planner/tinympc/tiny_api.cpp delete mode 100755 src/tongji/modules/planner/tinympc/tiny_api.hpp delete mode 100644 src/tongji/modules/planner/tinympc/tiny_api_constants.hpp delete mode 100755 src/tongji/modules/planner/tinympc/types.hpp diff --git a/src/tongji/modules/planner/tinympc/CMakeLists.txt b/src/tongji/modules/planner/tinympc/CMakeLists.txt deleted file mode 100755 index b8654af..0000000 --- a/src/tongji/modules/planner/tinympc/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -add_library(tinympcstatic STATIC - admm.cpp - tiny_api.cpp - codegen.cpp - rho_benchmark.cpp -) - -set_property(TARGET tinympcstatic PROPERTY POSITION_INDEPENDENT_CODE ON) - -# target_link_libraries(tinympcstatic PUBLIC Eigen) -target_include_directories(tinympcstatic PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/..) -target_include_directories(tinympcstatic PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../../include/Eigen) - - - -if(USING_CODEGEN) # Defined in top-level CMakeLists.txt - -# Files that are needed for embedded code generation -list( APPEND EMBEDDED_FILES - "${CMAKE_CURRENT_SOURCE_DIR}/admm.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/admm.hpp" - "${CMAKE_CURRENT_SOURCE_DIR}/tiny_api.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/tiny_api.hpp" - "${CMAKE_CURRENT_SOURCE_DIR}/types.hpp" - "${CMAKE_CURRENT_SOURCE_DIR}/tiny_api_constants.hpp" ) - - -foreach( f ${EMBEDDED_FILES} ) - get_filename_component( fname ${f} NAME ) - - set( dest_file "${EMBEDDED_BUILD_TINYMPC_DIR}/${fname}" ) - list( APPEND EMBEDDED_BUILD_TINYMPC_FILES "${dest_file}" ) - - add_custom_command(OUTPUT ${dest_file} - COMMAND ${CMAKE_COMMAND} -E copy "${f}" "${dest_file}" - DEPENDS ${f} - COMMENT "Copying ${fname}") -endforeach() - -add_custom_target( copy_codegen_tinympc_files DEPENDS ${EMBEDDED_BUILD_TINYMPC_FILES} ) -add_dependencies( copy_codegen_files copy_codegen_tinympc_files ) - -endif(USING_CODEGEN) diff --git a/src/tongji/modules/planner/tinympc/admm.cpp b/src/tongji/modules/planner/tinympc/admm.cpp deleted file mode 100755 index 7051a2b..0000000 --- a/src/tongji/modules/planner/tinympc/admm.cpp +++ /dev/null @@ -1,391 +0,0 @@ -#include - -#include "admm.hpp" -#include "rho_benchmark.hpp" - -#define DEBUG_MODULE "TINYALG" - -extern "C" { - -/** - * Update linear terms from Riccati backward pass -*/ -void backward_pass_grad(TinySolver *solver) -{ - for (int i = solver->work->N - 2; i >= 0; i--) - { - (solver->work->d.col(i)).noalias() = solver->cache->Quu_inv * (solver->work->Bdyn.transpose() * solver->work->p.col(i + 1) + solver->work->r.col(i) + solver->cache->BPf); - (solver->work->p.col(i)).noalias() = solver->work->q.col(i) + solver->cache->AmBKt.lazyProduct(solver->work->p.col(i + 1)) - (solver->cache->Kinf.transpose()).lazyProduct(solver->work->r.col(i)) + solver->cache->APf; - } -} - -/** - * Use LQR feedback policy to roll out trajectory -*/ -void forward_pass(TinySolver *solver) -{ - for (int i = 0; i < solver->work->N - 1; i++) - { - (solver->work->u.col(i)).noalias() = -solver->cache->Kinf.lazyProduct(solver->work->x.col(i)) - solver->work->d.col(i); - (solver->work->x.col(i + 1)).noalias() = solver->work->Adyn.lazyProduct(solver->work->x.col(i)) + solver->work->Bdyn.lazyProduct(solver->work->u.col(i)) + solver->work->fdyn; - } -} - -/** - * Project a vector s onto the second order cone defined by mu - * @param s, mu - * @return projection onto cone if s is outside cone. Return s if s is inside cone. -*/ -tinyVector project_soc(tinyVector s, float mu) { - tinytype u0 = s(Eigen::placeholders::last) * mu; - tinyVector u1 = s.head(s.rows()-1); - float a = u1.norm(); - tinyVector cone_origin(s.rows()); - cone_origin.setZero(); - - if (a <= -u0) { // below cone - return cone_origin; - } - else if (a <= u0) { // in cone - return s; - } - else if (a >= abs(u0)) { // outside cone - Matrix u2(u1.size() + 1); - u2 << u1, a/mu; - return 0.5 * (1 + u0/a) * u2; - } - else { - return cone_origin; - } -} - -/** - * Project a vector z onto a hyperplane defined by a^T z = b - * Implements equation (21): ΠH(z) = z - (⟨z, a⟩ − b)/||a||² * a - * @param z Vector to project - * @param a Normal vector of the hyperplane - * @param b Offset of the hyperplane - * @return Projection of z onto the hyperplane - */ -tinyVector project_hyperplane(const tinyVector& z, const tinyVector& a, tinytype b) { - tinytype dist = (a.dot(z) - b) / a.squaredNorm(); - return z - dist * a; -} - -/** - * Project slack (auxiliary) variables into their feasible domain, defined by - * projection functions related to each constraint - * TODO: pass in meta information with each constraint assigning it to a - * projection function -*/ -void update_slack(TinySolver *solver) -{ - - // Update bound constraint slack variables for state - solver->work->vnew = solver->work->x + solver->work->g; - - // Update bound constraint slack variables for input - solver->work->znew = solver->work->u + solver->work->y; - - // Box constraints on state - if (solver->settings->en_state_bound) { - solver->work->vnew = solver->work->x_max.cwiseMin(solver->work->x_min.cwiseMax(solver->work->vnew)); - } - - // Box constraints on input - if (solver->settings->en_input_bound) { - solver->work->znew = solver->work->u_max.cwiseMin(solver->work->u_min.cwiseMax(solver->work->znew)); - } - - - // Update second order cone slack variables for state - if (solver->settings->en_state_soc && solver->work->numStateCones > 0) { - solver->work->vcnew = solver->work->x + solver->work->gc; - } - - // Update second order cone slack variables for input - if (solver->settings->en_input_soc && solver->work->numInputCones > 0) { - solver->work->zcnew = solver->work->u + solver->work->yc; - } - - // Cone constraints on state - if (solver->settings->en_state_soc) { - for (int i=0; iwork->N; i++) { - for (int k=0; kwork->numStateCones; k++) { - int start = solver->work->Acx(k); - int num_xs = solver->work->qcx(k); - tinytype mu = solver->work->cx(k); - tinyVector col = solver->work->vcnew.block(start, i, num_xs, 1); - solver->work->vcnew.block(start, i, num_xs, 1) = project_soc(col, mu); - } - } - } - - // Cone constraints on input - if (solver->settings->en_input_soc) { - for (int i=0; iwork->N-1; i++) { - for (int k=0; kwork->numInputCones; k++) { - int start = solver->work->Acu(k); - int num_us = solver->work->qcu(k); - tinytype mu = solver->work->cu(k); - tinyVector col = solver->work->zcnew.block(start, i, num_us, 1); - solver->work->zcnew.block(start, i, num_us, 1) = project_soc(col, mu); - } - } - } - - // Update linear constraint slack variables for state - if (solver->settings->en_state_linear) { - solver->work->vlnew = solver->work->x + solver->work->gl; - } - - // Update linear constraint slack variables for input - if (solver->settings->en_input_linear) { - solver->work->zlnew = solver->work->u + solver->work->yl; - } - - // Linear constraints on state - if (solver->settings->en_state_linear) { - for (int i=0; iwork->N; i++) { - for (int k=0; kwork->numStateLinear; k++) { - tinyVector a = solver->work->Alin_x.row(k); - tinytype b = solver->work->blin_x(k); - tinytype constraint_value = a.dot(solver->work->vlnew.col(i)); - if (constraint_value > b) { // Only project if constraint is violated - solver->work->vlnew.col(i) = project_hyperplane(solver->work->vlnew.col(i), a, b); - } - } - } - } - - // Linear constraints on input - if (solver->settings->en_input_linear) { - for (int i=0; iwork->N-1; i++) { - for (int k=0; kwork->numInputLinear; k++) { - tinyVector a = solver->work->Alin_u.row(k); - tinytype b = solver->work->blin_u(k); - tinytype constraint_value = a.dot(solver->work->zlnew.col(i)); - if (constraint_value > b) { // Only project if constraint is violated - solver->work->zlnew.col(i) = project_hyperplane(solver->work->zlnew.col(i), a, b); - } - } - } - } - -} - -/** - * Update next iteration of dual variables by performing the augmented - * lagrangian multiplier update -*/ -void update_dual(TinySolver *solver) -{ - // Update bound constraint dual variables for state - solver->work->g = solver->work->g + solver->work->x - solver->work->vnew; - - // Update bound constraint dual variables for input - solver->work->y = solver->work->y + solver->work->u - solver->work->znew; - - // Update second order cone dual variables for state - if (solver->settings->en_state_soc && solver->work->numStateCones > 0) { - solver->work->gc = solver->work->gc + solver->work->x - solver->work->vcnew; - } - - // Update second order cone dual variables for input - if (solver->settings->en_input_soc && solver->work->numInputCones > 0) { - solver->work->yc = solver->work->yc + solver->work->u - solver->work->zcnew; - } - - // Update linear constraint dual variables for state - if (solver->settings->en_state_linear) { - solver->work->gl = solver->work->gl + solver->work->x - solver->work->vlnew; - } - - // Update linear constraint dual variables for input - if (solver->settings->en_input_linear) { - solver->work->yl = solver->work->yl + solver->work->u - solver->work->zlnew; - } -} - -/** - * Update linear control cost terms in the Riccati feedback using the changing - * slack and dual variables from ADMM -*/ -void update_linear_cost(TinySolver *solver) -{ - - // Update state cost terms - solver->work->q = -(solver->work->Xref.array().colwise() * solver->work->Q.array()); - (solver->work->q).noalias() -= solver->cache->rho * (solver->work->vnew - solver->work->g); - if (solver->settings->en_state_soc && solver->work->numStateCones > 0) { - (solver->work->q).noalias() -= solver->cache->rho * (solver->work->vcnew - solver->work->gc); - } - if (solver->settings->en_state_linear) { - (solver->work->q).noalias() -= solver->cache->rho * (solver->work->vlnew - solver->work->gl); - } - - // Update input cost terms - solver->work->r = -(solver->work->Uref.array().colwise() * solver->work->R.array()); - (solver->work->r).noalias() -= solver->cache->rho * (solver->work->znew - solver->work->y); - if (solver->settings->en_input_soc && solver->work->numInputCones > 0) { - (solver->work->r).noalias() -= solver->cache->rho * (solver->work->zcnew - solver->work->yc); - } - if (solver->settings->en_input_linear) { - (solver->work->r).noalias() -= solver->cache->rho * (solver->work->zlnew - solver->work->yl); - } - - // Update terminal cost - solver->work->p.col(solver->work->N - 1) = -(solver->work->Xref.col(solver->work->N - 1).transpose().lazyProduct(solver->cache->Pinf)); - (solver->work->p.col(solver->work->N - 1)).noalias() -= solver->cache->rho * (solver->work->vnew.col(solver->work->N - 1) - solver->work->g.col(solver->work->N - 1)); - - if (solver->settings->en_state_soc && solver->work->numStateCones > 0) { - solver->work->p.col(solver->work->N - 1) -= solver->cache->rho * (solver->work->vcnew.col(solver->work->N - 1) - solver->work->gc.col(solver->work->N - 1)); - } - if (solver->settings->en_state_linear) { - solver->work->p.col(solver->work->N - 1) -= solver->cache->rho * (solver->work->vlnew.col(solver->work->N - 1) - solver->work->gl.col(solver->work->N - 1)); - } -} - -/** - * Check for termination condition by evaluating whether the largest absolute - * primal and dual residuals for states and inputs are below threhold. -*/ -bool termination_condition(TinySolver *solver) -{ - if (solver->work->iter % solver->settings->check_termination == 0) - { - solver->work->primal_residual_state = (solver->work->x - solver->work->vnew).cwiseAbs().maxCoeff(); - solver->work->dual_residual_state = ((solver->work->v - solver->work->vnew).cwiseAbs().maxCoeff()) * solver->cache->rho; - solver->work->primal_residual_input = (solver->work->u - solver->work->znew).cwiseAbs().maxCoeff(); - solver->work->dual_residual_input = ((solver->work->z - solver->work->znew).cwiseAbs().maxCoeff()) * solver->cache->rho; - - if (solver->work->primal_residual_state < solver->settings->abs_pri_tol && - solver->work->primal_residual_input < solver->settings->abs_pri_tol && - solver->work->dual_residual_state < solver->settings->abs_dua_tol && - solver->work->dual_residual_input < solver->settings->abs_dua_tol) - { - return true; - } - } - return false; -} - - -int solve(TinySolver *solver) -{ - // Initialize variables - solver->solution->solved = 0; - solver->solution->iter = 0; - solver->work->status = 11; // TINY_UNSOLVED - solver->work->iter = 0; - - // Setup for adaptive rho - RhoAdapter adapter; - adapter.rho_min = solver->settings->adaptive_rho_min; - adapter.rho_max = solver->settings->adaptive_rho_max; - adapter.clip = solver->settings->adaptive_rho_enable_clipping; - - RhoBenchmarkResult rho_result; - - // Store previous values for residuals - tinyMatrix v_prev = solver->work->vnew; - tinyMatrix z_prev = solver->work->znew; - - // Initialize SOC slack variables if needed - if (solver->settings->en_state_soc && solver->work->numStateCones > 0) { - solver->work->vcnew = solver->work->x; - } - - if (solver->settings->en_input_soc && solver->work->numInputCones > 0) { - solver->work->zcnew = solver->work->u; - } - - // Initialize linear constraint slack variables if needed - if (solver->settings->en_state_linear) { - solver->work->vlnew = solver->work->x; - } - - if (solver->settings->en_input_linear) { - solver->work->zlnew = solver->work->u; - } - - for (int i = 0; i < solver->settings->max_iter; i++) - { - // Solve linear system with Riccati and roll out to get new trajectory - backward_pass_grad(solver); - - forward_pass(solver); - - // Project slack variables into feasible domain - update_slack(solver); - - // Compute next iteration of dual variables - update_dual(solver); - - // Update linear control cost terms using reference trajectory, duals, and slack variables - update_linear_cost(solver); - - solver->work->iter += 1; - - // Handle adaptive rho if enabled - if (solver->settings->adaptive_rho) { - // Calculate residuals for adaptive rho - tinytype pri_res_input = (solver->work->u - solver->work->znew).cwiseAbs().maxCoeff(); - tinytype pri_res_state = (solver->work->x - solver->work->vnew).cwiseAbs().maxCoeff(); - tinytype dua_res_input = solver->cache->rho * (solver->work->znew - z_prev).cwiseAbs().maxCoeff(); - tinytype dua_res_state = solver->cache->rho * (solver->work->vnew - v_prev).cwiseAbs().maxCoeff(); - - // Update rho every 5 iterations - if (i > 0 && i % 5 == 0) { - benchmark_rho_adaptation( - &adapter, - solver->work->x, - solver->work->u, - solver->work->vnew, - solver->work->znew, - solver->work->g, - solver->work->y, - solver->cache, - solver->work, - solver->work->N, - &rho_result - ); - - // Update matrices using Taylor expansion - update_matrices_with_derivatives(solver->cache, rho_result.final_rho); - } - } - - // Store previous values for next iteration - z_prev = solver->work->znew; - v_prev = solver->work->vnew; - - // Check for whether cost is minimized by calculating residuals - if (termination_condition(solver)) { - solver->work->status = 1; // TINY_SOLVED - - // Save solution - solver->solution->iter = solver->work->iter; - solver->solution->solved = 1; - solver->solution->x = solver->work->vnew; - solver->solution->u = solver->work->znew; - - // std::cout << "Solver converged in " << solver->work->iter << " iterations" << std::endl; - - return 0; - } - - // Save previous slack variables - solver->work->v = solver->work->vnew; - solver->work->z = solver->work->znew; - - } - - solver->solution->iter = solver->work->iter; - solver->solution->solved = 0; - solver->solution->x = solver->work->vnew; - solver->solution->u = solver->work->znew; - return 1; -} - -} /* extern "C" */ diff --git a/src/tongji/modules/planner/tinympc/admm.hpp b/src/tongji/modules/planner/tinympc/admm.hpp deleted file mode 100755 index f07536d..0000000 --- a/src/tongji/modules/planner/tinympc/admm.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include "types.hpp" - -#ifdef __cplusplus -extern "C" { -#endif - -int solve(TinySolver *solver); - -void update_primal(TinySolver *solver); -void backward_pass_grad(TinySolver *solver); -void forward_pass(TinySolver *solver); -void update_slack(TinySolver *solver); -void update_dual(TinySolver *solver); -void update_linear_cost(TinySolver *solver); -bool termination_condition(TinySolver *solver); - -/** - * Project a vector s onto the second order cone defined by mu - * @param s, mu - * @return projection onto cone if s is outside cone. Return s if s is inside cone. -*/ -tinyVector project_soc(tinyVector s, float mu); - -/** - * Project a vector z onto a hyperplane defined by a^T z = b - * Implements equation (21): ΠH(z) = z - (⟨z, a⟩ − b)/||a||² * a - * @param z Vector to project - * @param a Normal vector of the hyperplane - * @param b Offset of the hyperplane - * @return Projection of z onto the hyperplane - */ -tinyVector project_hyperplane(const tinyVector& z, const tinyVector& a, tinytype b); -#ifdef __cplusplus -} -#endif \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/codegen.cpp b/src/tongji/modules/planner/tinympc/codegen.cpp deleted file mode 100755 index cdc91f3..0000000 --- a/src/tongji/modules/planner/tinympc/codegen.cpp +++ /dev/null @@ -1,447 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -//#include -#include "error.hpp" - -#include -#include - -// #include "types.hpp" -#include "codegen.hpp" - -#ifdef __MINGW32__ -#include -inline int mkdir(const char *pathname, int flags) { - return _mkdir(pathname); -} -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -/* Define the maximum allowed length of the path (directory + filename + extension) */ -#define PATH_LENGTH 2048 - -using namespace Eigen; - -static void print_matrix(FILE *f, MatrixXd mat, int num_elements) -{ - // Check if matrix is uninitialized or too small - if (mat.size() == 0 || mat.size() < num_elements) { - // Print zeros for all elements - for (int i = 0; i < num_elements; i++) { - fprintf(f, "(tinytype)0.0000000000000000"); - if (i < num_elements - 1) - fprintf(f, ","); - } - return; - } - - // Matrix is properly initialized and has enough elements - for (int i = 0; i < num_elements; i++) { - fprintf(f, "(tinytype)%.16f", mat.reshaped()[i]); - if (i < num_elements - 1) - fprintf(f, ","); - } -} - - -static void create_directory(const char* dir, int verbose) { - // Attempt to create directory - if (mkdir(dir, S_IRWXU|S_IRWXG|S_IROTH)) { - if (errno == EEXIST) { // Skip if directory already exists - if (verbose) - std::cout << dir << " already exists, skipping." << std::endl; - } else { - ERROR_MSG(EXIT_FAILURE, "Failed to create directory %s", dir); - } - } -} - -// TODO: Make this fail if tiny_setup has not already been called -int tiny_codegen(TinySolver* solver, const char* output_dir, int verbose) { - if (!solver) { - std::cout << "Error in tiny_codegen: solver is nullptr" << std::endl; - return 1; - } - int status = 0; - status |= codegen_create_directories(output_dir, verbose); - status |= codegen_data_header(output_dir, verbose); - status |= codegen_data_source(solver, output_dir, verbose); - status |= codegen_example(output_dir, verbose); - - return status; -} - -int tiny_codegen_with_sensitivity(TinySolver* solver, const char* output_dir, - tinyMatrix* dK, tinyMatrix* dP, - tinyMatrix* dC1, tinyMatrix* dC2, int verbose) { - if (!solver) { - std::cout << "Error in tiny_codegen_with_sensitivity: solver is nullptr" << std::endl; - return 1; - } - - // Only store sensitivity matrices if adaptive rho is enabled - if (solver->settings->adaptive_rho) { - // Store the sensitivity matrices in the solver's cache - solver->cache->dKinf_drho = *dK; - solver->cache->dPinf_drho = *dP; - solver->cache->dC1_drho = *dC1; - solver->cache->dC2_drho = *dC2; - } - - // Call the regular codegen function which will now include the sensitivity matrices if adaptive_rho is enabled - return tiny_codegen(solver, output_dir, verbose); -} - -// Create code generation folder structure in whichever directory the executable calling tiny_codegen was called -int codegen_create_directories(const char* output_dir, int verbose) { - - // Create output folder (root folder for code generation) - create_directory(output_dir, verbose); - - // Create src folder - char src_dir[PATH_LENGTH]; - sprintf(src_dir, "%s/src/", output_dir); - create_directory(src_dir, verbose); - - // Create tinympc folder - char tinympc_dir[PATH_LENGTH]; - sprintf(tinympc_dir, "%s/tinympc/", output_dir); - create_directory(tinympc_dir, verbose); - - // // Create include folder - // char inc_dir[PATH_LENGTH]; - // sprintf(inc_dir, "%s/include/", output_dir); - // create_directory(inc_dir, verbose); - - return EXIT_SUCCESS; -} - -// Create inc/tiny_data.hpp file -int codegen_data_header(const char* output_dir, int verbose) { - char data_hpp_fname[PATH_LENGTH]; - FILE *data_hpp_f; - - sprintf(data_hpp_fname, "%s/tinympc/tiny_data.hpp", output_dir); - - // Open data header file - data_hpp_f = fopen(data_hpp_fname, "w+"); - if (data_hpp_f == NULL) - ERROR_MSG(EXIT_FAILURE, "Failed to open file %s", data_hpp_fname); - - // Preamble - time_t start_time; - time(&start_time); - fprintf(data_hpp_f, "/*\n"); - fprintf(data_hpp_f, " * This file was autogenerated by TinyMPC on %s", ctime(&start_time)); - fprintf(data_hpp_f, " */\n\n"); - - fprintf(data_hpp_f, "#pragma once\n\n"); - - fprintf(data_hpp_f, "#include \"types.hpp\"\n\n"); - - fprintf(data_hpp_f, "#ifdef __cplusplus\n"); - fprintf(data_hpp_f, "extern \"C\" {\n"); - fprintf(data_hpp_f, "#endif\n\n"); - - fprintf(data_hpp_f, "extern TinySolver tiny_solver;\n\n"); - - fprintf(data_hpp_f, "#ifdef __cplusplus\n"); - fprintf(data_hpp_f, "}\n"); - fprintf(data_hpp_f, "#endif\n"); - - // Close codegen data header file - fclose(data_hpp_f); - - if (verbose) { - printf("Data header generated in %s\n", data_hpp_fname); - } - return 0; -} - -// Create src/tiny_data.cpp file -int codegen_data_source(TinySolver* solver, const char* output_dir, int verbose) { - char data_cpp_fname[PATH_LENGTH]; - FILE *data_cpp_f; - - int nx = solver->work->nx; - int nu = solver->work->nu; - int N = solver->work->N; - - sprintf(data_cpp_fname, "%s/src/tiny_data.cpp", output_dir); - - // Open data source file - data_cpp_f = fopen(data_cpp_fname, "w+"); - if (data_cpp_f == NULL) - ERROR_MSG(EXIT_FAILURE, "Failed to open file %s", data_cpp_fname); - - // Preamble - time_t start_time; - time(&start_time); - fprintf(data_cpp_f, "/*\n"); - fprintf(data_cpp_f, " * This file was autogenerated by TinyMPC on %s", ctime(&start_time)); - fprintf(data_cpp_f, " */\n\n"); - - // Open extern C - fprintf(data_cpp_f, "#include \"tinympc/tiny_data.hpp\"\n\n"); - fprintf(data_cpp_f, "#ifdef __cplusplus\n"); - fprintf(data_cpp_f, "extern \"C\" {\n"); - fprintf(data_cpp_f, "#endif\n\n"); - - // Solution - fprintf(data_cpp_f, "/* Solution */\n"); - fprintf(data_cpp_f, "TinySolution solution = {\n"); - - fprintf(data_cpp_f, "\t%d,\t\t// iter\n", solver->solution->iter); - fprintf(data_cpp_f, "\t%d,\t\t// solved\n", solver->solution->solved); - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); - print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); - fprintf(data_cpp_f, ").finished(),\t// x\n"); // x solution - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); - print_matrix(data_cpp_f, MatrixXd::Zero(nu, N-1), nu * (N-1)); - fprintf(data_cpp_f, ").finished(),\t// x\n"); // u solution - - fprintf(data_cpp_f, "};\n\n"); - - // Cache - fprintf(data_cpp_f, "/* Matrices that must be recomputed with changes in time step, rho */\n"); - fprintf(data_cpp_f, "TinyCache cache = {\n"); - - fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// rho (step size/penalty)\n", solver->cache->rho); - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, nx); - print_matrix(data_cpp_f, solver->cache->Kinf, nu * nx); - fprintf(data_cpp_f, ").finished(),\t// Kinf\n"); // Kinf - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); - print_matrix(data_cpp_f, solver->cache->Pinf, nx * nx); - fprintf(data_cpp_f, ").finished(),\t// Pinf\n"); // Pinf - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, nu); - print_matrix(data_cpp_f, solver->cache->Quu_inv, nu * nu); - fprintf(data_cpp_f, ").finished(),\t// Quu_inv\n"); // Quu_inv - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); - print_matrix(data_cpp_f, solver->cache->AmBKt, nx * nx); - fprintf(data_cpp_f, ").finished(),\t// AmBKt\n"); // AmBKt - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); - print_matrix(data_cpp_f, solver->cache->C1, nx * nx); - fprintf(data_cpp_f, ").finished(),\t// C1\n"); // C1 - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); - print_matrix(data_cpp_f, solver->cache->C2, nx * nx); - fprintf(data_cpp_f, ").finished()"); // C2, no comma if no sensitivity matrices - - // Only print sensitivity matrices if adaptive rho is enabled - if (solver->settings->adaptive_rho) { - fprintf(data_cpp_f, ",\t// C2\n"); // Add comma and comment for C2 if we have more matrices - - // Add sensitivity matrices within the struct initialization - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, nx); - print_matrix(data_cpp_f, solver->cache->dKinf_drho, nu * nx); - fprintf(data_cpp_f, ").finished(),\t// dKinf_drho\n"); // dKinf_drho - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); - print_matrix(data_cpp_f, solver->cache->dPinf_drho, nx * nx); - fprintf(data_cpp_f, ").finished(),\t// dPinf_drho\n"); // dPinf_drho - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); - print_matrix(data_cpp_f, solver->cache->dC1_drho, nx * nx); - fprintf(data_cpp_f, ").finished(),\t// dC1_drho\n"); // dC1_drho - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); - print_matrix(data_cpp_f, solver->cache->dC2_drho, nx * nx); - fprintf(data_cpp_f, ").finished()\t// dC2_drho\n"); // dC2_drho - } else { - fprintf(data_cpp_f, "\t// C2\n"); // Just add comment for C2 - } - - fprintf(data_cpp_f, "};\n\n"); - - // Settings - fprintf(data_cpp_f, "/* User settings */\n"); - fprintf(data_cpp_f, "TinySettings settings = {\n"); - - fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// primal tolerance\n", solver->settings->abs_pri_tol); - fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// dual tolerance\n", solver->settings->abs_dua_tol); - fprintf(data_cpp_f, "\t%d,\t\t// max iterations\n", solver->settings->max_iter); - fprintf(data_cpp_f, "\t%d,\t\t// iterations per termination check\n", solver->settings->check_termination); - fprintf(data_cpp_f, "\t%d,\t\t// enable state constraints\n", solver->settings->en_state_bound); - fprintf(data_cpp_f, "\t%d\t\t// enable input constraints\n", solver->settings->en_input_bound); - - fprintf(data_cpp_f, "};\n\n"); - - // Workspace - fprintf(data_cpp_f, "/* Problem variables */\n"); - fprintf(data_cpp_f, "TinyWorkspace work = {\n"); - - fprintf(data_cpp_f, "\t%d,\t// Number of states\n", nx); - fprintf(data_cpp_f, "\t%d,\t// Number of control inputs\n", nu); - fprintf(data_cpp_f, "\t%d,\t// Number of knotpoints in the horizon\n", N); - - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); - print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); - fprintf(data_cpp_f, ").finished(),\t// x\n"); // x - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); - print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); - fprintf(data_cpp_f, ").finished(),\t// u\n"); // u - - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); - print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); - fprintf(data_cpp_f, ").finished(),\t// q\n"); // q - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); - print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); - fprintf(data_cpp_f, ").finished(),\t// r\n"); // r - - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); - print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); - fprintf(data_cpp_f, ").finished(),\t// p\n"); // p - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); - print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); - fprintf(data_cpp_f, ").finished(),\t// d\n"); // d - - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); - print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); - fprintf(data_cpp_f, ").finished(),\t// v\n"); // v - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); - print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); - fprintf(data_cpp_f, ").finished(),\t// vnew\n"); // vnew - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); - print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); - fprintf(data_cpp_f, ").finished(),\t// z\n"); // z - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); - print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); - fprintf(data_cpp_f, ").finished(),\t// znew\n"); // znew - - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); - print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); - fprintf(data_cpp_f, ").finished(),\t// g\n"); // g - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); - print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); - fprintf(data_cpp_f, ").finished(),\t// y\n"); // y - - fprintf(data_cpp_f, "\t(tinyVector(%d) << ", nx); - print_matrix(data_cpp_f, solver->work->Q, nx); - fprintf(data_cpp_f, ").finished(),\t// Q\n"); // Q - fprintf(data_cpp_f, "\t(tinyVector(%d) << ", nu); - print_matrix(data_cpp_f, solver->work->R, nu); - fprintf(data_cpp_f, ").finished(),\t// R\n"); // R - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nx); - print_matrix(data_cpp_f, solver->work->Adyn, nx * nx); - fprintf(data_cpp_f, ").finished(),\t// Adyn\n"); // Adyn - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, nu); - print_matrix(data_cpp_f, solver->work->Bdyn, nx * nu); - fprintf(data_cpp_f, ").finished(),\t// Bdyn\n"); // Bdyn - - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); - print_matrix(data_cpp_f, solver->work->x_min, nx * N); - fprintf(data_cpp_f, ").finished(),\t// x_min\n"); // x_min - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); - print_matrix(data_cpp_f, solver->work->x_max, nx * N); - fprintf(data_cpp_f, ").finished(),\t// x_max\n"); // x_max - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); - print_matrix(data_cpp_f, solver->work->u_min, nu * (N-1)); - fprintf(data_cpp_f, ").finished(),\t// u_min\n"); // u_min - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); - print_matrix(data_cpp_f, solver->work->u_max, nu * (N-1)); - fprintf(data_cpp_f, ").finished(),\t// u_max\n"); // u_max - - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nx, N); - print_matrix(data_cpp_f, MatrixXd::Zero(nx, N), nx * N); - fprintf(data_cpp_f, ").finished(),\t// Xref\n"); // Xref - fprintf(data_cpp_f, "\t(tinyMatrix(%d, %d) << ", nu, N-1); - print_matrix(data_cpp_f, MatrixXd::Zero(nu, N - 1), nu * (N-1)); - fprintf(data_cpp_f, ").finished(),\t// Uref\n"); // Uref - - fprintf(data_cpp_f, "\t(tinyVector(%d) << ", nu); - print_matrix(data_cpp_f, MatrixXd::Zero(nu, 1), nu); - fprintf(data_cpp_f, ").finished(),\t// Qu\n"); // Qu - - fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// state primal residual\n", 0.0); - fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// input primal residual\n", 0.0); - fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// state dual residual\n", 0.0); - fprintf(data_cpp_f, "\t(tinytype)%.16f,\t// input dual residual\n", 0.0); - fprintf(data_cpp_f, "\t%d,\t// solve status\n", 0); - fprintf(data_cpp_f, "\t%d,\t// solve iteration\n", 0); - - fprintf(data_cpp_f, "};\n\n"); - - // Write solver struct definition to workspace file - fprintf(data_cpp_f, "TinySolver tiny_solver = {&solution, &settings, &cache, &work};\n\n"); - - // Close extern C - fprintf(data_cpp_f, "#ifdef __cplusplus\n"); - fprintf(data_cpp_f, "}\n"); - fprintf(data_cpp_f, "#endif\n\n"); - - // Close codegen data file - fclose(data_cpp_f); - if (verbose) { - printf("Data generated in %s\n", data_cpp_fname); - } - return 0; -} - -int codegen_example(const char* output_dir, int verbose) { - char example_cpp_fname[PATH_LENGTH]; - FILE *example_cpp_f; - - sprintf(example_cpp_fname, "%s/src/tiny_main.cpp", output_dir); - - // Open example file - example_cpp_f = fopen(example_cpp_fname, "w+"); - if (example_cpp_f == NULL) - ERROR_MSG(EXIT_FAILURE, "Failed to open file %s", example_cpp_fname); - - // Preamble - time_t start_time; - time(&start_time); - fprintf(example_cpp_f, "/*\n"); - fprintf(example_cpp_f, " * This file was autogenerated by TinyMPC on %s", ctime(&start_time)); - fprintf(example_cpp_f, " */\n\n"); - - fprintf(example_cpp_f, "#include \n\n"); - - fprintf(example_cpp_f, "#include \n"); - fprintf(example_cpp_f, "#include \n\n"); - - fprintf(example_cpp_f, "using namespace Eigen;\n"); - fprintf(example_cpp_f, "IOFormat TinyFmt(4, 0, \", \", \"\\n\", \"[\", \"]\");\n\n"); - - fprintf(example_cpp_f, "#ifdef __cplusplus\n"); - fprintf(example_cpp_f, "extern \"C\" {\n"); - fprintf(example_cpp_f, "#endif\n\n"); - - fprintf(example_cpp_f, "int main()\n"); - fprintf(example_cpp_f, "{\n"); - fprintf(example_cpp_f, "\tint exitflag = 1;\n"); - fprintf(example_cpp_f, "\t// Double check some data\n"); - fprintf(example_cpp_f, "\tstd::cout << \"rho: \" << tiny_solver.cache->rho << std::endl;\n"); - fprintf(example_cpp_f, "\tstd::cout << \"\\nmax iters: \" << tiny_solver.settings->max_iter << std::endl;\n"); - fprintf(example_cpp_f, "\tstd::cout << \"\\nState transition matrix:\\n\" << tiny_solver.work->Adyn.format(TinyFmt) << std::endl;\n"); - fprintf(example_cpp_f, "\tstd::cout << \"\\nInput/control matrix:\\n\" << tiny_solver.work->Bdyn.format(TinyFmt) << std::endl;\n\n"); - - fprintf(example_cpp_f, "\t// Visit https://tinympc.org/ to see how to set the initial condition and update the reference trajectory.\n\n"); - - fprintf(example_cpp_f, "\tstd::cout << \"\\nSolving...\\n\" << std::endl;\n\n"); - fprintf(example_cpp_f, "\texitflag = tiny_solve(&tiny_solver);\n\n"); - fprintf(example_cpp_f, "\tif (exitflag == 0) printf(\"Hooray! Solved with no error!\\n\");\n"); - fprintf(example_cpp_f, "\telse printf(\"Oops! Something went wrong!\\n\");\n"); - - fprintf(example_cpp_f, "\treturn 0;\n"); - fprintf(example_cpp_f, "}\n\n"); - - fprintf(example_cpp_f, "#ifdef __cplusplus\n"); - fprintf(example_cpp_f, "} /* extern \"C\" */\n"); - fprintf(example_cpp_f, "#endif\n"); - - // Close codegen example main file - fclose(example_cpp_f); - if (verbose) { - printf("Example tinympc main generated in %s\n", example_cpp_fname); - } - return 0; -} - -#ifdef __cplusplus -} -#endif \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/codegen.hpp b/src/tongji/modules/planner/tinympc/codegen.hpp deleted file mode 100755 index 4495d30..0000000 --- a/src/tongji/modules/planner/tinympc/codegen.hpp +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -#include "types.hpp" - -#ifdef __cplusplus -extern "C" { -#endif - - int tiny_codegen(TinySolver* solver, const char* output_dir, int verbose); - - int tiny_codegen_with_sensitivity(TinySolver* solver, const char* output_dir, - tinyMatrix* dK, tinyMatrix* dP, - tinyMatrix* dC1, tinyMatrix* dC2, int verbose); - - int codegen_create_directories(const char* output_dir, int verbose); - int codegen_data_header(const char* output_dir, int verbose); - int codegen_data_source(TinySolver* solver, const char* output_dir, int verbose); - int codegen_example(const char* output_dir, int verbose); - - -#ifdef __cplusplus -} -#endif \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/error.hpp b/src/tongji/modules/planner/tinympc/error.hpp deleted file mode 100644 index 713c1ce..0000000 --- a/src/tongji/modules/planner/tinympc/error.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include -#include -#include - -// #if defined(__linux__) || defined(__unix__)// Check if Linux -// #include -// #define ERROR_MSG(exit_code, format, ...) error(exit_code, errno, format, ##__VA_ARGS__) - -// #elif defined(__APPLE__) || defined(__MACH__) // Check if macOS -#define ERROR_MSG(exit_code, format, ...) \ - { \ - fprintf(stderr, format ": %s\n", ##__VA_ARGS__, strerror(errno)); \ - exit(exit_code); \ - } - -// #else -// #error "Unsupported operating system" -// #endif - -#ifdef __cplusplus -} -#endif \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/rho_benchmark.cpp b/src/tongji/modules/planner/tinympc/rho_benchmark.cpp deleted file mode 100644 index f10848d..0000000 --- a/src/tongji/modules/planner/tinympc/rho_benchmark.cpp +++ /dev/null @@ -1,250 +0,0 @@ -#include "rho_benchmark.hpp" -#include -#include -#include -#ifdef ARDUINO -#include -#else -// For non-Arduino platforms -uint32_t micros() { - return 0; // Replace with appropriate timing function -} -#endif - -void initialize_format_matrices(RhoAdapter* adapter, int nx, int nu, int N) { - // Calculate dimensions - int x_decision_size = nx * N + nu * (N-1); - int constraint_rows = (nx + nu) * (N-1); - - // Pre-allocate matrices - adapter->A_matrix = tinyMatrix::Zero(constraint_rows, x_decision_size); - adapter->z_vector = tinyMatrix::Zero(constraint_rows, 1); - adapter->y_vector = tinyMatrix::Zero(constraint_rows, 1); - adapter->x_decision = tinyMatrix::Zero(x_decision_size, 1); - - // Pre-compute P matrix structure - adapter->P_matrix = tinyMatrix::Zero(x_decision_size, x_decision_size); - adapter->q_vector = tinyMatrix::Zero(x_decision_size, 1); - - // Pre-allocate residual computation matrices - adapter->Ax_vector = tinyMatrix::Zero(constraint_rows, 1); - adapter->r_prim_vector = tinyMatrix::Zero(constraint_rows, 1); - adapter->r_dual_vector = tinyMatrix::Zero(x_decision_size, 1); - adapter->Px_vector = tinyMatrix::Zero(x_decision_size, 1); - adapter->ATy_vector = tinyMatrix::Zero(x_decision_size, 1); - - // Store dimensions - adapter->format_nx = nx; - adapter->format_nu = nu; - adapter->format_N = N; - - adapter->matrices_initialized = true; -} - -void format_matrices( - RhoAdapter* adapter, - const tinyMatrix& x_prev, - const tinyMatrix& u_prev, - const tinyMatrix& v_prev, - const tinyMatrix& z_prev, - const tinyMatrix& g_prev, - const tinyMatrix& y_prev, - TinyCache* cache, - TinyWorkspace* work, - int N -) { - if (!adapter->matrices_initialized) { - initialize_format_matrices(adapter, x_prev.rows(), u_prev.rows(), N); - } - - int nx = adapter->format_nx; - int nu = adapter->format_nu; - - // Fill x_decision - int x_idx = 0; - for (int i = 0; i < N; i++) { - adapter->x_decision.block(x_idx, 0, nx, 1) = x_prev.col(i); - x_idx += nx; - if (i < N-1) { - adapter->x_decision.block(x_idx, 0, nu, 1) = u_prev.col(i); - x_idx += nu; - } - } - - // Clear A matrix for reuse - adapter->A_matrix.setZero(); - - // Fill A matrix with dynamics and input constraints - for (int i = 0; i < N-1; i++) { - // Input constraints - int row_start = i * nu; - int col_start = i * (nx+nu) + nx; - adapter->A_matrix.block(row_start, col_start, nu, nu) = tinyMatrix::Identity(nu, nu); - - // Dynamics constraints - row_start = (N-1) * nu + i * nx; - col_start = i * (nx+nu); - adapter->A_matrix.block(row_start, col_start, nx, nx) = work->Adyn; - adapter->A_matrix.block(row_start, col_start+nx, nx, nu) = work->Bdyn; - - int next_state_idx = col_start + nx + nu; - if (next_state_idx < adapter->A_matrix.cols()) { - adapter->A_matrix.block(row_start, next_state_idx, nx, nx) = -tinyMatrix::Identity(nx, nx); - } - } - - // Fill z and y vectors - for (int i = 0; i < N-1; i++) { - adapter->z_vector.block(i*nu, 0, nu, 1) = z_prev.col(i); - adapter->z_vector.block((N-1)*nu+i*nx, 0, nx, 1) = v_prev.col(i+1); - - adapter->y_vector.block(i*nu, 0, nu, 1) = y_prev.col(i); - adapter->y_vector.block((N-1)*nu+i*nx, 0, nx, 1) = g_prev.col(i+1); - } - - // Build P matrix (cost matrix) - adapter->P_matrix.setZero(); - - // Fill diagonal blocks - x_idx = 0; - for (int i = 0; i < N; i++) { - // State cost - if (i == N-1) { - adapter->P_matrix.block(x_idx, x_idx, nx, nx) = cache->Pinf; - } else { - adapter->P_matrix.block(x_idx, x_idx, nx, nx) = work->Q.asDiagonal(); - } - x_idx += nx; - - // Input cost - if (i < N-1) { - adapter->P_matrix.block(x_idx, x_idx, nu, nu) = work->R.asDiagonal(); - x_idx += nu; - } - } - - // Create q vector (linear cost vector) - x_idx = 0; - for (int i = 0; i < N; i++) { - // For simplicity, we'll use zero reference for now - // In a real implementation, you'd use your reference trajectory - tinyMatrix x_ref = tinyMatrix::Zero(nx, 1); - tinyMatrix delta_x = x_prev.col(i) - x_ref; - adapter->q_vector.block(x_idx, 0, nx, 1) = work->Q.asDiagonal() * delta_x; - x_idx += nx; - - if (i < N-1) { - // For simplicity, we'll use zero reference for now - tinyMatrix u_ref = tinyMatrix::Zero(nu, 1); - tinyMatrix delta_u = u_prev.col(i) - u_ref; - adapter->q_vector.block(x_idx, 0, nu, 1) = work->R.asDiagonal() * delta_u; - x_idx += nu; - } - } -} - -void compute_residuals( - RhoAdapter* adapter, - tinytype* pri_res, - tinytype* dual_res, - tinytype* pri_norm, - tinytype* dual_norm -) { - // Compute Ax - adapter->Ax_vector = adapter->A_matrix * adapter->x_decision; - - // Compute primal residual - adapter->r_prim_vector = adapter->Ax_vector - adapter->z_vector; - *pri_res = adapter->r_prim_vector.cwiseAbs().maxCoeff(); - *pri_norm = std::max(adapter->Ax_vector.cwiseAbs().maxCoeff(), adapter->z_vector.cwiseAbs().maxCoeff()); - - // Compute dual residual components - adapter->Px_vector = adapter->P_matrix * adapter->x_decision; - adapter->ATy_vector = adapter->A_matrix.transpose() * adapter->y_vector; - - // Compute full dual residual - adapter->r_dual_vector = adapter->Px_vector + adapter->q_vector + adapter->ATy_vector; - *dual_res = adapter->r_dual_vector.cwiseAbs().maxCoeff(); - - // Compute normalization - *dual_norm = std::max(std::max(adapter->Px_vector.cwiseAbs().maxCoeff(), - adapter->ATy_vector.cwiseAbs().maxCoeff()), - adapter->q_vector.cwiseAbs().maxCoeff()); -} - -tinytype predict_rho( - RhoAdapter* adapter, - tinytype pri_res, - tinytype dual_res, - tinytype pri_norm, - tinytype dual_norm, - tinytype current_rho -) { - const tinytype eps = 1e-10; - - tinytype normalized_pri = pri_res / (pri_norm + eps); - tinytype normalized_dual = dual_res / (dual_norm + eps); - - tinytype ratio = normalized_pri / (normalized_dual + eps); - - tinytype new_rho = current_rho * std::sqrt(ratio); - - if (adapter->clip) { - new_rho = std::min(std::max(new_rho, adapter->rho_min), adapter->rho_max); - } - - return new_rho; -} - -void update_matrices_with_derivatives(TinyCache* cache, tinytype new_rho) { - tinytype delta_rho = new_rho - cache->rho; - - - - cache->Kinf = cache->Kinf + delta_rho * cache->dKinf_drho; - cache->Pinf = cache->Pinf + delta_rho * cache->dPinf_drho; - cache->C1 = cache->C1 + delta_rho * cache->dC1_drho; - cache->C2 = cache->C2 + delta_rho * cache->dC2_drho; - - cache->rho = new_rho; - - -} - -void benchmark_rho_adaptation( - RhoAdapter* adapter, - const tinyMatrix& x_prev, - const tinyMatrix& u_prev, - const tinyMatrix& v_prev, - const tinyMatrix& z_prev, - const tinyMatrix& g_prev, - const tinyMatrix& y_prev, - TinyCache* cache, - TinyWorkspace* work, - int N, - RhoBenchmarkResult* result -) { - uint32_t start_time = micros(); - - // Format matrices - format_matrices(adapter, x_prev, u_prev, v_prev, z_prev, g_prev, y_prev, cache, work, N); - - // Compute residuals - tinytype pri_res, dual_res, pri_norm, dual_norm; - compute_residuals(adapter, &pri_res, &dual_res, &pri_norm, &dual_norm); - - // Predict new rho - tinytype new_rho = predict_rho(adapter, pri_res, dual_res, pri_norm, dual_norm, cache->rho); - - // Update matrices - update_matrices_with_derivatives(cache, new_rho); - - // Store results - result->time_us = micros() - start_time; - result->initial_rho = cache->rho; - result->final_rho = new_rho; - result->pri_res = pri_res; - result->dual_res = dual_res; - result->pri_norm = pri_norm; - result->dual_norm = dual_norm; -} \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/rho_benchmark.hpp b/src/tongji/modules/planner/tinympc/rho_benchmark.hpp deleted file mode 100644 index 7f69230..0000000 --- a/src/tongji/modules/planner/tinympc/rho_benchmark.hpp +++ /dev/null @@ -1,95 +0,0 @@ -#pragma once -#include -#include "types.hpp" - -struct RhoAdapter { - tinytype rho_min; - tinytype rho_max; - bool clip; - bool matrices_initialized; - - // Pre-allocated matrices for formatting - tinyMatrix A_matrix; - tinyMatrix z_vector; - tinyMatrix y_vector; - tinyMatrix x_decision; - tinyMatrix P_matrix; - tinyMatrix q_vector; - - // Pre-allocated matrices for residual computation - tinyMatrix Ax_vector; - tinyMatrix r_prim_vector; - tinyMatrix r_dual_vector; - tinyMatrix Px_vector; - tinyMatrix ATy_vector; - - // Dimensions - int format_nx; - int format_nu; - int format_N; -}; - -struct RhoBenchmarkResult { - uint32_t time_us; - tinytype initial_rho; - tinytype final_rho; - tinytype pri_res; - tinytype dual_res; - tinytype pri_norm; - tinytype dual_norm; -}; - -// Initialize matrices for formatting -void initialize_format_matrices(RhoAdapter* adapter, int nx, int nu, int N); - -// Format matrices for residual computation -void format_matrices( - RhoAdapter* adapter, - const tinyMatrix& x_prev, - const tinyMatrix& u_prev, - const tinyMatrix& v_prev, - const tinyMatrix& z_prev, - const tinyMatrix& g_prev, - const tinyMatrix& y_prev, - TinyCache* cache, - TinyWorkspace* work, - int N -); - -// Compute residuals -void compute_residuals( - RhoAdapter* adapter, - tinytype* pri_res, - tinytype* dual_res, - tinytype* pri_norm, - tinytype* dual_norm -); - -// Predict new rho value -tinytype predict_rho( - RhoAdapter* adapter, - tinytype pri_res, - tinytype dual_res, - tinytype pri_norm, - tinytype dual_norm, - tinytype current_rho -); - -// Update matrices using derivatives -void update_matrices_with_derivatives(TinyCache* cache, tinytype new_rho); - - -// Main benchmark function -void benchmark_rho_adaptation( - RhoAdapter* adapter, - const tinyMatrix& x_prev, - const tinyMatrix& u_prev, - const tinyMatrix& v_prev, - const tinyMatrix& z_prev, - const tinyMatrix& g_prev, - const tinyMatrix& y_prev, - TinyCache* cache, - TinyWorkspace* work, - int N, - RhoBenchmarkResult* result -); \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/tiny_api.cpp b/src/tongji/modules/planner/tinympc/tiny_api.cpp deleted file mode 100755 index d05f40f..0000000 --- a/src/tongji/modules/planner/tinympc/tiny_api.cpp +++ /dev/null @@ -1,476 +0,0 @@ -#include "tiny_api.hpp" -#include "tiny_api_constants.hpp" - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -using namespace Eigen; -IOFormat TinyApiFmt(4, 0, ", ", "\n", "[", "]"); - -static int check_dimension(std::string matrix_name, std::string rows_or_columns, int actual, int expected) { - if (actual != expected) { - std::cout << matrix_name << " has " << actual << " " << rows_or_columns << ". Expected " << expected << "." << std::endl; - return 1; - } - return 0; -} - -int tiny_setup(TinySolver** solverp, - tinyMatrix Adyn, tinyMatrix Bdyn, tinyMatrix fdyn, tinyMatrix Q, tinyMatrix R, - tinytype rho, int nx, int nu, int N, int verbose) { - - TinySolution *solution = new TinySolution(); - TinyCache *cache = new TinyCache(); - TinySettings *settings = new TinySettings(); - TinyWorkspace *work = new TinyWorkspace(); - TinySolver *solver = new TinySolver(); - - solver->solution = solution; - solver->cache = cache; - solver->settings = settings; - solver->work = work; - - *solverp = solver; - - // Initialize solution - solution->iter = 0; - solution->solved = 0; - solution->x = tinyMatrix::Zero(nx, N); - solution->u = tinyMatrix::Zero(nu, N-1); - - // Initialize settings - tiny_set_default_settings(settings); - - // Initialize workspace - work->nx = nx; - work->nu = nu; - work->N = N; - - // Make sure arguments are the correct shapes - int status = 0; - status |= check_dimension("State transition matrix (A)", "rows", Adyn.rows(), nx); - status |= check_dimension("State transition matrix (A)", "columns", Adyn.cols(), nx); - status |= check_dimension("Input matrix (B)", "rows", Bdyn.rows(), nx); - status |= check_dimension("Input matrix (B)", "columns", Bdyn.cols(), nu); - status |= check_dimension("Affine vector (f)", "rows", fdyn.rows(), nx); - status |= check_dimension("Affine vector (f)", "columns", fdyn.cols(), 1); - status |= check_dimension("State stage cost (Q)", "rows", Q.rows(), nx); - status |= check_dimension("State stage cost (Q)", "columns", Q.cols(), nx); - status |= check_dimension("State input cost (R)", "rows", R.rows(), nu); - status |= check_dimension("State input cost (R)", "columns", R.cols(), nu); - if (status) { - return status; - } - - work->x = tinyMatrix::Zero(nx, N); - work->u = tinyMatrix::Zero(nu, N-1); - - work->q = tinyMatrix::Zero(nx, N); - work->r = tinyMatrix::Zero(nu, N-1); - - work->p = tinyMatrix::Zero(nx, N); - work->d = tinyMatrix::Zero(nu, N-1); - - // Bound constraint slack variables - work->v = tinyMatrix::Zero(nx, N); - work->vnew = tinyMatrix::Zero(nx, N); - work->z = tinyMatrix::Zero(nu, N-1); - work->znew = tinyMatrix::Zero(nu, N-1); - - // Bound constraint dual variables - work->g = tinyMatrix::Zero(nx, N); - work->y = tinyMatrix::Zero(nu, N-1); - - // Cone constraint slack variables - work->vc = tinyMatrix::Zero(nx, N); - work->vcnew = tinyMatrix::Zero(nx, N); - work->zc = tinyMatrix::Zero(nu, N-1); - work->zcnew = tinyMatrix::Zero(nu, N-1); - - // Cone constraint dual variables - work->gc = tinyMatrix::Zero(nx, N); - work->yc = tinyMatrix::Zero(nu, N-1); - - // Linear constraint slack variables - work->vl = tinyMatrix::Zero(nx, N); - work->vlnew = tinyMatrix::Zero(nx, N); - work->zl = tinyMatrix::Zero(nu, N-1); - work->zlnew = tinyMatrix::Zero(nu, N-1); - - // Linear constraint dual variables - work->gl = tinyMatrix::Zero(nx, N); - work->yl = tinyMatrix::Zero(nu, N-1); - - work->Q = (Q + rho * tinyMatrix::Identity(nx, nx)).diagonal(); - work->R = (R + rho * tinyMatrix::Identity(nu, nu)).diagonal(); - work->Adyn = Adyn; // State transition matrix - work->Bdyn = Bdyn; // Input matrix - work->fdyn = fdyn; // Affine offset vector - - work->Xref = tinyMatrix::Zero(nx, N); - work->Uref = tinyMatrix::Zero(nu, N-1); - - work->Qu = tinyVector::Zero(nu); - - work->primal_residual_state = 0; - work->primal_residual_input = 0; - work->dual_residual_state = 0; - work->dual_residual_input = 0; - work->status = 0; - work->iter = 0; - - // Initialize cache - status = tiny_precompute_and_set_cache(cache, Adyn, Bdyn, fdyn, work->Q.asDiagonal(), work->R.asDiagonal(), nx, nu, rho, verbose); - if (status) { - return status; - } - - // Initialize sensitivity matrices for adaptive rho - if (solver->settings->adaptive_rho) { - tiny_initialize_sensitivity_matrices(solver); - } - - return 0; -} - -int tiny_set_bound_constraints(TinySolver* solver, - tinyMatrix x_min, tinyMatrix x_max, - tinyMatrix u_min, tinyMatrix u_max) { - if (!solver) { - std::cout << "Error in tiny_set_bound_constraints: solver is nullptr" << std::endl; - return 1; - } - - // Make sure all bound constraint matrix sizes are self-consistent - int status = 0; - status |= check_dimension("Lower state bounds (x_min)", "rows", x_min.rows(), solver->work->nx); - status |= check_dimension("Lower state bounds (x_min)", "cols", x_min.cols(), solver->work->N); - status |= check_dimension("Lower state bounds (x_max)", "rows", x_max.rows(), solver->work->nx); - status |= check_dimension("Lower state bounds (x_max)", "cols", x_max.cols(), solver->work->N); - status |= check_dimension("Lower input bounds (u_min)", "rows", u_min.rows(), solver->work->nu); - status |= check_dimension("Lower input bounds (u_min)", "cols", u_min.cols(), solver->work->N-1); - status |= check_dimension("Lower input bounds (u_max)", "rows", u_max.rows(), solver->work->nu); - status |= check_dimension("Lower input bounds (u_max)", "cols", u_max.cols(), solver->work->N-1); - - solver->work->x_min = x_min; - solver->work->x_max = x_max; - solver->work->u_min = u_min; - solver->work->u_max = u_max; - - return 0; -} - -int tiny_set_cone_constraints(TinySolver* solver, - VectorXi Acx, VectorXi qcx, tinyVector cx, - VectorXi Acu, VectorXi qcu, tinyVector cu) { - if (!solver) { - std::cout << "Error in tiny_set_cone_constraints: solver is nullptr" << std::endl; - return 1; - } - - // Make sure all cone constraint vector sizes are self-consistent - int num_state_cones = Acx.rows(); - int num_input_cones = Acu.rows(); - int status = 0; - status |= check_dimension("Cone state size (qcx)", "rows", qcx.rows(), num_state_cones); - status |= check_dimension("Cone mu value for state (cx)", "rows", cx.rows(), num_state_cones); - status |= check_dimension("Cone input size (qcu)", "rows", qcu.rows(), num_input_cones); - status |= check_dimension("Cone mu value for input (cu)", "rows", cu.rows(), num_input_cones); - if (status) { - return status; - } - - solver->work->numStateCones = num_state_cones; - solver->work->numInputCones = num_input_cones; - - solver->work->Acx = Acx; - solver->work->qcx = qcx; - solver->work->cx = cx; - - solver->work->Acu = Acu; - solver->work->qcu = qcu; - solver->work->cu = cu; - - return 0; -} - -int tiny_set_linear_constraints(TinySolver* solver, - tinyMatrix Alin_x, tinyVector blin_x, - tinyMatrix Alin_u, tinyVector blin_u) { - if (!solver) { - std::cout << "Error in tiny_set_linear_constraints: solver is nullptr" << std::endl; - return 1; - } - - // Make sure all linear constraint matrix sizes are self-consistent - int num_state_linear = Alin_x.rows(); - int num_input_linear = Alin_u.rows(); - int status = 0; - - // Check state constraint dimensions - if (num_state_linear > 0) { - status |= check_dimension("State linear constraint matrix (Alin_x)", "rows", Alin_x.rows(), num_state_linear); - status |= check_dimension("State linear constraint matrix (Alin_x)", "columns", Alin_x.cols(), solver->work->nx); - status |= check_dimension("State linear constraint vector (blin_x)", "rows", blin_x.rows(), num_state_linear); - status |= check_dimension("State linear constraint vector (blin_x)", "columns", blin_x.cols(), 1); - } - - // Check input constraint dimensions - if (num_input_linear > 0) { - status |= check_dimension("Input linear constraint matrix (Alin_u)", "rows", Alin_u.rows(), num_input_linear); - status |= check_dimension("Input linear constraint matrix (Alin_u)", "columns", Alin_u.cols(), solver->work->nu); - status |= check_dimension("Input linear constraint vector (blin_u)", "rows", blin_u.rows(), num_input_linear); - status |= check_dimension("Input linear constraint vector (blin_u)", "columns", blin_u.cols(), 1); - } - - if (status) { - return status; - } - - solver->work->numStateLinear = num_state_linear; - solver->work->numInputLinear = num_input_linear; - - solver->work->Alin_x = Alin_x; - solver->work->blin_x = blin_x; - solver->work->Alin_u = Alin_u; - solver->work->blin_u = blin_u; - - return 0; -} - -int tiny_precompute_and_set_cache(TinyCache *cache, - tinyMatrix Adyn, tinyMatrix Bdyn, tinyMatrix fdyn, tinyMatrix Q, tinyMatrix R, - int nx, int nu, tinytype rho, int verbose) { - - if (!cache) { - std::cout << "Error in tiny_precompute_and_set_cache: cache is nullptr" << std::endl; - return 1; - } - - // Update by adding rho * identity matrix to Q, R - tinyMatrix Q1 = Q + rho * tinyMatrix::Identity(nx, nx); - tinyMatrix R1 = R + rho * tinyMatrix::Identity(nu, nu); - - // Printing - if (verbose) { - std::cout << "A = " << Adyn.format(TinyApiFmt) << std::endl; - std::cout << "B = " << Bdyn.format(TinyApiFmt) << std::endl; - std::cout << "Q = " << Q1.format(TinyApiFmt) << std::endl; - std::cout << "R = " << R1.format(TinyApiFmt) << std::endl; - std::cout << "rho = " << rho << std::endl; - } - - // Riccati recursion to get Kinf, Pinf - tinyMatrix Ktp1 = tinyMatrix::Zero(nu, nx); - tinyMatrix Ptp1 = rho * tinyMatrix::Ones(nx, 1).array().matrix().asDiagonal(); - tinyMatrix Kinf = tinyMatrix::Zero(nu, nx); - tinyMatrix Pinf = tinyMatrix::Zero(nx, nx); - - for (int i = 0; i < 1000; i++) - { - Kinf = (R1 + Bdyn.transpose() * Ptp1 * Bdyn).inverse() * Bdyn.transpose() * Ptp1 * Adyn; - Pinf = Q1 + Adyn.transpose() * Ptp1 * (Adyn - Bdyn * Kinf); - // if Kinf converges, break - if ((Kinf - Ktp1).cwiseAbs().maxCoeff() < 1e-5) - { - if (verbose) { - std::cout << "Kinf converged after " << i + 1 << " iterations" << std::endl; - } - break; - } - Ktp1 = Kinf; - Ptp1 = Pinf; - } - - // Compute cached matrices - tinyMatrix Quu_inv = (R1 + Bdyn.transpose() * Pinf * Bdyn).inverse(); - tinyMatrix AmBKt = (Adyn - Bdyn * Kinf).transpose(); - - // Precomputation for affine term - tinyVector APf = AmBKt*Pinf*fdyn; - tinyVector BPf = Bdyn.transpose()*Pinf*fdyn; - - if (verbose) { - std::cout << "Kinf = " << Kinf.format(TinyApiFmt) << std::endl; - std::cout << "Pinf = " << Pinf.format(TinyApiFmt) << std::endl; - std::cout << "Quu_inv = " << Quu_inv.format(TinyApiFmt) << std::endl; - std::cout << "AmBKt = " << AmBKt.format(TinyApiFmt) << std::endl; - std::cout << "APf = " << APf.format(TinyApiFmt) << std::endl; - std::cout << "BPf = " << BPf.format(TinyApiFmt) << std::endl; - - std::cout << "\nPrecomputation finished!\n" << std::endl; - } - - cache->rho = rho; - cache->Kinf = Kinf; - cache->Pinf = Pinf; - cache->Quu_inv = Quu_inv; - cache->AmBKt = AmBKt; - cache->C1 = Quu_inv; - cache->C2 = AmBKt; - cache->APf = APf; - cache->BPf = BPf; - - return 0; // return success -} - - -int tiny_solve(TinySolver* solver) { - return solve(solver); -} - -int tiny_update_settings(TinySettings* settings, tinytype abs_pri_tol, tinytype abs_dua_tol, - int max_iter, int check_termination, - int en_state_bound, int en_input_bound, - int en_state_soc, int en_input_soc, - int en_state_linear, int en_input_linear) { - if (!settings) { - std::cout << "Error in tiny_update_settings: settings is nullptr" << std::endl; - return 1; - } - settings->abs_pri_tol = abs_pri_tol; - settings->abs_dua_tol = abs_dua_tol; - settings->max_iter = max_iter; - settings->check_termination = check_termination; - settings->en_state_bound = en_state_bound; - settings->en_input_bound = en_input_bound; - settings->en_state_soc = en_state_soc; - settings->en_input_soc = en_input_soc; - settings->en_state_linear = en_state_linear; - settings->en_input_linear = en_input_linear; - return 0; -} - -int tiny_set_default_settings(TinySettings* settings) { - if (!settings) { - std::cout << "Error in tiny_set_default_settings: settings is nullptr" << std::endl; - return 1; - } - settings->abs_pri_tol = TINY_DEFAULT_ABS_PRI_TOL; - settings->abs_dua_tol = TINY_DEFAULT_ABS_DUA_TOL; - settings->max_iter = TINY_DEFAULT_MAX_ITER; - settings->check_termination = TINY_DEFAULT_CHECK_TERMINATION; - - // Turn off constraints until they are set by tiny_set_bound_constraints or tiny_set_cone_constraints - settings->en_state_bound = TINY_DEFAULT_EN_STATE_BOUND; - settings->en_input_bound = TINY_DEFAULT_EN_INPUT_BOUND; - settings->en_state_soc = TINY_DEFAULT_EN_STATE_SOC; - settings->en_input_soc = TINY_DEFAULT_EN_INPUT_SOC; - settings->en_state_linear = TINY_DEFAULT_EN_STATE_LINEAR; - settings->en_input_linear = TINY_DEFAULT_EN_INPUT_LINEAR; - - // Initialize adaptive rho settings - // NOTE : Adaptive rho currently supports only quadrotor system - settings->adaptive_rho = 0; // Disabled by default - settings->adaptive_rho_min = 1.0; - settings->adaptive_rho_max = 100.0; - settings->adaptive_rho_enable_clipping = 1; - - return 0; -} - -int tiny_set_x0(TinySolver* solver, tinyVector x0) { - if (!solver) { - std::cout << "Error in tiny_set_x0: solver is nullptr" << std::endl; - return 1; - } - if (x0.rows() != solver->work->nx) { - perror("Error in tiny_set_x0: x0 is not the correct length"); - } - solver->work->x.col(0) = x0; - return 0; -} - -int tiny_set_x_ref(TinySolver* solver, tinyMatrix x_ref) { - if (!solver) { - std::cout << "Error in tiny_set_x_ref: solver is nullptr" << std::endl; - return 1; - } - int status = 0; - status |= check_dimension("State reference trajectory (x_ref)", "rows", x_ref.rows(), solver->work->nx); - status |= check_dimension("State reference trajectory (x_ref)", "columns", x_ref.cols(), solver->work->N); - solver->work->Xref = x_ref; - return 0; -} - -int tiny_set_u_ref(TinySolver* solver, tinyMatrix u_ref) { - if (!solver) { - std::cout << "Error in tiny_set_u_ref: solver is nullptr" << std::endl; - return 1; - } - int status = 0; - status |= check_dimension("Control/input reference trajectory (u_ref)", "rows", u_ref.rows(), solver->work->nu); - status |= check_dimension("Control/input reference trajectory (u_ref)", "columns",u_ref.cols(), solver->work->N-1); - solver->work->Uref = u_ref; - return 0; -} - -void tiny_initialize_sensitivity_matrices(TinySolver *solver) { - - int nu = solver->work->nu; - int nx = solver->work->nx; - // Initialize matrices with zeros - solver->cache->dKinf_drho = tinyMatrix::Zero(nu, nx); - solver->cache->dPinf_drho = tinyMatrix::Zero(nx, nx); - solver->cache->dC1_drho = tinyMatrix::Zero(nu, nu); - solver->cache->dC2_drho = tinyMatrix::Zero(nx, nx); - - const float dKinf_drho[4][12] = { - { 0.0001, -0.0001, -0.0025, 0.0003, 0.0007, 0.0050, 0.0001, -0.0001, -0.0008, 0.0000, 0.0001, 0.0008}, - { -0.0001, -0.0000, -0.0025, -0.0001, -0.0006, -0.0050, -0.0001, 0.0000, -0.0008, -0.0000, -0.0001, -0.0008}, - { 0.0000, 0.0000, -0.0025, 0.0001, 0.0004, 0.0050, 0.0000, 0.0000, -0.0008, 0.0000, 0.0000, 0.0008}, - { -0.0000, 0.0001, -0.0025, -0.0003, -0.0004, -0.0050, -0.0000, 0.0001, -0.0008, -0.0000, -0.0000, -0.0008} - }; - - const float dPinf_drho[12][12] = { - { 0.0494, -0.0045, -0.0000, 0.0110, 0.1300, -0.0283, 0.0280, -0.0026, -0.0000, 0.0004, 0.0070, -0.0094}, - { -0.0045, 0.0491, 0.0000, -0.1320, -0.0111, 0.0114, -0.0026, 0.0279, 0.0000, -0.0076, -0.0004, 0.0038}, - { -0.0000, 0.0000, 2.4450, 0.0000, -0.0000, -0.0000, -0.0000, 0.0000, 1.2593, 0.0000, 0.0000, 0.0000}, - { 0.0110, -0.1320, 0.0000, 0.3913, 0.0592, 0.3108, 0.0080, -0.0776, 0.0000, 0.0254, 0.0068, 0.0750}, - { 0.1300, -0.0111, -0.0000, 0.0592, 0.4420, 0.7771, 0.0797, -0.0081, -0.0000, 0.0068, 0.0350, 0.1875}, - { -0.0283, 0.0114, -0.0000, 0.3108, 0.7771, 10.0441, 0.0272, -0.0109, 0.0000, 0.0655, 0.1639, 2.6362}, - { 0.0280, -0.0026, -0.0000, 0.0080, 0.0797, 0.0272, 0.0163, -0.0016, -0.0000, 0.0005, 0.0047, 0.0032}, - { -0.0026, 0.0279, 0.0000, -0.0776, -0.0081, -0.0109, -0.0016, 0.0161, 0.0000, -0.0046, -0.0005, -0.0013}, - { -0.0000, 0.0000, 1.2593, 0.0000, -0.0000, 0.0000, -0.0000, 0.0000, 0.9232, 0.0000, 0.0000, 0.0000}, - { 0.0004, -0.0076, 0.0000, 0.0254, 0.0068, 0.0655, 0.0005, -0.0046, 0.0000, 0.0022, 0.0017, 0.0244}, - { 0.0070, -0.0004, 0.0000, 0.0068, 0.0350, 0.1639, 0.0047, -0.0005, 0.0000, 0.0017, 0.0054, 0.0610}, - { -0.0094, 0.0038, 0.0000, 0.0750, 0.1875, 2.6362, 0.0032, -0.0013, 0.0000, 0.0244, 0.0610, 0.9869} - }; - - const float dC1_drho[4][4] = { - { -0.0000, 0.0000, -0.0000, 0.0000}, - { 0.0000, -0.0000, 0.0000, -0.0000}, - { -0.0000, 0.0000, -0.0000, 0.0000}, - { 0.0000, -0.0000, 0.0000, -0.0000} - }; - - const float dC2_drho[12][12] = { - { 0.0000, -0.0000, 0.0000, 0.0000, 0.0000, -0.0000, 0.0000, -0.0000, 0.0000, 0.0000, 0.0000, -0.0000}, - { -0.0000, 0.0000, 0.0000, -0.0000, -0.0000, 0.0000, -0.0000, 0.0000, 0.0000, -0.0000, -0.0000, 0.0000}, - { -0.0000, 0.0000, 0.0001, 0.0000, -0.0000, -0.0000, -0.0000, 0.0000, 0.0000, 0.0000, -0.0000, -0.0000}, - { 0.0000, -0.0000, -0.0000, 0.0001, 0.0000, -0.0000, 0.0000, -0.0000, -0.0000, 0.0000, 0.0000, -0.0000}, - { 0.0000, -0.0000, -0.0000, 0.0000, 0.0001, -0.0000, 0.0000, -0.0000, -0.0000, 0.0000, 0.0000, -0.0000}, - { -0.0000, 0.0000, -0.0000, -0.0000, 0.0000, 0.0001, -0.0000, 0.0000, -0.0000, 0.0000, 0.0000, 0.0000}, - { 0.0000, -0.0000, 0.0000, 0.0000, 0.0000, -0.0000, 0.0000, -0.0000, 0.0000, 0.0000, 0.0000, -0.0000}, - { -0.0000, 0.0000, 0.0000, -0.0000, -0.0000, 0.0000, -0.0000, 0.0000, 0.0000, -0.0000, -0.0000, 0.0000}, - { -0.0000, 0.0000, 0.0021, 0.0000, -0.0000, -0.0000, -0.0000, 0.0000, 0.0006, 0.0000, -0.0000, -0.0000}, - { 0.0002, -0.0027, -0.0000, 0.0068, 0.0005, -0.0005, 0.0001, -0.0015, -0.0000, 0.0004, 0.0000, -0.0001}, - { 0.0027, -0.0002, 0.0000, 0.0005, 0.0066, -0.0011, 0.0015, -0.0001, 0.0000, 0.0000, 0.0004, -0.0002}, - { -0.0001, 0.0001, 0.0000, -0.0000, 0.0000, 0.0041, -0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0006} - }; - - - - // Map arrays to Eigen matrices - solver->cache->dKinf_drho = Map>(dKinf_drho[0]).cast(); - solver->cache->dPinf_drho = Map>(dPinf_drho[0]).cast(); - solver->cache->dC1_drho = Map>(dC1_drho[0]).cast(); - solver->cache->dC2_drho = Map>(dC2_drho[0]).cast(); -} - -#ifdef __cplusplus -} -#endif \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/tiny_api.hpp b/src/tongji/modules/planner/tinympc/tiny_api.hpp deleted file mode 100755 index 5d9ce04..0000000 --- a/src/tongji/modules/planner/tinympc/tiny_api.hpp +++ /dev/null @@ -1,62 +0,0 @@ -#pragma once - -#include -#include "admm.hpp" - -#ifdef __cplusplus -extern "C" { -#endif - -int tiny_setup(TinySolver** solverp, - tinyMatrix Adyn, tinyMatrix Bdyn, tinyMatrix fdyn, tinyMatrix Q, tinyMatrix R, - tinytype rho, int nx, int nu, int N, int verbose); -int tiny_set_bound_constraints(TinySolver* solver, - tinyMatrix x_min, tinyMatrix x_max, - tinyMatrix u_min, tinyMatrix u_max); -int tiny_set_cone_constraints(TinySolver* solver, - VectorXi Acu, VectorXi qcu, tinyVector cu, - VectorXi Acx, VectorXi qcx, tinyVector cx); -int tiny_set_linear_constraints(TinySolver* solver, - tinyMatrix Alin_x, tinyVector blin_x, - tinyMatrix Alin_u, tinyVector blin_u); -int tiny_precompute_and_set_cache(TinyCache *cache, - tinyMatrix Adyn, tinyMatrix Bdyn, tinyMatrix fdyn, tinyMatrix Q, tinyMatrix R, - int nx, int nu, tinytype rho, int verbose); - -void compute_sensitivity_matrices(TinyCache *cache, - tinyMatrix Adyn, tinyMatrix Bdyn, tinyMatrix Q, tinyMatrix R, - int nx, int nu, tinytype rho, int verbose); - -int tiny_update_matrices_with_derivatives(TinyCache *cache, tinytype delta_rho); -int tiny_solve(TinySolver *solver); - -int tiny_update_settings(TinySettings* settings, - tinytype abs_pri_tol, tinytype abs_dua_tol, - int max_iter, int check_termination, - int en_state_bound, int en_input_bound, - int en_state_soc, int en_input_soc, - int en_state_linear, int en_input_linear); -int tiny_set_default_settings(TinySettings* settings); - -int tiny_set_x0(TinySolver* solver, tinyVector x0); -int tiny_set_x_ref(TinySolver* solver, tinyMatrix x_ref); -int tiny_set_u_ref(TinySolver* solver, tinyMatrix u_ref); - -/** - * Initialize sensitivity matrices for adaptive rho - * - * @param solver Pointer to solver - */ -void tiny_initialize_sensitivity_matrices(TinySolver *solver); - -int tiny_setup_state_soc_constraints(TinySolver *solver, - tinyVector Acx, tinyVector qcx, tinyVector cx, - int numStateCones); - -int tiny_setup_input_soc_constraints(TinySolver *solver, - tinyVector Acu, tinyVector qcu, tinyVector cu, - int numInputCones); - -#ifdef __cplusplus -} -#endif \ No newline at end of file diff --git a/src/tongji/modules/planner/tinympc/tiny_api_constants.hpp b/src/tongji/modules/planner/tinympc/tiny_api_constants.hpp deleted file mode 100644 index d99e96a..0000000 --- a/src/tongji/modules/planner/tinympc/tiny_api_constants.hpp +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - - -// Default settings -#define TINY_DEFAULT_ABS_PRI_TOL (1e-03) -#define TINY_DEFAULT_ABS_DUA_TOL (1e-03) -#define TINY_DEFAULT_MAX_ITER (1000) -#define TINY_DEFAULT_CHECK_TERMINATION (1) -#define TINY_DEFAULT_EN_STATE_BOUND (1) -#define TINY_DEFAULT_EN_INPUT_BOUND (1) -#define TINY_DEFAULT_EN_STATE_SOC (0) -#define TINY_DEFAULT_EN_INPUT_SOC (0) -#define TINY_DEFAULT_EN_STATE_LINEAR (0) -#define TINY_DEFAULT_EN_INPUT_LINEAR (0) diff --git a/src/tongji/modules/planner/tinympc/types.hpp b/src/tongji/modules/planner/tinympc/types.hpp deleted file mode 100755 index 409023a..0000000 --- a/src/tongji/modules/planner/tinympc/types.hpp +++ /dev/null @@ -1,204 +0,0 @@ -#pragma once - -#include -// #include -// #include - -using namespace Eigen; - -#ifdef __cplusplus -extern "C" { -#endif - -typedef double tinytype; // should be double if you want to generate code -typedef Matrix tinyMatrix; -typedef Matrix tinyVector; - -// typedef Matrix tiny_VectorNx; -// typedef Matrix tiny_VectorNu; -// typedef Matrix tiny_MatrixNxNx; -// typedef Matrix tiny_MatrixNxNu; -// typedef Matrix tiny_MatrixNuNx; -// typedef Matrix tiny_MatrixNuNu; - -// typedef Matrix tiny_MatrixNxNh; // Nu x Nh -// typedef Matrix tiny_MatrixNuNhm1; // Nu x Nh-1 - -/** - * Solution - */ -typedef struct { - int iter; - int solved; - tinyMatrix x; // nx x N - tinyMatrix u; // nu x N-1 -} TinySolution; - - -/** -* Matrices that must be recomputed with changes in time step, rho -*/ -typedef struct { - tinytype rho; - tinyMatrix Kinf; // nu x nx - tinyMatrix Pinf; // nx x nx - tinyMatrix Quu_inv; // nu x nu - tinyMatrix AmBKt; // nx x nx - tinyVector APf; // nx x 1 - tinyVector BPf; // nu x 1 - tinyMatrix C1; // From adaptive rho - tinyMatrix C2; // From adaptive rho - - // Sensitivity matrices for adaptive rho - tinyMatrix dKinf_drho; - tinyMatrix dPinf_drho; - tinyMatrix dC1_drho; - tinyMatrix dC2_drho; -} TinyCache; -/** -* User settings -*/ -typedef struct { - tinytype abs_pri_tol; - tinytype abs_dua_tol; - int max_iter; - int check_termination; - int en_state_bound; - int en_input_bound; - int en_state_soc; - int en_input_soc; - int en_state_linear; - int en_input_linear; - - // Add adaptive rho parameters - int adaptive_rho; // Enable/disable adaptive rho (1/0) - tinytype adaptive_rho_min; // Minimum value for rho - tinytype adaptive_rho_max; // Maximum value for rho - int adaptive_rho_enable_clipping; // Enable/disable clipping of rho (1/0) -} TinySettings; - - -/** - * Problem variables - */ -typedef struct { - int nx; // Number of states - int nu; // Number of control inputs - int N; // Number of knotpoints in the horizon - - // State and input - tinyMatrix x; // nx x N - tinyMatrix u; // nu x N-1 - - // Linear control cost terms - tinyMatrix q; // nx x N - tinyMatrix r; // nu x N-1 - - // Linear Riccati backward pass terms - tinyMatrix p; // nx x N - tinyMatrix d; // nu x N-1 - - // Bound constraint variables - // Slack variables - tinyMatrix v; // nx x N - tinyMatrix vnew; // nx x N - tinyMatrix z; // nu x N-1 - tinyMatrix znew; // nu x N-1 - - // Dual variables - tinyMatrix g; // nx x N - tinyMatrix y; // nu x N-1 - - // State and input bounds - tinyMatrix x_min; // nx x N - tinyMatrix x_max; // nx x N - tinyMatrix u_min; // nu x N-1 - tinyMatrix u_max; // nu x N-1 - - // Cone constraint variables - // Variables to keep track of general cone information - int numStateCones; // Number of cone constraints on states at each time step - int numInputCones; // Number of cone constraints on inputs at each time step - tinyVector cx; // One coefficient for each state cone - tinyVector cu; // One coefficient for each input cone - VectorXi Acx; // Start indices for each state cone - VectorXi Acu; // Start indices for each input cone - VectorXi qcx; // Dimension for each state cone - VectorXi qcu; // Dimension for each input cone - - // Slack variables - tinyMatrix vc; // nx x N - tinyMatrix vcnew; // nx x N - tinyMatrix zc; // nu x N-1 - tinyMatrix zcnew; // nu x N-1 - - // Dual variables - tinyMatrix gc; // nx x N - tinyMatrix yc; // nu x N-1 - - // Linear constraint variables - // Variables to keep track of general linear constraint information - int numStateLinear; // Number of linear constraints on states at each time step - int numInputLinear; // Number of linear constraints on inputs at each time step - - // Constraint matrices and vectors - tinyMatrix Alin_x; // Normal vectors for state linear constraints (numStateLinear x nx) - tinyVector blin_x; // Offset values for state linear constraints (numStateLinear x 1) - tinyMatrix Alin_u; // Normal vectors for input linear constraints (numInputLinear x nu) - tinyVector blin_u; // Offset values for input linear constraints (numInputLinear x 1) - - // Slack variables for linear constraints - tinyMatrix vl; // nx x N - tinyMatrix vlnew; // nx x N - tinyMatrix zl; // nu x N-1 - tinyMatrix zlnew; // nu x N-1 - - // Dual variables for linear constraints - tinyMatrix gl; // nx x N - tinyMatrix yl; // nu x N-1 - - - - // Q, R, A, B, f given by user - tinyVector Q; // nx x 1 - tinyVector R; // nu x 1 - tinyMatrix Adyn; // nx x nx (state transition matrix) - tinyMatrix Bdyn; // nx x nu (control matrix) - tinyVector fdyn; // nx x 1 (affine vector) - - // Reference trajectory to track for one horizon - tinyMatrix Xref; // nx x N - tinyMatrix Uref; // nu x N-1 - - // Temporaries - tinyVector Qu; // nu x 1 - - - - // Variables for keeping track of solve status - tinytype primal_residual_state; - tinytype primal_residual_input; - tinytype dual_residual_state; - tinytype dual_residual_input; - int status; - int iter; -} TinyWorkspace; - -/** - * Main TinyMPC solver structure that holds all information. - */ -typedef struct { - TinySolution *solution; // Solution - TinySettings *settings; // Problem settings - TinyCache *cache; // Problem cache - TinyWorkspace *work; // Solver workspace -} TinySolver; - - -// Add at the top with other definitions -#define BENCH_NX 12 -#define BENCH_NU 4 - -#ifdef __cplusplus -} -#endif From d4629a82d8625f85fef15255a644c248bb7dea6e Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 6 Nov 2025 11:10:24 +0800 Subject: [PATCH 86/93] Revert "refactor(architecture): extract shared utilities from t v1 mo" This reverts commit c26fbac51e06d4574d8283388c48118bba7c9dea. --- .../modules/identifier/armor_filter.hpp | 4 +- .../modules/identifier/identified_armor.hpp | 10 ++-- src/tongji/modules/identifier/identifier.cpp | 12 ++--- .../car_predictor/car_predictor_manager.cpp | 4 +- .../modules/solver/reprojection_util.hpp | 4 +- src/tongji/modules/solver/solver.cpp | 4 +- src/tongji/utils/coordinate.hpp | 2 +- src/tongji/utils/index.hpp | 49 ------------------- src/util/index.hpp | 14 +++++- src/{tongji/utils => util}/logger.cpp | 2 +- src/{tongji/utils => util}/logger.hpp | 2 +- src/{tongji/utils => util}/stringifier.hpp | 2 +- tests/tongji_tests/utils/coordinate_test.cpp | 2 +- 13 files changed, 37 insertions(+), 74 deletions(-) delete mode 100644 src/tongji/utils/index.hpp rename src/{tongji/utils => util}/logger.cpp (95%) rename src/{tongji/utils => util}/logger.hpp (70%) rename src/{tongji/utils => util}/stringifier.hpp (95%) diff --git a/src/tongji/modules/identifier/armor_filter.hpp b/src/tongji/modules/identifier/armor_filter.hpp index 89a590b..21699e0 100644 --- a/src/tongji/modules/identifier/armor_filter.hpp +++ b/src/tongji/modules/identifier/armor_filter.hpp @@ -7,7 +7,7 @@ #include "data/armor_image_spaceing.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" -#include "tongji/utils/index.hpp" +#include "util/index.hpp" namespace world_exe::tongji::identifier { @@ -34,7 +34,7 @@ class ArmorFilter { void Update(enumeration::CarIDFlag ids) { invincible_armor_.clear(); - for (auto id : tongji::utils::index::ExpandArmorIdFlags(ids)) { + for (auto id : util::enumeration::ExpandArmorIdFlags(ids)) { invincible_armor_.emplace(std::move(id)); } } diff --git a/src/tongji/modules/identifier/identified_armor.hpp b/src/tongji/modules/identifier/identified_armor.hpp index d9d97b0..079c0c4 100644 --- a/src/tongji/modules/identifier/identified_armor.hpp +++ b/src/tongji/modules/identifier/identified_armor.hpp @@ -1,9 +1,9 @@ #pragma once -#include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "interfaces/armor_in_image.hpp" -#include "tongji/utils/index.hpp" +#include "data/time_stamped.hpp" +#include "util/index.hpp" namespace world_exe::tongji::identifier { @@ -11,7 +11,7 @@ class IdentifiedArmor final : public interfaces::IArmorInImage { public: explicit IdentifiedArmor(const std::vector& armors) { for (const auto& armor : armors) { - armors_[utils::index::GetIndex(armor.id)].emplace_back(armor); + armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); } } @@ -19,7 +19,7 @@ class IdentifiedArmor final : public interfaces::IArmorInImage { const std::vector& GetArmors( const enumeration::ArmorIdFlag& armor_id) const override { - return armors_[utils::index::GetIndex(armor_id)]; + return armors_[util::enumeration::GetIndex(armor_id)]; } static IdentifiedArmor DecorateIArmorInImage(const interfaces::IArmorInImage& armor) { @@ -30,4 +30,4 @@ class IdentifiedArmor final : public interfaces::IArmorInImage { data::TimeStamp time_stamp_ { std::chrono::steady_clock::now().time_since_epoch() }; std::array, 8> armors_; }; -} +} \ No newline at end of file diff --git a/src/tongji/modules/identifier/identifier.cpp b/src/tongji/modules/identifier/identifier.cpp index fafdb1c..bf03a41 100644 --- a/src/tongji/modules/identifier/identifier.cpp +++ b/src/tongji/modules/identifier/identifier.cpp @@ -21,8 +21,8 @@ #include "enum/armor_id.hpp" #include "identified_armor.hpp" #include "interfaces/armor_in_image.hpp" -#include "tongji/utils/logger.hpp" -#include "tongji/utils/stringifier.hpp" +#include "util/logger.hpp" +#include "util/stringifier.hpp" namespace world_exe::tongji::identifier { class Identifier::Impl { @@ -196,7 +196,7 @@ class Identifier::Impl { void Save(const cv::Mat& armor_pattern, enumeration::ArmorIdFlag armor_id) const { auto file_name = fmt::format("{:%Y-%m-%d_%H-%M-%S}", std::chrono::system_clock::now()); auto img_path = fmt::format( - "{}/{}_{}.jpg", save_path_, utils::stringifier::ToString(armor_id), file_name); + "{}/{}_{}.jpg", save_path_, util::stringifier::ToString(armor_id), file_name); cv::imwrite(img_path, armor_pattern); } @@ -297,7 +297,7 @@ class Identifier::Impl { // 出现 5号 则显示 debug 信息。但不过滤。 if (armor_id == enumeration::ArmorIdFlag::InfantryV && debug_) - utils::logger::logger()->debug("See pattern 5 : InfantryV "); + world_exe::util::logger::logger()->debug("See pattern 5 : InfantryV "); return name_ok && confidence_ok; } @@ -311,8 +311,8 @@ class Identifier::Impl { // 保存异常的图案，用于分类器的迭代 if (!name_ok && debug_) { - utils::logger::logger()->debug( - "see strange armor: {}", utils::stringifier::ToString(armor_id)); + util::logger::logger()->debug( + "see strange armor: {}", util::stringifier::ToString(armor_id)); Save(armor_pattern, armor_id); } return name_ok; diff --git a/src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp b/src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp index cd15f4a..458dde0 100644 --- a/src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp +++ b/src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp @@ -12,8 +12,8 @@ #include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" -#include "tongji/utils/index.hpp" #include "tongji/utils/math.hpp" +#include "util/index.hpp" namespace world_exe::tongji::predictor { @@ -28,7 +28,7 @@ class CarPredictorManager::Impl { const enumeration::ArmorIdFlag& flag, const data::TimeStamp& time_stamp) { std::vector result; - for (const auto& car_id : utils::index::ExpandArmorIdFlags(flag)) { + for (const auto& car_id : util::enumeration::ExpandArmorIdFlags(flag)) { if (const auto it = targets_map_.find(car_id); it != targets_map_.end()) { const auto& [id, predictor] = *it; if (!predictor->IsConverged()) { diff --git a/src/tongji/modules/solver/reprojection_util.hpp b/src/tongji/modules/solver/reprojection_util.hpp index 1bb0592..2cd6638 100644 --- a/src/tongji/modules/solver/reprojection_util.hpp +++ b/src/tongji/modules/solver/reprojection_util.hpp @@ -61,11 +61,11 @@ class ReprojectionUtil { // get R_armor2camera t_armor2camera const Eigen::Vector3d& t_armor2gimbal = armor_xyz_in_gimbal; Eigen::Matrix3d R_armor2camera = R_camera2gimbal.transpose() * R_armor2gimbal; - Eigen::Matrix3d R_armor2camera_cv = utils::coordinate::ros2opencv_rotation(R_armor2camera); + Eigen::Matrix3d R_armor2camera_cv = util::coordinate::ros2opencv_rotation(R_armor2camera); Eigen::Vector3d t_armor2camera = R_camera2gimbal.transpose() * (armor_xyz_in_gimbal) + t_gimbal2camera; - Eigen::Vector3d t_armor2camera_cv = utils::coordinate::ros2opencv_position(t_armor2camera); + Eigen::Vector3d t_armor2camera_cv = util::coordinate::ros2opencv_position(t_armor2camera); // get rvec tvec cv::Vec3d rvec; diff --git a/src/tongji/modules/solver/solver.cpp b/src/tongji/modules/solver/solver.cpp index 6ffb8fa..957282e 100644 --- a/src/tongji/modules/solver/solver.cpp +++ b/src/tongji/modules/solver/solver.cpp @@ -117,14 +117,14 @@ class Solver::Impl { // 1. P_C_cv -> P_C_ros (位置) Eigen::Vector3d xyz_in_camera_cv; cv::cv2eigen(tvec, xyz_in_camera_cv); - const auto xyz_in_camera = utils::coordinate::opencv2ros_position(xyz_in_camera_cv); + const auto xyz_in_camera = util::coordinate::opencv2ros_position(xyz_in_camera_cv); // 2. R_A->C_cv -> R_A->C_ros (姿态) cv::Mat rmat; cv::Rodrigues(rvec, rmat); Eigen::Matrix3d R_armor2camera_cv; cv::cv2eigen(rmat, R_armor2camera_cv); - const auto R_armor2camera = utils::coordinate::opencv2ros_rotation(R_armor2camera_cv); + const auto R_armor2camera = util::coordinate::opencv2ros_rotation(R_armor2camera_cv); return { xyz_in_camera, R_armor2camera }; } diff --git a/src/tongji/utils/coordinate.hpp b/src/tongji/utils/coordinate.hpp index 77015ac..4f632e0 100644 --- a/src/tongji/utils/coordinate.hpp +++ b/src/tongji/utils/coordinate.hpp @@ -2,7 +2,7 @@ #include -namespace world_exe::tongji::utils::coordinate { +namespace world_exe::util::coordinate { static inline Eigen::Vector3d opencv2ros_position(const Eigen::Vector3d& position) { return Eigen::Vector3d(position.z(), -position.x(), -position.y()); diff --git a/src/tongji/utils/index.hpp b/src/tongji/utils/index.hpp deleted file mode 100644 index c328ebb..0000000 --- a/src/tongji/utils/index.hpp +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once -#define BACKWARD_HAS_DW 1 - -#include "enum/armor_id.hpp" -#include -#include - -namespace world_exe::tongji::utils::index { - -static std::vector ExpandArmorIdFlags( - ::world_exe::enumeration::ArmorIdFlag flags) { - std::vector<::world_exe::enumeration::ArmorIdFlag> result; - for (int i = 0; i < static_cast(::world_exe::enumeration::ArmorIdFlag::Count); ++i) { - auto single = static_cast<::world_exe::enumeration::ArmorIdFlag>( - static_cast(::world_exe::enumeration::ArmorIdFlag::Hero) << i); - if ((static_cast(flags) & static_cast(single)) != 0) { - result.push_back(single); - } - } - return result; -} - -static inline int GetIndex(const world_exe::enumeration::ArmorIdFlag& flag) { - const auto value = static_cast(flag); - if (value != 0 && (value & (value - 1)) == 0) return std::countr_zero(value); - else { - - // auto st = backward::StackTrace(); - // st.load_here(4); - // backward::Printer ptr; - // ptr.object = false; - // ptr.color_mode = backward::ColorMode::always; - // ptr.address = false; - // ptr.snippet = false; - // ptr.print(st, stderr); - throw std::runtime_error("Invalid ArmorIdFlag value: " + std::to_string(value)); - } -} - -static const world_exe::enumeration::ArmorIdFlag GetArmorIdFlag(int index) { - if (index >= 0 && index < static_cast(world_exe::enumeration::ArmorIdFlag::Count)) { - uint32_t value = 1U << index; - return static_cast(value); - } else { - throw std::out_of_range("Invalid index for ArmorIdFlag conversion."); - } -} - -} \ No newline at end of file diff --git a/src/util/index.hpp b/src/util/index.hpp index 6e8bdef..7a300a8 100644 --- a/src/util/index.hpp +++ b/src/util/index.hpp @@ -1,5 +1,6 @@ #pragma once +#include #define BACKWARD_HAS_DW 1 // #include "backward.hpp" #include "enum/armor_id.hpp" @@ -32,6 +33,17 @@ static const world_exe::enumeration::ArmorIdFlag GetArmorIdFlag(int index) { } } - +static std::vector<::world_exe::enumeration::ArmorIdFlag> ExpandArmorIdFlags( + ::world_exe::enumeration::ArmorIdFlag flags) { + std::vector<::world_exe::enumeration::ArmorIdFlag> result; + for (int i = 0; i < static_cast(::world_exe::enumeration::ArmorIdFlag::Count); ++i) { + auto single = static_cast<::world_exe::enumeration::ArmorIdFlag>( + static_cast(::world_exe::enumeration::ArmorIdFlag::Hero) << i); + if ((static_cast(flags) & static_cast(single)) != 0) { + result.push_back(single); + } + } + return result; +} } \ No newline at end of file diff --git a/src/tongji/utils/logger.cpp b/src/util/logger.cpp similarity index 95% rename from src/tongji/utils/logger.cpp rename to src/util/logger.cpp index f0ce339..43e45e2 100644 --- a/src/tongji/utils/logger.cpp +++ b/src/util/logger.cpp @@ -7,7 +7,7 @@ #include #include -namespace world_exe::tongji::utils::logger { +namespace world_exe::util::logger { std::shared_ptr logger_ = nullptr; void set_logger() { diff --git a/src/tongji/utils/logger.hpp b/src/util/logger.hpp similarity index 70% rename from src/tongji/utils/logger.hpp rename to src/util/logger.hpp index 24706ec..801db97 100644 --- a/src/tongji/utils/logger.hpp +++ b/src/util/logger.hpp @@ -2,7 +2,7 @@ #include -namespace world_exe::tongji::utils::logger { +namespace world_exe::util::logger { std::shared_ptr logger(); } // namespace logger diff --git a/src/tongji/utils/stringifier.hpp b/src/util/stringifier.hpp similarity index 95% rename from src/tongji/utils/stringifier.hpp rename to src/util/stringifier.hpp index 6916248..0fe4974 100644 --- a/src/tongji/utils/stringifier.hpp +++ b/src/util/stringifier.hpp @@ -4,7 +4,7 @@ #include "enum/armor_id.hpp" -namespace world_exe::tongji::utils::stringifier { +namespace world_exe::util::stringifier { static inline std::string ToString(const world_exe::enumeration::ArmorIdFlag& id) { switch (id) { diff --git a/tests/tongji_tests/utils/coordinate_test.cpp b/tests/tongji_tests/utils/coordinate_test.cpp index 371cdc6..df1e8d4 100644 --- a/tests/tongji_tests/utils/coordinate_test.cpp +++ b/tests/tongji_tests/utils/coordinate_test.cpp @@ -3,7 +3,7 @@ #include "gtest/gtest.h" #include -namespace coord = world_exe::tongji::utils::coordinate; +namespace coord = world_exe::util::coordinate; TEST(CoordinateTest, Opencv2RosPosition_SimpleAxis) { // 1. 准备 (Arrange) - 在 OpenCV 坐标系下，沿着每个轴单位移动 From 69ec5b8041cc59118ebcb4071c9d4d0876843c3a Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 6 Nov 2025 11:14:49 +0800 Subject: [PATCH 87/93] Revert "test(math): verify mathematical utilities functionality" This reverts commit 0477bbf8b38199d01d3893262fbf30250143c60d. --- src/tongji/utils/math.hpp | 134 -------------------- tests/tongji_tests/utils/math_test.cpp | 165 ------------------------- 2 files changed, 299 deletions(-) delete mode 100644 src/tongji/utils/math.hpp delete mode 100644 tests/tongji_tests/utils/math_test.cpp diff --git a/src/tongji/utils/math.hpp b/src/tongji/utils/math.hpp deleted file mode 100644 index 5001675..0000000 --- a/src/tongji/utils/math.hpp +++ /dev/null @@ -1,134 +0,0 @@ -#pragma once - -#include - -namespace world_exe::tongji::utils::math { - -// [−π,π] 注意边界值 -static constexpr double clamp_pm_pi(auto angle) { - return std::remainder(angle, 2.0 * std::numbers::pi); -} -// zyx order -static inline Eigen::Vector3d get_ypr_from_quaternion(const Eigen::Quaterniond& quaternion) { - const Eigen::Matrix3d R = quaternion.toRotationMatrix(); - return R.eulerAngles(2, 1, 0); -} - -/** - * @brief 将笛卡尔坐标点 (x, y) 绕原点旋转。 - * delta_angle > 0 时，执行逆时针旋转。 - * @param x 点的 x 坐标。 - * @param y 点的 y 坐标。 - * @param delta_angle 旋转角度 (弧度)。 - * @return std::pair 旋转后的新坐标 (x', y')。 - */ -static inline std::pair rotate_point_ccw(double x, double y, double delta_angle) { - const double cos_d = std::cos(delta_angle); - const double sin_d = std::sin(delta_angle); - - const double x_new = x * cos_d - y * sin_d; // x' = x*cos(d) - y*sin(d) - const double y_new = x * sin_d + y * cos_d; // y' = x*sin(d) + y*cos(d) - - return { x_new, y_new }; -} - -// zyx order -static Eigen::Matrix3d euler_to_matrix(const Eigen::Vector3d& ypr) { - Eigen::AngleAxisd rollAngle(ypr[2], Eigen::Vector3d::UnitX()); - Eigen::AngleAxisd pitchAngle(ypr[1], Eigen::Vector3d::UnitY()); - Eigen::AngleAxisd yawAngle(ypr[0], Eigen::Vector3d::UnitZ()); - Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle; - - return q.toRotationMatrix(); -} - -static inline Eigen::Quaterniond euler_to_quaternion( - double yaw_rad, double pitch_rad, double roll_rad) { - return Eigen::AngleAxisd(yaw_rad, Eigen::Vector3d::UnitZ()) - * Eigen::AngleAxisd(pitch_rad, Eigen::Vector3d::UnitY()) - * Eigen::AngleAxisd(roll_rad, Eigen::Vector3d::UnitX()); -} - -static inline Eigen::Quaterniond euler_to_quaternion(const Eigen::Vector3d& ypr) { - return Eigen::AngleAxisd(ypr[0], Eigen::Vector3d::UnitZ()) - * Eigen::AngleAxisd(ypr[1], Eigen::Vector3d::UnitY()) - * Eigen::AngleAxisd(ypr[2], Eigen::Vector3d::UnitX()); -} -// zyx order -static Eigen::Vector3d quaternion_to_euler(Eigen::Quaterniond q) { - const Eigen::Matrix3d R = q.toRotationMatrix(); - return R.eulerAngles(2, 1, 0); -} - -static Eigen::Vector3d matrix_to_euler(Eigen::Matrix3d R) { return R.eulerAngles(2, 1, 0); } - -static inline Eigen::Vector3d xyz2ypd(const Eigen::Vector3d& xyz) { - const double x = xyz[0]; - const double y = xyz[1]; - const double z = xyz[2]; - - const double r_xy = std::hypot(x, y); - - const double distance = std::hypot(r_xy, z); - const double yaw = std::atan2(y, x); - const double pitch = std::atan2(z, r_xy); - - return { yaw, pitch, distance }; -} - -static Eigen::Matrix xyz2ypd_jacobian(const Eigen::Vector3d& xyz) { - const auto x = xyz[0], y = xyz[1], z = xyz[2]; - - const double r_xy_sq = x * x + y * y; // r_xy^2 - const double r_xy = std::sqrt(r_xy_sq); // r_xy - const double D_sq = r_xy_sq + z * z; // D^2 (总距离平方) - const double D = std::sqrt(D_sq); // D (总距离) - - if (r_xy_sq < 1e-12) { - return Eigen::Matrix3d::Zero(); - } - - // I. Yaw (行 0): J[0, *] - const double r_xy_sq_inv = 1.0 / r_xy_sq; - const double dyaw_dx = -y * r_xy_sq_inv; - const double dyaw_dy = x * r_xy_sq_inv; - const double dyaw_dz = 0.0; - - // II. Pitch (行 1): J[1, *] - // 简化后的公式使用 D_sq 和 r_xy - const double D_sq_inv = 1.0 / D_sq; - const double r_xy_inv = 1.0 / r_xy; - const double factor = z * D_sq_inv * r_xy_inv; // z / (D^2 * r_xy) - - const double dpitch_dx = -x * factor; - const double dpitch_dy = -y * factor; - // dpitch_dz = r_xy / D^2 - const double dpitch_dz = r_xy * D_sq_inv; - - // III. Distance (行 2): J[2, *] - // dD/dx = x/D - const double D_inv = 1.0 / D; - const double ddistance_dx = x * D_inv; - const double ddistance_dy = y * D_inv; - const double ddistance_dz = z * D_inv; - - Eigen::Matrix3d J; - // clang-format off - J << dyaw_dx , dyaw_dy , dyaw_dz , - dpitch_dx , dpitch_dy , dpitch_dz , - ddistance_dx, ddistance_dy, ddistance_dz; - // clang-format on - - return J; -} - -template T square(T const& a) { return a * a; }; - -static inline double get_abs_angle(const Eigen::Vector2d& vec1, const Eigen::Vector2d& vec2) { - if (vec1.norm() == 0. || vec2.norm() == 0.) { - return 0.; - } - return std::acos(vec1.dot(vec2) / (vec1.norm() * vec2.norm())); //(0~pi) -} - -} // namespace rmcs_auto_aim::util::math diff --git a/tests/tongji_tests/utils/math_test.cpp b/tests/tongji_tests/utils/math_test.cpp deleted file mode 100644 index 5c43d8d..0000000 --- a/tests/tongji_tests/utils/math_test.cpp +++ /dev/null @@ -1,165 +0,0 @@ - -#include "tongji/utils/math.hpp" - -#include -#include - -#include "gtest/gtest.h" -#include - -namespace math = world_exe::tongji::utils::math; - -constexpr double TOLERANCE = 1e-6; - -// --- 1. 角度钳位测试 (clamp_pm_pi) --- -// 目标：确保将角度标准化到 [−π, π] 范围内。std::remainder(angle, 2*PI) 保证了结果在 [-PI, PI] - -TEST(MathUtilsTest, ClampPi_NormalRange) { - // 1. 正常范围内的角度 - EXPECT_NEAR(math::clamp_pm_pi(M_PI / 2.0), M_PI / 2.0, TOLERANCE); - EXPECT_NEAR(math::clamp_pm_pi(-M_PI / 2.0), -M_PI / 2.0, TOLERANCE); -} - -TEST(MathUtilsTest, ClampPi_BoundaryValues) { - // 容忍度常量 - constexpr double TOLERANCE = 1e-6; - - // --- 1. 测试 PI + eps -> -PI + eps 的行为 --- - // 确保大于 PI 的值被正确拉回负半轴。 - EXPECT_NEAR(math::clamp_pm_pi(std::numbers::pi + 1e-7), -std::numbers::pi + 1e-7, TOLERANCE) - << "Failed for PI + epsilon. Expected: -PI + epsilon."; - - // --- 2. 奇数倍 PI 测试 (绝对值断言) --- - // 目标：确保结果的绝对值是 PI，因为 std::remainder(x, 2PI) 不保证符号。 - - // 测试 3*PI: 结果必须是 PI 或 -PI - double result_3pi = math::clamp_pm_pi(3.0 * std::numbers::pi); - EXPECT_NEAR(std::abs(result_3pi), std::numbers::pi, TOLERANCE) - << "Failed for 3*PI. Expected absolute value PI, got: " << result_3pi; - - // 测试 -3*PI: 结果必须是 PI 或 -PI - double result_neg_3pi = math::clamp_pm_pi(-3.0 * std::numbers::pi); - EXPECT_NEAR(std::abs(result_neg_3pi), std::numbers::pi, TOLERANCE) - << "Failed for -3*PI. Expected absolute value PI, got: " << result_neg_3pi; - - // --- 3. 严格的 PI 边界 --- - // 测试 PI 本身。由于 std::remainder(PI, 2PI) 可能为 PI 或 -PI，我们只断言绝对值。 - double result_pi = math::clamp_pm_pi(std::numbers::pi); - EXPECT_NEAR(std::abs(result_pi), std::numbers::pi, TOLERANCE) - << "Failed for PI. Expected absolute value PI, got: " << result_pi; -} - -// --- 2. 坐标旋转测试 (rotate_point_ccw) --- -// 目标：确保点 (x, y) 绕原点正确地执行逆时针 (CCW) 旋转。 - -TEST(MathTransformTest, RotatePointCCW_ZeroRotation) { - // 1. 零旋转 - auto [x_new, y_new] = math::rotate_point_ccw(10.0, 5.0, 0.0); - EXPECT_NEAR(x_new, 10.0, TOLERANCE); - EXPECT_NEAR(y_new, 5.0, TOLERANCE); -} - -TEST(MathTransformTest, RotatePointCCW_90Degrees) { - // 2. 逆时针旋转 90 度 (PI/2)： (1, 0) -> (0, 1) - auto [x1, y1] = math::rotate_point_ccw(1.0, 0.0, M_PI / 2.0); - EXPECT_NEAR(x1, 0.0, TOLERANCE); - EXPECT_NEAR(y1, 1.0, TOLERANCE); - - // (0, 1) -> (-1, 0) - auto [x2, y2] = math::rotate_point_ccw(0.0, 1.0, M_PI / 2.0); - EXPECT_NEAR(x2, -1.0, TOLERANCE); - EXPECT_NEAR(y2, 0.0, TOLERANCE); -} - -TEST(MathTransformTest, RotatePointCCW_RoundTrip) { - // 3. 来回旋转：旋转 delta，再旋转 -delta，应回到原点 - double x = 3.0, y = 4.0; - double delta_angle = M_PI / 6.0; // 30度 - - auto [x_rot, y_rot] = math::rotate_point_ccw(x, y, delta_angle); - auto [x_final, y_final] = math::rotate_point_ccw(x_rot, y_rot, -delta_angle); - - EXPECT_NEAR(x_final, x, TOLERANCE); - EXPECT_NEAR(y_final, y, TOLERANCE); -} - -// --- 3. 欧拉角/四元数/矩阵互转 (Round Trip Tests) --- -// 目标：确保所有 ZYX 顺序转换函数是互逆的。 - -TEST(MathConversionTest, EulerToQuaternionAndBack_RoundTrip) { - // 1. Euler -> Quaternion -> Euler (使用统一的 ZYX 顺序) - Eigen::Vector3d original_ypr(0.5, -0.4, 0.3); // YAW, PITCH, ROLL - - Eigen::Quaterniond q = math::euler_to_quaternion(original_ypr); - Eigen::Vector3d final_ypr = math::quaternion_to_euler(q); - - // 两个函数都使用了 Eigen 的 ZYX 接口，结果应该匹配 - ASSERT_TRUE(final_ypr.isApprox(original_ypr, TOLERANCE)) - << "E->Q->E failed." - << "\nOriginal YPR: " << original_ypr.transpose() - << "\nFinal YPR: " << final_ypr.transpose(); -} - -TEST(MathConversionTest, EulerToMatrixAndBack_RoundTrip) { - // 2. Euler -> Matrix -> Euler - Eigen::Vector3d original_ypr(0.8, -0.1, 0.4); - - Eigen::Matrix3d R = math::euler_to_matrix(original_ypr); - Eigen::Vector3d final_ypr = math::matrix_to_euler(R); - - ASSERT_TRUE(final_ypr.isApprox(original_ypr, TOLERANCE)) - << "E->M->E failed." - << "\nOriginal YPR: " << original_ypr.transpose() - << "\nFinal YPR: " << final_ypr.transpose(); -} - -TEST(MathConversionTest, QuaternionToEuler_DirectAccess) { - // 3. get_ypr_from_quaternion 测试 - Eigen::Vector3d expected_ypr(1.0, 0.5, -0.3); - Eigen::Quaterniond q = math::euler_to_quaternion(expected_ypr); - Eigen::Vector3d actual_ypr = math::get_ypr_from_quaternion(q); - - ASSERT_TRUE(actual_ypr.isApprox(expected_ypr, TOLERANCE)) << "get_ypr_from_quaternion failed."; -} - -// --- 4. 坐标转换测试 (xyz2ypd, xyz2ypd_jacobian) --- - -TEST(MathConversionTest, XYZ2YPD_KnownPoint) { - // 1. 沿 X 轴 (前方) - Eigen::Vector3d xyz(10.0, 0.0, 0.0); - Eigen::Vector3d ypd = math::xyz2ypd(xyz); // 预期 Yaw=0, Pitch=0, Dist=10 - - Eigen::Vector3d expected_ypd(0.0, 0.0, 10.0); - - ASSERT_TRUE(ypd.isApprox(expected_ypd, TOLERANCE)) - << "Expected YPD: " << expected_ypd.transpose() << "\nActual YPD: " << ypd.transpose(); -} - -TEST(MathConversionTest, XYZ2YPD_Jacobian_KnownPoint) { - // 2. 雅可比矩阵测试 - 检查一个已知点 (1, 0, 0) - Eigen::Vector3d xyz(1.0, 0.0, 0.0); - Eigen::Matrix3d J = math::xyz2ypd_jacobian(xyz); - - // 预期结果 (x=1, y=0, z=0) - // J[0, *] (Yaw): dyaw/dx=0, dyaw/dy=1, dyaw/dz=0 - // J[1, *] (Pitch): dpitch/dx=0, dpitch/dy=0, dpitch/dz=1 - // J[2, *] (Dist): dD/dx=1, dD/dy=0, dD/dz=0 - Eigen::Matrix3d expected_J; - expected_J << 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0; - - ASSERT_TRUE(J.isApprox(expected_J, TOLERANCE)) << "Jacobian for (1,0,0) failed." - << "\nExpected:\n" - << expected_J << "\nActual:\n" - << J; -} - -TEST(MathConversionTest, Jacobian_Singularity) { - // 3. 奇点测试：(0, 0, z) 处，r_xy_sq < 1e-12 应该返回零矩阵 - Eigen::Vector3d xyz_singularity(0.0, 0.0, 5.0); - Eigen::Matrix3d J = math::xyz2ypd_jacobian(xyz_singularity); - - ASSERT_TRUE(J.isApprox(Eigen::Matrix3d::Zero(), TOLERANCE)) << "Jacobian failed at singularity " - "(0, 0, z)." - << "\nActual:\n" - << J; -} From 2cb071332d3cf2bdbbf814a66db7d60454e5ea47 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 6 Nov 2025 11:34:49 +0800 Subject: [PATCH 88/93] revert: restore to version before directory restructuring --- src/tongji/auto_aim_system.cpp | 8 +- .../fire_controller/aim_point_chooser.hpp | 6 +- .../fire_controller/aim_solver.hpp | 6 +- .../fire_controller/fire_controller.cpp | 6 +- .../fire_controller/fire_controller.hpp | 0 .../fire_controller/fire_decision.hpp | 1 + .../fire_controller/trajectory.hpp | 0 .../{modules => }/identifier/armor_filter.hpp | 0 .../{modules => }/identifier/classifier.hpp | 0 .../{modules => }/identifier/decider.cpp | 0 .../{modules => }/identifier/decider.hpp | 0 .../identifier/identified_armor.hpp | 0 .../{modules => }/identifier/identifier.cpp | 1 + .../{modules => }/identifier/identifier.hpp | 0 .../{modules => }/identifier/tracker.hpp | 0 .../predictor/car_predictor/car_predictor.hpp | 11 +- .../car_predictor/car_predictor_manager.cpp | 8 +- .../car_predictor/car_predictor_manager.hpp | 0 .../predictor/in_gimbal_control_armor.hpp | 0 .../kalman_filter/extended_kalman_filter.hpp | 0 .../predictor/kalman_filter/predict_model.hpp | 42 ++--- .../solver/reprojection_util.hpp | 10 +- .../{modules => }/solver/solved_armor.hpp | 0 src/tongji/{modules => }/solver/solver.cpp | 4 +- src/tongji/{modules => }/solver/solver.hpp | 0 .../state_machine/state_machine.cpp | 0 .../state_machine/state_machine.hpp | 0 src/{tongji/utils => util}/coordinate.hpp | 0 src/util/math.hpp | 160 ++++++++++++++++-- tests/tongji_tests/utils/coordinate_test.cpp | 2 +- 30 files changed, 201 insertions(+), 64 deletions(-) rename src/tongji/{modules => }/fire_controller/aim_point_chooser.hpp (94%) rename src/tongji/{modules => }/fire_controller/aim_solver.hpp (95%) rename src/tongji/{modules => }/fire_controller/fire_controller.cpp (95%) rename src/tongji/{modules => }/fire_controller/fire_controller.hpp (100%) rename src/tongji/{modules => }/fire_controller/fire_decision.hpp (98%) rename src/tongji/{modules => }/fire_controller/trajectory.hpp (100%) rename src/tongji/{modules => }/identifier/armor_filter.hpp (100%) rename src/tongji/{modules => }/identifier/classifier.hpp (100%) rename src/tongji/{modules => }/identifier/decider.cpp (100%) rename src/tongji/{modules => }/identifier/decider.hpp (100%) rename src/tongji/{modules => }/identifier/identified_armor.hpp (100%) rename src/tongji/{modules => }/identifier/identifier.cpp (99%) rename src/tongji/{modules => }/identifier/identifier.hpp (100%) rename src/tongji/{modules => }/identifier/tracker.hpp (100%) rename src/tongji/{modules => }/predictor/car_predictor/car_predictor.hpp (92%) rename src/tongji/{modules => }/predictor/car_predictor/car_predictor_manager.cpp (93%) rename src/tongji/{modules => }/predictor/car_predictor/car_predictor_manager.hpp (100%) rename src/tongji/{modules => }/predictor/in_gimbal_control_armor.hpp (100%) rename src/tongji/{modules => }/predictor/kalman_filter/extended_kalman_filter.hpp (100%) rename src/tongji/{modules => }/predictor/kalman_filter/predict_model.hpp (87%) rename src/tongji/{modules => }/solver/reprojection_util.hpp (93%) rename src/tongji/{modules => }/solver/solved_armor.hpp (100%) rename src/tongji/{modules => }/solver/solver.cpp (98%) rename src/tongji/{modules => }/solver/solver.hpp (100%) rename src/tongji/{modules => }/state_machine/state_machine.cpp (100%) rename src/tongji/{modules => }/state_machine/state_machine.hpp (100%) rename src/{tongji/utils => util}/coordinate.hpp (100%) diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 7692b31..9789324 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -6,12 +6,12 @@ #include "../v1/sync/syncer.hpp" #include "core/event_bus.hpp" #include "data/predictor_update_package.hpp" -#include "modules/fire_controller/fire_controller.hpp" -#include "modules/predictor/car_predictor/car_predictor_manager.hpp" -#include "modules/solver/solver.hpp" -#include "modules/state_machine/state_machine.hpp" #include "parameters/params_system_v1.hpp" #include "parameters/profile.hpp" +#include "tongji/fire_controller/fire_controller.hpp" +#include "tongji/predictor/car_predictor/car_predictor_manager.hpp" +#include "tongji/solver/solver.hpp" +#include "tongji/state_machine/state_machine.hpp" #include "v1/identifier/identifier.hpp" namespace world_exe::tongji { diff --git a/src/tongji/modules/fire_controller/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp similarity index 94% rename from src/tongji/modules/fire_controller/aim_point_chooser.hpp rename to src/tongji/fire_controller/aim_point_chooser.hpp index 8dd834b..51e2c71 100644 --- a/src/tongji/modules/fire_controller/aim_point_chooser.hpp +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -2,7 +2,7 @@ #include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" -#include "tongji/utils/math.hpp" +#include "util/math.hpp" #include @@ -29,8 +29,8 @@ class AimPointChooser { std::vector> delta_angle_list; for (int i = 0; i < armor_num; i++) { - const auto ypr = utils::math::get_ypr_from_quaternion(armors[i].orientation); - auto delta_angle = utils::math::clamp_pm_pi(ypr[0] - center_yaw); + auto delta_angle = util::math::clamp_pm_pi( + util::math::get_yaw_from_quaternion(armors[i].orientation) - center_yaw); delta_angle_list.emplace_back(std::make_tuple(i, delta_angle)); } std::sort(delta_angle_list.begin(), delta_angle_list.end(), diff --git a/src/tongji/modules/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp similarity index 95% rename from src/tongji/modules/fire_controller/aim_solver.hpp rename to src/tongji/fire_controller/aim_solver.hpp index 25ebc6a..cdeeb8f 100644 --- a/src/tongji/modules/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -8,10 +8,10 @@ #include #include +#include "../predictor/car_predictor/car_predictor.hpp" #include "aim_point_chooser.hpp" -#include "tongji/modules/predictor/car_predictor/car_predictor.hpp" -#include "tongji/modules/predictor/kalman_filter/extended_kalman_filter.hpp" -#include "tongji/modules/predictor/kalman_filter/predict_model.hpp" +#include "tongji/predictor/kalman_filter/extended_kalman_filter.hpp" +#include "tongji/predictor/kalman_filter/predict_model.hpp" #include "trajectory.hpp" namespace world_exe::tongji::fire_control { diff --git a/src/tongji/modules/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp similarity index 95% rename from src/tongji/modules/fire_controller/fire_controller.cpp rename to src/tongji/fire_controller/fire_controller.cpp index ce925eb..1e3a21e 100644 --- a/src/tongji/modules/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -5,13 +5,13 @@ #include +#include "../identifier/identified_armor.hpp" +#include "../state_machine/state_machine.hpp" #include "aim_solver.hpp" #include "data/fire_control.hpp" #include "fire_decision.hpp" #include "interfaces/target_predictor.hpp" -#include "tongji/modules/identifier/identified_armor.hpp" -#include "tongji/modules/predictor/car_predictor/car_predictor_manager.hpp" -#include "tongji/modules/state_machine/state_machine.hpp" +#include "tongji/predictor/car_predictor/car_predictor_manager.hpp" namespace world_exe::tongji::fire_control { diff --git a/src/tongji/modules/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp similarity index 100% rename from src/tongji/modules/fire_controller/fire_controller.hpp rename to src/tongji/fire_controller/fire_controller.hpp diff --git a/src/tongji/modules/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp similarity index 98% rename from src/tongji/modules/fire_controller/fire_decision.hpp rename to src/tongji/fire_controller/fire_decision.hpp index 619500e..39125c5 100644 --- a/src/tongji/modules/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -2,6 +2,7 @@ #include #include +#include #include diff --git a/src/tongji/modules/fire_controller/trajectory.hpp b/src/tongji/fire_controller/trajectory.hpp similarity index 100% rename from src/tongji/modules/fire_controller/trajectory.hpp rename to src/tongji/fire_controller/trajectory.hpp diff --git a/src/tongji/modules/identifier/armor_filter.hpp b/src/tongji/identifier/armor_filter.hpp similarity index 100% rename from src/tongji/modules/identifier/armor_filter.hpp rename to src/tongji/identifier/armor_filter.hpp diff --git a/src/tongji/modules/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp similarity index 100% rename from src/tongji/modules/identifier/classifier.hpp rename to src/tongji/identifier/classifier.hpp diff --git a/src/tongji/modules/identifier/decider.cpp b/src/tongji/identifier/decider.cpp similarity index 100% rename from src/tongji/modules/identifier/decider.cpp rename to src/tongji/identifier/decider.cpp diff --git a/src/tongji/modules/identifier/decider.hpp b/src/tongji/identifier/decider.hpp similarity index 100% rename from src/tongji/modules/identifier/decider.hpp rename to src/tongji/identifier/decider.hpp diff --git a/src/tongji/modules/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp similarity index 100% rename from src/tongji/modules/identifier/identified_armor.hpp rename to src/tongji/identifier/identified_armor.hpp diff --git a/src/tongji/modules/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp similarity index 99% rename from src/tongji/modules/identifier/identifier.cpp rename to src/tongji/identifier/identifier.cpp index bf03a41..2f716f0 100644 --- a/src/tongji/modules/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include diff --git a/src/tongji/modules/identifier/identifier.hpp b/src/tongji/identifier/identifier.hpp similarity index 100% rename from src/tongji/modules/identifier/identifier.hpp rename to src/tongji/identifier/identifier.hpp diff --git a/src/tongji/modules/identifier/tracker.hpp b/src/tongji/identifier/tracker.hpp similarity index 100% rename from src/tongji/modules/identifier/tracker.hpp rename to src/tongji/identifier/tracker.hpp diff --git a/src/tongji/modules/predictor/car_predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor/car_predictor.hpp similarity index 92% rename from src/tongji/modules/predictor/car_predictor/car_predictor.hpp rename to src/tongji/predictor/car_predictor/car_predictor.hpp index 0ba2230..05136f7 100644 --- a/src/tongji/modules/predictor/car_predictor/car_predictor.hpp +++ b/src/tongji/predictor/car_predictor/car_predictor.hpp @@ -1,18 +1,19 @@ #pragma once +#include #include #include #include #include +#include "../in_gimbal_control_armor.hpp" +#include "../kalman_filter/extended_kalman_filter.hpp" +#include "../kalman_filter/predict_model.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "data/time_stamped.hpp" #include "enum/car_id.hpp" #include "interfaces/predictor.hpp" -#include "tongji/modules/predictor/in_gimbal_control_armor.hpp" -#include "tongji/modules/predictor/kalman_filter/extended_kalman_filter.hpp" -#include "tongji/modules/predictor/kalman_filter/predict_model.hpp" namespace world_exe::tongji::predictor { @@ -58,13 +59,13 @@ class CarPredictor final : public interfaces::IPredictor { const auto ekf_x = this->GetPredictedX((time_stamp - time_stamp_).to_seconds()); std::vector armors; for (int id = 0; id < model_.GetArmorNum(); id++) { - auto angle = utils::math::clamp_pm_pi(ekf_x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto angle = util::math::clamp_pm_pi(ekf_x[6] + id * 2 * CV_PI / model_.GetArmorNum()); auto xyz = model_.h_armor_xyz(ekf_x, id); data::ArmorGimbalControlSpacing armor; armor.id = model_.GetID(); armor.position = xyz; - armor.orientation = utils::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); armors.emplace_back(std::move(armor)); } return std::make_shared(armors, time_stamp_); diff --git a/src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp similarity index 93% rename from src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp rename to src/tongji/predictor/car_predictor/car_predictor_manager.cpp index 458dde0..1eaca95 100644 --- a/src/tongji/modules/predictor/car_predictor/car_predictor_manager.cpp +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp @@ -12,8 +12,8 @@ #include "data/time_stamped.hpp" #include "enum/armor_id.hpp" #include "enum/car_id.hpp" -#include "tongji/utils/math.hpp" #include "util/index.hpp" +#include "util/math.hpp" namespace world_exe::tongji::predictor { @@ -71,12 +71,12 @@ class CarPredictorManager::Impl { if (!targets_map_.contains(armor.id)) { targets_map_.try_emplace(armor.id, std::make_unique(armor.position, - utils::math::quaternion_to_euler(armor.orientation), armor.id, + util::math::quaternion_to_euler(armor.orientation, 2, 1, 0), armor.id, data->GetTimeStamp())); } else { targets_map_.at(armor.id)->Update(data->GetTimeStamp(), armor.position, - utils::math::quaternion_to_euler(armor.orientation), - utils::math::xyz2ypd(armor.position)); + util::math::quaternion_to_euler(armor.orientation, 2, 1, 0), + util::math::xyz2ypd(armor.position)); } } diff --git a/src/tongji/modules/predictor/car_predictor/car_predictor_manager.hpp b/src/tongji/predictor/car_predictor/car_predictor_manager.hpp similarity index 100% rename from src/tongji/modules/predictor/car_predictor/car_predictor_manager.hpp rename to src/tongji/predictor/car_predictor/car_predictor_manager.hpp diff --git a/src/tongji/modules/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp similarity index 100% rename from src/tongji/modules/predictor/in_gimbal_control_armor.hpp rename to src/tongji/predictor/in_gimbal_control_armor.hpp diff --git a/src/tongji/modules/predictor/kalman_filter/extended_kalman_filter.hpp b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp similarity index 100% rename from src/tongji/modules/predictor/kalman_filter/extended_kalman_filter.hpp rename to src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp diff --git a/src/tongji/modules/predictor/kalman_filter/predict_model.hpp b/src/tongji/predictor/kalman_filter/predict_model.hpp similarity index 87% rename from src/tongji/modules/predictor/kalman_filter/predict_model.hpp rename to src/tongji/predictor/kalman_filter/predict_model.hpp index 412cc94..e811698 100644 --- a/src/tongji/modules/predictor/kalman_filter/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -7,7 +7,7 @@ #include #include "enum/car_id.hpp" -#include "tongji/utils/math.hpp" +#include "util/math.hpp" namespace world_exe::tongji::predictor { template @@ -75,15 +75,15 @@ class EKFModel { // 防止夹角求和出现异常值 constexpr auto x_add(const XVec& a, const XVec& b) const -> const auto { XVec c = a + b; - c(6) = utils::math::clamp_pm_pi(c(6)); + c(6) = util ::math::clamp_pm_pi(c(6)); return c; } constexpr auto z_substract(const ZVec& a, const ZVec& b) const -> const auto { auto c = a - b; - c(0) = utils::math::clamp_pm_pi(c(0)); - c(1) = utils::math::clamp_pm_pi(c(1)); - c(3) = utils::math::clamp_pm_pi(c(3)); + c(0) = util::math::clamp_pm_pi(c(0)); + c(1) = util::math::clamp_pm_pi(c(1)); + c(3) = util::math::clamp_pm_pi(c(3)); return c; } @@ -111,7 +111,7 @@ class EKFModel { // 防止夹角求和出现异常值 auto f(const XVec& x, const double& dt)const ->const auto{ XVec x_prior = this->A(dt) * x; - x_prior(6) = utils::math::clamp_pm_pi(x_prior(6)); + x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); return x_prior; }; @@ -128,8 +128,8 @@ class EKFModel { } std::sort(xyza_i_list.begin(), xyza_i_list.end(), [](const auto& a, const auto& b) { - auto ypd1 = utils::math::xyz2ypd(a.first.head(3)); - auto ypd2 = utils::math::xyz2ypd(b.first.head(3)); + auto ypd1 = util::math::xyz2ypd(a.first.head(3)); + auto ypd2 = util::math::xyz2ypd(b.first.head(3)); return ypd1(2) < ypd2(2); }); @@ -138,9 +138,9 @@ class EKFModel { for (int i = 0; i < std::min(3, armor_num_); ++i) { const auto& xyza = xyza_i_list[i].first; - auto ypd = utils::math::xyz2ypd(xyza.head(3)); - double error = std::abs(utils::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - xyza(3))) - + std::abs(utils::math::clamp_pm_pi(armor_ypd_in_gimbal(0) - ypd(0))); + auto ypd = util::math::xyz2ypd(xyza.head(3)); + double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - xyza(3))) + + std::abs(util::math::clamp_pm_pi(armor_ypd_in_gimbal(0) - ypd(0))); if (error < min_error) { min_error = error; @@ -152,7 +152,7 @@ class EKFModel { // 计算出装甲板中心的坐标（考虑长短轴） auto h_armor_xyz(const XVec& x, int id) const ->const auto { - auto angle = utils::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); + auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); auto r = (use_l_h) ? x(8) + x(9) : x(8); @@ -164,7 +164,7 @@ class EKFModel { } constexpr auto H(const XVec& x, int id) const->HMat const { - auto angle = utils::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); + auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto cos_angle=std::cos(angle); auto sin_angle=std::sin(angle); @@ -192,7 +192,7 @@ class EKFModel { // clang-format on auto armor_xyz = h_armor_xyz(x, id); - auto H_armor_ypd = utils::math::xyz2ypd_jacobian(armor_xyz); + auto H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); // clang-format off Eigen::MatrixH_armor_ypda; H_armor_ypda<< @@ -208,7 +208,7 @@ class EKFModel { const Eigen::Vector3d& armor_ypd_in_gimbal, int id) const -> RMat const { // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; auto center_yaw = std::atan2(armor_xyz_in_gimbal(1), armor_xyz_in_gimbal(0)); - auto delta_angle = utils::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - center_yaw); + auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - center_yaw); RDig R_dig(4); R_dig << 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, log(std::abs(armor_ypd_in_gimbal(2)) + 1) / 200 + 9e-2; @@ -254,17 +254,17 @@ class EKFModel { constexpr auto h(const XVec& x, const int& id) const -> const auto { auto xyz = this->h_armor_xyz(x, id); - auto ypd = utils::math::xyz2ypd(xyz); - auto angle = utils::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); + auto ypd = util::math::xyz2ypd(xyz); + auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; } // 防止夹角求差出现异常值 constexpr auto z_subtract(const ZVec& a, const ZVec& b) const -> const auto { ZVec c = a - b; - c(0) = utils::math::clamp_pm_pi(c(0)); - c(1) = utils::math::clamp_pm_pi(c(1)); - c(3) = utils::math::clamp_pm_pi(c(3)); + c(0) = util::math::clamp_pm_pi(c(0)); + c(1) = util::math::clamp_pm_pi(c(1)); + c(3) = util::math::clamp_pm_pi(c(3)); return c; }; @@ -272,7 +272,7 @@ class EKFModel { std::vector _armor_xyza_list; for (int i = 0; i < armor_num_; i++) { - auto angle = utils::math::clamp_pm_pi(ekf_x(6) + i * 2 * CV_PI / armor_num_); + auto angle = util::math::clamp_pm_pi(ekf_x(6) + i * 2 * CV_PI / armor_num_); auto xyz = h_armor_xyz(ekf_x, i); _armor_xyza_list.push_back({ xyz(0), xyz(1), xyz(2), angle }); } diff --git a/src/tongji/modules/solver/reprojection_util.hpp b/src/tongji/solver/reprojection_util.hpp similarity index 93% rename from src/tongji/modules/solver/reprojection_util.hpp rename to src/tongji/solver/reprojection_util.hpp index 2cd6638..109163a 100644 --- a/src/tongji/modules/solver/reprojection_util.hpp +++ b/src/tongji/solver/reprojection_util.hpp @@ -11,8 +11,8 @@ #include "data/armor_image_spaceing.hpp" #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" -#include "tongji/utils/coordinate.hpp" -#include "tongji/utils/math.hpp" +#include "util/coordinate.hpp" +#include "util/math.hpp" namespace world_exe::tongji::solver { @@ -105,11 +105,11 @@ class ReprojectionUtil { + std::fabs(ref_d.norm() - pt_d.norm())) / ref_d.norm(); double angular_dis = - ref_d.norm() * utils::math::get_abs_angle(ref_d, pt_d) / ref_d.norm(); + ref_d.norm() * util::math::get_abs_angle(ref_d, pt_d) / ref_d.norm(); // 平方可能是为了配合 sin 和 cos // 弧度差代价（0 度左右占比应该大） - double cost_i = utils::math::square(pixel_dis * std::sin(inclined)) - + utils::math::square(angular_dis * std::cos(inclined)) + double cost_i = util::math::square(pixel_dis * std::sin(inclined)) + + util::math::square(angular_dis * std::cos(inclined)) * 2.0; // DETECTOR_ERROR_PIXEL_BY_SLOPE // 重投影像素误差越大，越相信斜率 cost += std::sqrt(cost_i); diff --git a/src/tongji/modules/solver/solved_armor.hpp b/src/tongji/solver/solved_armor.hpp similarity index 100% rename from src/tongji/modules/solver/solved_armor.hpp rename to src/tongji/solver/solved_armor.hpp diff --git a/src/tongji/modules/solver/solver.cpp b/src/tongji/solver/solver.cpp similarity index 98% rename from src/tongji/modules/solver/solver.cpp rename to src/tongji/solver/solver.cpp index 957282e..a9a91d4 100644 --- a/src/tongji/modules/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -11,7 +11,6 @@ #include #include -#include "../solver/reprojection_util.hpp" #include "data/armor_camera_spacing.hpp" #include "data/armor_image_spaceing.hpp" #include "data/time_stamped.hpp" @@ -19,7 +18,8 @@ #include "parameters/profile.hpp" #include "parameters/rm_parameters.hpp" #include "solved_armor.hpp" -#include "tongji/utils/coordinate.hpp" +#include "tongji/solver/reprojection_util.hpp" +#include "util/coordinate.hpp" #include "util/index.hpp" #include "util/math.hpp" namespace world_exe::tongji::solver { diff --git a/src/tongji/modules/solver/solver.hpp b/src/tongji/solver/solver.hpp similarity index 100% rename from src/tongji/modules/solver/solver.hpp rename to src/tongji/solver/solver.hpp diff --git a/src/tongji/modules/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp similarity index 100% rename from src/tongji/modules/state_machine/state_machine.cpp rename to src/tongji/state_machine/state_machine.cpp diff --git a/src/tongji/modules/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp similarity index 100% rename from src/tongji/modules/state_machine/state_machine.hpp rename to src/tongji/state_machine/state_machine.hpp diff --git a/src/tongji/utils/coordinate.hpp b/src/util/coordinate.hpp similarity index 100% rename from src/tongji/utils/coordinate.hpp rename to src/util/coordinate.hpp diff --git a/src/util/math.hpp b/src/util/math.hpp index 65b7b8b..6d91a7e 100644 --- a/src/util/math.hpp +++ b/src/util/math.hpp @@ -3,6 +3,23 @@ #include namespace world_exe::util::math { +static constexpr double ratio(const auto& point) { return atan2(point.y, point.x); } +static constexpr double clamp_pm_pi(auto&& angle) { + while (angle >= std::numbers::pi) + angle -= std::numbers::pi; + while (angle <= -std::numbers::pi) + angle += std::numbers::pi; + + return angle; +} +static constexpr double clamp_pm_tau(auto&& angle) { + while (angle >= 2 * std::numbers::pi) + angle -= 2 * std::numbers::pi; + while (angle <= -2 * std::numbers::pi) + angle += 2 * std::numbers::pi; + + return angle; +} static inline double get_yaw_from_quaternion(const Eigen::Quaterniond& quaternion) { @@ -46,6 +63,27 @@ static inline std::tuple remap( return { distance * std::cos(distance_angle), distance * std::sin(distance_angle) }; } +// zyx order +static Eigen::Matrix3d euler_to_matrix(const Eigen::Vector3d& ypr) { + double roll = ypr[2]; + double pitch = ypr[1]; + double yaw = ypr[0]; + double cos_yaw = cos(yaw); + double sin_yaw = sin(yaw); + double cos_pitch = cos(pitch); + double sin_pitch = sin(pitch); + double cos_roll = cos(roll); + double sin_roll = sin(roll); + // clang-format off + Eigen::Matrix3d R{ + {cos_yaw * cos_pitch, cos_yaw * sin_pitch * sin_roll - sin_yaw * cos_roll, cos_yaw * sin_pitch * cos_roll + sin_yaw * sin_roll}, + {sin_yaw * cos_pitch, sin_yaw * sin_pitch * sin_roll + cos_yaw * cos_roll, sin_yaw * sin_pitch * cos_roll - cos_yaw * sin_roll}, + { -sin_pitch, cos_pitch * sin_roll, cos_pitch * cos_roll} + }; + // clang-format on + return R; +} + static inline Eigen::Quaterniond euler_to_quaternion( const double& yaw_rad, const double& pitch_rad, const double& roll_rad) { Eigen::AngleAxisd rollAngle(roll_rad, Eigen::Vector3d::UnitX()); @@ -56,6 +94,81 @@ static inline Eigen::Quaterniond euler_to_quaternion( return q; } +static Eigen::Vector3d quaternion_to_euler( + Eigen::Quaterniond q, int axis0, int axis1, int axis2, bool extrinsic = false) { + if (!extrinsic) std::swap(axis0, axis2); + + auto i = axis0, j = axis1, k = axis2; + auto is_proper = (i == k); + if (is_proper) k = 3 - i - j; + auto sign = (i - j) * (j - k) * (k - i) / 2; + + double a, b, c, d; + Eigen::Vector4d xyzw = q.coeffs(); + if (is_proper) { + a = xyzw[3]; + b = xyzw[i]; + c = xyzw[j]; + d = xyzw[k] * sign; + } else { + a = xyzw[3] - xyzw[j]; + b = xyzw[i] + xyzw[k] * sign; + c = xyzw[j] + xyzw[3]; + d = xyzw[k] * sign - xyzw[i]; + } + + Eigen::Vector3d eulers; + auto n2 = a * a + b * b + c * c + d * d; + eulers[1] = std::acos(2 * (a * a + b * b) / n2 - 1); + + auto half_sum = std::atan2(b, a); + auto half_diff = std::atan2(-d, c); + + auto eps = 1e-7; + auto safe1 = std::abs(eulers[1]) >= eps; + auto safe2 = std::abs(eulers[1] - std::numbers::pi) >= eps; + auto safe = safe1 && safe2; + if (safe) { + eulers[0] = half_sum + half_diff; + eulers[2] = half_sum - half_diff; + } else { + if (!extrinsic) { + eulers[0] = 0; + if (!safe1) eulers[2] = 2 * half_sum; + if (!safe2) eulers[2] = -2 * half_diff; + } else { + eulers[2] = 0; + if (!safe1) eulers[0] = 2 * half_sum; + if (!safe2) eulers[0] = 2 * half_diff; + } + } + + for (int i = 0; i < 3; i++) + eulers[i] = clamp_pm_pi(eulers[i]); + + if (!is_proper) { + eulers[2] *= sign; + eulers[1] -= std::numbers::pi / 2; + } + + if (!extrinsic) std::swap(eulers[0], eulers[2]); + + return eulers; +} + +static Eigen::Vector3d matrix_to_euler( + Eigen::Matrix3d R, int axis0, int axis1, int axis2, bool extrinsic = false) { + Eigen::Quaterniond q(R); + return quaternion_to_euler(q, axis0, axis1, axis2, extrinsic); +} + +static inline double get_abs_angle(const Eigen::Vector2d& vec1, const Eigen::Vector2d& vec2) { + if (vec1.norm() == 0. || vec2.norm() == 0.) { + return 0.; + } + return std::acos(vec1.dot(vec2) / (vec1.norm() * vec2.norm())); //(0~pi) +} + static inline double get_angle_err_rad_from_quaternion( const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2) { double yaw1 = get_yaw_from_quaternion(q1); @@ -67,6 +180,7 @@ static inline double get_angle_err_rad_from_quaternion( if (yaw_err > std::numbers::pi) yaw_err = 2 * std::numbers::pi - yaw_err; return yaw_err; } + static inline double get_distance_err_rad_from_vector3d( const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) { double d1 = v1.norm(); @@ -75,21 +189,41 @@ static inline double get_distance_err_rad_from_vector3d( return derr; } -static constexpr double ratio(const auto& point) { return atan2(point.y, point.x); } -static constexpr double clamp_pm_pi(auto&& angle) { - while (angle >= std::numbers::pi) - angle -= std::numbers::pi; - while (angle <= -std::numbers::pi) - angle += std::numbers::pi; - return angle; +static inline Eigen::Vector3d xyz2ypd(const Eigen::Vector3d& xyz) { + auto x = xyz[0], y = xyz[1], z = xyz[2]; + auto yaw = std::atan2(y, x); + auto pitch = std::atan2(z, std::sqrt(x * x + y * y)); + auto distance = std::sqrt(x * x + y * y + z * z); + return { yaw, pitch, distance }; } -static constexpr double clamp_pm_tau(auto&& angle) { - while (angle >= 2 * std::numbers::pi) - angle -= 2 * std::numbers::pi; - while (angle <= -2 * std::numbers::pi) - angle += 2 * std::numbers::pi; - return angle; +static Eigen::Matrix xyz2ypd_jacobian(const Eigen::Vector3d& xyz) { + auto x = xyz[0], y = xyz[1], z = xyz[2]; + + auto dyaw_dx = -y / (x * x + y * y); + auto dyaw_dy = x / (x * x + y * y); + auto dyaw_dz = 0.0; + + auto dpitch_dx = -(x * z) / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 1.5)); + auto dpitch_dy = -(y * z) / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 1.5)); + auto dpitch_dz = 1 / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 0.5)); + + auto ddistance_dx = x / std::pow((x * x + y * y + z * z), 0.5); + auto ddistance_dy = y / std::pow((x * x + y * y + z * z), 0.5); + auto ddistance_dz = z / std::pow((x * x + y * y + z * z), 0.5); + + // clang-format off + Eigen::Matrix J{ + {dyaw_dx, dyaw_dy, dyaw_dz}, + {dpitch_dx, dpitch_dy, dpitch_dz}, + {ddistance_dx, ddistance_dy, ddistance_dz} + }; + // clang-format on + + return J; } + +template T square(T const& a) { return a * a; }; + } // namespace rmcs_auto_aim::util::math \ No newline at end of file diff --git a/tests/tongji_tests/utils/coordinate_test.cpp b/tests/tongji_tests/utils/coordinate_test.cpp index df1e8d4..ea0139f 100644 --- a/tests/tongji_tests/utils/coordinate_test.cpp +++ b/tests/tongji_tests/utils/coordinate_test.cpp @@ -1,4 +1,4 @@ -#include "tongji/utils/coordinate.hpp" +#include "util/coordinate.hpp" #include "gtest/gtest.h" #include From 85dd33b805d47ea08a1957e0d87c57d03baf094d Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 6 Nov 2025 11:55:19 +0800 Subject: [PATCH 89/93] fix(merge): resolve conflicts after revert operation --- tests/CMakeLists.txt | 11 +- tests/tongji_tests/utils/coordinate_test.cpp | 119 ------------------- 2 files changed, 4 insertions(+), 126 deletions(-) delete mode 100644 tests/tongji_tests/utils/coordinate_test.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index b16f2d7..8e8c08e 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -7,10 +7,7 @@ set(CMAKE_BUILD_TYPE Debug) # 包含主项目头文件 include_directories(../include) -file(GLOB_RECURSE TONGJI_TEST_SOURCES "./tongji_tests/*.cpp") - -add_executable(all_tests - ${TONGJI_TEST_SOURCES}) +add_executable(test_main test_main.cpp) add_executable(runable runable_main.cpp) @@ -28,15 +25,15 @@ FetchContent_Declare( set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) FetchContent_MakeAvailable(googletest) -target_link_libraries(all_tests +target_link_libraries(test_main GTest::gtest_main GTest::gmock_main alliance_auto_aim ) + target_link_libraries(runable alliance_auto_aim ) -include(GoogleTest) -gtest_discover_tests(all_tests) + diff --git a/tests/tongji_tests/utils/coordinate_test.cpp b/tests/tongji_tests/utils/coordinate_test.cpp deleted file mode 100644 index ea0139f..0000000 --- a/tests/tongji_tests/utils/coordinate_test.cpp +++ /dev/null @@ -1,119 +0,0 @@ -#include "util/coordinate.hpp" - -#include "gtest/gtest.h" -#include - -namespace coord = world_exe::util::coordinate; - -TEST(CoordinateTest, Opencv2RosPosition_SimpleAxis) { - // 1. 准备 (Arrange) - 在 OpenCV 坐标系下，沿着每个轴单位移动 - Eigen::Vector3d opencv_pos_x(1.0, 0.0, 0.0); // 沿 X 轴 (右) - Eigen::Vector3d opencv_pos_y(0.0, 1.0, 0.0); // 沿 Y 轴 (下) - Eigen::Vector3d opencv_pos_z(0.0, 0.0, 1.0); // 沿 Z 轴 (前) - - // 预期结果 (ROS 坐标系: x->前, y->左, z->上) - // (x, y, z) -> (z, -x, -y) - Eigen::Vector3d ros_expected_x(0.0, -1.0, 0.0); // (0, -1, 0) - Eigen::Vector3d ros_expected_y(0.0, 0.0, -1.0); // (0, 0, -1) - Eigen::Vector3d ros_expected_z(1.0, 0.0, 0.0); // (1, 0, 0) - - // 2. 执行 (Act) - Eigen::Vector3d ros_actual_x = coord::opencv2ros_position(opencv_pos_x); - Eigen::Vector3d ros_actual_y = coord::opencv2ros_position(opencv_pos_y); - Eigen::Vector3d ros_actual_z = coord::opencv2ros_position(opencv_pos_z); - - // 3. 断言 (Assert) - // 使用 ASSERT_TRUE 结合 Eigen 的 isApprox() 方法进行浮点数向量的精确比较 - ASSERT_TRUE(ros_actual_x.isApprox(ros_expected_x)) << "X-axis conversion failed."; - ASSERT_TRUE(ros_actual_y.isApprox(ros_expected_y)) << "Y-axis conversion failed."; - ASSERT_TRUE(ros_actual_z.isApprox(ros_expected_z)) << "Z-axis conversion failed."; -} - -TEST(CoordinateTest, Ros2OpencvPosition_SimpleAxis) { - // 1. 准备 (Arrange) - ROS 坐标系下的单位向量 - Eigen::Vector3d ros_pos_x(1.0, 0.0, 0.0); // 沿 X 轴 (前) - Eigen::Vector3d ros_pos_y(0.0, 1.0, 0.0); // 沿 Y 轴 (左) - Eigen::Vector3d ros_pos_z(0.0, 0.0, 1.0); // 沿 Z 轴 (上) - - // 预期结果 (OpenCV 坐标系: x->右, y->下, z->前) - // (-y, -z, x) - Eigen::Vector3d opencv_expected_x(0.0, 0.0, 1.0); // (0, 0, 1) - Eigen::Vector3d opencv_expected_y(-1.0, 0.0, 0.0); // (-1, 0, 0) - Eigen::Vector3d opencv_expected_z(0.0, -1.0, 0.0); // (0, -1, 0) - - // 2. 执行 (Act) - Eigen::Vector3d opencv_actual_x = coord::ros2opencv_position(ros_pos_x); - Eigen::Vector3d opencv_actual_y = coord::ros2opencv_position(ros_pos_y); - Eigen::Vector3d opencv_actual_z = coord::ros2opencv_position(ros_pos_z); - - // 3. 断言 (Assert) - ASSERT_TRUE(opencv_actual_x.isApprox(opencv_expected_x)); - ASSERT_TRUE(opencv_actual_y.isApprox(opencv_expected_y)); - ASSERT_TRUE(opencv_actual_z.isApprox(opencv_expected_z)); -} - -// **关键交叉测试 (Round Trip Test):** 确保来回转换后结果不变 -TEST(CoordinateTest, Position_RoundTrip) { - // 1. 准备：一个复杂的随机向量 - Eigen::Vector3d original_pos(1.23, -4.56, 7.89); - - // 2. 执行：OpenCV -> ROS -> OpenCV - Eigen::Vector3d ros_pos = coord::opencv2ros_position(original_pos); - Eigen::Vector3d final_pos = coord::ros2opencv_position(ros_pos); - - // 3. 断言：最终位置应该近似等于原始位置 - ASSERT_TRUE(final_pos.isApprox(original_pos)) - << "Round trip conversion failed. Original: " << original_pos.transpose() - << ", Final: " << final_pos.transpose(); -} - -TEST(CoordinateTest, Opencv2RosRotation_Identity) { - // 1. 准备：单位旋转矩阵 (无旋转) - Eigen::Matrix3d identity_rot = Eigen::Matrix3d::Identity(); - - // 2. 执行 - Eigen::Matrix3d ros_rot = coord::opencv2ros_rotation(identity_rot); - - // 3. 断言：转换一个单位矩阵，结果应该是一个单位矩阵 - ASSERT_TRUE(ros_rot.isApprox(identity_rot)) << "Identity rotation failed."; -} - -TEST(CoordinateTest, Opencv2RosRotation_KnownRotation) { - // 1. 准备：绕 OpenCV 坐标系 Z 轴旋转 90 度 (Z-axis 绕前方向) - // OpenCV 坐标系：X(右), Y(下), Z(前) - // 绕 Z 轴旋转 90 度 (右转 90) => X->Y, Y->-X - double angle = M_PI / 2.0; - Eigen::Matrix3d rot_opencv; - rot_opencv << cos(angle), -sin(angle), 0, sin(angle), cos(angle), 0, 0, 0, 1; - - // 2. 执行 - Eigen::Matrix3d ros_rot = coord::opencv2ros_rotation(rot_opencv); - - // 3. 预期结果：验证一个点。 - // 在 OpenCV 中 (1, 0, 0) 旋转后变成 (0, 1, 0)。 - // 转换到 ROS 坐标系后: - // (1, 0, 0) -> (0, -1, 0) (ROS X) - // (0, 1, 0) -> (0, 0, -1) (ROS Y) - // ROS 旋转矩阵应该把 (0, -1, 0) 变成 (0, 0, -1)。 - - Eigen::Vector3d ros_vector_in(0.0, -1.0, 0.0); // 相当于 OpenCV 的 X 轴 - Eigen::Vector3d ros_vector_out = ros_rot * ros_vector_in; - - Eigen::Vector3d ros_expected_out(0.0, 0.0, -1.0); // 相当于 OpenCV 的 Y 轴 - - // 4. 断言 - ASSERT_TRUE(ros_vector_out.isApprox(ros_expected_out)) << "Known rotation test failed."; -} - -TEST(CoordinateTest, Rotation_RoundTrip) { - // 1. 准备：一个绕随机轴旋转的复杂旋转矩阵 - Eigen::AngleAxisd aa(0.5, Eigen::Vector3d(0.1, 0.5, -0.3).normalized()); - Eigen::Matrix3d original_rot = aa.toRotationMatrix(); - - // 2. 执行：OpenCV -> ROS -> OpenCV - Eigen::Matrix3d ros_rot = coord::opencv2ros_rotation(original_rot); - Eigen::Matrix3d final_rot = coord::ros2opencv_rotation(ros_rot); // 使用 ros2opencv_rotation - - // 3. 断言：最终矩阵应该近似等于原始矩阵 - ASSERT_TRUE(final_rot.isApprox(original_rot, 1e-6)) << "Rotation Round trip conversion failed."; -} From 08eef45093289b1d8d47dba5bace51893581bb0c Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 6 Nov 2025 13:01:02 +0800 Subject: [PATCH 90/93] test: synchronize test-related modifications --- include/enum/system_version.hpp | 11 +-- src/core/system_factory.cpp | 5 +- src/tongji/auto_aim_system.cpp | 90 ++++++++++++++++--- src/tongji/identifier/armor_filter.hpp | 22 +++-- src/tongji/identifier/tracker.hpp | 7 +- .../kalman_filter/extended_kalman_filter.hpp | 6 +- .../predictor/kalman_filter/predict_model.hpp | 8 +- src/tongji/state_machine/state_machine.cpp | 1 + src/util/math.hpp | 16 ++-- src/util/parameters/params_system_v1.cpp | 5 +- src/v1/identifier/identifier.cpp | 23 ++--- tests/mocks/MockCamera.hpp | 52 +++++++++++ 12 files changed, 191 insertions(+), 55 deletions(-) create mode 100644 tests/mocks/MockCamera.hpp diff --git a/include/enum/system_version.hpp b/include/enum/system_version.hpp index 4ad0094..1ac896a 100644 --- a/include/enum/system_version.hpp +++ b/include/enum/system_version.hpp @@ -6,11 +6,12 @@ namespace world_exe::enumeration { enum class SystemVersion : uint32_t { - V1 = 0x00010000, - V1Debug = 0x00010001, - V2 = 0x00020000, - Balabala = 0x00030000, - Default = V1 + V1 = 0x00010000, + V1Debug = 0x00010001, + V2 = 0x00020000, + V2Debug = 0x00020001, + Balabala = 0x00030000, + Default = V1 }; } \ No newline at end of file diff --git a/src/core/system_factory.cpp b/src/core/system_factory.cpp index 91dc0d4..0e6fc7f 100644 --- a/src/core/system_factory.cpp +++ b/src/core/system_factory.cpp @@ -17,6 +17,9 @@ void world_exe::core::SystemFactory::Build(const enumeration::SystemVersion& ver case world_exe::enumeration::SystemVersion::V2: world_exe::tongji::AutoAimSystem::build(false); // 构建调试运行的V1版本 break; + case world_exe::enumeration::SystemVersion::V2Debug: + world_exe::tongji::AutoAimSystem::build(true); // 构建调试运行的V2版本 + break; default: #if __cplusplus >= 202002L throw std::runtime_error(std::format("Target version 0x{:x} is not impleme \n Factory " @@ -24,7 +27,7 @@ void world_exe::core::SystemFactory::Build(const enumeration::SystemVersion& ver (int)version)); #else throw std::runtime_error("Target version is not impleme \n Factory " - "core/system_factory.cpp : Build(SystemVersion) "); + "core/system_factory.cpp : Build(SystemVersion) "); #endif break; diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 5cc3488..3c1986a 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -1,18 +1,29 @@ #include "auto_aim_system.hpp" #include +#include +#include +#include #include +#include +#include +#include #include "../v1/sync/syncer.hpp" #include "core/event_bus.hpp" #include "data/mat_stamped.hpp" #include "data/predictor_update_package.hpp" +#include "data/time_stamped.hpp" #include "parameters/params_system_v1.hpp" #include "parameters/profile.hpp" +#include "parameters/rm_parameters.hpp" #include "tongji/fire_controller/fire_controller.hpp" +#include "tongji/identifier/identifier.hpp" #include "tongji/predictor/car_predictor/car_predictor_manager.hpp" #include "tongji/solver/solver.hpp" #include "tongji/state_machine/state_machine.hpp" +#include "utils/fps_counter.hpp" +#include "utils/visualization.hpp" #include "v1/identifier/identifier.hpp" namespace world_exe::tongji { @@ -22,12 +33,15 @@ class AutoAimSystem::Impl { public: Impl(const bool& debug) : debug(debug) - , config_path_("../../configs/example.yaml") { - + , config_path_("/workspaces/src/alliance_ros_auto_aim/alliance_auto_aim/configs/" + "example.yaml") + , fps_() { identifier_ = std::make_unique( parameters::ParamsForSystemV1::szu_model_path(), parameters::ParamsForSystemV1::device(), parameters::HikCameraProfile::get_width(), parameters::HikCameraProfile::get_height()); + // identifier_ = std::make_unique(config_path_, + // "."); pnp_solver_ = std::make_unique(); live_target_manager_ = std::make_shared(config_path_); state_machine_ = std::make_shared(); @@ -36,7 +50,8 @@ class AutoAimSystem::Impl { time_stamp_ = std::chrono::steady_clock::now(); syncer_ = std::make_unique(seconds(2), 6e-6); - core::EventBus::Subscript(parameters::ParamsForSystemV1::raw_image_event, + core::EventBus::Subscript( + parameters::ParamsForSystemV1::raw_image_event, [this](const world_exe::data::MatStamped& mat) { Solve(mat); }); core::EventBus::Subscript( parameters::ParamsForSystemV1::camera_capture_transforms, @@ -44,15 +59,33 @@ class AutoAimSystem::Impl { } auto Solve(const data::MatStamped& raw) -> void { + if (identifier_ == nullptr) std::terminate(); const auto& [armors_in_image, flag] = identifier_->identify(raw.mat); + + // if (armors_in_image) { + // auto visualized = raw.mat.clone(); + // util::visualization::draw_armor_in_image(*armors_in_image, visualized); + // cv::imshow("identified", visualized); + // cv::waitKey(1); + // } else { + // std::printf("No identified armors/n"); + // } + // if (fps_.count()) std::cout << fps_.fps() << std::endl; + // return; + + // std::cout << "here" << std::endl; + if (flag == enumeration::ArmorIdFlag::None) return; + // std::cout << "here" << std::endl; state_machine_->Update(armors_in_image, std::chrono::duration_cast( std::chrono::steady_clock::now() - time_stamp_)); // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分 - const auto& [pack, check] = syncer_->get_data(raw.stamp); - if (!check) return; + auto [pack, check] = syncer_->get_data(raw.stamp); + if (!check) + pack.camera_capture_begin_time_stamp = + data::TimeStamp(steady_clock::now().time_since_epoch()); const auto R_camera2gimbal = pack.camera_to_gimbal.rotation(); const auto t_camera2gimbal = pack.camera_to_gimbal.translation(); @@ -61,9 +94,12 @@ class AutoAimSystem::Impl { const auto& armors_in_camera = pnp_solver_->SolvePnp(armors_in_image); auto combined = std::make_shared(pack, armors_in_camera); - live_target_manager_->Update(combined); + core::EventBus::Publish>( + parameters::ParamsForSystemV1::tracker_current_armors_event, + live_target_manager_->Predict(flag, pack.camera_capture_begin_time_stamp)); + state_machine_ = std::make_shared(); const auto target_id = state_machine_->GetAllowdToFires(); @@ -76,6 +112,40 @@ class AutoAimSystem::Impl { core::EventBus::Publish( parameters::ParamsForSystemV1::fire_control_event, GetControlCommand()); + + if (!debug) [[likely]] + return; + + core::EventBus::Publish( + parameters::ParamsForSystemV1::car_id_identify_event, flag); + core::EventBus::Publish>( + parameters::ParamsForSystemV1::armors_in_image_identify_event, armors_in_image); + core::EventBus::Publish>( + parameters::ParamsForSystemV1::armors_in_camera_pnp_event, armors_in_camera); + core::EventBus::Publish>( + parameters::ParamsForSystemV1::tracker_update_event, combined); + core::EventBus::Publish( + parameters::ParamsForSystemV1::car_tracing_event, state_machine_->GetAllowdToFires()); + // std::cout << "here" << std::endl; + // if (armors_in_image) { + // auto visualized = raw.mat.clone(); + // util::visualization::draw_armor_in_image(*armors_in_image, visualized); + // cv::imshow("identified", visualized); + // cv::waitKey(1); + // } else { + // std::printf("No identified armors/n"); + // } + // if (armors_in_camera) { + // auto visualized = raw.mat.clone(); + // util::visualization::draw_armor_in_camera(*armors_in_camera, + // parameters::HikCameraProfile::get_intrinsic_parameters(), + // parameters::HikCameraProfile::get_distortion_parameters(), + // parameters::Robomaster::NormalArmorObjectPointsRos, visualized); + // cv::imshow("pnp", visualized); + // cv::waitKey(1); + // } else { + // std::printf("No pnp armors/n"); + // } } void SetTransfroms(const data::CameraGimbalMuzzleSyncData& data) { syncer_->set_data(data); } @@ -89,9 +159,9 @@ class AutoAimSystem::Impl { private: bool debug; const std::string config_path_; - + world_exe::util::FpsCounter fps_; std::chrono::steady_clock::time_point time_stamp_; - std::unique_ptr identifier_; + std::unique_ptr identifier_; std::unique_ptr pnp_solver_; std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; @@ -104,8 +174,8 @@ AutoAimSystem::AutoAimSystem(const bool& debug) AutoAimSystem::~AutoAimSystem() = default; std::unique_ptr AutoAimSystem::v2; -void AutoAimSystem::build(bool debug){ - if(v2 != nullptr) return; +void AutoAimSystem::build(bool debug) { + if (v2 != nullptr) return; v2 = std::make_unique(debug); } } diff --git a/src/tongji/identifier/armor_filter.hpp b/src/tongji/identifier/armor_filter.hpp index 21699e0..8189cd3 100644 --- a/src/tongji/identifier/armor_filter.hpp +++ b/src/tongji/identifier/armor_filter.hpp @@ -18,24 +18,28 @@ class ArmorFilter { std::vector FilterArmor( std::vector armors) const { + armors.erase(std::remove_if(armors.begin(), armors.end(), [&](const auto& armor) { // 25赛季没有5号装甲板 - armors.erase(std::remove_if(armors.begin(), armors.end(), - [](const auto& armor) { return armor.id == enumeration::ArmorIdFlag::InfantryV; })); // 不打前哨站 - armors.erase(std::remove_if(armors.begin(), armors.end(), - [](const auto& armor) { return armor.id == enumeration::ArmorIdFlag::Outpost; })); // 过滤掉刚复活无敌的装甲板 - armors.erase(std::remove_if(armors.begin(), armors.end(), - [&](const auto& armor) { return invincible_armor_.count(armor.id); }), - armors.end()); - return std::move(armors); + return armor.id == enumeration::ArmorIdFlag::InfantryV + || armor.id == enumeration::ArmorIdFlag::Outpost + || invincible_armor_.count(armor.id); + })); + // armors.erase(std::remove_if(armors.begin(), armors.end(), + // [](const auto& armor) { return armor.id == enumeration::ArmorIdFlag::Outpost; })); + // // 过滤掉刚复活无敌的装甲板 + // armors.erase(std::remove_if(armors.begin(), armors.end(), + // [&](const auto& armor) { return invincible_armor_.count(armor.id); })); + + return armors; } void Update(enumeration::CarIDFlag ids) { invincible_armor_.clear(); for (auto id : util::enumeration::ExpandArmorIdFlags(ids)) { - invincible_armor_.emplace(std::move(id)); + invincible_armor_.insert(std::move(id)); } } diff --git a/src/tongji/identifier/tracker.hpp b/src/tongji/identifier/tracker.hpp index aca3538..e11a045 100644 --- a/src/tongji/identifier/tracker.hpp +++ b/src/tongji/identifier/tracker.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -32,8 +33,8 @@ class Tracker final { auto SelectTrackingTargetID(const std::shared_ptr& armors_in_image, const std::chrono::milliseconds& duration_from_last_update) noexcept - -> enumeration::ArmorIdFlag const { + CheckCameraOffline(duration_from_last_update); auto filtered_ids = enumeration::ArmorIdFlag::None; @@ -52,11 +53,11 @@ class Tracker final { if (!filtered_armors.empty()) { filtered_ids = static_cast(static_cast(filtered_ids) - | static_cast(filtered_armors.at(0).id)); + | static_cast(filtered_armors[0].id)); } } - UpdateState(!(filtered_ids == enumeration::ArmorIdFlag::None)); + // UpdateState(!(filtered_ids == enumeration::ArmorIdFlag::None)); tracking_car_id_ = decider_->GetBestArmor(filtered_armors); return tracking_car_id_; diff --git a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp index e7972f0..bd95c1c 100644 --- a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp +++ b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp @@ -74,15 +74,15 @@ template class ExtendedKalmanFilter { const RMat S = H * P_prior * H.transpose() + R; const Eigen::MatrixXd K = P_prior * H.transpose() * S.inverse(); - // std::cout << P << "\n" << std::endl; + // std::cout << P << "\n" << std::endl; x << model_.x_add(x_prior, K * residual); // Stable Compution of the Posterior Covariance // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb - // FIXME: P -> P^- - P << (I - K * H) * P_prior; + // FIXME: P -> P^- // P << (I - K * H) * P_prior * (I - K * H).transpose() + K * R * K.transpose(); + P << (I - K * H) * P_prior; const auto P_inverse = P_prior.inverse(); const auto error = x - x_prior; diff --git a/src/tongji/predictor/kalman_filter/predict_model.hpp b/src/tongji/predictor/kalman_filter/predict_model.hpp index b0180e1..25946c4 100644 --- a/src/tongji/predictor/kalman_filter/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -228,10 +228,10 @@ class EKFModel { v1 = 100; // 加速度方差 v2 = 400; // 角加速度方差 } - auto dt_ = dt ; - auto a = dt_ * dt_ * dt_ * dt_ / 4; - auto b = dt_ * dt_ * dt_ / 2; - auto c = dt_ * dt_; + auto dt_ = dt * 1e4; + auto a = dt_ * dt_ * dt_ * dt_ / 4; + auto b = dt_ * dt_ * dt_ / 2; + auto c = dt_ * dt_; // 预测过程噪声偏差的方差 QMat _Q; diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 284e855..0115ade 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -1,5 +1,6 @@ #include "state_machine.hpp" +#include #include #include "../identifier/tracker.hpp" diff --git a/src/util/math.hpp b/src/util/math.hpp index 154ff44..c825b15 100644 --- a/src/util/math.hpp +++ b/src/util/math.hpp @@ -4,18 +4,19 @@ namespace world_exe::util::math { static constexpr double ratio(const auto& point) { return atan2(point.y, point.x); } -static constexpr double clamp_pm_pi(auto&& angle) { - while (angle >= std::numbers::pi) + +static constexpr double clamp_pm_tau(auto&& angle) { + while (angle >= 2 * std::numbers::pi) angle -= 2 * std::numbers::pi; - while (angle <= -std::numbers::pi) + while (angle <= -2 * std::numbers::pi) angle += 2 * std::numbers::pi; return angle; } -static constexpr double clamp_pm_tau(auto&& angle) { - while (angle >= 2 * std::numbers::pi) +static constexpr double clamp_pm_pi(auto&& angle) { + while (angle > std::numbers::pi) angle -= 2 * std::numbers::pi; - while (angle <= -2 * std::numbers::pi) + while (angle <= -std::numbers::pi) angle += 2 * std::numbers::pi; return angle; @@ -205,7 +206,6 @@ static Eigen::Matrix xyz2ypd_jacobian(const Eigen::Vector3d& xyz) auto dyaw_dy = x / (x * x + y * y); auto dyaw_dz = 0.0; - auto dpitch_dx = -(x * z) / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 1.5)); auto dpitch_dy = -(y * z) / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 1.5)); auto dpitch_dz = 1 / ((z * z / (x * x + y * y) + 1) * std::pow((x * x + y * y), 0.5)); @@ -227,4 +227,4 @@ static Eigen::Matrix xyz2ypd_jacobian(const Eigen::Vector3d& xyz) template T square(T const& a) { return a * a; }; -} // namespace rmcs_auto_aim::util::math \ No newline at end of file +} // namespace rmcs_auto_aim::util::math diff --git a/src/util/parameters/params_system_v1.cpp b/src/util/parameters/params_system_v1.cpp index 392a545..f86d58d 100644 --- a/src/util/parameters/params_system_v1.cpp +++ b/src/util/parameters/params_system_v1.cpp @@ -6,8 +6,9 @@ namespace world_exe::parameters { struct ParamsForSystemV1::Impl { public: - std::string model_path = "/home/alray/workspace/ros_alliance_ws/alliance_ros_auto_aim/alliance_auto_aim/models/szu_identify_model.onnx"; - std::string device = "AUTO"; + std::string model_path = "/workspaces/src/alliance_ros_auto_aim/alliance_auto_aim/models/" + "szu_identify_model.onnx"; + std::string device = "AUTO"; double control_delay_in_second = 0.05; double velocity_begin = 26; double gravity = 9.81; diff --git a/src/v1/identifier/identifier.cpp b/src/v1/identifier/identifier.cpp index c0afbeb..6343c81 100644 --- a/src/v1/identifier/identifier.cpp +++ b/src/v1/identifier/identifier.cpp @@ -16,8 +16,8 @@ #include "openvino/core/preprocess/pre_post_process.hpp" #include "openvino/runtime/core.hpp" -#include #include +#include #include namespace world_exe::v1::identifier { @@ -76,14 +76,16 @@ class Identifier::Impl { */ std::tuple, enumeration::CarIDFlag> Identify( const cv::Mat& input_image) { - + // 首先使用深度学习模型进行装甲板检测得到roi区域 const auto armor_infos = model_infer(input_image); // 然后进行灯条匹配验证 auto [a, b] = matchPlate(input_image, armor_infos); - if(a != nullptr) - a->time_stamp_ = std::chrono::steady_clock::now().time_since_epoch(); - return {a, b}; + if (!a) { + return { a, enumeration::CarIDFlag::None }; + } + a->time_stamp_ = std::chrono::steady_clock::now().time_since_epoch(); + return { a, b }; } /** @@ -141,7 +143,7 @@ class Identifier::Impl { if (confidence < conf_threshold_) continue; // 过滤低置信度检测 // 解析颜色分类结果（蓝、红、紫、无色） - const auto color_scores = output_buffer.row(i).colRange(9, 13); // color + const auto color_scores = output_buffer.row(i).colRange(9, 13); // color // 解析车辆类型分类结果（哨兵、英雄、工程等） const auto classes_scores = output_buffer.row(i).colRange(13, 22); // num cv::Point class_id, color_id; @@ -207,6 +209,7 @@ class Identifier::Impl { cv::dnn::NMSBoxes(boxes, confidences, conf_threshold_, nms_threshold_, indices); // 构建最终的检测结果 + objects_.clear(); for (const std::size_t valid_index : indices) if (valid_index <= boxes.size()) { auto object = tmp_objects_[valid_index]; @@ -233,8 +236,8 @@ class Identifier::Impl { }; std::tuple, enumeration::CarIDFlag> matchPlate( - const cv::Mat& img, const std::vector& armor_infos) { - if (armor_infos.empty()) return {}; + const cv::Mat& img, const std::vector armor_infos) { + if (armor_infos.empty()) return { }; // 图像预处理：转换为灰度图并二值化，为后续灯条检测做准备,由于我们前面通过模型拿到了灯条的roi区域，在区域内基本不会误识别，所以后面我们通过传统方式来匹配时各种阈值都可以给松一些，注意，这里的阈值可能要根据实际情况做出调整 cv::cvtColor(img, gray_img_, cv::COLOR_BGR2GRAY); @@ -253,7 +256,7 @@ class Identifier::Impl { 0, image_width_), std::clamp(static_cast(armor.rect_.y - armor.rect_.height / 2. * (match_magnification_ratio_ - 1.)), - 0, image_width_) + 0, image_height_) }; cv::Size rect_size { std::clamp(static_cast( armor.rect_.width * match_magnification_ratio_), @@ -407,7 +410,7 @@ class Identifier::Impl { bool target_color_ { false }; ///< 目标颜色：false=蓝色，true=红色 ov::CompiledModel compiled_model_; ///< 编译后的 OpenVINO 模型 - std::vector objects_{}; + std::vector objects_ { }; }; Identifier::Identifier(const std::string& model_path, const std::string& device, diff --git a/tests/mocks/MockCamera.hpp b/tests/mocks/MockCamera.hpp new file mode 100644 index 0000000..e58392a --- /dev/null +++ b/tests/mocks/MockCamera.hpp @@ -0,0 +1,52 @@ +#include + +#include "data/time_stamped.hpp" + +namespace world_exe::tests::mock { + +struct MockCamera { + Eigen::Vector3d position; + Eigen::Matrix3d orientation; + + MockCamera() + : position(Eigen::Vector3d::Zero()) + , orientation(Eigen::Matrix3d::Identity()) { } +}; + +class MockCameraInGimbal final { +public: + MockCameraInGimbal(double angular_speed = 1) + : time(std::chrono::steady_clock::now().time_since_epoch()) + , camera({ }) + , gimbal_origin(Eigen::Vector3d(0, 0, 0)) { } + + const data::TimeStamp& GetTimeStamp() const { return time; } + + void RotateYawPitchAndTranslate( + double t, double yaw_speed, double pitch_speed, const Eigen::Vector3d& linear_velocity) { + double yaw_angle = yaw_speed * t; + double pitch_angle = pitch_speed * t; + + Eigen::Matrix3d R_yaw = + Eigen::AngleAxisd(yaw_angle, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + Eigen::Matrix3d R_pitch = + Eigen::AngleAxisd(pitch_angle, Eigen::Vector3d::UnitY()).toRotationMatrix(); + Eigen::Matrix3d rotation = R_yaw * R_pitch; + + Eigen::Vector3d translation = linear_velocity * t; + + MoveCamera(rotation, translation); + } + +private: + void MoveCamera(const Eigen::Matrix3d& rotation_matrix, const Eigen::Vector3d& translation) { + Eigen::Vector3d relative = camera.position - gimbal_origin; + camera.position = gimbal_origin + rotation_matrix * relative + translation; + camera.orientation = rotation_matrix * camera.orientation; + } + + data::TimeStamp time; + MockCamera camera; + Eigen::Vector3d gimbal_origin; +}; +} \ No newline at end of file From 1c690408092ea617740f07b6e1aedf4458e1d6b3 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 7 Nov 2025 00:14:51 +0800 Subject: [PATCH 91/93] test:add gimbal mock test --- tests/mock_gimbal_test.cpp | 74 +++++++++++++++++++++ tests/mocks/mock_camera_tranform.hpp | 97 ++++++++++++++++++++++++++++ 2 files changed, 171 insertions(+) create mode 100644 tests/mock_gimbal_test.cpp create mode 100644 tests/mocks/mock_camera_tranform.hpp diff --git a/tests/mock_gimbal_test.cpp b/tests/mock_gimbal_test.cpp new file mode 100644 index 0000000..e5752e8 --- /dev/null +++ b/tests/mock_gimbal_test.cpp @@ -0,0 +1,74 @@ + +#include "mocks/MockArmorInCamera.hpp" +#include "mocks/MockArmorInWorld.hpp" +#include "mocks/mock_camera_tranform.hpp" +#include +#include + +#include +#include +#include + +#include + +#include +#include + +int main() { + using namespace world_exe::util::visualization; + + Eigen::Vector3d V_input(1., 0., 0.); + double Omega_Yaw = 0.0; + double Omega_Pitch = 0.0; + double Max_Pitch = M_PI / 48.0; + auto transformer = world_exe::tests::mock::Camera2GimbalTransformer( + V_input, Omega_Yaw, Omega_Pitch, Max_Pitch); + + double dt = 0.01; + + cv::Mat image = cv::Mat { 1080, 1440, CV_8UC3, { 255, 255, 255 } }; + auto tmp = image.clone(); + + auto armor_in_camera = std::make_shared(0, 0); + auto armor_in_gimbal = std::make_shared(0, 0); + + auto armors_in_camera_3 = armor_in_camera->GetArmors(armor_in_camera->armorid); + auto armors_in_gimbal_3 = armor_in_gimbal->GetArmors(armor_in_gimbal->armorid); + + auto num = armors_in_camera_3.size(); + while (true) { + image.copyTo(tmp); + + draw_armor_in_camera(*armor_in_camera, + world_exe::parameters::HikCameraProfile::get_intrinsic_parameters(), + world_exe::parameters::HikCameraProfile::get_distortion_parameters(), + world_exe::parameters::Robomaster::NormalArmorObjectPointsRos, tmp); + + auto transform = transformer.updateAndGetTransform(dt); + + for (int i = 0; i < num; ++i) { + armors_in_gimbal_3[i].position = transform * armors_in_camera_3[i].position; + armors_in_gimbal_3[i].orientation = + Quaterniond(transform.rotation()) * armors_in_camera_3[i].orientation; + } + + // draw_armor_in_camera(*armor_in_camera, + // world_exe::parameters::HikCameraProfile::get_intrinsic_parameters(), + // world_exe::parameters::HikCameraProfile::get_distortion_parameters(), + // world_exe::parameters::Robomaster::NormalArmorObjectPointsRos, tmp); + + draw_armor_in_gimbal(*armor_in_gimbal, + world_exe::parameters::HikCameraProfile::get_intrinsic_parameters(), + world_exe::parameters::HikCameraProfile::get_distortion_parameters(), + world_exe::parameters::Robomaster::NormalArmorObjectPointsRos, transform.inverse(), + tmp); + cv::imshow("imshow", tmp); + cv::waitKey(1); + + // std::cout << armors_in_camera_3[0].position << std::endl; + std::cout << armors_in_gimbal_3[0].position << std::endl; + std::cout << std::endl; + + // std::this_thread::sleep_for(std::chrono::milliseconds { }); + } +} \ No newline at end of file diff --git a/tests/mocks/mock_camera_tranform.hpp b/tests/mocks/mock_camera_tranform.hpp new file mode 100644 index 0000000..2cfb60e --- /dev/null +++ b/tests/mocks/mock_camera_tranform.hpp @@ -0,0 +1,97 @@ +#include + +#include +#include +#include + +using namespace Eigen; +using AffineTransform = Affine3d; // 仿射变换矩阵 (4x4) +using Vector3D = Vector3d; +using QuaternionType = Quaterniond; + +namespace world_exe::tests::mock { + +class Camera2GimbalTransformer { +private: + // --- 状态变量 --- + // 当前的仿射变换 (包含位置和姿态) + AffineTransform T_current; + + // 当前时间 + double current_time; + + // --- 输入参数 --- + // 1. 水平平移速度 (在世界坐标系下) + const Vector3D V_horizontal; + // 2. 绕竖直轴 (Z轴) 的角速度 (在世界坐标系下) + const double Omega_Yaw; + // 3. 绕 Pitch 轴的角速度 (在世界坐标系下) + double Omega_Pitch; + // 4. 最大 Pitch 角度 (用于约束) + const double Max_Pitch_Angle; + + // 用于追踪当前 Pitch 角度，以实现角度约束 (假设Pitch轴是Y轴) + double current_pitch_angle; + +public: + /** + * @brief 构造函数 + * @param v_horiz 水平速度向量 (例如: 1.0, 0.0, 0.0) + * @param omega_yaw 绕 Z 轴角速度 (rad/s) + * @param omega_pitch 绕 Pitch 轴角速度 (rad/s) + * @param max_pitch_angle 绕 Pitch 轴的最大角度限制 (rad) + */ + Camera2GimbalTransformer( + const Vector3D& v_horiz, double omega_yaw, double omega_pitch, double max_pitch_angle) + : V_horizontal(v_horiz) + , Omega_Yaw(omega_yaw) + , Omega_Pitch(omega_pitch) + , Max_Pitch_Angle(std::abs(max_pitch_angle)) + , // 确保最大角度是正值 + T_current(AffineTransform::Identity()) + , current_time(0.0) + , current_pitch_angle(0.0) { } + + /** + * @brief 按照给定的时间步长进行一次状态积分，并返回更新后的 Affine 变换 + * @param dt 时间步长 (秒) + * @return 更新后的 Affine 变换矩阵 + */ + AffineTransform updateAndGetTransform(double dt) { + Vector3D delta_t = V_horizontal * dt; + + double delta_yaw = Omega_Yaw * dt; + AngleAxisd R_yaw(delta_yaw, Vector3D::UnitZ()); + + double delta_pitch = Omega_Pitch * dt; + double next_pitch_angle = current_pitch_angle + delta_pitch; + if (next_pitch_angle > Max_Pitch_Angle || next_pitch_angle < -Max_Pitch_Angle) { + + Omega_Pitch = -Omega_Pitch; + delta_pitch = Omega_Pitch * dt; + } + + current_pitch_angle = current_pitch_angle + delta_pitch; + + AngleAxisd R_pitch(delta_pitch, Vector3D::UnitY()); + + // 2c. 组合旋转增量：R_total = R_yaw * R_pitch + // 假设 Pitch 和 Yaw 是在世界坐标系下连续作用的 (即增量也是在世界坐标系下) + // 结果是一个 AngleAxisd 或 Quaternion，需要转换为 Affine + AffineTransform T_rot_increment = AffineTransform(R_yaw * R_pitch); + + // --- 3. 组合总增量 T_delta --- + + // T_delta = T_平移 * T_旋转 + AffineTransform T_delta = Translation3d(delta_t) * T_rot_increment; + + // --- 4. 状态更新 --- + + // T_new = T_delta * T_old (因为 T_delta 是相对于世界坐标系W的增量) + T_current = T_delta * T_current; + current_time += dt; + + return T_current; + } +}; +} From fa2f06dfc272903df32030648b280687997441a4 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 7 Nov 2025 00:15:09 +0800 Subject: [PATCH 92/93] test:add gimbal mock test --- src/tongji/auto_aim_system.cpp | 18 ++++++++---- src/util/visaulization.cpp | 14 ++++----- tests/CMakeLists.txt | 7 +++++ tests/mocks/MockCamera.hpp | 52 ---------------------------------- 4 files changed, 25 insertions(+), 66 deletions(-) delete mode 100644 tests/mocks/MockCamera.hpp diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 3c1986a..28fc87e 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -1,29 +1,32 @@ #include "auto_aim_system.hpp" #include + #include #include #include #include #include #include -#include + +#include #include "../v1/sync/syncer.hpp" #include "core/event_bus.hpp" #include "data/mat_stamped.hpp" #include "data/predictor_update_package.hpp" +#include "data/sync_data.hpp" #include "data/time_stamped.hpp" +#include "enum/car_id.hpp" #include "parameters/params_system_v1.hpp" #include "parameters/profile.hpp" -#include "parameters/rm_parameters.hpp" + +#include "../tests/mocks/mock_camera_tranform.hpp" #include "tongji/fire_controller/fire_controller.hpp" -#include "tongji/identifier/identifier.hpp" #include "tongji/predictor/car_predictor/car_predictor_manager.hpp" #include "tongji/solver/solver.hpp" #include "tongji/state_machine/state_machine.hpp" #include "utils/fps_counter.hpp" -#include "utils/visualization.hpp" #include "v1/identifier/identifier.hpp" namespace world_exe::tongji { @@ -47,8 +50,10 @@ class AutoAimSystem::Impl { state_machine_ = std::make_shared(); fire_controller_ = std::make_unique( config_path_, state_machine_, live_target_manager_); - time_stamp_ = std::chrono::steady_clock::now(); - syncer_ = std::make_unique(seconds(2), 6e-6); + time_stamp_ = std::chrono::steady_clock::now(); + syncer_ = std::make_unique(seconds(2), 6e-6); + mock_camera_tranform_ = std::make_unique( + Vector3D(1, 1, 0.0), 0, 0, M_PI / 6.0); core::EventBus::Subscript( parameters::ParamsForSystemV1::raw_image_event, @@ -167,6 +172,7 @@ class AutoAimSystem::Impl { std::shared_ptr live_target_manager_; std::unique_ptr syncer_; std::unique_ptr fire_controller_; + std::unique_ptr mock_camera_tranform_; }; AutoAimSystem::AutoAimSystem(const bool& debug) diff --git a/src/util/visaulization.cpp b/src/util/visaulization.cpp index 518a68e..e8b62a8 100644 --- a/src/util/visaulization.cpp +++ b/src/util/visaulization.cpp @@ -75,7 +75,7 @@ void world_exe::util::visualization ::draw_armor_in_camera( for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); i++) for (const auto& armor : camera_armor.GetArmors(static_cast(1 << i))) { - data::ArmorImageSpacing img_armor {}; + data::ArmorImageSpacing img_armor { }; world_exe::util::cast::armor_3d_camera_to_armor_2d_image(armor, intrinsic_parameters, distortion_parameters, points_in_armor_spacing, img_armor); draw_armor_2d(img_armor, in_out_mat); @@ -87,17 +87,15 @@ void world_exe::util::visualization ::draw_armor_in_camera( void world_exe::util::visualization::draw_armor_in_gimbal( const interfaces::IArmorInGimbalControl& camera_armor, const cv::Mat& intrinsic_parameters, const cv::Mat& distortion_parameters, const std::vector& points_in_armor_spacing, - const Eigen::Affine3d gimal_to_camera, - cv::Mat& in_out_mat) { + const Eigen::Affine3d gimal_to_camera, cv::Mat& in_out_mat) { for (int i = 0; i < static_cast(world_exe::enumeration::ArmorIdFlag::Count); i++) for (const auto& armor_gimbal : camera_armor.GetArmors(static_cast(1 << i))) { - world_exe::data::ArmorImageSpacing img_armor {}; - world_exe::data::ArmorCameraSpacing armor{ - armor_gimbal.id, + world_exe::data::ArmorImageSpacing img_armor { }; + world_exe::data::ArmorCameraSpacing armor { armor_gimbal.id, gimal_to_camera * armor_gimbal.position, - Eigen::Quaterniond{gimal_to_camera * armor_gimbal.orientation.toRotationMatrix()} - }; + Eigen::Quaterniond { + gimal_to_camera.rotation() * armor_gimbal.orientation.toRotationMatrix() } }; world_exe::util::cast::armor_3d_camera_to_armor_2d_image(armor, intrinsic_parameters, distortion_parameters, points_in_armor_spacing, img_armor); draw_armor_2d(img_armor, in_out_mat); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 8e8c08e..a85408c 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -11,6 +11,9 @@ add_executable(test_main test_main.cpp) add_executable(runable runable_main.cpp) +add_executable(gimbal_mock mock_gimbal_test.cpp) + + # 确保测试使用 C++20 target_compile_features(test_main PRIVATE cxx_std_20) target_compile_features(runable PRIVATE cxx_std_20) @@ -35,5 +38,9 @@ target_link_libraries(runable alliance_auto_aim ) +target_link_libraries(gimbal_mock + alliance_auto_aim +) + diff --git a/tests/mocks/MockCamera.hpp b/tests/mocks/MockCamera.hpp deleted file mode 100644 index e58392a..0000000 --- a/tests/mocks/MockCamera.hpp +++ /dev/null @@ -1,52 +0,0 @@ -#include - -#include "data/time_stamped.hpp" - -namespace world_exe::tests::mock { - -struct MockCamera { - Eigen::Vector3d position; - Eigen::Matrix3d orientation; - - MockCamera() - : position(Eigen::Vector3d::Zero()) - , orientation(Eigen::Matrix3d::Identity()) { } -}; - -class MockCameraInGimbal final { -public: - MockCameraInGimbal(double angular_speed = 1) - : time(std::chrono::steady_clock::now().time_since_epoch()) - , camera({ }) - , gimbal_origin(Eigen::Vector3d(0, 0, 0)) { } - - const data::TimeStamp& GetTimeStamp() const { return time; } - - void RotateYawPitchAndTranslate( - double t, double yaw_speed, double pitch_speed, const Eigen::Vector3d& linear_velocity) { - double yaw_angle = yaw_speed * t; - double pitch_angle = pitch_speed * t; - - Eigen::Matrix3d R_yaw = - Eigen::AngleAxisd(yaw_angle, Eigen::Vector3d::UnitZ()).toRotationMatrix(); - Eigen::Matrix3d R_pitch = - Eigen::AngleAxisd(pitch_angle, Eigen::Vector3d::UnitY()).toRotationMatrix(); - Eigen::Matrix3d rotation = R_yaw * R_pitch; - - Eigen::Vector3d translation = linear_velocity * t; - - MoveCamera(rotation, translation); - } - -private: - void MoveCamera(const Eigen::Matrix3d& rotation_matrix, const Eigen::Vector3d& translation) { - Eigen::Vector3d relative = camera.position - gimbal_origin; - camera.position = gimbal_origin + rotation_matrix * relative + translation; - camera.orientation = rotation_matrix * camera.orientation; - } - - data::TimeStamp time; - MockCamera camera; - Eigen::Vector3d gimbal_origin; -}; -} \ No newline at end of file From 2c3751dc54074c900ced82cb359a5439c4f7196d Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 7 Nov 2025 22:43:38 +0800 Subject: [PATCH 93/93] test:add gimbal mock test --- src/tongji/auto_aim_system.cpp | 14 +++----------- tests/CMakeLists.txt | 3 ++- tests/mock_gimbal_test.cpp | 21 ++------------------- tests/mocks/mock_camera_tranform.hpp | 1 - 4 files changed, 7 insertions(+), 32 deletions(-) diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 28fc87e..52f671a 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -4,11 +4,9 @@ #include #include -#include #include #include #include - #include #include "../v1/sync/syncer.hpp" @@ -20,8 +18,6 @@ #include "enum/car_id.hpp" #include "parameters/params_system_v1.hpp" #include "parameters/profile.hpp" - -#include "../tests/mocks/mock_camera_tranform.hpp" #include "tongji/fire_controller/fire_controller.hpp" #include "tongji/predictor/car_predictor/car_predictor_manager.hpp" #include "tongji/solver/solver.hpp" @@ -50,10 +46,8 @@ class AutoAimSystem::Impl { state_machine_ = std::make_shared(); fire_controller_ = std::make_unique( config_path_, state_machine_, live_target_manager_); - time_stamp_ = std::chrono::steady_clock::now(); - syncer_ = std::make_unique(seconds(2), 6e-6); - mock_camera_tranform_ = std::make_unique( - Vector3D(1, 1, 0.0), 0, 0, M_PI / 6.0); + time_stamp_ = std::chrono::steady_clock::now(); + syncer_ = std::make_unique(seconds(2), 6e-6); core::EventBus::Subscript( parameters::ParamsForSystemV1::raw_image_event, @@ -78,8 +72,6 @@ class AutoAimSystem::Impl { // if (fps_.count()) std::cout << fps_.fps() << std::endl; // return; - // std::cout << "here" << std::endl; - if (flag == enumeration::ArmorIdFlag::None) return; // std::cout << "here" << std::endl; state_machine_->Update(armors_in_image, @@ -172,7 +164,7 @@ class AutoAimSystem::Impl { std::shared_ptr live_target_manager_; std::unique_ptr syncer_; std::unique_ptr fire_controller_; - std::unique_ptr mock_camera_tranform_; + // std::unique_ptr mock_camera_tranform_; }; AutoAimSystem::AutoAimSystem(const bool& debug) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index a85408c..0c5665a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -43,4 +43,5 @@ target_link_libraries(gimbal_mock ) - +include(GoogleTest) +gtest_discover_tests(test_main) \ No newline at end of file diff --git a/tests/mock_gimbal_test.cpp b/tests/mock_gimbal_test.cpp index e5752e8..c9090f9 100644 --- a/tests/mock_gimbal_test.cpp +++ b/tests/mock_gimbal_test.cpp @@ -18,8 +18,8 @@ int main() { using namespace world_exe::util::visualization; Eigen::Vector3d V_input(1., 0., 0.); - double Omega_Yaw = 0.0; - double Omega_Pitch = 0.0; + double Omega_Yaw = 0.1; + double Omega_Pitch = 0.05; double Max_Pitch = M_PI / 48.0; auto transformer = world_exe::tests::mock::Camera2GimbalTransformer( V_input, Omega_Yaw, Omega_Pitch, Max_Pitch); @@ -46,17 +46,6 @@ int main() { auto transform = transformer.updateAndGetTransform(dt); - for (int i = 0; i < num; ++i) { - armors_in_gimbal_3[i].position = transform * armors_in_camera_3[i].position; - armors_in_gimbal_3[i].orientation = - Quaterniond(transform.rotation()) * armors_in_camera_3[i].orientation; - } - - // draw_armor_in_camera(*armor_in_camera, - // world_exe::parameters::HikCameraProfile::get_intrinsic_parameters(), - // world_exe::parameters::HikCameraProfile::get_distortion_parameters(), - // world_exe::parameters::Robomaster::NormalArmorObjectPointsRos, tmp); - draw_armor_in_gimbal(*armor_in_gimbal, world_exe::parameters::HikCameraProfile::get_intrinsic_parameters(), world_exe::parameters::HikCameraProfile::get_distortion_parameters(), @@ -64,11 +53,5 @@ int main() { tmp); cv::imshow("imshow", tmp); cv::waitKey(1); - - // std::cout << armors_in_camera_3[0].position << std::endl; - std::cout << armors_in_gimbal_3[0].position << std::endl; - std::cout << std::endl; - - // std::this_thread::sleep_for(std::chrono::milliseconds { }); } } \ No newline at end of file diff --git a/tests/mocks/mock_camera_tranform.hpp b/tests/mocks/mock_camera_tranform.hpp index 2cfb60e..a130dad 100644 --- a/tests/mocks/mock_camera_tranform.hpp +++ b/tests/mocks/mock_camera_tranform.hpp @@ -2,7 +2,6 @@ #include #include -#include using namespace Eigen; using AffineTransform = Affine3d; // 仿射变换矩阵 (4x4)