recharge

src/common/3rd/x86/math/include/math/aabox2d.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "math/vec2d.h"
+
+#include <string>
+#include <vector>
+
+namespace decision::math {
+
+class AABox2d {
+public:
+    AABox2d() = default;
+    AABox2d(const Vec2d& center, const double length, const double width);
+    AABox2d(const Vec2d& one_corner, const Vec2d& opponent_corner);
+    AABox2d(const std::vector<Vec2d>& points);
+
+    const Vec2d& center() const { return _center; }
+    double center_x() const { return _center.x(); }
+    double center_y() const { return _center.y(); }
+    double length() const { return _length; }
+    double width() const { return _width; }
+    double half_length() const { return _half_length; }
+    double half_width() const { return _half_width; }
+    double area() const { return _length * _width; }
+    double min_x() const { return _center.x() - _half_length; }
+    double max_x() const { return _center.x() + _half_length; }
+    double min_y() const { return _center.y() - _half_width; }
+    double max_y() const { return _center.y() + _half_width; }
+
+    // Get all corners in counter clockwise order.
+    void get_all_corners(std::vector<Vec2d>* const corners) const;
+
+    bool is_point_in(const Vec2d& point) const;
+    bool is_point_on_boundary(const Vec2d& point) const;
+
+    double distance_to(const Vec2d& point) const;
+    double distance_to(const AABox2d& box) const;
+
+    bool has_overlap(const AABox2d& box) const;
+
+    // Shift the center of AABox by the input vector.
+    void shift(const Vec2d& shift_vec);
+
+    void merge_from(const AABox2d& other_box);
+    void merge_from(const Vec2d& other_point);
+
+    std::string debug_string() const;
+
+protected:
+    Vec2d _center;
+    double _length = 0.0;
+    double _width = 0.0;
+    double _half_length = 0.0;
+    double _half_width = 0.0;
+};
+
+}

src/common/3rd/x86/math/include/math/aabox3d.h

@@ -0,0 +1,57 @@
+// Copyright 2018 Baidu Inc. All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+//
+// Description: aabox for 3d
+
+#pragma once
+
+#include "math/aabox2d.h"
+#include "math/vec3d.h"
+
+namespace decision::math {
+
+class AABox3d {
+public:
+    AABox3d() = default;
+    AABox3d(const Vec3d& center, const double length, const double width, const double height);
+    AABox3d(const Vec3d& one_corner, const Vec3d& opponent_corner);
+    AABox3d(const std::vector<Vec3d>& points);
+
+    AABox2d aabox2d() const;
+    const Vec3d& center() const { return _center; }
+    double length() const { return _length; }
+    double width() const { return _width; }
+    double height() const { return _height; }
+    double min_x() const { return _center.x() - _half_length; }
+    double max_x() const { return _center.x() + _half_length; }
+    double min_y() const { return _center.y() - _half_width; }
+    double max_y() const { return _center.y() + _half_width; }
+    double min_z() const { return _center.z() - _half_height; }
+    double max_z() const { return _center.z() + _half_height; }
+
+    // Get all corners in counter clockwise order.
+    void get_all_corners(std::vector<Vec3d>* const corners) const;
+
+    bool is_point_in(const Vec3d& point) const;
+    bool is_point_on_boundary(const Vec3d& point) const;
+
+    double distance_to(const Vec3d& point) const;
+
+    // Shift the center of AABox by the input vector.
+    void shift(const Vec3d& shift_vec);
+
+    void merge_from(const AABox3d& other_box);
+
+    std::string debug_string() const;
+
+protected:
+    Vec3d _center;
+    double _length = 0.0;
+    double _width = 0.0;
+    double _height = 0.0;
+    double _half_length = 0.0;
+    double _half_width = 0.0;
+    double _half_height = 0.0;
+};
+
+}

src/common/3rd/x86/math/include/math/aaboxkdtree2d.h

