compliete initbase

vision_test/configs/error_configs/report_logging_define.json

@@ -0,0 +1,31 @@
+{
+  "info": {},
+
+  "warring": {
+    "0000": "Success",
+
+    "0100": "Cannot read calibration file, use E(4, 4)",
+
+    "1000": "Detected object count is 0",
+    "1001": "Depth crop is None",
+    "1003": "Failed to detect a valid pose",
+
+    "1100": "Object point cloud contains excessive noise",
+    "1101": "The point cloud is empty",
+
+    "1200": "The number of points is insufficient to compute an OBB",
+    "1201": "PCA output vector is None",
+    "1202": "This pose cannot be grab, and position refine fail",
+
+    "1300": "E2E model input data 'coors' are fewer than 128",
+    "1301": "E2E model input data 'point_clouds' are fewer than 128",
+    "1302": "The 'true num' of points is 0; No graspable points are available",
+    "1303": "The model returned no predictions",
+    "1304": "All rotation vector processing failed; no valid rotation matrix was generated"
+  },
+
+  "error": {},
+
+  "fatal": {}
+}
+

vision_test/include/eigen-3.4.1/.gitignore

@@ -1,41 +0,0 @@
-qrc_*cxx
-*.orig
-*.pyc
-*.diff
-diff
-*.save
-save
-*.old
-*.gmo
-*.qm
-core
-core.*
-*.bak
-*~
-*build*
-*.moc.*
-*.moc
-ui_*
-CMakeCache.txt
-tags
-.*.swp
-activity.png
-*.out
-*.php*
-*.log
-*.orig
-*.rej
-log
-patch
-*.patch
-a
-a.*
-lapack/testing
-lapack/reference
-.*project
-.settings
-Makefile
-!ci/build.gitlab-ci.yml
-!scripts/buildtests.in
-!Eigen/Core
-!Eigen/src/Core

vision_test/include/eigen-3.4.1/.gitlab-ci.yml

@@ -1,34 +0,0 @@
-# This file is part of Eigen, a lightweight C++ template library
-# for linear algebra.
-#
-# Copyright (C) 2023, The Eigen Authors
-#
-# This Source Code Form is subject to the terms of the Mozilla
-# Public License v. 2.0. If a copy of the MPL was not distributed
-# with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-stages:
-  - checkformat
-  - build
-  - test
-  - deploy
-
-variables:
-  # CMake build directory.
-  EIGEN_CI_BUILDDIR: .build
-  # Specify the CMake build target.
-  EIGEN_CI_BUILD_TARGET: ""
-  # If a test regex is specified, that will be selected.
-  # Otherwise, we will try a label if specified.
-  EIGEN_CI_CTEST_REGEX: ""
-  EIGEN_CI_CTEST_LABEL: ""
-  EIGEN_CI_CTEST_ARGS: ""
-
-include:
-  - "/ci/checkformat.gitlab-ci.yml"
-  - "/ci/common.gitlab-ci.yml"
-  - "/ci/build.linux.gitlab-ci.yml"
-  - "/ci/build.windows.gitlab-ci.yml"
-  - "/ci/test.linux.gitlab-ci.yml"
-  - "/ci/test.windows.gitlab-ci.yml"
-  - "/ci/deploy.gitlab-ci.yml"

vision_test/include/vision_test/detect_node.hpp

@@ -1,12 +0,0 @@
-
-
-
-
-#include "vision_test/init_base.hpp"
-
-
-class DetectNode: public InitBase {
-    public:
-        explicit DetectNode(const std::string name);
-
-};

vision_test/include/vision_test/init_base.hpp

@@ -1,41 +0,0 @@
-
-
-#include "vision_test/config_base.hpp"
-
-
-class InitBase: public ConfigBase {
-    public:
-        explicit InitBase(const std::string name);
-
-    protected:
-        // Functions
-        void init_model();
-        void init_publisher();
-        void init_subscriber();
-        void init_service();
-        void init_sync_sub();
-
-        virtual void camera_info_callback(
-            const sensor_msgs::msg::CameraInfo::SharedPtr &msg
-        );
-        
-        virtual void sync_sub_callback(
-            const sensor_msgs::msg::Image::ConstSharedPtr &color_msg,
-            const sensor_msgs::msg::Image::ConstSharedPtr &depth_msg
-        );
-
-        virtual void service_sub_callback(
-            const sensor_msgs::msg::Image::ConstSharedPtr &msg
-        );
-
-        virtual void service_callback(
-            const std::shared_ptr<interfaces::srv::VisionObjectRecognition::Request> &request,
-            std::shared_ptr<interfaces::srv::VisionObjectRecognition::Response> response
-        );
-        
-        virtual void seg_image();
-        virtual void seg_color();
-        virtual void seg_crossboard();
-};
-
-

vision_test/include/vision_test/node.h

@@ -0,0 +1,5 @@
+#pragma once
+
+
+#include "vision_test/node/detect_node.hpp"
+

vision_test/include/vision_test/node/config_base.hpp

@@ -1,7 +1,11 @@
+#pragma once
+
 #include <string>
 #include <vector>
 #include <array>
 #include <cstdint>
+#include <functional>
+#include <unordered_map>
 
 #include <nlohmann/json.hpp>
 #include <Eigen/Dense>
@@ -15,10 +19,7 @@
 
 #include "interfaces/srv/vision_object_recognition.hpp"
 #include "interfaces/msg/pose_array_class_and_id.hpp"
-
-#include "ament_index_cpp/get_package_share_directory.hpp"
-
-#include "vision_test/utils/read_file_tools.hpp"
+#include "interfaces/msg/img_msg.hpp"
 
 
 namespace policy {
@@ -28,6 +29,8 @@ namespace policy {
 
 namespace path {
     extern std::string shared_path;
+    inline std::string logging_define_path = "configs/error_configs/report_logging_define.json";
+    
 }
 
 
@@ -49,32 +52,17 @@ class ConfigBase: public rclcpp::Node {
         std::string position;
         std::string checkpoint_path;
 
+        std::unordered_map<std::string, std::string> logging_code_map;
+
         float confidence;
+        std::vector<int> classes;
+
         std::uint16_t distance;
+        std::vector<std::vector<std::vector<int>>> color_range;
 
         std::array<int, 2> pattern_size;
-        std::array<double, 9> k;
-        std::array<double, 5> d;
-
-        Eigen::Matrix4d left_hand_mat, right_hand_mat, head_mat, hand_eye_mat;
-
-        void (ConfigBase::*detect_function)();
-        void (ConfigBase::*calculate_function)();
-
-        rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_detect_img;
-        rclcpp::Publisher<interfaces::msg::PoseArrayClassAndID>::SharedPtr pub_pose_list;
-
-        message_filters::Subscriber<sensor_msgs::msg::Image> color_img_sub;
-        message_filters::Subscriber<sensor_msgs::msg::Image> depth_img_sub;
-        std::shared_ptr<policy::SyncPolicy> sync_sub;
-
-        rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub;
-        rclcpp::Service<interfaces::srv::VisionObjectRecognition>::SharedPtr detect_server;
-
-        std::vector<std::vector<std::vector<int>>> color_range;
-        std::vector<int> classes;
         
-        // Functions
-        void read_calibration_mat();
-
+        int camera_size[2];
+        std::array<double, 9> k;
+        std::vector<double> d;       
 };

vision_test/include/vision_test/node/detect_node.hpp

@@ -0,0 +1,38 @@
+#pragma once
+
+
+
+#include "vision_test/node/init_base.hpp"
+
+
+class DetectNode: public InitBase {
+    public:
+        explicit DetectNode(const std::string name);
+
+    private:
+        // Functions
+        void camera_info_callback(
+            const sensor_msgs::msg::CameraInfo::SharedPtr &msg
+        ) override;
+        
+        void sync_sub_callback(
+            const sensor_msgs::msg::Image::ConstSharedPtr &color_msg,
+            const sensor_msgs::msg::Image::ConstSharedPtr &depth_msg
+        ) override;
+
+        void service_sub_callback(
+            const sensor_msgs::msg::Image::ConstSharedPtr &msg
+        ) override;
+
+
+        std::vector<Eigen::Matrix4d> seg_image(
+            const cv::Mat &color, const cv::Mat &depth, int &code
+        ) override;
+        std::vector<Eigen::Matrix4d> seg_color(
+            const cv::Mat &color, const cv::Mat &depth, int &code
+        ) override;
+        std::vector<Eigen::Matrix4d> seg_crossboard(
+            const cv::Mat &color, const cv::Mat &depth, int &code
+        ) override;
+
+};

vision_test/include/vision_test/node/init_base.hpp

@@ -0,0 +1,90 @@
+#pragma once
+
+#include "opencv2/opencv.hpp"
+#include "vision_test/node/config_base.hpp"
+
+
+class InitBase: public ConfigBase {
+    public:
+        explicit InitBase(const std::string name);
+
+    protected:
+        std::promise<bool> camera_info_promise_;
+        std::shared_future<bool> camera_info_future_;
+        bool future_ready_;
+
+        void init_flow();
+
+        // Functions
+        void read_calibration_mat();
+        void init_model();
+        void init_publisher();
+        void init_subscriber();
+        void init_service();
+        void init_sync_sub();
+
+        virtual void camera_info_callback(
+            const sensor_msgs::msg::CameraInfo::SharedPtr &msg
+        ) = 0;
+        
+        virtual void sync_sub_callback(
+            const sensor_msgs::msg::Image::ConstSharedPtr &color_msg,
+            const sensor_msgs::msg::Image::ConstSharedPtr &depth_msg
+        ) = 0;
+
+        virtual void service_sub_callback(
+            const sensor_msgs::msg::Image::ConstSharedPtr &msg
+        ) = 0;
+
+        virtual void service_callback(
+            const std::shared_ptr<interfaces::srv::VisionObjectRecognition::Request> &request,
+            std::shared_ptr<interfaces::srv::VisionObjectRecognition::Response> response
+        ) = 0;
+        
+        virtual std::vector<Eigen::Matrix4d> seg_image(
+            const cv::Mat &color, const cv::Mat &depth, int &code
+        ) = 0;
+        virtual std::vector<Eigen::Matrix4d> seg_color(
+            const cv::Mat &color, const cv::Mat &depth, int &code
+        ) = 0;
+        virtual std::vector<Eigen::Matrix4d> seg_crossboard(
+            const cv::Mat &color, const cv::Mat &depth, int &code
+        ) = 0;
+
+
+        std::function<std::vector<Eigen::Matrix4d>(
+            const cv::Mat&, const cv::Mat&, int&
+        )> detect_function;
+
+        std::function<std::vector<Eigen::Matrix4d>(
+            const cv::Mat&, const cv::Mat&, const double(&)[6], const nlohmann::json&, int&
+        )> calculate_function;
+
+
+        Eigen::Matrix4d left_hand_mat, right_hand_mat, head_mat, hand_eye_mat;
+
+        rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_detect_img;
+        rclcpp::Publisher<interfaces::msg::PoseArrayClassAndID>::SharedPtr pub_pose_list;
+
+        std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> color_img_sub;
+        std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>> depth_img_sub;
+        std::shared_ptr<message_filters::Synchronizer<policy::SyncPolicy>> sync_sub;
+
+        rclcpp::Subscription<interfaces::msg::ImgMsg>::SharedPtr sub_img;
+        rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub;
+
+        rclcpp::Service<interfaces::srv::VisionObjectRecognition>::SharedPtr detect_server;
+
+        private:
+            using ConfigBase::get_camera_mode; 
+            using ConfigBase::detect_mode;
+            using ConfigBase::calculate_mode;
+            using ConfigBase::depth_topic_name;
+            using ConfigBase::color_topic_name;
+            using ConfigBase::camera_info_topic_name;
+            using ConfigBase::checkpoint_path;
+            using ConfigBase::service_name;
+            using ConfigBase::topic_name;
+};
+
+

vision_test/include/vision_test/os.hpp

@@ -0,0 +1,4 @@
+#pragma once
+
+
+#include "vision_test/os/join.hpp"

vision_test/include/vision_test/os/join.hpp

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <string>
+#include <filesystem>
+
+
+namespace os::path {
+    inline std::string join(const std::string &path1, const std::string &path2) {
+        if (path2.empty()) return path1;
+        if (path2.front() == '/') {  // path2 是绝对路径，直接返回
+            return path2;
+        }
+
+        if (path1.empty()) return path2;
+
+        std::string result = path1;
+        // 确保 path1 末尾有 /
+        if (result.back() != '/') result += '/';
+        result += path2;
+        return result;
+    }
+}

vision_test/include/vision_test/ultralytics/inference/model_onnxruntime.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <string>
+#include <vector>
+#include <tuple>
+
+#include <opencv2/opencv.hpp>
+#include <onnxruntime_cxx_api.h>
+
+
+namespace inference {
+
+    class OnnxRuntimeInference {
+        public:
+            explicit OnnxRuntimeInference(const std::string& checkpoint_path);
+            
+            std::tuple<Ort::Value, Ort::Value> forward(const cv::Mat &img);
+
+        private:
+
+            Ort::Env env;
+            Ort::SessionOptions session_options;
+            Ort::Session session;
+            Ort::MemoryInfo mem_info;
+            
+    };  
+}

vision_test/include/vision_test/utils.hpp

@@ -0,0 +1,5 @@
+#pragma once
+
+#include "vision_test/utils/calculate_tools.hpp"
+#include "vision_test/utils/read_json.hpp"
+#include "vision_test/utils/format04d.hpp"

vision_test/include/vision_test/utils/calculate_tools.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <vector>
+
+#include <Eigen/Dense>
+#include <opencv2/opencv.hpp>
+#include <nlohmann/json.hpp>
+
+
+namespace utils::calculate {
+    std::vector<Eigen::Matrix4d> calculate_pose_pca(
+        const cv::Mat &color,
+        const cv::Mat &depth,
+        const double (&k)[6],
+        const nlohmann::json &kwargs,
+        int &code
+    );
+
+
+    std::vector<Eigen::Matrix4d> calculate_pose_icp(
+        const cv::Mat &color,
+        const cv::Mat &depth,
+        const double (&k)[6],
+        const nlohmann::json &kwargs,
+        int &code
+    );
+
+
+    std::vector<Eigen::Matrix4d> calculate_pose_e2e(
+        const cv::Mat &color,
+        const cv::Mat &depth,
+        const double (&k)[6],
+        const nlohmann::json &kwargs,
+        int &code
+    );
+}

vision_test/include/vision_test/utils/format04d.hpp

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <cstdio>
+#include <string>
+
+namespace utils::format {
+    inline std::string format04d(int code)
+    {
+        char buf[16];
+        std::snprintf(buf, sizeof(buf), "%04d", code);
+
+        return std::string(buf);
+    }
+}

vision_test/include/vision_test/utils/image_tools.hpp

@@ -0,0 +1,13 @@
+#pragma once
+
+#include <array>
+#include <tuple>
+#include <vector>
+#include "opencv2/opencv.hpp"
+
+std::tuple<cv::Mat, cv::Mat>
+get_map(
+    std::array<double, 9> &K,
+    const std::vector<double> &D,
+    const int &camera_size
+);

vision_test/include/vision_test/utils/read_file_tools.hpp

@@ -1,6 +0,0 @@
-#include <string>
-
-#include <Eigen/Dense>
-
-
-Eigen::Matrix4d read_data_from_json(const std::string &file_path, std::string &info, bool &sign);

vision_test/include/vision_test/utils/read_json.hpp

@@ -0,0 +1,13 @@
+#pragma once
+
+#include <string>
+#include <unordered_map>
+
+#include <Eigen/Dense>
+
+
+namespace utils::io::json {
+    Eigen::Matrix4d load_calibration_mat(const std::string &file_path, int &code);
+
+    std::unordered_map<std::string, std::string> load_error_map(const std::string& path);
+}

vision_test/include/vision_test/vision_detect_node.hpp

@@ -1,3 +1,5 @@
+#pragma once
+
 #include <string>
 #include <nlohmann/json.hpp>
 #include <Eigen/Dense>

vision_test/src/detect_node.cpp

@@ -1,8 +0,0 @@
-
-
-#include "vision_test/detect_node.hpp"
-
-
-DetectNode::DetectNode(const std::string name): InitBase(name) {
-
-}

vision_test/src/init_base.cpp

@@ -1,10 +0,0 @@
-
-
-#include "vision_test/init_base.hpp"
-
-
-
-InitBase::InitBase(const std::string name): ConfigBase(name) {
-
-    
-}

vision_test/src/main.cpp

@@ -1,7 +1,9 @@
 #include <iostream>
 #include <string>
 #include "rclcpp/rclcpp.hpp"
-#include "vision_test/detect_node.hpp"
+#include "ament_index_cpp/get_package_share_directory.hpp"
+
+#include "vision_test/node/detect_node.hpp"
 
 
 int main(int argc, char ** argv) {

vision_test/src/node/config_base.cpp

@@ -2,7 +2,10 @@
 #include <algorithm>
 #include <nlohmann/json.hpp>
 
-#include "vision_test/config_base.hpp"
+#include "vision_test/os.hpp"
+#include "vision_test/utils.hpp"
+
+#include "vision_test/node/config_base.hpp"
 
 
 namespace path {
@@ -11,116 +14,67 @@ namespace path {
 
 
 ConfigBase::ConfigBase(const std::string name): Node(name) {
+    this->logging_code_map = utils::io::json::load_error_map(
+        os::path::join(path::shared_path, path::logging_define_path)
+    );
+
+    
     this->declare_parameter("configs_path", "configs/launch_configs/default_config.json");
     std::string config_path = this->get_parameter("configs_path").as_string();
-
-    std::ifstream file(config_path);
+    std::ifstream file(os::path::join(path::shared_path, config_path));
     file >> this->configs;
 
+
     this->output_box = (this->configs["output_box"].get<std::string>()) == "True";
     this->output_mask = (this->configs["output_mask"].get<std::string>()) == "True";
-
     this->get_camera_mode = this->configs["get_camera_mode"].get<std::string>();
     this->detect_mode = this->configs["detect_mode"].get<std::string>();
     this->calculate_mode = this->configs["calculate_mode"].get<std::string>();
 
-    this->read_calibration_mat();
 
     if (this->get_camera_mode == "Service") {
         this->service_name = this->configs["service_name"].get<std::string>();
-    }
-    else if (this->get_camera_mode == "Topic") {
+    } else if (this->get_camera_mode == "Topic") {
         this->topic_name = this->configs["topic_name"].get<std::string>();
         this->depth_topic_name = this->configs["depth_topic_name"].get<std::string>();
         this->color_topic_name = this->configs["color_topic_name"].get<std::string>();
         this->camera_info_topic_name = this->configs["camera_info_topic_name"].get<std::string>();
-    }
-    else {
+    } else {
         RCLCPP_WARN(this->get_logger(), "Unknown get_camera_mode, Use Service");
         this->service_name = this->configs["service_name"].get<std::string>();
     }
 
+
     if (this->detect_mode == "Detect") {
         this->confidence = this->configs["confidence"].get<float>();
         this->checkpoint_path = this->configs["checkpoint_path"].get<std::string>();
         this->classes = this->configs["classes"].get<std::vector<int>>();
-    }
-    else if (this->detect_mode == "Color") {
+    } else if (this->detect_mode == "Color") {
         this->color_range = this->configs["color_range"].get<std::vector<std::vector<std::vector<int>>>>();
         this->distance = this->configs["distance"].get<std::uint16_t>();
-    }
-    else if (this->detect_mode == "Crossboard") {
+    } else if (this->detect_mode == "Crossboard") {
         this->pattern_size = this->configs["pattern_size"].get<std::array<int, 2>>();
-    }
-    else {
+    } else {
         RCLCPP_WARN(this->get_logger(), "Unknown detect_mode, Use Detect");
         this->confidence = this->configs["confidence"].get<float>();
         this->checkpoint_path = this->configs["checkpoint_path"].get<std::string>();
         this->classes = this->configs["classes"].get<std::vector<int>>();
     }
 
+
     if (this->calculate_mode == "PCA") {
         this->calculate_configs = this->configs["PCA_configs"];
-    }
-    else if (this->calculate_mode == "ICP") {
+    } else if (this->calculate_mode == "ICP" && this->detect_mode == "Detect") {
         this->calculate_configs = this->configs["ICP_configs"];
-        // source
-    }
-    else {
-        RCLCPP_WARN(this->get_logger(), "Unknown calculate_mode, Use PCA");
+        // source to be added
+    } else if (this->calculate_mode == "E2E" && this->detect_mode == "Detect") {
+        this->calculate_configs = this->configs["E2E_configs"];
+        // wait to be added
+    } else {
+        RCLCPP_WARN(this->get_logger(), "Unknown calculate_mode or not Detect mode, Use PCA");
         this->calculate_configs = this->configs["PCA_configs"];
     }
+
     
     RCLCPP_INFO(this->get_logger(), "Get parameters done"); 
-    // Done
 }
-
-
-void ConfigBase::read_calibration_mat() {
-    /*Init Calibration Mat*/
-    std::string info;
-    bool sign;
-
-    info = "left";
-    this->left_hand_mat = read_data_from_json(
-        this->configs["calibration"]["left_hand"].get<std::string>(), 
-        info, 
-        sign
-    );
-    if (sign) {
-        RCLCPP_INFO(this->get_logger(), info.c_str());
-    }
-    else {
-        RCLCPP_WARN(this->get_logger(), info.c_str());
-    }
-
-    info = "right";
-    this->right_hand_mat = read_data_from_json(
-        this->configs["calibration"]["right_hand"].get<std::string>(), 
-        info, 
-        sign
-    );
-    if (sign) {
-        RCLCPP_INFO(this->get_logger(), info.c_str());
-    }
-    else {
-        RCLCPP_WARN(this->get_logger(), info.c_str());
-    }
-
-    info = "head";
-    this->head_mat = read_data_from_json(
-        this->configs["calibration"]["head"].get<std::string>(), 
-        info, 
-        sign
-    );
-    if (sign) {
-        RCLCPP_INFO(this->get_logger(), info.c_str());
-    }
-    else {
-        RCLCPP_WARN(this->get_logger(), info.c_str());
-    }
-    // Done
-}
-
-
-

vision_test/src/node/detect_node.cpp

@@ -0,0 +1,40 @@
+
+
+#include "vision_test/node/detect_node.hpp"
+
+
+DetectNode::DetectNode(const std::string name): InitBase(name) {
+
+}
+
+void DetectNode::camera_info_callback(
+    const sensor_msgs::msg::CameraInfo::SharedPtr &msg
+) {
+    // 1. 读取 K 和 D
+    k = msg->k;
+    d = msg->d;
+
+    // 2. 图像尺寸
+    camera_size[0] = msg->width;
+    camera_size[1] = msg->height;
+
+    // 3. 判断有效性（等价于 Python len(...) > 0）
+    if (!k.empty() && !d.empty()) {
+
+        // 4. 计算 map
+        // std::tie(map1, map2, k) =
+        //     get_map(k, d, camera_size);
+
+        // 5. future 未完成 → set_value
+        if (!future_ready_) {
+            camera_info_promise_.set_value(true);
+            this->future_ready_ = true;
+        }
+
+        // 6. 销毁 subscription（只触发一次）
+        camera_info_sub.reset();
+    }
+    else {
+        RCLCPP_WARN(this->get_logger(), "K and d are not defined");
+    }
+}

vision_test/src/node/init_base.cpp

@@ -0,0 +1,284 @@
+#include <fstream>
+#include <algorithm>
+#include <nlohmann/json.hpp>
+
+#include "message_filters/subscriber.hpp"
+#include "message_filters/sync_policies/approximate_time.hpp"
+
+#include "vision_test/os.hpp"
+#include "vision_test/utils.hpp"
+
+#include "interfaces/msg/img_msg.hpp"
+#include "interfaces/msg/pose_array_class_and_id.hpp"
+#include "interfaces/srv/vision_object_recognition.hpp"
+
+#include "vision_test/node/init_base.hpp"
+
+
+
+InitBase::InitBase(const std::string name): ConfigBase(name) {
+    
+    
+}
+
+
+void InitBase::init_flow() {
+    /*Init Flow*/
+    this->camera_info_future_ = camera_info_promise_.get_future().share();
+    this->read_calibration_mat();
+    this->init_publisher();
+
+    // choose detect model
+    if (get_camera_mode == "Service") {
+        this->init_service();
+        this->init_subscriber();
+    } else if (get_camera_mode == "Topic") {
+        // choose witch position of camera
+        if (position == "left") {
+            hand_eye_mat = left_hand_mat;
+        } else if (position == "right") {
+            hand_eye_mat = right_hand_mat;
+        } else {
+            hand_eye_mat = head_mat;
+        }
+        this->init_sync_sub();
+    } else {
+        this->init_service();
+        this->init_subscriber();
+    }
+
+
+    if (detect_mode == "Detect") {
+        this->detect_function = 
+            [this] (const cv::Mat &c, const cv::Mat &d, int &code) {
+                return this->seg_image(c, d, code);
+            };
+        // if not self.classes:
+        //     self.classes = None
+        this->init_model();
+    } else if (detect_mode == "Color") {
+        this->detect_function = 
+            [this] (const cv::Mat &c, const cv::Mat &d, int &code) {
+                return this->seg_color(c, d, code);
+            };
+    } else if (detect_mode == "CrossBoard") {
+        this->detect_function = 
+            [this] (const cv::Mat &c, const cv::Mat &d, int &code) {
+                return this->seg_crossboard(c, d, code);
+            };
+    } else {
+        this->detect_function = 
+            [this] (const cv::Mat &c, const cv::Mat &d, int &code) {
+                return this->seg_image(c, d, code);
+            };
+        // if not self.classes:
+        //     self.classes = None
+        this->init_model();
+    }
+
+
+    if (calculate_mode == "PCA") {
+        this->calculate_function = 
+            [this] (
+                const cv::Mat &c, 
+                const cv::Mat &d, 
+                const double (&k)[6], 
+                const nlohmann::json &cam_data, 
+                int &code
+            ) {
+                return utils::calculate::calculate_pose_pca(c, d, k, cam_data, code);
+            };
+    } else if (calculate_mode == "ICP" && this->detect_mode == "Detect") {
+        this->calculate_function = 
+            [this] (
+                const cv::Mat &c, 
+                const cv::Mat &d, 
+                const double (&k)[6], 
+                const nlohmann::json &cam_data, 
+                int &code
+            ) {
+                return utils::calculate::calculate_pose_icp(c, d, k, cam_data, code);
+            };    
+    } else if (calculate_mode == "E2E" && this->detect_mode == "Detect") {
+        // e2e_model wait to implement
+        this->calculate_function = 
+            [this] (
+                const cv::Mat &c, 
+                const cv::Mat &d, 
+                const double (&k)[6], 
+                const nlohmann::json &cam_data, 
+                int &code
+            ) {
+                return utils::calculate::calculate_pose_e2e(c, d, k, cam_data, code);
+            };
+    } else {
+        this->calculate_function = 
+            [this] (
+                const cv::Mat &c, 
+                const cv::Mat &d, 
+                const double (&k)[6], 
+                const nlohmann::json &cam_data, 
+                int &code
+            ) {
+                return utils::calculate::calculate_pose_pca(c, d, k, cam_data, code);
+            };
+    }
+
+    RCLCPP_INFO(this->get_logger(), "Initialization complete.");
+}
+
+
+void InitBase::read_calibration_mat() {
+    /*Init Calibration Mat*/
+    int code;
+
+    // read left
+    this->left_hand_mat = utils::io::json::load_calibration_mat(
+        os::path::join(
+            path::shared_path, 
+            this->configs["calibration"]["left_hand"].get<std::string>()
+        ), 
+        code
+    );
+    if (code == 0) {
+        RCLCPP_INFO(this->get_logger(), "left: Success read calibration file");
+    } else {
+        RCLCPP_WARN(
+            this->get_logger(), 
+            "left: %s", 
+            this->logging_code_map[utils::format::format04d(code)].c_str()
+        );
+    }
+
+    // read right
+    this->right_hand_mat = utils::io::json::load_calibration_mat(
+        os::path::join(
+            path::shared_path, 
+            this->configs["calibration"]["right_hand"].get<std::string>()
+        ), 
+        code
+    );
+    if (code == 0) {
+        RCLCPP_INFO(this->get_logger(), "right: Success read calibration file");
+    } else {
+        RCLCPP_WARN(
+            this->get_logger(), 
+            "right: %s", 
+            this->logging_code_map[utils::format::format04d(code)].c_str()
+        );
+    }
+
+    // read head
+    this->head_mat = utils::io::json::load_calibration_mat(
+        os::path::join(
+            path::shared_path, 
+            this->configs["calibration"]["head"].get<std::string>()
+        ), 
+        code
+    );
+    if (code == 0) {
+        RCLCPP_INFO(this->get_logger(), "head: Success read calibration file");
+    } else {
+        RCLCPP_WARN(
+            this->get_logger(), 
+            "head: %s", 
+            this->logging_code_map[utils::format::format04d(code)].c_str()
+        );
+    }
+    // Done
+}
+
+
+void InitBase::init_model() {
+    /*load detect model*/
+    checkpoint_path = os::path::join(path::shared_path, checkpoint_path);
+    
+    // wait to implement
+}
+
+
+void InitBase::init_publisher() {
+    /*Init Publisher*/
+    if (output_box || output_mask) {
+        this->pub_detect_img = this->create_publisher<sensor_msgs::msg::Image>(
+            "/image/detect_image", 10
+        );
+    }
+
+    if (get_camera_mode == "Topic") {
+        this->pub_pose_list = this->create_publisher<interfaces::msg::PoseArrayClassAndID>(
+            "/pose/cv_detect_pose", 10
+        );
+    }
+}
+
+
+void InitBase::init_subscriber() {
+    /*Init Subscriber*/
+    this->sub_img = this->create_subscription<interfaces::msg::ImgMsg>(
+        "/img_msg", 10, std::bind(
+            &InitBase::service_sub_callback,
+            this, 
+            std::placeholders::_1
+        )
+    );
+}
+
+
+void InitBase::init_service() {
+    /*Init Service*/
+    this->detect_server = this->create_service<interfaces::srv::VisionObjectRecognition>(
+        this->service_name,
+        std::bind(
+            &InitBase::service_callback,
+            this, 
+            std::placeholders::_1, 
+            std::placeholders::_2
+        )
+    );
+}
+
+
+void InitBase::init_sync_sub() {
+    /*Init Sync Subscriber*/
+    this->camera_info_sub = this->create_subscription<sensor_msgs::msg::CameraInfo>(
+        this->camera_info_topic_name, 
+        10, 
+        std::bind(
+            &InitBase::camera_info_callback,
+            this, 
+            std::placeholders::_1
+        )
+    );
+
+    future_ready_ = false;
+    RCLCPP_INFO(this->get_logger(), "Waiting for camera info...");
+    rclcpp::spin_until_future_complete(
+        this->get_node_base_interface(),
+        camera_info_future_
+    );
+    RCLCPP_INFO(this->get_logger(), "Camera info received.");
+
+    // sync subscriber
+    this->color_img_sub = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::Image>>(
+        this, 
+        this->color_topic_name
+    );
+    this->depth_img_sub = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::Image>>(
+        this, 
+        this->depth_topic_name
+    );
+    this->sync_sub = std::make_shared<message_filters::Synchronizer<policy::SyncPolicy>>(
+        policy::SyncPolicy(10), 
+        *this->color_img_sub, 
+        *this->depth_img_sub
+    );
+    this->sync_sub->registerCallback(
+        std::bind(
+            &InitBase::sync_sub_callback,
+            this, 
+            std::placeholders::_1, 
+            std::placeholders::_2
+        )
+    );
+}

vision_test/src/read_file_tools.cpp

@@ -1,49 +0,0 @@
-#include <string>
-#include <fstream>
-#include <Eigen/Dense>
-#include <nlohmann/json.hpp>
-
-#include "vision_test/utils/read_file_tools.hpp"
-
-
-Eigen::Matrix4d read_data_from_json(const std::string &file_path, std::string &info, bool &sign) {
-    Eigen::Matrix4d mat; 
-    nlohmann::json jfile;
-    std::ifstream file(file_path);
-    if(!file.is_open()) {
-        if (info == "right") {
-            info = "Cannot open right_hand file! use E(4, 4)";
-        }
-        else if (info == "left") {
-            info = "Cannot open left_hand file! use E(4, 4)";
-        }
-        else {
-            info = "Cannot open head file! use E(4, 4)";
-        }
-        mat = Eigen::Matrix4d::Identity();
-        sign = false;
-    }
-    else {
-        file >> jfile;
-        std::vector<std::vector<double>> data = jfile["T"];
-        for(int i = 0; i < 4; i++) {
-            for(int j = 0; j < 4; j++) {
-                mat(i, j) = data[i][j];
-            }
-        }
-        if (info == "right") {
-            info = "Success read right_hand data";
-        }
-        else if (info == "left") {
-            info = "Success read left_hand data";
-        }
-        else {
-            info = "Success read head data";
-        }
-        sign = true;
-    }
-    return mat;
-}    
-    
-    
-    

vision_test/src/ultralytics/inference/model_onnxruntime.cpp

@@ -0,0 +1,63 @@
+#include "vision_test/ultralytics/inference/model_onnxruntime.hpp"
+
+
+
+namespace {
+
+    Ort::SessionOptions make_session_options()
+    {
+        Ort::SessionOptions opts;
+        opts.SetGraphOptimizationLevel(
+            GraphOptimizationLevel::ORT_ENABLE_ALL
+        );
+        return opts;
+    }
+
+    void get_input_shape(const Ort::Session &session, std::vector<int64_t> &dims) {
+        size_t num_inputs = session.GetInputCount();
+        for (size_t i = 0; i < num_inputs; ++i) {
+            Ort::TypeInfo type_info = session.GetInputTypeInfo(i);
+            auto tensor_info = type_info.GetTensorTypeAndShapeInfo();
+            dims = tensor_info.GetShape();
+        }
+    }
+}
+
+
+namespace inference {
+
+    OnnxRuntimeInference::OnnxRuntimeInference(const std::string& checkpoint_path) : 
+    env(ORT_LOGGING_LEVEL_WARNING, "segmentation_model"),
+    session_options(make_session_options()),
+    session(env, checkpoint_path.c_str(), session_options),
+    mem_info(Ort::MemoryInfo::CreateCpu(OrtArenaAllocator, OrtMemTypeDefault))
+    {
+        session_options.SetGraphOptimizationLevel(GraphOptimizationLevel::ORT_ENABLE_ALL);
+         
+        
+    }
+
+
+    std::tuple<Ort::Value, Ort::Value> OnnxRuntimeInference::forward(const cv::Mat &img) {
+        std::vector<int64_t> input_shape = {1, 3, 640, 640};
+
+
+        Ort::AllocatorWithDefaultOptions allocator;
+        Ort::AllocatedStringPtr input_name_ptr = session.GetInputNameAllocated(0, allocator);
+        const char* input_name = input_name_ptr.get();
+
+        Ort::AllocatedStringPtr detection_name = session.GetOutputNameAllocated(0, allocator);
+        Ort::AllocatedStringPtr prototype_name = session.GetOutputNameAllocated(1, allocator);
+        const char* output_name[] = {detection_name.get(), prototype_name.get()};
+        
+        auto output_tensors = session.Run(
+            Ort::RunOptions{nullptr},
+            &input_name,
+            &input_tensor,
+            1,
+            output_name,
+            2
+        );
+    }
+}
+

vision_test/src/utils/read_json.cpp

@@ -0,0 +1,44 @@
+#include <fstream>
+#include <nlohmann/json.hpp>
+
+#include "vision_test/utils/read_json.hpp"
+
+
+namespace utils::io::json {
+    Eigen::Matrix4d load_calibration_mat(const std::string &file_path, int &code) {
+        Eigen::Matrix4d mat;    
+        nlohmann::json jfile;
+        std::ifstream file(file_path);
+        if(!file.is_open()) {
+            code = 100;
+            mat = Eigen::Matrix4d::Identity();
+        }
+        else {
+            file >> jfile;
+            std::vector<std::vector<double>> data = jfile["T"];
+            for(int i = 0; i < 4; i++) {
+                for(int j = 0; j < 4; j++) {
+                    mat(i, j) = data[i][j];
+                }
+            }
+            code = 0;
+        }
+        return mat;
+    }
+
+    std::unordered_map<std::string, std::string> load_error_map(const std::string& path)
+    {
+        std::ifstream ifs(path);
+        nlohmann::json j;
+        ifs >> j;
+
+        std::unordered_map<std::string, std::string> map;
+        for (auto& [code, msg] : j["warring"].items()) {
+            map[code] = msg.get<std::string>();
+        }
+        return map;
+    }    
+}
+    
+    
+