@@ -0,0 +1,387 @@
+#pragma once
+
+#include "math/aabox2d.h"
+#include "math/math_utils.h"
+
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <vector>
+
+namespace decision::math {
+
+struct AABoxKDTreeParams {
+    // The maximum depth of the kdtree.
+    int max_depth = -1;
+    // The maximum number of items in one leaf node.
+    int max_leaf_size = -1;
+    // The maximum dimension size of leaf node.
+    double max_leaf_dimension = -1.0;
+};
+
+template <class ObjectType>
+class AABoxKDTree2dNode {
+public:
+    using ObjectPtr = const ObjectType*;
+
+    AABoxKDTree2dNode(const std::vector<ObjectPtr>& objects,
+        const AABoxKDTreeParams& params, int depth)
+        : _depth(depth)
+    {
+        CHECK(!objects.empty());
+
+        compute_boundary(objects);
+        compute_partition();
+
+        if (split_to_subnodes(objects, params)) {
+            std::vector<ObjectPtr> left_subnode_objects;
+            std::vector<ObjectPtr> right_subnode_objects;
+            partition_objects(objects, &left_subnode_objects, &right_subnode_objects);
+
+            // Split to sub-nodes.
+            if (!left_subnode_objects.empty()) {
+                _left_subnode.reset(new AABoxKDTree2dNode<ObjectType>(
+                    left_subnode_objects, params, depth + 1));
+            }
+            if (!right_subnode_objects.empty()) {
+                _right_subnode.reset(new AABoxKDTree2dNode<ObjectType>(
+                    right_subnode_objects, params, depth + 1));
+            }
+        } else {
+            init_objects(objects);
+        }
+    }
+
+    ObjectPtr get_nearest_object(const Vec2d& point) const
+    {
+        ObjectPtr nearest_object = nullptr;
+        double min_distance_sqr = std::numeric_limits<double>::infinity();
+        get_nearest_object_internal(point, &min_distance_sqr, &nearest_object);
+        return nearest_object;
+    }
+
+    std::vector<ObjectPtr> get_objects(const Vec2d& point, const double distance) const
+    {
+        std::vector<ObjectPtr> result_objects;
+        get_objects_internal(point, distance, sqr(distance), &result_objects);
+        return result_objects;
+    }
+
+    AABox2d get_bounding_box() const
+    {
+        return AABox2d({ _min_x, _min_y }, { _max_x, _max_y });
+    }
+
+private:
+    void init_objects(const std::vector<ObjectPtr>& objects)
+    {
+        _num_objects = objects.size();
+        _objects_sorted_by_min = objects;
+        _objects_sorted_by_max = objects;
+        std::sort(_objects_sorted_by_min.begin(), _objects_sorted_by_min.end(),
+            [&](ObjectPtr obj1, ObjectPtr obj2) {
+                return _partition == PARTITION_X ? obj1->aabox().min_x() < obj2->aabox().min_x()
+                                                 : obj1->aabox().min_y() < obj2->aabox().min_y();
+            });
+        std::sort(_objects_sorted_by_max.begin(), _objects_sorted_by_max.end(),
+            [&](ObjectPtr obj1, ObjectPtr obj2) {
+                return _partition == PARTITION_X ? obj1->aabox().max_x() > obj2->aabox().max_x()
+                                                 : obj1->aabox().max_y() > obj2->aabox().max_y();
+            });
+        _objects_sorted_by_min_bound.reserve(_num_objects);
+        for (ObjectPtr object : _objects_sorted_by_min) {
+            _objects_sorted_by_min_bound.push_back(
+                _partition == PARTITION_X ? object->aabox().min_x() : object->aabox().min_y());
+        }
+        _objects_sorted_by_max_bound.reserve(_num_objects);
+        for (ObjectPtr object : _objects_sorted_by_max) {
+            _objects_sorted_by_max_bound.push_back(
+                _partition == PARTITION_X ? object->aabox().max_x() : object->aabox().max_y());
+        }
+    }
+
+    bool split_to_subnodes(const std::vector<ObjectPtr>& objects, const AABoxKDTreeParams& params)
+    {
+        if (params.max_depth >= 0 && _depth >= params.max_depth) {
+            return false;
+        }
+        if (static_cast<int>(objects.size()) <= std::max(1, params.max_leaf_size)) {
+            return false;
+        }
+        if (params.max_leaf_dimension >= 0.0 && std::max(_max_x - _min_x, _max_y - _min_y) <= params.max_leaf_dimension) {
+            return false;
+        }
+        return true;
+    }
+
+    double lowerbound_distance_sqr_to_point(const Vec2d& point) const
+    {
+        double dx = 0.0;
+        if (point.x() < _min_x) {
+            dx = _min_x - point.x();
+        } else if (point.x() > _max_x) {
+            dx = point.x() - _max_x;
+        }
+        double dy = 0.0;
+        if (point.y() < _min_y) {
+            dy = _min_y - point.y();
+        } else if (point.y() > _max_y) {
+            dy = point.y() - _max_y;
+        }
+        return dx * dx + dy * dy;
+    }
+
+    double upperbound_distance_sqr_to_point(const Vec2d& point) const
+    {
+        const double dx = (point.x() > _mid_x ? (point.x() - _min_x) : (point.x() - _max_x));
+        const double dy = (point.y() > _mid_y ? (point.y() - _min_y) : (point.y() - _max_y));
+        return dx * dx + dy * dy;
+    }
+
+    void get_all_objects(std::vector<ObjectPtr>* const result_objects) const
+    {
+        result_objects->insert(result_objects->end(),
+            _objects_sorted_by_min.begin(), _objects_sorted_by_min.end());
+        if (_left_subnode != nullptr) {
+            _left_subnode->get_all_objects(result_objects);
+        }
+        if (_right_subnode != nullptr) {
+            _right_subnode->get_all_objects(result_objects);
+        }
+    }
+
+    void get_objects_internal(const Vec2d& point,
+        const double distance,
+        const double distance_sqr,
+        std::vector<ObjectPtr>* const result_objects) const
+    {
+        if (lowerbound_distance_sqr_to_point(point) > distance_sqr) {
+            return;
+        }
+        if (upperbound_distance_sqr_to_point(point) <= distance_sqr) {
+            get_all_objects(result_objects);
+            return;
+        }
+        const double pvalue = (_partition == PARTITION_X ? point.x() : point.y());
+        if (pvalue < _partition_position) {
+            const double limit = pvalue + distance;
+            for (int i = 0; i < _num_objects; ++i) {
+                if (_objects_sorted_by_min_bound[i] > limit) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_min[i];
+                if (object->distance_sqr_to(point) <= distance_sqr) {
+                    result_objects->push_back(object);
+                }
+            }
+        } else {
+            const double limit = pvalue - distance;
+            for (int i = 0; i < _num_objects; ++i) {
+                if (_objects_sorted_by_max_bound[i] < limit) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_max[i];
+                if (object->distance_sqr_to(point) <= distance_sqr) {
+                    result_objects->push_back(object);
+                }
+            }
+        }
+        if (_left_subnode != nullptr) {
+            _left_subnode->get_objects_internal(point, distance, distance_sqr, result_objects);
+        }
+        if (_right_subnode != nullptr) {
+            _right_subnode->get_objects_internal(point, distance, distance_sqr, result_objects);
+        }
+    }
+
+    void get_nearest_object_internal(const Vec2d& point,
+        double* const min_distance_sqr,
+        ObjectPtr* const nearest_object) const
+    {
+        if (lowerbound_distance_sqr_to_point(point) >= *min_distance_sqr - kMathEpsilon) {
+            return;
+        }
+        const double pvalue = (_partition == PARTITION_X ? point.x() : point.y());
+        const bool search_left_first = (pvalue < _partition_position);
+        if (search_left_first) {
+            if (_left_subnode != nullptr) {
+                _left_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        } else {
+            if (_right_subnode != nullptr) {
+                _right_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        }
+        if (*min_distance_sqr <= kMathEpsilon) {
+            return;
+        }
+
+        if (search_left_first) {
+            for (int i = 0; i < _num_objects; ++i) {
+                const double bound = _objects_sorted_by_min_bound[i];
+                if (bound > pvalue && sqr(bound - pvalue) > *min_distance_sqr) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_min[i];
+                const double distance_sqr = object->distance_sqr_to(point);
+                if (distance_sqr < *min_distance_sqr) {
+                    *min_distance_sqr = distance_sqr;
+                    *nearest_object = object;
+                }
+            }
+        } else {
+            for (int i = 0; i < _num_objects; ++i) {
+                const double bound = _objects_sorted_by_max_bound[i];
+                if (bound < pvalue && sqr(bound - pvalue) > *min_distance_sqr) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_max[i];
+                const double distance_sqr = object->distance_sqr_to(point);
+                if (distance_sqr < *min_distance_sqr) {
+                    *min_distance_sqr = distance_sqr;
+                    *nearest_object = object;
+                }
+            }
+        }
+        if (*min_distance_sqr <= kMathEpsilon) {
+            return;
+        }
+        if (search_left_first) {
+            if (_right_subnode != nullptr) {
+                _right_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        } else {
+            if (_left_subnode != nullptr) {
+                _left_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        }
+    }
+
+    void compute_boundary(const std::vector<ObjectPtr>& objects)
+    {
+        _min_x = std::numeric_limits<double>::infinity();
+        _min_y = std::numeric_limits<double>::infinity();
+        _max_x = -std::numeric_limits<double>::infinity();
+        _max_y = -std::numeric_limits<double>::infinity();
+        for (ObjectPtr object : objects) {
+            _min_x = std::min(_min_x, object->aabox().min_x());
+            _max_x = std::max(_max_x, object->aabox().max_x());
+            _min_y = std::min(_min_y, object->aabox().min_y());
+            _max_y = std::max(_max_y, object->aabox().max_y());
+        }
+        _mid_x = (_min_x + _max_x) / 2.0;
+        _mid_y = (_min_y + _max_y) / 2.0;
+    }
+
+    void compute_partition()
+    {
+        if (_max_x - _min_x >= _max_y - _min_y) {
+            _partition = PARTITION_X;
+            _partition_position = (_min_x + _max_x) / 2.0;
+        } else {
+            _partition = PARTITION_Y;
+            _partition_position = (_min_y + _max_y) / 2.0;
+        }
+    }
+
+    void partition_objects(const std::vector<ObjectPtr>& objects,
+        std::vector<ObjectPtr>* const left_subnode_objects,
+        std::vector<ObjectPtr>* const right_subnode_objects)
+    {
+        left_subnode_objects->clear();
+        right_subnode_objects->clear();
+        std::vector<ObjectPtr> other_objects;
+        if (_partition == PARTITION_X) {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_x() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_x() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        } else {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_y() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_y() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        }
+        init_objects(other_objects);
+    }
+
+private:
+    int _num_objects = 0;
+    std::vector<ObjectPtr> _objects_sorted_by_min;
+    std::vector<ObjectPtr> _objects_sorted_by_max;
+    std::vector<double> _objects_sorted_by_min_bound;
+    std::vector<double> _objects_sorted_by_max_bound;
+    int _depth = 0;
+
+    // Boundary
+    double _min_x = 0.0;
+    double _max_x = 0.0;
+    double _min_y = 0.0;
+    double _max_y = 0.0;
+    double _mid_x = 0.0;
+    double _mid_y = 0.0;
+
+    enum Partition {
+        PARTITION_X = 1,
+        PARTITION_Y = 2,
+    };
+    Partition _partition = PARTITION_X;
+    double _partition_position = 0.0;
+
+    std::unique_ptr<AABoxKDTree2dNode<ObjectType>> _left_subnode = nullptr;
+    std::unique_ptr<AABoxKDTree2dNode<ObjectType>> _right_subnode = nullptr;
+};
+
+template <class ObjectType>
+class AABoxKDTree2d {
+public:
+    using ObjectPtr = const ObjectType*;
+
+    AABoxKDTree2d(const std::vector<ObjectType>& objects, const AABoxKDTreeParams& params)
+    {
+        if (!objects.empty()) {
+            std::vector<ObjectPtr> object_ptrs;
+            for (const auto& object : objects) {
+                object_ptrs.push_back(&object);
+            }
+            _root.reset(new AABoxKDTree2dNode<ObjectType>(object_ptrs, params, 0));
+        }
+    }
+
+    ObjectPtr get_nearest_object(const Vec2d& point) const
+    {
+        return _root == nullptr ? nullptr : _root->get_nearest_object(point);
+    }
+
+    std::vector<ObjectPtr> get_objects(const Vec2d& point, const double distance) const
+    {
+        if (_root == nullptr) {
+            return {};
+        }
+        return _root->get_objects(point, distance);
+    }
+
+    AABox2d get_bounding_box() const
+    {
+        return _root == nullptr ? AABox2d() : _root->get_bounding_box();
+    }
+
+private:
+    std::unique_ptr<AABoxKDTree2dNode<ObjectType>> _root = nullptr;
+};
+
+}

src/common/3rd/x86/math/include/math/aaboxkdtree3d.h

@@ -0,0 +1,441 @@
+
+// Copyright 2018 Baidu. Inc All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+// Date: 2018-06-19 14:44
+//
+// Description: aaboxkdtree for 3d element
+
+#ifndef _MATH_COMMON_AABOXKDTREE3D_H_
+#define _MATH_COMMON_AABOXKDTREE3D_H_
+
+#pragma once
+
+#include "math/aabox3d.h"
+#include "math/math_utils.h"
+
+#include <algorithm>
+#include <limits>
+#include <memory>
+#include <vector>
+
+namespace decision::math {
+
+struct AABox3dKDTreeParams {
+    // The maximum depth of the kdtree.
+    int max_depth = -1;
+    // The maximum number of items in one leaf node.
+    int max_leaf_size = -1;
+    // The maximum dimension size of leaf node.
+    double max_leaf_dimension = -1.0;
+};
+
+template <class ObjectType>
+class AABoxKDTree3dNode {
+public:
+    using ObjectPtr = const ObjectType*;
+
+    AABoxKDTree3dNode(const std::vector<ObjectPtr>& objects,
+        const AABox3dKDTreeParams& params, int depth)
+        : _depth(depth)
+    {
+        CHECK(!objects.empty());
+
+        compute_boundary(objects);
+        compute_partition();
+
+        if (split_to_subnodes(objects, params)) {
+            std::vector<ObjectPtr> left_subnode_objects;
+            std::vector<ObjectPtr> right_subnode_objects;
+            partition_objects(objects, &left_subnode_objects, &right_subnode_objects);
+
+            // Split to sub-nodes.
+            if (!left_subnode_objects.empty()) {
+                _left_subnode.reset(new AABoxKDTree3dNode<ObjectType>(
+                    left_subnode_objects, params, depth + 1));
+            }
+            if (!right_subnode_objects.empty()) {
+                _right_subnode.reset(new AABoxKDTree3dNode<ObjectType>(
+                    right_subnode_objects, params, depth + 1));
+            }
+        } else {
+            init_objects(objects);
+        }
+    }
+
+    ObjectPtr get_nearest_object(const Vec3d& point) const
+    {
+        ObjectPtr nearest_object = nullptr;
+        double min_distance_sqr = std::numeric_limits<double>::infinity();
+        get_nearest_object_internal(point, &min_distance_sqr, &nearest_object);
+        return nearest_object;
+    }
+
+    std::vector<ObjectPtr> get_objects(const Vec3d& point, const double distance) const
+    {
+        std::vector<ObjectPtr> result_objects;
+        get_objects_internal(point, distance, sqr(distance), &result_objects);
+        return result_objects;
+    }
+
+    AABox3d get_bounding_box() const
+    {
+        return AABox3d({ _min_x, _min_y, _min_z }, { _max_x, _max_y, _max_z });
+    }
+
+private:
+    void init_objects(const std::vector<ObjectPtr>& objects)
+    {
+        _num_objects = objects.size();
+        _objects_sorted_by_min = objects;
+        _objects_sorted_by_max = objects;
+        std::sort(_objects_sorted_by_min.begin(), _objects_sorted_by_min.end(),
+            [&](ObjectPtr obj1, ObjectPtr obj2) {
+                return _partition == PARTITION_X ? obj1->aabox().min_x() < obj2->aabox().min_x()
+                                                 : (_partition == PARTITION_Y ? obj1->aabox().min_y() < obj2->aabox().min_y()
+                                                                              : obj1->aabox().min_z() < obj2->aabox().min_z());
+            });
+        std::sort(_objects_sorted_by_max.begin(), _objects_sorted_by_max.end(),
+            [&](ObjectPtr obj1, ObjectPtr obj2) {
+                return _partition == PARTITION_X ? obj1->aabox().max_x() > obj2->aabox().max_x()
+                                                 : (_partition == PARTITION_Y ? obj1->aabox().max_y() > obj2->aabox().max_y()
+                                                                              : obj1->aabox().max_z() > obj2->aabox().max_z());
+            });
+        _objects_sorted_by_min_bound.reserve(_num_objects);
+        for (ObjectPtr object : _objects_sorted_by_min) {
+            _objects_sorted_by_min_bound.push_back(
+                _partition == PARTITION_X ? object->aabox().min_x()
+                                          : (_partition == PARTITION_Y ? object->aabox().min_y()
+                                                                       : object->aabox().min_z()));
+        }
+        _objects_sorted_by_max_bound.reserve(_num_objects);
+        for (ObjectPtr object : _objects_sorted_by_max) {
+            _objects_sorted_by_max_bound.push_back(
+                _partition == PARTITION_X ? object->aabox().max_x()
+                                          : (_partition == PARTITION_Y ? object->aabox().max_y()
+                                                                       : object->aabox().max_z()));
+        }
+    }
+
+    bool split_to_subnodes(const std::vector<ObjectPtr>& objects, const AABox3dKDTreeParams& params)
+    {
+        if (params.max_depth >= 0 && _depth >= params.max_depth) {
+            return false;
+        }
+        if (static_cast<int>(objects.size()) <= std::max(1, params.max_leaf_size)) {
+            return false;
+        }
+        double max_dim = std::max(_max_x - _min_x, _max_y - _min_y);
+        max_dim = std::max(max_dim, _max_z - _min_z);
+        if (params.max_leaf_dimension >= 0.0 && max_dim <= params.max_leaf_dimension) {
+            return false;
+        }
+        return true;
+    }
+
+    double lowerbound_distance_sqr_to_point(const Vec3d& point) const
+    {
+        double dx = 0.0;
+        if (point.x() < _min_x) {
+            dx = _min_x - point.x();
+        } else if (point.x() > _max_x) {
+            dx = point.x() - _max_x;
+        }
+        double dy = 0.0;
+        if (point.y() < _min_y) {
+            dy = _min_y - point.y();
+        } else if (point.y() > _max_y) {
+            dy = point.y() - _max_y;
+        }
+        double dz = 0.0;
+        if (point.z() < _min_z) {
+            dz = _min_z - point.z();
+        } else if (point.z() > _max_z) {
+            dz = point.z() - _max_z;
+        }
+        return dx * dx + dy * dy + dz * dz;
+    }
+
+    double upperbound_distance_sqr_to_point(const Vec3d& point) const
+    {
+        const double dx = (point.x() > _mid_x ? (point.x() - _min_x) : (point.x() - _max_x));
+        const double dy = (point.y() > _mid_y ? (point.y() - _min_y) : (point.y() - _max_y));
+        const double dz = (point.z() > _mid_z ? (point.z() - _min_z) : (point.z() - _max_z));
+        return dx * dx + dy * dy + dz * dz;
+    }
+
+    void get_all_objects(std::vector<ObjectPtr>* const result_objects) const
+    {
+        result_objects->insert(result_objects->end(),
+            _objects_sorted_by_min.begin(), _objects_sorted_by_min.end());
+        if (_left_subnode != nullptr) {
+            _left_subnode->get_all_objects(result_objects);
+        }
+        if (_right_subnode != nullptr) {
+            _right_subnode->get_all_objects(result_objects);
+        }
+    }
+
+    void get_objects_internal(const Vec3d& point,
+        const double distance,
+        const double distance_sqr,
+        std::vector<ObjectPtr>* const result_objects) const
+    {
+        if (lowerbound_distance_sqr_to_point(point) > distance_sqr) {
+            return;
+        }
+        if (upperbound_distance_sqr_to_point(point) <= distance_sqr) {
+            get_all_objects(result_objects);
+            return;
+        }
+        const double pvalue = (_partition == PARTITION_X ? point.x()
+                                                         : (_partition == PARTITION_Y ? point.y() : point.z()));
+        if (pvalue < _partition_position) {
+            const double limit = pvalue + distance;
+            for (int i = 0; i < _num_objects; ++i) {
+                if (_objects_sorted_by_min_bound[i] > limit) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_min[i];
+                if (object->distance_sqr_to(point) <= distance_sqr) {
+                    result_objects->push_back(object);
+                }
+            }
+        } else {
+            const double limit = pvalue - distance;
+            for (int i = 0; i < _num_objects; ++i) {
+                if (_objects_sorted_by_max_bound[i] < limit) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_max[i];
+                if (object->distance_sqr_to(point) <= distance_sqr) {
+                    result_objects->push_back(object);
+                }
+            }
+        }
+        if (_left_subnode != nullptr) {
+            _left_subnode->get_objects_internal(point, distance, distance_sqr, result_objects);
+        }
+        if (_right_subnode != nullptr) {
+            _right_subnode->get_objects_internal(point, distance, distance_sqr, result_objects);
+        }
+    }
+
+    void get_nearest_object_internal(const Vec3d& point,
+        double* const min_distance_sqr,
+        ObjectPtr* const nearest_object) const
+    {
+        if (lowerbound_distance_sqr_to_point(point) >= *min_distance_sqr - kMathEpsilon) {
+            return;
+        }
+        const double pvalue = (_partition == PARTITION_X ? point.x()
+                                                         : (_partition == PARTITION_Y ? point.y() : point.z()));
+        const bool search_left_first = (pvalue < _partition_position);
+        if (search_left_first) {
+            if (_left_subnode != nullptr) {
+                _left_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        } else {
+            if (_right_subnode != nullptr) {
+                _right_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        }
+        if (*min_distance_sqr <= kMathEpsilon) {
+            return;
+        }
+
+        if (search_left_first) {
+            for (int i = 0; i < _num_objects; ++i) {
+                const double bound = _objects_sorted_by_min_bound[i];
+                if (bound > pvalue && sqr(bound - pvalue) > *min_distance_sqr) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_min[i];
+                const double distance_sqr = object->distance_sqr_to(point);
+                if (distance_sqr < *min_distance_sqr) {
+                    *min_distance_sqr = distance_sqr;
+                    *nearest_object = object;
+                }
+            }
+        } else {
+            for (int i = 0; i < _num_objects; ++i) {
+                const double bound = _objects_sorted_by_max_bound[i];
+                if (bound < pvalue && sqr(bound - pvalue) > *min_distance_sqr) {
+                    break;
+                }
+                ObjectPtr object = _objects_sorted_by_max[i];
+                const double distance_sqr = object->distance_sqr_to(point);
+                if (distance_sqr < *min_distance_sqr) {
+                    *min_distance_sqr = distance_sqr;
+                    *nearest_object = object;
+                }
+            }
+        }
+        if (*min_distance_sqr <= kMathEpsilon) {
+            return;
+        }
+        if (search_left_first) {
+            if (_right_subnode != nullptr) {
+                _right_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        } else {
+            if (_left_subnode != nullptr) {
+                _left_subnode->get_nearest_object_internal(
+                    point, min_distance_sqr, nearest_object);
+            }
+        }
+    }
+
+    void compute_boundary(const std::vector<ObjectPtr>& objects)
+    {
+        _min_x = std::numeric_limits<double>::infinity();
+        _min_y = std::numeric_limits<double>::infinity();
+        _min_z = std::numeric_limits<double>::infinity();
+        _max_x = -std::numeric_limits<double>::infinity();
+        _max_y = -std::numeric_limits<double>::infinity();
+        _max_z = -std::numeric_limits<double>::infinity();
+        for (ObjectPtr object : objects) {
+            _min_x = std::min(_min_x, object->aabox().min_x());
+            _max_x = std::max(_max_x, object->aabox().max_x());
+            _min_y = std::min(_min_y, object->aabox().min_y());
+            _max_y = std::max(_max_y, object->aabox().max_y());
+            _min_z = std::min(_min_z, object->aabox().min_z());
+            _max_z = std::max(_max_z, object->aabox().max_z());
+        }
+        _mid_x = (_min_x + _max_x) / 2.0;
+        _mid_y = (_min_y + _max_y) / 2.0;
+        _mid_z = (_min_z + _max_z) / 2.0;
+    }
+
+    void compute_partition()
+    {
+        double dx = _max_x - _min_x;
+        double dy = _max_y - _min_y;
+        double dz = _max_z - _min_z;
+        if (dx >= dy && dx >= dz) {
+            _partition = PARTITION_X;
+            _partition_position = (_min_x + _max_x) / 2.0;
+        } else if (dy >= dx && dy >= dz) {
+            _partition = PARTITION_Y;
+            _partition_position = (_min_y + _max_y) / 2.0;
+        } else {
+            _partition = PARTITION_Z;
+            _partition_position = (_min_z + _max_z) / 2.0;
+        }
+    }
+
+    void partition_objects(const std::vector<ObjectPtr>& objects,
+        std::vector<ObjectPtr>* const left_subnode_objects,
+        std::vector<ObjectPtr>* const right_subnode_objects)
+    {
+        left_subnode_objects->clear();
+        right_subnode_objects->clear();
+        std::vector<ObjectPtr> other_objects;
+        if (_partition == PARTITION_X) {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_x() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_x() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        } else if (_partition == PARTITION_Y) {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_y() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_y() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        } else {
+            for (ObjectPtr object : objects) {
+                if (object->aabox().max_z() <= _partition_position) {
+                    left_subnode_objects->push_back(object);
+                } else if (object->aabox().min_z() >= _partition_position) {
+                    right_subnode_objects->push_back(object);
+                } else {
+                    other_objects.push_back(object);
+                }
+            }
+        }
+        init_objects(other_objects);
+    }
+
+private:
+    int _num_objects = 0;
+    std::vector<ObjectPtr> _objects_sorted_by_min;
+    std::vector<ObjectPtr> _objects_sorted_by_max;
+    std::vector<double> _objects_sorted_by_min_bound;
+    std::vector<double> _objects_sorted_by_max_bound;
+    int _depth = 0;
+
+    // Boundary
+    double _min_x = 0.0;
+    double _max_x = 0.0;
+    double _min_y = 0.0;
+    double _max_y = 0.0;
+    double _min_z = 0.0;
+    double _max_z = 0.0;
+    double _mid_x = 0.0;
+    double _mid_y = 0.0;
+    double _mid_z = 0.0;
+
+    enum Partition {
+        PARTITION_X = 1,
+        PARTITION_Y = 2,
+        PARTITION_Z = 3,
+    };
+    Partition _partition = PARTITION_X;
+    double _partition_position = 0.0;
+
+    std::unique_ptr<AABoxKDTree3dNode<ObjectType>> _left_subnode = nullptr;
+    std::unique_ptr<AABoxKDTree3dNode<ObjectType>> _right_subnode = nullptr;
+};
+
+template <class ObjectType>
+class AABoxKDTree3d {
+public:
+    using ObjectPtr = const ObjectType*;
+
+    AABoxKDTree3d(const std::vector<ObjectType>& objects, const AABox3dKDTreeParams& params)
+    {
+        if (!objects.empty()) {
+            std::vector<ObjectPtr> object_ptrs;
+            for (const auto& object : objects) {
+                object_ptrs.push_back(&object);
+            }
+            _root.reset(new AABoxKDTree3dNode<ObjectType>(object_ptrs, params, 0));
+        }
+    }
+
+    ObjectPtr get_nearest_object(const Vec3d& point) const
+    {
+        return _root == nullptr ? nullptr : _root->get_nearest_object(point);
+    }
+
+    std::vector<ObjectPtr> get_objects(const Vec3d& point, const double distance) const
+    {
+        if (_root == nullptr) {
+            return {};
+        }
+        return _root->get_objects(point, distance);
+    }
+
+    AABox3d get_bounding_box() const
+    {
+        return _root == nullptr ? AABox3d() : _root->get_bounding_box();
+    }
+
+private:
+    std::unique_ptr<AABoxKDTree3dNode<ObjectType>> _root = nullptr;
+};
+
+}
+
+#endif // _MATH_COMMON_AABOXKDTREE3D_H_

src/common/3rd/x86/math/include/math/angle.h

@@ -0,0 +1,173 @@
+// Copyright (c) 2016 Baidu.com, Inc. All Rights Reserved
+// Author: hengliang
+
+// This file defines the Angle class, which uses an integer to represent an angle, and supports
+// commonly-used operations such as addition, subtraction, and trigonometry. Using an integer gives
+// the following advantages over the traditional floating-point representations:
+// - A signed integer automatically wraps an angle to the interval [-pi, pi).
+// - It makes the delta angle calculation easier and safer. For example, the difference between -179
+//   degrees and 179 degrees longitude is automatically 2 degrees.
+// - It uses less storage to achieve similar level of accuracy: Angle8 < Angle16 < float < Angle32 <
+//   double < Angle64, where < means "less precise than."
+// - Integer operations are more efficient.
+// - Angle8 and Angle16 allow super fast trigonometric functions via a 64-KiB lookup table.
+//
+// It is recommended to use the Angle class as much as possible, especially for the following
+// scenarios:
+// - Latitude/longitude: Angle32 can achieve < 1 cm resolution.
+// - Euler angles: Angle16 is precise enough for localization/object detection.
+//
+// Usage examples: Please refer to angle_test.cpp.
+
+#pragma once
+
+#include <cmath>
+#include <cstdint>
+#include <limits>
+
+namespace decision::math {
+
+template <typename T>
+class Angle {
+public:
+    static_assert(std::numeric_limits<T>::is_integer && std::numeric_limits<T>::is_signed,
+        "T must be a signed integer type");
+
+    static constexpr T RAW_PI = std::numeric_limits<T>::min();
+    static constexpr T RAW_PI_2 = -(std::numeric_limits<T>::min() >> 1);
+    static constexpr double DEG_TO_RAW = RAW_PI / -180.0;
+    static constexpr double RAD_TO_RAW = RAW_PI * -M_1_PI;
+    static constexpr double RAW_TO_DEG = -180.0 / RAW_PI;
+    static constexpr double RAW_TO_RAD = -M_PI / RAW_PI;
+
+    // Construct an object from an angle in degrees.
+    static Angle from_deg(double value)
+    {
+        Angle a(lround(value * DEG_TO_RAW));
+        return a;
+    }
+
+    // Construct an object from an angle in radians.
+    static Angle from_rad(double value)
+    {
+        Angle a(lround(value * RAD_TO_RAW));
+        return a;
+    }
+
+    explicit Angle(T value = 0)
+        : _value(value)
+    {
+    }
+
+    double to_deg() const
+    {
+        return _value * RAW_TO_DEG;
+    }
+
+    double to_rad() const
+    {
+        return _value * RAW_TO_RAD;
+    }
+
+    T raw() const
+    {
+        return _value;
+    }
+
+    Angle operator+=(Angle other)
+    {
+        _value += other._value;
+        return *this;
+    }
+
+    Angle operator-=(Angle other)
+    {
+        _value -= other._value;
+        return *this;
+    }
+
+    template <typename Scalar>
+    Angle operator*=(Scalar s)
+    {
+        _value = lround(_value * s);
+        return *this;
+    }
+
+    template <typename Scalar>
+    Angle operator/=(Scalar s)
+    {
+        _value = lround(_value / s);
+        return *this;
+    }
+
+private:
+    T _value;
+};
+
+using Angle8 = Angle<int8_t>;
+using Angle16 = Angle<int16_t>;
+using Angle32 = Angle<int32_t>;
+using Angle64 = Angle<int64_t>;
+
+template <typename T>
+Angle<T> operator+(Angle<T> lhs, Angle<T> rhs)
+{
+    lhs += rhs;
+    return lhs;
+}
+
+template <typename T>
+Angle<T> operator-(Angle<T> lhs, Angle<T> rhs)
+{
+    lhs -= rhs;
+    return lhs;
+}
+
+template <typename T, typename Scalar>
+Angle<T> operator*(Angle<T> lhs, Scalar rhs)
+{
+    lhs *= rhs;
+    return lhs;
+}
+
+template <typename T, typename Scalar>
+Angle<T> operator*(Scalar lhs, Angle<T> rhs)
+{
+    rhs *= lhs;
+    return rhs;
+}
+
+template <typename T, typename Scalar>
+Angle<T> operator/(Angle<T> lhs, Scalar rhs)
+{
+    lhs /= rhs;
+    return lhs;
+}
+
+template <typename T>
+double operator/(Angle<T> lhs, Angle<T> rhs)
+{
+    return double(lhs.raw()) / rhs.raw();
+}
+
+template <typename T>
+bool operator==(Angle<T> lhs, Angle<T> rhs)
+{
+    return lhs.raw() == rhs.raw();
+}
+
+template <typename T>
+bool operator!=(Angle<T> lhs, Angle<T> rhs)
+{
+    return !(lhs == rhs);
+}
+
+// Fast trigonometric functions. Single precision is sufficient for Angle16 and Angle8.
+float sin(Angle16 a);
+float cos(Angle16 a);
+float tan(Angle16 a);
+float sin(Angle8 a);
+float cos(Angle8 a);
+float tan(Angle8 a);
+
+}

src/common/3rd/x86/math/include/math/box2d.h

@@ -0,0 +1,63 @@
+#pragma once
+
+#include "math/aabox2d.h"
+#include "math/segment2d.h"
+#include "math/vec2d.h"
+
+#include <string>
+#include <vector>
+
+namespace decision::math {
+
+class Box2d {
+public:
+    Box2d(const Vec2d& center, const double heading,
+        const double length, const double width);
+    Box2d(const Segment2d& axis, const double width);
+    explicit Box2d(const AABox2d& aabox);
+    static Box2d create_aa_box(const Vec2d& one_corner, const Vec2d& opponent_corner);
+
+    const Vec2d& center() const { return _center; }
+    double center_x() const { return _center.x(); }
+    double center_y() const { return _center.y(); }
+    double length() const { return _length; }
+    double width() const { return _width; }
+    double half_length() const { return _half_length; }
+    double half_width() const { return _half_width; }
+    double heading() const { return _heading; }
+    double cos_heading() const { return _cos_heading; }
+    double sin_heading() const { return _sin_heading; }
+    double area() const { return _length * _width; }
+    double diagonal() const { return hypot(_length, _width); }
+
+    void get_all_corners(std::vector<Vec2d>* const corners) const;
+
+    bool is_point_in(const Vec2d& point) const;
+    bool is_point_on_boundary(const Vec2d& point) const;
+
+    double distance_to(const Vec2d& point) const;
+    double distance_to(const Segment2d& segment) const;
+    double distance_to(const Box2d& box) const;
+
+    bool has_overlap(const Segment2d& segment) const;
+    bool has_overlap(const Box2d& box) const;
+
+    AABox2d get_aa_box() const;
+
+    void rotate_from_center(double rotate_angle);
+    void shift(const Vec2d& shift_vec);
+
+    std::string debug_string() const;
+
+protected:
+    Vec2d _center;
+    double _length = 0.0;
+    double _width = 0.0;
+    double _half_length = 0.0;
+    double _half_width = 0.0;
+    double _heading = 0.0;
+    double _cos_heading = 1.0;
+    double _sin_heading = 0.0;
+};
+
+}

src/common/3rd/x86/math/include/math/euler_angles_zxy.h

@@ -0,0 +1,118 @@
+// Copyright 2016 Baidu Inc. All Rights Reserved.
+// Author: Heng, Liang (hengliang@baidu.com)
+//
+// A class of Euler angles with the intrinsic sequence ZXY.
+//
+// Our vehicle reference frame follows NovAtel's convention right/forward/up. The Euler angles
+// represents the rotation from the local frame (east/north/up) to the vehicle frame, as explained
+// below:
+//   roll: is zero when the car is level, and positive when the left side of the car is up;
+//   pitch: is zero when the car is level, and positive when the car is nose up;
+//   yaw: is zero when the car is pointing to north, and positive when the car turns left.
+// When using Euler angles to represent the rotation, we should use the intrinsic rotations ZXY. See
+// below for more explanations.
+//   http://danceswithcode.net/engineeringnotes/rotations_in_3d/rotations_in_3d_part1.html
+
+#ifndef ADU_COMMON_MATH_EULER_ANGLES_ZXY_H
+#define ADU_COMMON_MATH_EULER_ANGLES_ZXY_H
+
+#include "Eigen/Geometry"
+#include "math/math_utils.h"
+
+#include <cmath>
+
+namespace adu {
+namespace common {
+namespace math {
+
+template <typename T>
+struct EulerAnglesZXY {
+    T roll, pitch, yaw;
+
+    // Constructs an identity rotation.
+    EulerAnglesZXY()
+        : roll(0)
+        , pitch(0)
+        , yaw(0)
+    {
+    }
+
+    // Constructs a rotation using only yaw.
+    explicit EulerAnglesZXY(T yaw)
+        : roll(0)
+        , pitch(0)
+        , yaw(yaw)
+    {
+    }
+
+    // Constructs a rotation using roll, pitch, and yaw.
+    EulerAnglesZXY(T roll, T pitch, T yaw)
+        : roll(roll)
+        , pitch(pitch)
+        , yaw(yaw)
+    {
+    }
+
+    // Constructs a rotation using components of a quaternion.
+    EulerAnglesZXY(T qw, T qx, T qy, T qz)
+        : roll(atan2(2.0 * (qw * qy - qx * qz), 2.0 * (sqr<T>(qw) + sqr<T>(qz)) - 1.0))
+        , pitch(asin(2.0 * (qw * qx + qy * qz)))
+        , yaw(atan2(2.0 * (qw * qz - qx * qy), 2.0 * (sqr<T>(qw) + sqr<T>(qy)) - 1.0))
+    {
+    }
+
+    // Constructs a rotation using a quaternion.
+    explicit EulerAnglesZXY(const Eigen::Quaternion<T>& q)
+        : EulerAnglesZXY(q.w(), q.x(), q.y(), q.z())
+    {
+    }
+
+    // Normalizes roll, pitch, and yaw to [-PI, PI).
+    void normalize()
+    {
+        roll = normalize_angle(roll);
+        pitch = normalize_angle(pitch);
+        yaw = normalize_angle(yaw);
+    }
+
+    // Check if the rotation is valid. A valid rotation must have -PI/2 < pitch < PI/2.
+    bool is_valid()
+    {
+        normalize();
+        return pitch < M_PI_2 && pitch > -M_PI_2;
+    }
+
+    // Converts to a quaternion. The scalar part is guarantee to be non-negative.
+    Eigen::Quaternion<T> to_quaternion() const
+    {
+        // Uses double for internal calculation for better precision.
+        double r = roll * 0.5;
+        double p = pitch * 0.5;
+        double y = yaw * 0.5;
+
+        double sr = sin(r);
+        double sp = sin(p);
+        double sy = sin(y);
+
+        double cr = cos(r);
+        double cp = cos(p);
+        double cy = cos(y);
+
+        T qw = cr * cp * cy - sr * sp * sy;
+        T qx = cr * sp * cy - sr * cp * sy;
+        T qy = cr * sp * sy + sr * cp * cy;
+        T qz = cr * cp * sy + sr * sp * cy;
+        if (qw < 0.0)
+            return { -qw, -qx, -qy, -qz };
+        return { qw, qx, qy, qz };
+    }
+};
+
+using EulerAnglesZXYf = EulerAnglesZXY<float>;
+using EulerAnglesZXYd = EulerAnglesZXY<double>;
+
+} // namespace math
+} // namespace common
+} // namespace adu
+
+#endif // ADU_COMMON_MATH_EULER_ANGLES_ZXY_H

src/common/3rd/x86/math/include/math/heading.h

@@ -0,0 +1,45 @@
+// Copyright 2016 Baidu Inc. All Rights Reserved.
+// Author: Heng, Liang (hengliang@baidu.com)
+//
+// Helper functions that converts heading from/to a quaternion.
+//
+// We have permanently defined that the heading of car is zero when the car is pointing to east, and
+// positive when the car turns left. This function guarantees correct heading calculation,
+// regardless any future changes in the definition of vehicle reference frame.
+
+#pragma once
+
+#include "Eigen/Geometry"
+#include "math/euler_angles_zxy.h"
+#include "math/math_utils.h"
+
+#include <cmath>
+
+namespace decision::math {
+
+// Returns heading (in radians) in [-PI, PI).
+inline double quaternion_to_heading(double qw, double qx, double qy, double qz)
+{
+    EulerAnglesZXYd euler_angles(qw, qx, qy, qz);
+    // Needs adding pi/2 to yaw because when the vehicle reference frame is RFU, yaw is zero when
+    // the car is pointing to north.
+    return normalize_angle(euler_angles.yaw + M_PI_2);
+}
+
+// Similar to above but takes a Quaternion object.
+template <typename T>
+inline double quaternion_to_heading(const Eigen::Quaternion<T>& q)
+{
+    return quaternion_to_heading(q.w(), q.x(), q.y(), q.z());
+}
+
+// Returns a quaternion with zero roll, zero pitch, and the specified heading.
+template <typename T>
+inline Eigen::Quaternion<T> heading_to_quaternion(double heading)
+{
+    // Needs deducting pi/2 from heading because the vehicle reference frame is RFU.
+    EulerAnglesZXY<T> euler_angles(heading - M_PI_2);
+    return euler_angles.to_quaternion();
+}
+
+}

src/common/3rd/x86/math/include/math/math_utils.h

@@ -0,0 +1,55 @@
+#pragma once
+
+#include "math/box2d.h"
+#include "math/polygon2d.h"
+#include "math/vec2d.h"
+#include "math/vec3d.h"
+
+namespace decision::math {
+
+constexpr double kMathEpsilon = 1e-10;
+
+inline double cross_prod(const Vec2d& point1, const Vec2d& point2, const Vec2d& point3)
+{
+    return (point2.x() - point1.x()) * (point3.y() - point1.y()) - (point3.x() - point1.x()) * (point2.y() - point1.y());
+}
+
+inline Vec3d cross_prod(const Vec3d& point1, const Vec3d& point2, const Vec3d& point3)
+{
+    Vec3d v12 = point2 - point1;
+    Vec3d v13 = point3 - point1;
+    return v12.cross_prod(v13);
+}
+
+inline double inner_prod(const Vec2d& point1, const Vec2d& point2, const Vec2d& point3)
+{
+    return (point2.x() - point1.x()) * (point3.x() - point1.x()) + (point2.y() - point1.y()) * (point3.y() - point1.y());
+}
+
+// Wrap angle to [0, 2 * PI).
+inline double wrap_angle(const double angle)
+{
+    const double new_angle = fmod(angle, M_PI * 2.0);
+    return new_angle < 0 ? new_angle + M_PI * 2.0 : new_angle;
+}
+
+// Normalize angle to [-PI, PI).
+inline double normalize_angle(const double angle)
+{
+    const double new_angle = fmod(angle + M_PI, M_PI * 2.0);
+    return (new_angle < 0 ? new_angle + M_PI * 2.0 : new_angle) - M_PI;
+}
+
+template <typename T>
+inline T sqr(const T value)
+{
+    return value * value;
+}
+
+}
+
+#include <iostream>
+#define CHECK(v) (void)(v)
+#define CHECK_NOTNULL(v) (void)(v)
+#define CHECK_GE(a, b)
+#define CHECK_GT(a, b)

src/common/3rd/x86/math/include/math/polygon2d.h

@@ -0,0 +1,77 @@
+#pragma once
+
+#include "math/box2d.h"
+#include "math/segment2d.h"
+#include "math/vec2d.h"
+
+#include <string>
+#include <vector>
+
+namespace decision::math{
+
+class Polygon2d {
+public:
+    Polygon2d() = default;
+    explicit Polygon2d(const Box2d& box);
+    explicit Polygon2d(std::vector<Vec2d> points);
+
+    const std::vector<Vec2d>& points() const { return _points; }
+    const std::vector<Segment2d>& segments() const { return _segments; }
+    int num_points() const { return _num_points; }
+    bool is_convex() const { return _is_convex; }
+    double area() const { return _area; }
+
+    double distance_to_boundary(const Vec2d& point) const;
+    double distance_to(const Vec2d& point) const;
+    double distance_to(const Segment2d& segment) const;
+    double distance_to(const Box2d& box) const;
+    double distance_to(const Polygon2d& polygon) const;
+    double distance_sqr_to(const Vec2d& point) const;
+
+    bool is_point_in(const Vec2d& point) const;
+    bool is_point_on_boundary(const Vec2d& point) const;
+
+    bool is_contain(const Segment2d& segment) const;
+    bool is_contain(const Polygon2d& polygon) const;
+
+    static bool compute_convex_hull(const std::vector<Vec2d>& points, Polygon2d* const polygon);
+
+    bool has_overlap(const Segment2d& segment) const;
+    bool get_overlap(const Segment2d& segment, Vec2d* const first, Vec2d* const last) const;
+    std::vector<Segment2d> get_all_overlaps(const Segment2d& segment) const;
+
+    bool has_overlap(const Polygon2d& polygon) const;
+    // Only compute overlaps between two convex polygons.
+    bool compute_overlap(const Polygon2d& other_polygon, Polygon2d* const overlap_polygon) const;
+
+    AABox2d aa_bounding_box() const;
+    Box2d bounding_box_with_heading(const double heading) const;
+    Box2d min_area_bounding_box() const;
+    void extreme_points(const double heading, Vec2d* const first, Vec2d* const last) const;
+
+    Polygon2d expand_by_distance(const double distance) const;
+
+    std::string debug_string() const;
+
+protected:
+    void build_from_points();
+
+protected:
+    int next(int at) const;
+    int prev(int at) const;
+
+    // Remove point p if and only if outer_prod(segment.start, segment.end, p) < 0.
+    static bool clip_convex_hull(const Segment2d& segment, std::vector<Vec2d>* const points);
+
+    std::vector<Vec2d> _points;
+    int _num_points = 0;
+    std::vector<Segment2d> _segments;
+    bool _is_convex = false;
+    double _area = 0.0;
+    double _min_x = 0.0;
+    double _max_x = 0.0;
+    double _min_y = 0.0;
+    double _max_y = 0.0;
+};
+
+}

src/common/3rd/x86/math/include/math/polygon3d.h

@@ -0,0 +1,38 @@
+// Copyright 2018 Baidu Inc. All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+//
+// Description: polygon for 3d point, the polygon is constructed on 2d space, but the point is 3d
+
+#pragma once
+
+#include "math/polygon2d.h"
+#include "math/segment3d.h"
+#include "math/vec2d.h"
+#include "math/vec3d.h"
+
+namespace decision::math {
+
+class Polygon3d {
+public:
+    Polygon3d() = default;
+    explicit Polygon3d(const std::vector<Vec3d>& points);
+
+    Polygon2d polygon2d() const;
+    const std::vector<Vec3d>& points() const { return _points; }
+    const std::vector<Segment3d>& segments() const { return _segments; }
+
+    std::string debug_string() const;
+
+protected:
+    void build_from_points();
+
+protected:
+    inline size_t next(size_t at) const;
+    inline size_t prev(size_t at) const;
+
+    std::vector<Vec3d> _points;
+    std::vector<Vec2d> _points2d;
+    std::vector<Segment3d> _segments;
+};
+
+}

src/common/3rd/x86/math/include/math/segment2d.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include "math/vec2d.h"
+
+#include <string>
+
+namespace decision::math {
+
+class Segment2d {
+public:
+    Segment2d();
+    Segment2d(const Vec2d& start, const Vec2d& end);
+
+    const Vec2d& start() const { return _start; }
+    const Vec2d& end() const { return _end; }
+    const Vec2d& unit_direction() const { return _unit_direction; }
+    Vec2d center() const { return (_start + _end) / 2.0; }
+    double heading() const { return _heading; }
+    double cos_heading() const { return _unit_direction.x(); }
+    double sin_heading() const { return _unit_direction.y(); }
+
+    double length() const;
+    double length_sqr() const;
+
+    double distance_to(const Vec2d& point) const;
+    double distance_to(const Vec2d& point, Vec2d* const nearest_pt) const;
+    double distance_sqr_to(const Vec2d& point) const;
+    double distance_sqr_to(const Vec2d& point, Vec2d* const nearest_pt) const;
+
+    bool is_point_in(const Vec2d& point) const;
+
+    bool has_intersect(const Segment2d& other_segment) const;
+    bool get_intersect(const Segment2d& other_segment, Vec2d* const point) const;
+
+    double project_onto_unit(const Vec2d& point) const;
+    double product_onto_unit(const Vec2d& point) const;
+    double get_perpendicular_foot(const Vec2d& point, Vec2d* const foot_point) const;
+
+    std::string debug_string() const;
+
+protected:
+    Vec2d _start;
+    Vec2d _end;
+    Vec2d _unit_direction;
+    double _heading = 0.0;
+    double _length = 0.0;
+};
+
+}

src/common/3rd/x86/math/include/math/segment3d.h

@@ -0,0 +1,51 @@
+// Copyright 2018 Baidu Inc. All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+//
+// Description: segment for 3d
+
+#pragma once
+
+#include "math/segment2d.h"
+#include "math/vec3d.h"
+
+namespace decision::math {
+
+class Segment3d {
+public:
+    Segment3d();
+    Segment3d(const Vec3d& start, const Vec3d& end);
+
+    const Vec3d& start() const { return _start; }
+    const Vec3d& end() const { return _end; }
+    const Vec3d& unit_direction() const { return _unit_direction; }
+    Vec3d center() const { return (_start + _end) / 2.0; }
+
+    double length() const;
+    double length_sqr() const;
+
+    double distance_to(const Vec3d& point) const;
+    double distance_to(const Vec3d& point, Vec3d* const nearest_pt) const;
+    double distance_sqr_to(const Vec3d& point) const;
+    double distance_sqr_to(const Vec3d& point, Vec3d* const nearest_pt) const;
+
+    bool is_point_in(const Vec3d& point) const;
+
+    bool has_intersect(const Segment3d& other_segment) const;
+    bool get_intersect(const Segment3d& other_segment, Vec3d* const point) const;
+
+    double project_onto_unit(const Vec3d& point) const;
+    double product_onto_unit(const Vec3d& point) const;
+    double get_perpendicular_foot(const Vec3d& point, Vec3d* const foot_point) const;
+
+    Segment2d segment2d() const;
+
+    std::string debug_string() const;
+
+protected:
+    Vec3d _start;
+    Vec3d _end;
+    Vec3d _unit_direction;
+    double _length = 0.0;
+};
+
+}

src/common/3rd/x86/math/include/math/sin_table.h

@@ -0,0 +1,7 @@
+#pragma once
+
+namespace decision::math {
+
+extern const float SIN_TABLE[16385];
+
+}

src/common/3rd/x86/math/include/math/vec2d.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <cmath>
+#include <string>
+
+namespace decision::math {
+
+class Vec2d {
+public:
+    constexpr Vec2d() noexcept
+        : Vec2d(0, 0)
+    {
+    }
+    constexpr Vec2d(const double x, const double y) noexcept
+        : _x(x)
+        , _y(y)
+    {
+    }
+    static Vec2d create_unit_vec(const double angle);
+
+    double x() const { return _x; }
+    double y() const { return _y; }
+    double angle() const { return atan2(_y, _x); }
+    void set_x(const double x) { _x = x; }
+    void set_y(const double y) { _y = y; }
+    void normalize();
+
+    double length() const;
+    double length_sqr() const;
+    double distance_to(const Vec2d& other) const;
+    double distance_sqr_to(const Vec2d& other) const;
+
+    Vec2d operator+(const Vec2d& other) const;
+    Vec2d operator-(const Vec2d& other) const;
+    Vec2d operator*(const double ratio) const;
+    Vec2d operator/(const double ratio) const;
+    Vec2d& operator+=(const Vec2d& other);
+    Vec2d& operator-=(const Vec2d& other);
+    Vec2d& operator*=(const double ratio);
+    Vec2d& operator/=(const double ratio);
+    bool operator==(const Vec2d& other) const;
+
+    //! rotate the vector by angle.
+    Vec2d rotate(const double angle) const;
+
+    double cross_prod(const Vec2d& other) const;
+    double inner_prod(const Vec2d& other) const;
+
+    std::string debug_string() const;
+
+protected:
+    double _x = 0.0;
+    double _y = 0.0;
+};
+
+Vec2d operator*(const double ratio, const Vec2d& vec);
+
+}

src/common/3rd/x86/math/include/math/vec3d.h

@@ -0,0 +1,66 @@
+// Copyright 2018 Baidu Inc. All Rights Reserved
+// Author: Fuxiangyu (fuxiangyu@baidu.com)
+//
+// Description: supporting for 3D element
+
+#pragma once
+
+// #include <cmath>
+// #include <string>
+#include "math/vec2d.h"
+
+namespace decision::math {
+
+class Vec3d {
+public:
+    constexpr Vec3d() noexcept
+        : Vec3d(0, 0, 0)
+    {
+    }
+    constexpr Vec3d(const double x, const double y, const double z) noexcept
+        : _x(x)
+        , _y(y)
+        , _z(z)
+    {
+    }
+    static Vec3d create_unit_vec(const double yaw_angle, const double pitch_angle);
+
+    double x() const { return _x; }
+    double y() const { return _y; }
+    double z() const { return _z; }
+    double yaw_angle() const { return atan2(_y, _x); }
+    void set_x(const double x) { _x = x; }
+    void set_y(const double y) { _y = y; }
+    void set_z(const double z) { _z = z; }
+    void normalize();
+
+    double length() const;
+    double length_sqr() const;
+    double distance_to(const Vec3d& other) const;
+    double distance_sqr_to(const Vec3d& other) const;
+
+    Vec3d operator+(const Vec3d& other) const;
+    Vec3d operator-(const Vec3d& other) const;
+    Vec3d operator*(const double ratio) const;
+    Vec3d operator/(const double ratio) const;
+    Vec3d& operator+=(const Vec3d& other);
+    Vec3d& operator-=(const Vec3d& other);
+    Vec3d& operator*=(const double ratio);
+    Vec3d& operator/=(const double ratio);
+    bool operator==(const Vec3d& other) const;
+
+    Vec3d cross_prod(const Vec3d& other) const;
+    double inner_prod(const Vec3d& other) const;
+
+    Vec2d vec2d() const { return Vec2d(_x, _y); }
+    std::string debug_string() const;
+
+protected:
+    double _x = 0.0;
+    double _y = 0.0;
+    double _z = 0.0;
+};
+
+Vec3d operator*(const double ratio, const Vec3d& vec);
+
+}

src/common/3rd/x86/utils/include/utils/bevdetConfig.h

@@ -0,0 +1,6 @@
+#pragma once
+
+constexpr const char* bevDetConfigurePath = "/cfgs/configure.yaml";
+constexpr const char* bevDetImgStageEngine = "model/img_stage_lt_d_fp16.engine";
+constexpr const char* bevDetBEVStageEngine = "model/bev_stage_lt_d_fp16.engine";
+constexpr const char* bevDetDepth = "cfgs/bevdet_lt_depth.yaml";

src/common/3rd/x86/utils/include/utils/defer.h

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <functional>
+
+class defer_t final {
+public:
+    template <typename F>
+    explicit defer_t(F&& f) noexcept
+        : f_ {
+            std::forward<F>(f)
+        }
+    {
+    }
+
+    ~defer_t()
+    {
+
+        if (f_) {
+
+            f_();
+        }
+    }
+
+    defer_t(const defer_t&) = delete;
+    defer_t& operator=(const defer_t&) = delete;
+
+    defer_t(defer_t&& rhs) noexcept
+        : f_ {
+            std::move(rhs.f_)
+        }
+    {
+    }
+
+    defer_t& operator=(defer_t&& rhs) noexcept
+    {
+
+        f_ = std::move(rhs.f_);
+        return *this;
+    }
+
+private:
+    std::function<void()> f_;
+};
+
+class defer_maker final {
+
+public:
+    template <typename F>
+    defer_t operator+(F&& f)
+    {
+
+        return defer_t {
+            std::forward<F>(f)
+        };
+    }
+};
+
+#define DEFER_CONCAT_NAME(l, r) l##r
+#define DEFER_CREATE_NAME(l, r) DEFER_CONCAT_NAME(l, r)
+#define defer auto DEFER_CREATE_NAME(defer_, __COUNTER__) = defer_maker {} +
+#define defer_scope defer[&]

src/common/3rd/x86/utils/include/utils/error_event.h

@@ -0,0 +1,135 @@
+#pragma once
+
+/**
+0x01	系统基础	SYS
+0x02	can0	CAN0
+0x03	can1	CAN1
+0x04	can2	CAN2
+0x10	IMU	IMU
+0x11	GPS	GPS
+0x12	单线雷达 #1 左	LIDAR1
+0x13	单线雷达 #2 右	LIDAR2
+0x14	单线雷达 #3 左斜	LIDAR3
+0x15	单线雷达 #4 右斜	LIDAR4
+0x16	单线雷达 #5 后	LIDAR5
+0x17	相机 #1（RGB）	CAM1
+0x18	相机 #2（RGB-D/后）	CAM2
+0x19	屏幕	TFT
+
+0x20	电机驱动器	MOTOR
+0x21	上控	TOP
+ */
+enum class ErrorEventCategory {
+    SYS = 0x01,
+    CAN0 = 0x02,
+    CAN1 = 0x03,
+    CAN2 = 0x04,
+    IMU = 0x10,
+    GPS = 0x11,
+    LIDAR1 = 0x12,
+    LIDAR2 = 0x13,
+    LIDAR3 = 0x14,
+    LIDAR4 = 0x15,
+    LIDAR5 = 0x16,
+    CAM1 = 0x17,
+    CAM2 = 0x18,
+    TFT = 0x19,
+    MOTOR = 0x20,
+    TOP = 0x21,
+};
+//  01 发生  02 恢复
+enum class ErrorEventLevel {
+    OCCUR = 0x01,
+    RECOVER = 0x02,
+};
+
+#define SYS_CONFIG_ERROR 0x010101 // 配置读取失败
+#define SYS_INIT_FAIL 0x010102 // 初始化失败
+#define SYS_FILE_ERROR 0x010103 // 文件 IO 异常
+#define SYS_NETWORK_FAIL 0x010201 // 4G / 网络错误
+#define SYS_TIME_SYNC_ERROR 0x010501 // 系统时间异常
+
+#define CAN0_NOT_FOUND 0x020201 // 未检测到 CAN0
+#define CAN0_DISCONNECTED 0x020202 // CAN0 断连
+
+#define CAN1_NOT_FOUND 0x030201 // 未检测到 CAN1
+#define CAN1_DISCONNECTED 0x030202 // CAN1 断连
+
+#define CAN2_NOT_FOUND 0x040201 // 未检测到 CAN2
+#define CAN2_DISCONNECTED 0x040202 // CAN2 断连
+
+#define IMU_NOT_FOUND 0x100201 // 未检测到 IMU
+#define IMU_DISCONNECTED 0x100202 // IMU 断连
+#define IMU_TIMEOUT 0x100303 // 超时无数据
+#define IMU_DATA_INVALID 0x100304 // 数据异常（NaN/饱和）
+#define IMU_CALIB_FAIL 0x100407 // 校准失败
+#define IMU_TIME_SYNC_ERROR 0x100508 // 时间戳跳变
+#define IMU_DRIVER_ERROR 0x100609 // SDK/协议解析失败
+
+#define GPS_NOT_FOUND 0x110201 // 未检测到 GPS
+#define GPS_DISCONNECTED 0x110202 // 断连
+#define GPS_NO_SIGNAL 0x110304 // 无卫星 / SNR 太低
+#define GPS_DATA_LOSS 0x110305 // 数据丢包
+#define GPS_TIME_SYNC_ERROR 0x110508 // PPS/时间异常
+#define GPS_DRIVER_ERROR 0x110609 // 协议解析失败（NMEA/UBlox）
+
+// 左雷达
+#define LIDAR1_NOT_FOUND 0x120201 // 未检测到雷达
+#define LIDAR1_DISCONNECTED 0x120202 // 断连
+#define LIDAR1_TIMEOUT 0x120303 // 无数据
+#define LIDAR1_DATA_INVALID 0x120304 // 数据无效
+#define LIDAR1_TIME_SYNC_ERROR 0x120508 // 时间戳异常
+#define LIDAR1_DRIVER_ERROR 0x120609 // 协议解析失败
+// 右雷达
+#define LIDAR2_NOT_FOUND 0x130201 // 未检测到雷达
+#define LIDAR2_DISCONNECTED 0x130202 // 断连
+#define LIDAR2_TIMEOUT 0x130303 // 无数据
+#define LIDAR2_DATA_INVALID 0x130304 // 数据无效
+#define LIDAR2_TIME_SYNC_ERROR 0x130508 // 时间戳异常
+#define LIDAR2_DRIVER_ERROR 0x130609 // 协议解析失败
+// 左斜雷达
+#define LIDAR3_NOT_FOUND 0x140201 // 未检测到雷达
+#define LIDAR3_DISCONNECTED 0x140202 // 断连
+#define LIDAR3_TIMEOUT 0x140303 // 无数据
+#define LIDAR3_DATA_INVALID 0x140304 // 数据无效
+#define LIDAR3_TIME_SYNC_ERROR 0x140508 // 时间戳异常
+#define LIDAR3_DRIVER_ERROR 0x140609 // 协议解析失败
+// 右斜雷达
+#define LIDAR4_NOT_FOUND 0x150201 // 未检测到雷达
+#define LIDAR4_DISCONNECTED 0x150202 // 断连
+#define LIDAR4_TIMEOUT 0x150303 // 无数据
+#define LIDAR4_DATA_INVALID 0x150304 // 数据无效
+#define LIDAR4_TIME_SYNC_ERROR 0x150508 // 时间戳异常
+#define LIDAR4_DRIVER_ERROR 0x150609 // 协议解析失败
+// 后雷达
+#define LIDAR5_NOT_FOUND 0x160201 // 未检测到雷达
+#define LIDAR5_DISCONNECTED 0x160202 // 断连
+#define LIDAR5_TIMEOUT 0x160303 // 无数据
+#define LIDAR5_DATA_INVALID 0x160304 // 数据无效
+#define LIDAR5_TIME_SYNC_ERROR 0x160508 // 时间戳异常
+#define LIDAR5_DRIVER_ERROR 0x160609 // 协议解析失败
+
+#define CAM1_NOT_FOUND 0x170201 // 未检测到相机
+#define CAM1_DISCONNECTED 0x170202 // 断连
+#define CAM1_FRAME_DROP 0x170305 // 丢帧过多
+#define CAM1_DATA_INVALID 0x170304 // 图像损坏/格式错误
+#define CAM1_TIME_SYNC_ERROR 0x170508 // 时间异常
+#define CAM1_DRIVER_ERROR 0x170609 // SDK/驱动错误
+
+#define CAM2_NOT_FOUND 0x180201 // 未检测到相机
+#define CAM2_DISCONNECTED 0x180202 // 断连
+#define CAM2_DEPTH_INVALID 0x180304 // 深度数据异常
+#define CAM2_FRAME_DROP 0x180305 // 丢帧过多
+#define CAM2_TIME_SYNC_ERROR 0x180508 // 时间异常
+#define CAM2_DRIVER_ERROR 0x180609 // SDK/驱动错误
+
+#define TFT_NOT_FOUND 0x190201 // 未检测到TFT
+#define TFT_DISCONNECTED 0x190202 // 断连
+#define TFT_DEPTH_INVALID 0x190304 //
+#define TFT_FRAME_DROP 0x190305 //
+#define TFT_TIME_SYNC_ERROR 0x190508 // 时间异常
+#define TFT_DRIVER_ERROR 0x190609 // SDK/驱动错误
+
+inline uint32_t encodeErrorLevel(uint32_t error_code, ErrorEventLevel level) {
+    return (error_code << 8) | (static_cast<uint32_t>(level) & 0xFF);
+}

src/common/3rd/x86/utils/include/utils/non_timer.h

@@ -0,0 +1,62 @@
+#include <atomic>
+#include <chrono>
+#include <functional>
+#include <iostream>
+#include <memory>
+#include <thread>
+namespace utils {
+// 定义定时器类
+class NonBlockingTimer {
+public:
+    // 构造函数，接受定时器时间（毫秒）和回调函数
+    NonBlockingTimer(int milliseconds, std::function<void()> callback)
+        : duration(std::chrono::milliseconds(milliseconds))
+        , callback(callback)
+        , is_running(false)
+    {
+    }
+
+    // 启动定时器
+    void start()
+    {
+        if (is_running)
+            return; // 如果定时器已经在运行，返回
+        is_running = true;
+        timer_thread = std::thread([this]() {
+            while (is_running) {
+                std::this_thread::sleep_for(duration); // 睡眠指定的时间
+                if (is_running) {
+                    callback(); // 时间到后执行回调函数
+                }
+            }
+        });
+        timer_thread.detach(); // 分离线程，非阻塞
+    }
+
+    // 停止定时器
+    void stop()
+    {
+        is_running = false;
+    }
+
+    // 检查定时器是否正在运行
+    bool running() const
+    {
+        return is_running;
+    }
+
+    ~NonBlockingTimer()
+    {
+        stop(); // 确保在销毁时停止定时器
+        if (timer_thread.joinable()) {
+            timer_thread.join(); // 如果线程可加入，加入线程
+        }
+    }
+
+private:
+    std::chrono::milliseconds duration; // 定时器的时间（毫秒）
+    std::function<void()> callback; // 回调函数
+    std::atomic<bool> is_running; // 定时器是否正在运行
+    std::thread timer_thread; // 定时器线程
+};
+}

src/common/3rd/x86/utils/include/utils/noncopyable.h

@@ -0,0 +1,15 @@
+#pragma once
+
+namespace utils {
+
+class Noncopyable {
+public:
+    Noncopyable(const Noncopyable&) = delete;
+    void operator=(const Noncopyable&) = delete;
+
+protected:
+    Noncopyable() = default;
+    ~Noncopyable() = default;
+};
+
+}

src/common/3rd/x86/utils/include/utils/serial_port.h

@@ -0,0 +1,102 @@
+/*
+ * @copyright: Copyright (c) 2024 zhengqi.com, Inc. All Rights Reserved
+ * @Author: likang(likang@baidu.com)
+ * @Date: 2024-01-05 16:32:38
+ * @FilePath: mem/src/sensor/serial_port/serial_port.h
+ * @brief:
+ */
+#ifndef SERIALPORT_H
+#define SERIALPORT_H
+#include <functional>
+#include <memory>
+#include <string>
+#include <vector>
+
+struct termios;
+namespace utils {
+
+enum BaudRate {
+    BR0 = 0000000,
+    BR50 = 0000001,
+    BR75 = 0000002,
+    BR110 = 0000003,
+    BR134 = 0000004,
+    BR150 = 0000005,
+    BR200 = 0000006,
+    BR300 = 0000007,
+    BR600 = 0000010,
+    BR1200 = 0000011,
+    BR1800 = 0000012,
+    BR2400 = 0000013,
+    BR4800 = 0000014,
+    BR9600 = 0000015,
+    BR19200 = 0000016,
+    BR38400 = 0000017,
+    BR57600 = 0010001,
+    BR115200 = 0010002,
+    BR230400 = 0010003,
+    BR460800 = 0010004,
+    BR500000 = 0010005,
+    BR576000 = 0010006,
+    BR921600 = 0010007,
+    BR1000000 = 0010010,
+    BR1152000 = 0010011,
+    BR1500000 = 0010012,
+    BR2000000 = 0010013,
+    BR2500000 = 0010014,
+    BR3000000 = 0010015,
+    BR3500000 = 0010016,
+    BR4000000 = 0010017
+};
+
+enum DataBits {
+    DATA_BITS_5,
+    DATA_BITS_6,
+    DATA_BITS_7,
+    DATA_BITS_8,
+};
+
+enum StopBits { STOP_BITS_1,
+    STOP_BITS_2 };
+
+enum Parity { PARITY_NONE,
+    PARITY_EVEN,
+    PARITY_MARK,
+    PARITY_ODD,
+    PARITY_SPACE };
+
+struct OpenOptions {
+    bool auto_open;
+    BaudRate baud_rate;
+    DataBits data_bits;
+    StopBits stop_bits;
+    Parity parity;
+    bool xon; // 当一端的数据连接不再能接受更多数据（或者接近这个状态），它发送XOFF字节给另一端。另一端收到XOFF字节，挂起数据发送。
+    bool xoff; // 一端如果准备好继续接收数据，它发送XON字节给另一端，另一端恢复数据发送。
+    bool xany;
+    int vmin;
+    int vtime;
+};
+
+class SerialPort {
+public:
+    static BaudRate baud_rate_make(unsigned long baud_rate);
+    static const OpenOptions default_options;
+    SerialPort(const std::string& devpath, const OpenOptions& options, bool isblock = false);
+    ~SerialPort();
+    bool start();
+    int read(char* buffer, int buf_len);
+    int write(char* buffer, int buf_len);
+    int get_tty_fd();
+
+private:
+    bool _is_block;
+    bool serial_port_init();
+    void termios_options(termios& tios, const OpenOptions& options);
+    std::string _path;
+    OpenOptions _open_options;
+    int _tty_fd = 0;
+};
+
+} // namespace sensor
+#endif // SERIALPORT_H

src/common/3rd/x86/utils/include/utils/singleton.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "noncopyable.h"
+#include <memory>
+
+namespace utils {
+
+template <typename T>
+class Singleton : public Noncopyable {
+public:
+    template <typename... Args>
+    static T& getInstance(Args&&... args)
+    {
+        static T t(std::forward<Args>(args)...);
+        return t;
+    }
+};
+
+}

src/common/3rd/x86/utils/include/utils/topic.h

@@ -0,0 +1,147 @@
+#pragma once
+
+constexpr const char *imagesTopic = "/sensor/camera/images";
+constexpr const char *cameraFusedImagesTopic = "/perception/cameraFused/images";
+
+constexpr const char *cameraFusedObjectsTopic = "/perception/cameraFused/objects";
+constexpr const char *multiFusedObjectsTopic = "/perception/multiFused/objects";
+constexpr const char *multiFusedObjectsTopicDecision = "/perception/decision/multiFused/objects";
+constexpr const char *obstacleMapTopicDecision = "/perception/obstacle_map";
+constexpr const char *eventTopic = "/decision/event";
+
+constexpr const char *danceStatusTopic = "/xhi/status";
+
+constexpr const char *backupCarTopic = "/sensor/backup/car";
+constexpr const char *handleControlTopic = "/handle/control";              // 手柄控制
+constexpr const char *nav2ControlTopic = "/nav/control";                   // nva2控制
+constexpr const char *appControlTopic = "/app/control";                    // app控制
+constexpr const char *carWheelSpeedTopic = "/car/speed";                   // 车辆底盘信息
+constexpr const char *carWheelSpeedStaTopic = "/wheel/twist";              // 车辆底盘信息
+constexpr const char *carEncoderTopic = "/car/encoder";                    // 车辆电机圈数
+constexpr const char *healthDataTopic = "/sensor/health/data";             // 健康计信息
+constexpr const char *healthTftDataTopic = "/sensor/tft/health/data";      // tft健康计信息
+constexpr const char *healthStatusTopic = "/sensor/health/start";          // 健康计启动
+constexpr const char *sensorhandleControlTopic = "/sensor/handle/control"; // 上控健康控制
+constexpr const char *gpsTopic = "/sensor/gps/data";
+constexpr const char *imuTopic = "/sensor/imu/data";
+constexpr const char *hrvStartTopic = "/sensor/hrv/start";
+constexpr const char *ultrasoundLeftTopic = "/sensor/left/ultrasound";
+constexpr const char *ultrasoundRightTopic = "/sensor/right/ultrasound";
+constexpr const char *ultrasoundBackTopic = "/sensor/back/ultrasound";
+constexpr const char *joystickTopic = "/sensor/joystick";
+constexpr const char *carInfoTopic = "/vlago/car/info";
+constexpr const char *operationSceTopic = "/service/operation/scenario";
+constexpr const char *serviceColourScreenTopic = "/service/colour_screen";
+constexpr const char *serviceGearTopic = "/service/gear";
+constexpr const char *modeChangeTopic = "/mode/change";
+constexpr const char *cameraH264Topic = "/sensor/backup/camera/h264";
+constexpr const char *leftLaserTopic = "/scan/left";
+constexpr const char *rightLaserTopic = "/scan/right";
+constexpr const char *leftTiltLaserTopic = "/scan/left_tilt";
+constexpr const char *rightTiltLaserTopic = "/scan/right_tilt";
+constexpr const char *backLaserTopic = "/scan/back";
+constexpr const char *leftTiltPointTopice = "/perception/left_tilt/pointcloud";
+constexpr const char *rightTiltPointTopice = "/perception/right_tilt/pointcloud";
+
+// 场景辅助测试相关topic
+constexpr const char *leftLaserROITopic = "/scene_auxiliary/left_laser/roi_filtered";
+constexpr const char *rightLaserROITopic = "/scene_auxiliary/right_laser/roi_filtered";
+constexpr const char *leftLaserClusterTopic = "/scene_auxiliary/left_laser/clusters";
+constexpr const char *rightLaserClusterTopic = "/scene_auxiliary/right_laser/clusters";
+constexpr const char *leftTiltClusterTopic = "/scene_auxiliary/left_tilt/clusters";
+constexpr const char *rightTiltClusterTopic = "/scene_auxiliary/right_tilt/clusters";
+constexpr const char *backClusterTopic = "/scene_auxiliary/back_laser/clusters";
+
+// base_link坐标系转换后的点云话题
+constexpr const char *leftLaserBaseLinkTopic = "/scene_auxiliary/left_laser/base_link";
+constexpr const char *rightLaserBaseLinkTopic = "/scene_auxiliary/right_laser/base_link";
+constexpr const char *leftTiltBaseLinkTopic = "/scene_auxiliary/left_tilt/base_link";
+constexpr const char *rightTiltBaseLinkTopic = "/scene_auxiliary/right_tilt/base_link";
+constexpr const char *backLaserBaseLinkTopic = "/scene_auxiliary/back_laser/base_link";
+
+constexpr const char *modeStatusTopic = "/vlago/mode/status";
+constexpr const char *rechargeStatusTopic = "/vlago/recharge/status";  //废弃
+constexpr const char *rechargeStateTopic = "/vlago/recharge/state";
+constexpr const char *sceneServiceModeTopic = "/scene_services/mode";
+constexpr const char *rgbPointCloudTopic = "/rgb/pointcloud";
+
+// 自车
+// constexpr const char* carInfo = "/sensor/car/information";
+
+// service
+constexpr const char *hrvStartService = "/vlago/hrv/start";
+constexpr const char *hrvInfoService = "/vlago/hrv/info";
+constexpr const char *operationSceService = "/vlago/operation/scenario";
+constexpr const char *operationStatusService = "/vlago/operation/status";
+constexpr const char *carControlService = "/vlago/car/control";
+// constexpr const char* carInfoService = "/vlago/car/info";
+constexpr const char *followControlService = "/vlago/follow_me";
+constexpr const char *followControlStatusService = "/vlago/follow_me/status";
+constexpr const char *summonService = "/valgo/car/summon";
+constexpr const char *summonStatusService = "/valgo/car/summon/status";
+constexpr const char *highService = "/vaglo/car/high";
+constexpr const char *gearsControlService = "/vlago/gear";
+constexpr const char *sensorGearControlService = "/sensor/vlago/gear";
+constexpr const char *idControlService = "/vlago/car_id";
+constexpr const char *narrowControlService = "/vlago/narrow_assistant_mode";
+constexpr const char *standardControlService = "/vlago/standard_mode";
+constexpr const char *cruiseControlService = "/vlago/cruise_mode";
+constexpr const char *auxiliaryControlService = "/vlago/safe_mode";
+constexpr const char *tftControlService = "/vlago/lamp_language_screen";
+constexpr const char *sensorTftControlService = "/sensor/vlago/lamp_language_screen";
+constexpr const char *danceControlService = "/vlago/dance_show";
+constexpr const char *modeSwitchService = "/mode/switch";
+constexpr const char *vlagoSettingService = "/vlago/setting";
+constexpr const char *vlagoModeEnableService = "/vlago/mode/enable";
+constexpr const char *rechargeService = "/vlago/auto_charge";
+constexpr const char *manageFollowmeService = "/manage/followme";
+constexpr const char *manageCruiseService = "/manage/cruise";
+constexpr const char *autoindoorService = "/vlago/autoDriveMode";
+constexpr const char *autoindoorNavGotoService = "/vlago/autoindoor/goto";
+constexpr const char *autoindoorNavCannelService = "/vlago/autoindoor/cannel";
+
+constexpr const char *feedNarrowService = "/feed/narrow/ctrl";
+constexpr const char *feedRechargeService = "/feed/recharge/ctrl";
+constexpr const char *feedFollwmeService = "/feed/follwme/ctrl";
+constexpr const char *feedDanceService = "/feed/dance/ctrl";
+//
+constexpr const char *ServiceFollowControlTopic = "/service/follow/control";
+// 模式
+constexpr const char *sceneAuxiliaryService = "/scene/auxiliary";
+constexpr const char *sceneNarrowService = "/scene/narrow";
+constexpr const char *sceneStandardService = "/scene/standard";
+constexpr const char *sceneCruiseService = "/scene/cruise";
+constexpr const char *sceneHelpcarService = "/scene/helpcar";
+constexpr const char *sceneDanceService = "/scene/dance";
+constexpr const char *sceneRechargeService = "/scene/recharge";
+constexpr const char *sceneFollowmeService = "/scene/followme";
+// 德国版本健康健 启动舞蹈秀
+constexpr const char *sceneKeyDanceTopic = "/scene/dance/key";
+
+constexpr const char *manageFollowMe = "/manage/followme";
+constexpr const char *manageDance = "/manage/dance";
+constexpr const char *manageStandard = "/manage/standard";
+
+// 模型
+constexpr const char *perTracekerTopic = "/perception/tracker/data";
+constexpr const char *perFeatuerTopic = "/perception/localization/data";
+constexpr const char *perDetctorTopic = "/perception/detector/data";
+constexpr const char *perMonnaiTopic = "/perception/end_points";
+constexpr const char *perSpeedbumpTopic = "/speedbump_marker_array";
+constexpr const char *perPoDaoTopic = "/perception/podao_detect_status";
+// 预警
+constexpr const char *auxiliaryAlarmTopic = "/vlago/auxiliary/alarm";
+constexpr const char *auxiliaryAlarmBackTopic = "/vlago/backcar/alarm";
+constexpr const char *cursorAlarmTopic = "/vlago/cruise/status";
+constexpr const char *speedbumpAlarmTopic = "/speed_bump/status";
+constexpr const char *downhillAlarmTopic = "/downhill/status";
+constexpr const char *uphillAlarmTopic = "/uphill/status";
+constexpr const char *footLimiterTopic = "/foot/limiter/status";
+
+// tft模式
+constexpr const char *tftModeTopic = "/vlago/tft/mode";
+
+constexpr const char *audioTopic = "/vlago/audio/play";
+
+// 错误码topic
+constexpr const char* errorEventTopic = "/error_events";

src/common/3rd/x86/utils/include/utils/trigger_logger.h

@@ -0,0 +1,174 @@
+#pragma once
+
+#include <chrono>
+#include <map>
+#include <memory>
+#include <mutex>
+#include <queue>
+#include <string>
+#include <thread>
+#include <atomic>
+
+namespace utils {
+
+/**
+ * @brief 埋点数据统一格式
+ * 
+ * 统一的埋点数据格式，用于云端上传
+ */
+struct TriggerData {
+    // 基础字段
+    std::string from;              // 数据来源: "pad" 或 "vlago"
+    std::string type;              // 事件类型: "trigger" 固定值
+    std::string event_name;        // 事件名称: 如 "mode_switch_start"
+    int64_t timestamp;             // 时间戳(毫秒)
+    std::string timestamp_str;     // 时间戳字符串: "YYYY-MM-DD HH:MM:SS"
+    
+    // 位置信息
+    std::string node_name;         // ROS节点名称
+    std::string file_path;        // 源文件路径
+    int line_number;               // 源代码行号
+    std::string function_name;     // 函数名称
+    
+    // 业务数据
+    std::map<std::string, std::string> data;  // 业务数据键值对
+    
+    // 系统信息
+    std::string vehicle_id;        // 车辆ID (可选)
+    std::string version;           // 软件版本 (可选)
+};
+
+/**
+ * @brief 埋点日志收集器
+ * 
+ * 单例模式，负责收集埋点数据并上传到云端
+ */
+class TriggerLogger {
+public:
+    /**
+     * @brief 获取单例实例
+     */
+    static TriggerLogger& getInstance();
+    
+    /**
+     * @brief 初始化埋点日志器
+     * @param websocket_url WebSocket服务器地址
+     * @param vehicle_id 车辆ID
+     * @param version 软件版本
+     */
+    void initialize(const std::string& websocket_url = "ws://60.205.235.193:3009",
+                   const std::string& vehicle_id = "",
+                   const std::string& version = "");
+    
+    /**
+     * @brief 记录埋点事件
+     * @param event_name 事件名称
+     * @param data 业务数据
+     * @param file_path 源文件路径
+     * @param line_number 源代码行号
+     * @param function_name 函数名称
+     * @param node_name ROS节点名称
+     */
+    void log(const std::string& event_name,
+             const std::map<std::string, std::string>& data,
+             const std::string& file_path = "",
+             int line_number = 0,
+             const std::string& function_name = "",
+             const std::string& node_name = "");
+    
+    /**
+     * @brief 设置车辆ID
+     */
+    void setVehicleId(const std::string& vehicle_id);
+    
+    /**
+     * @brief 设置软件版本
+     */
+    void setVersion(const std::string& version);
+    
+    /**
+     * @brief 停止日志器
+     */
+    void shutdown();
+    
+    /**
+     * @brief 检查是否已初始化
+     */
+    bool isInitialized() const { return initialized_.load(); }
+
+private:
+    TriggerLogger() = default;
+    ~TriggerLogger();
+    TriggerLogger(const TriggerLogger&) = delete;
+    TriggerLogger& operator=(const TriggerLogger&) = delete;
+    
+    /**
+     * @brief 将TriggerData转换为JSON字符串
+     */
+    std::string toJson(const TriggerData& trigger_data);
+    
+    /**
+     * @brief 上传线程函数
+     */
+    void uploadThread();
+    
+    /**
+     * @brief 获取当前时间戳(毫秒)
+     */
+    int64_t getCurrentTimestamp();
+    
+    /**
+     * @brief 格式化时间戳字符串
+     */
+    std::string formatTimestamp(int64_t timestamp_ms);
+    
+    // WebSocket客户端前向声明
+    class WebSocketClient;
+    
+    std::unique_ptr<WebSocketClient> ws_client_;
+    std::string websocket_url_;
+    std::string vehicle_id_;
+    std::string version_;
+    
+    std::mutex queue_mutex_;
+    std::queue<TriggerData> data_queue_;
+    std::atomic<bool> initialized_{false};
+    std::atomic<bool> should_stop_{false};
+    std::thread upload_thread_;
+    
+    // 队列大小限制
+    static constexpr size_t MAX_QUEUE_SIZE = 10000;
+};
+
+/**
+ * @brief 便捷宏定义，用于快速埋点
+ * 
+ * 使用示例:
+ *   TRIGGER_LOG("mode_switch_start", {
+ *       {"old_mode", "standard"},
+ *       {"new_mode", "auxiliary"}
+ *   });
+ */
+#define TRIGGER_LOG(event_name, data) \
+    utils::TriggerLogger::getInstance().log( \
+        event_name, \
+        data, \
+        __FILE__, \
+        __LINE__, \
+        __FUNCTION__, \
+        "")
+
+/**
+ * @brief 带节点名称的埋点宏
+ */
+#define TRIGGER_LOG_WITH_NODE(event_name, data, node_name) \
+    utils::TriggerLogger::getInstance().log( \
+        event_name, \
+        data, \
+        __FILE__, \
+        __LINE__, \
+        __FUNCTION__, \
+        node_name)
+
+} // namespace utils
+

src/common/3rd/x86/utils/include/utils/utils.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "string"
+
+#include <list>
+
+namespace utils {
+
+extern double legalizeAngle(double angle);
+extern double angleDiff(double a, double b);
+extern double angleAverage(std::list<double> angles);
+extern uint64_t format_time();
+}

src/common/3rd/x86/utils/include/utils/vl_common.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <string>
+
+#define UNUSED(name) ((void)name)
+#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0])
+
+namespace utils {
+
+/**
+ * @brief
+ * @param pidDirPath [in]存放pid的路径,如果运行中出错会强制退出程序
+ */
+extern void instanceRun(const std::string& pidDirPath);
+/**
+ * @brief get the process path
+ * @return success:name fault:empty
+ */
+extern std::string getProcessPath(void);
+/**
+ * @brief get the process name
+ * @return success:name fault:empty
+ */
+extern std::string getProcessName(void);
+/**
+ * @brief Check if the folder exists
+ * @param filePath[in] file path
+ * @return true
+ * @return false
+ */
+extern bool isFileExist(const std::string& filePath);
+/**
+ * @brief Read all data from the file into a string
+ * @param path[in] file path
+ * @param out[out] result
+ * @return true
+ * @return false
+ */
+extern bool readFileAllDataToString(const std::string& path, std::string& out);
+/**
+ * @brief write all data to path
+ * @param in[in] content
+ * @param path[in] file path
+ * @param isApp[in] is append
+ * @return true
+ * @return false
+ */
+extern bool writeStringAllDataToFile(const std::string& in, const std::string& path, bool isApp = false);
+/**
+ * @brief 返回uuid
+ * @return std::string
+ */
+extern std::string getUUID(void);
+/**
+ * @brief 随机生成字符串
+ * @param len 随机生成字符串的长度
+ * @return std::string
+ */
+extern std::string generateRandomString(size_t len);
+/**
+ * @brief hex打印
+ * @param data[in] 需要打印的数据
+ * @param size[in] 需要打印数据的字节个数
+ */
+extern void hexPrint(const char* data, size_t size);
+/**
+ * @brief hex打印
+ * @param data[in] 需要打印的数据
+ */
+extern void hexPrint(const std::string& data);
+/**
+ * @brief 时间字符串转unix时间戳
+ * @param timeStamp[in]时间字符串,格式为%d-%d-%d %d:%d:%d
+ * @return time_t
+ */
+extern time_t strTimeToUnixTimestamp(const std::string& timeStamp);
+
+}

src/common/3rd/x86/utils/include/utils/warning_type.h

@@ -0,0 +1,45 @@
+#pragma once
+//告警等级  (地面以上) 1 前向减速 2 侧向减速 3 后向减速 4 前方刹停 5 侧向刹停 6 后向刹停 7 驶离减速区 8 驶离刹停区
+//          (地面以下) 9 前向减速区 10 侧向减速区 11 前方刹停区 12 侧向刹停区  13 驶离减速区 14 驶离刹停区
+enum class WarningType {
+    //正常
+    FORWARD_WARNING = 1, // 前向减速
+    LATERAL_WARNING = 2, // 侧向减速
+    BACKWARD_WARNING = 3, // 后向减速
+    FORWARD_BRAKE_WARNING = 4, // 前方刹停
+    LATERAL_BRAKE_WARNING = 5, // 侧向刹停
+    BACKWARD_BRAKE_WARNING = 6, // 后向刹停
+    FORWARD_LEAVE_WARNING = 7, // 驶离减速区
+    LATERAL_LEAVE_WARNING = 8, // 驶离刹停区
+
+    FORWARD_NEGATIVE_PLANE_WARNING = 9, // 前向负平面减速区
+    LATERAL_NEGATIVE_PLANE_WARNING = 10, // 侧向负平面减速区
+    FORWARD_NEGATIVE_PLANE_BRAKE_WARNING = 11, // 前向负平面刹停区
+    LATERAL_NEGATIVE_PLANE_BRAKE_WARNING = 12, // 侧向负平面刹停区
+    FORWARD_NEGATIVE_PLANE_LEAVE_WARNING = 13, // 驶离负平面减速区
+    LATERAL_NEGATIVE_PLANE_LEAVE_WARNING = 14, // 驶离负平面刹停区
+};
+
+// 对应灯带效果
+enum class LightEffect {
+    FORWARD_WARNING = 1, // 前向减速
+    LATERAL_WARNING = 2, // 侧向减速
+    BACKWARD_WARNING = 3, // 后向减速
+    FORWARD_BRAKE_WARNING = 4, // 前方刹停
+    LATERAL_BRAKE_WARNING = 5, // 侧向刹停
+    BACKWARD_BRAKE_WARNING = 6, // 后向刹停
+    FORWARD_LEAVE_WARNING = 7, // 驶离减速区
+    LATERAL_LEAVE_WARNING = 8, // 驶离刹停区
+};
+
+// 对应语音效果
+enum class VoiceEffect {
+    FORWARD_WARNING = 1, // 前向减速
+    LATERAL_WARNING = 2, // 侧向减速
+    BACKWARD_WARNING = 3, // 后向减速
+    FORWARD_BRAKE_WARNING = 4, // 前方刹停
+    LATERAL_BRAKE_WARNING = 5, // 侧向刹停
+    BACKWARD_BRAKE_WARNING = 6, // 后向刹停
+    FORWARD_LEAVE_WARNING = 7, // 驶离减速区
+    LATERAL_LEAVE_WARNING = 8, // 驶离刹停区
+};

src/recharge/include/ipad_manager.hpp

@@ -24,6 +24,8 @@ public:
 
     // 状态查询（可选）
     bool isRechargeActive() const { return recharge_active_; }
+    void startRecharge();
+    void cancelRecharge();
 
 private:
     rclcpp::Node *node_;
@@ -36,9 +38,7 @@ private:
     bool recharge_active_;
     std::string cmd_enable_;
 
-    // 内部方法
-    void startRecharge();
-    void cancelRecharge();
+    
     void logInfo(const std::string &message);
 };
 

src/recharge/include/recharge_status.hpp

@@ -6,7 +6,7 @@ enum RechargeReportStatus
 {
     RECHARGE_EVENT_OBSTACLE_DETECTED = 1,   // 前往充电站中
     RECHARGE_EVENT_STATION_NOT_FOUND = 2,   // 未找到充电站
-    RECHARGE_FEEDBACK_SUCCESS_DOCKED = 3      // 成功对接
+    RECHARGE_FEEDBACK_SUCCESS_DOCKED = 100      // 成功对接
     // RECHARGE_EVENT_OBSTACLE_DETECTED = 4,   // 检测到障碍物
     // RECHARGE_EVENT_START_CHARGING = 5,      // 开始充电
     // RECHARGE_EVENT_STOP_CHARGING = 6        // 停止充电

src/recharge/include/report.hpp

@@ -15,6 +15,9 @@
 #include <std_msgs/msg/u_int32.hpp>
 #include "../../common/utils/topic.h"
 #include <interface/srv/scene_controls.hpp>
+#include "ipad_manager.hpp"
+#include <memory>
+
 
 class ReportManager
 {
@@ -22,7 +25,7 @@ public:
     using StatusReportCallback = std::function<void(const std::string &)>;
     using ErrorCallback = std::function<void(const ErrorInfo &)>;
 
-    ReportManager(rclcpp::Node *node);
+    ReportManager(rclcpp::Node *node,IpadManager* ipad);
 
     void handleRechargeService(const std::shared_ptr<interface::srv::SceneControls::Request> request, std::shared_ptr<interface::srv::SceneControls::Response> response);
     // 报告生成接口
@@ -74,6 +77,7 @@ public:
 
 private:
     rclcpp::Node *node_;
+    std::unique_ptr<IpadManager> _ipad;
     rclcpp::Publisher<std_msgs::msg::String>::SharedPtr report_publisher_;
     rclcpp::Service<interface::srv::SceneControls>::SharedPtr recharge_service_;     ///< 回充功能服务
     std::atomic<bool> recharge_mode_enabled_{false};        ///< 回充模式是否启用

src/recharge/src/event_input.cpp

@@ -27,10 +27,10 @@ void EventInputHandler::initializeModules()
     // 初始化工作流管理器
     workflow_manager_ = std::make_unique<WorkflowManager>(node_);
     // 初始化报告管理器
-    report_manager_ = std::make_unique<ReportManager>(node_);
+    report_manager_ = std::make_unique<ReportManager>(node_,ipad_manager_.get());
 
     setupModuleCallbacks();
-    setupSubscriptions();
+    // setupSubscriptions();
 
     RCLCPP_INFO(node_->get_logger(), "所有模块初始化完成");
 }
@@ -266,18 +266,19 @@ void EventInputHandler::setupModuleCallbacks()
     RCLCPP_INFO(node_->get_logger(), "模块回调设置完成");
 }
 
-void EventInputHandler::setupSubscriptions()
-{
-    // 创建命令使能订阅者
-    cmd_enable_sub_ = node_->create_subscription<std_msgs::msg::String>(
-        "/scene_services/mode", 10,
-        [this](const std_msgs::msg::String::SharedPtr msg)
-        {
-            this->ipad_manager_->onCmdEnable(msg);
-        });
-
-    RCLCPP_INFO(node_->get_logger(), "订阅器设置完成");
-}
+// void EventInputHandler::setupSubscriptions()
+// {
+//     // 创建命令使能订阅者
+//     // cmd_enable_sub_ = node_->create_subscription<std_msgs::msg::String>(
+//     //     "/scene_services/mode", 10,
+//     //     [this](const std_msgs::msg::String::SharedPtr msg)
+//     //     {
+//     //         this->ipad_manager_->onCmdEnable(msg);
+//     //     });
+//     report_manager_->
+
+//     RCLCPP_INFO(node_->get_logger(), "订阅器设置完成");
+// }
 
 void EventInputHandler::startNavigationControl()
 {

src/recharge/src/ipad_manager.cpp

@@ -10,24 +10,24 @@ IpadManager::IpadManager(rclcpp::Node *node)
     logInfo("iPad管理器初始化完成");
 }
 
-void IpadManager::onCmdEnable(const std_msgs::msg::String::SharedPtr msg)
-{
-    std::string new_enable = msg->data;
-    // 状态变化检查
-    if (cmd_enable_ == "auxiliary" && new_enable == "recharge")
-    {
-        cmd_enable_ = "recharge";
-        startRecharge();
-    }
-    else if (cmd_enable_ == "recharge" && new_enable == "auxiliary")
-    {
-        cmd_enable_ = "auxiliary";
-        cancelRecharge();
-    }
-    else
-    {
-    }
-}
+// void IpadManager::onCmdEnable(const std_msgs::msg::String::SharedPtr msg)
+// {
+//     std::string new_enable = msg->data;
+//     // 状态变化检查
+//     if (cmd_enable_ == "auxiliary" && new_enable == "recharge")
+//     {
+//         cmd_enable_ = "recharge";
+//         startRecharge();
+//     }
+//     else if (cmd_enable_ == "recharge" && new_enable == "auxiliary")
+//     {
+//         cmd_enable_ = "auxiliary";
+//         cancelRecharge();
+//     }
+//     else
+//     {
+//     }
+// }
 
 void IpadManager::startRecharge()
 {

src/recharge/src/report.cpp

@@ -9,8 +9,9 @@ std::map<uint32_t, std::string> ReportManager::error_descriptions_;
 std::map<ErrorModule, std::string> ReportManager::module_names_;
 std::map<ErrorSubModule, std::string> ReportManager::sub_module_names_;
 
-ReportManager::ReportManager(rclcpp::Node *node)
-    : node_(node)
+ReportManager::ReportManager(rclcpp::Node *node,IpadManager* ipad)
+    : node_(node),
+     _ipad(ipad)
 {
     // 创建报告发布者
     report_publisher_ = node_->create_publisher<std_msgs::msg::String>(
@@ -38,14 +39,14 @@ void ReportManager::handleRechargeService(
     if (request->start == 1) {
         recharge_mode_enabled_.store(true, std::memory_order_release);
         //TODO 启动回充
-
+        _ipad->startRecharge();
         RCLCPP_INFO(node_->get_logger(), "回充功能已开启 (启用执行器，记录最大线速度)");
         response->code = 0;
         response->message = "回充功能已开启";
     } else {
         recharge_mode_enabled_.store(false, std::memory_order_release);
         //TODO 关闭回充
-
+        _ipad->cancelRecharge();
         RCLCPP_INFO(node_->get_logger(), "回充功能已关闭");
         response->code = 0;
         response->message = "回充功能已关闭";