提交更新

Tracking/TrackingObj.cpp

+
+
+#include "FusionObj.h"
+#include "TimeFunction.h"
+
+
+namespace juefx_tracking 
+{
+    void TrackingObj::Init(std::string root_dir,YAML::Node config,TimeQueueDataProcessCallback cb)
+    {
+        m_config = config;
+        m_cb = cb;
+        int gpu = 0;
+        if(m_config["kf_gpu"])
+            gpu = m_config["kf_gpu"].as<int32_t>();
+        else
+            SDK_LOG(SDK_INFO, "can not load kf_gpu ");
+        SDK_LOG(SDK_INFO, "kf_gpu = %d",gpu);
+        m_tracker.SetGPU(gpu);
+
+        if(m_config["lidar_x_angle"])
+            m_lidar_x_angle = m_config["lidar_x_angle"].as<float>();
+        SDK_LOG(SDK_INFO, "m_lidar_x_angle = %f", m_lidar_x_angle);
+
+        float threshold = 0.00001;
+        if(m_config["iou_threshold"])
+            threshold = m_config["iou_threshold"].as<float>();
+        SDK_LOG(SDK_INFO, "iou_threshold = %f", threshold);
+        m_tracker.SetIouThreshold(threshold);
+        int max_coastcycles = 1;
+        if(m_config["max_coastcycles"])
+            max_coastcycles = m_config["max_coastcycles"].as<int32_t>();
+        SDK_LOG(SDK_INFO, "max_coastcycles = %d", max_coastcycles);
+        m_tracker.SetMaxCoastCycles(max_coastcycles);
+        int update_valid_count = 2;
+        if(m_config["update_valid_count"])
+            update_valid_count = m_config["update_valid_count"].as<int32_t>();
+        SDK_LOG(SDK_INFO, "update_valid_count = %d", update_valid_count);
+        m_tracker.SetValidUpdateCount(update_valid_count);
+        float matrix_angle_r_value = 1;
+        if(m_config["matrix_angle_r_value"])
+            matrix_angle_r_value = m_config["matrix_angle_r_value"].as<float>();
+        SDK_LOG(SDK_INFO, "matrix_angle_r_value = %f", matrix_angle_r_value);
+        std::vector<float> values;
+        values.push_back(matrix_angle_r_value);
+        m_tracker.SetValues(values);
+
+        if (config["TRANS"])
+            m_trans = config["TRANS"].as<std::vector<std::vector<double>>>();
+        else
+            SDK_LOG(SDK_INFO, "can not load TRANS from yaml");
+
+        if(config["KITTI2ORIGIN"])
+            m_kitti2origin = config["KITTI2ORIGIN"].as<std::vector<std::vector<double>>>();
+        else
+            SDK_LOG(SDK_INFO, "can not load KITTI2ORIGIN from yaml");
+
+        SDK_LOG(SDK_INFO, "trans = [%s]", GetMatrixStr(m_trans, 4, 4).c_str());
+        SDK_LOG(SDK_INFO, "kitti2origin = [%s]", GetMatrixStr(m_kitti2origin,4,4).c_str());
+        ADD_DEF_LOG(1, "tracking.csv");
+        SDK_IDX_LOG(1, SDK_INFO, "date,frameNum,time,id,input_radian,input_angle,predict_radian,predict_angle,output_radian,output_angle,input_type,input_x,input_y,input_z,input_l,input_w,input_h,last_type,predict_x,predict_y,predict_z,predict_l,predict_w,preditc_h,output_type,output_x,output_y,output_z,output_l,output_w,output_h,output_rot_y,v_x,v_y,v_z,lat,lon");
+        //m_coordinate.Init(value[0][0], value[0][1], value[0][2], value[0][3], value[1][0], value[1][1]);
+
+        m_objsQueue.set_max_num_items(2);
+
+        m_isTrackingRun = true;
+        m_trackingThread = std::thread(&TrackingObj::ThreadTrackingProcess, this);
+
+    }
+    void TrackingObj::PushData(objTrackListPtr objsPtr)
+    {
+        m_objsQueue.push(ptr);
+    }
+
+    void TrackingObj::ThreadTrackingProcess()
+    {
+        SDK_LOG(SDK_INFO, "TrackingObj::ThreadTrackingProcess begin");
+        while (m_isTrackingRun)
+        {
+            objTrackListPtr objsPtr;
+            bool ret = m_objsQueue.timeout_pop(objsPtr, 2000);
+            if (ret)
+            {
+                if(m_cb)
+                    m_cb(objsPtr);
+                static int countCb = 0;
+                countCb++;
+                static uint64_t countBeginCb = 0;
+                if (countBeginCb == 0)
+                    countBeginCb = GetCurTime();
+                if (GetCurTime() - countBeginCb > 3000 * 1000)
+                {
+                    SDK_LOG(SDK_INFO, "deal tracking obj count = %d, average rate = %f", countCb, (float)countCb / 3);
+                    countBeginCb = GetCurTime();
+                    countCb = 0;
+                }                    
+            }
+        }
+    }
+
+
+}

Tracking/TrackingObj.h

+/**
+ * @file FusinObj.h
+ * @brief the fusion of multi sensors's perception results
+ * @author juefx
+ * @date 2022/05/19
+ */
+#pragma once
+
+// headers in ROS
+#include <yaml-cpp/yaml.h>
+
+#ifdef _USING_ROS_MSG_
+#include "RosMsg.h"
+#else
+using objTrackListPtr = std::shared_ptr<int>;
+#endif
+
+#include "BaseTracker.h"
+#include "Track3D.h"
+#include "SafeQueue.hpp"
+//#include "TrackingMsg.h"
+
+namespace juefx_tracking 
+{
+
+    typedef void (TrackingProcessCall)(objTrackListPtr objsPtr);
+    typedef std::function<TrackingProcessCall> TrackingProcessCallback;
+    class TrackingObj
+    {
+    public:
+        TrackingObj(){}
+        ~TrackingObj(){}
+
+        void Init(std::string root_dir,YAML::Node config,TrackingProcessCallback cb);
+        void PushData(objTrackListPtr objsPtr);
+        void ThreadTrackingProcess();
+
+        SafeQueue<objTrackListPtr> m_objsQueue;//队列
+        BaseTracker<Track3D> m_tracker;
+        bool m_isTrackingRun = false;//记录是否在运行
+        std::thread m_trackingThread;//运行事件线程
+
+        std::vector<std::vector<double>> m_trans;
+        std::vector<std::vector<double>> m_kitti2origin;  
+        
+        float m_lidar_x_angle = 0.0f;//修改rot_y的参数，从配置里读取
+
+        YAML::Node m_config;
+        TrackingProcessCallback m_cb= nullptr;
+    };
+
+
+}
\ No newline at end of file

ros2/RosMsg.h

+#pragma once
+
+#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp"
+#include "tier4_perception_msgs/msg/detected_object_with_feature.hpp"
+#include "autoware_auto_perception_msgs/msg/object_classification.hpp"
+
+using objTrackListPtr = std::shared_ptr<tier4_perception_msgs::msg::DetectedObjectsWithFeature>;

ros2/TrackingRos2.cpp

+                                                                                                                                                                                                                                                                           
+#include "TrackingRos2.h"
+#include "Component.h"
+#include <eigen3/Eigen/Dense>
+#include <math.h>
+#include <yaml-cpp/yaml.h>
+#include "yaml_parser.h"
+// #include "TransformPos.h"
+#include <chrono>
+
+using namespace sensor_msgs;
+// using namespace message_filters;
+
+namespace juefx_tracking {
+    TrackingRos2::TrackingRos2(const rclcpp::NodeOptions &node_options)
+    : rclcpp::Node("camobjdet_node", node_options) 
+    {
+        std::string topic;
+
+        m_fusion_res = this->create_subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>(
+                    camera0_topic, 10, 
+                    std::bind(&TrackingRos2::ReceiveFusionResMsg, 
+                              this, std::placeholders::_1));
+        std::string pub_topic;
+        m_fusion_res = this->create_publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>(pub_topic, 10);
+
+        //获取yaml文件
+        std::string root_dir = this->declare_parameter<std::string>("root_dir", std::string(""));
+        int run_mode = this->declare_parameter<int>("run_mode", 1);
+        YAML::Node config;
+        if (run_mode == 1)
+        {
+            std::string entry_yaml = this->declare_parameter<std::string>("entry_yaml", std::string(""));
+            QichechengYamlParser parser;
+            int ret = parser.parse_yaml(entry_yaml, root_dir);
+            if (ret != 0)return;
+            config = parser.entry["TRACKING_YAML_NODE"];
+        }
+        else
+        {
+            std::string config_yaml = this->declare_parameter<std::string>("unittest_yaml", std::string(""));
+            config = YAML::LoadFile(config_yaml);
+        }
+
+        m_tracking.Init(root_dir,config,[=](objTrackListPtr objsPtr){
+            TrackingPorcess(objsPtr);
+        });
+    }
+
+TrackingRos2::~TrackingRos2() {}
+void TrackingRos2::ReceiveFusionResMsg(const tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg)
+{
+    objTrackListPtr objsPtr = std::make_shared<tier4_perception_msgs::msg::DetectedObjectsWithFeature>();
+    
+    *objsPtr = msg;
+    m_tracking.PushData(objsPtr);
+    
+    static int countR = 0;
+    countR++;
+    static uint64_t countBeginR = 0;
+    if (countBeginR == 0)
+        countBeginR = GetCurTime();
+    if (GetCurTime() - countBeginR > 3000 * 1000)
+    {
+        SDK_LOG(SDK_INFO, "recv msg count = %d, average rate = %f", countR, (float)countR/3);
+        countBeginR = GetCurTime();
+        countR = 0;
+    }    
+}
+void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr)
+{
+
+}
+
+}  // namespace juefx_yolov5                                                                              

ros2/TrackingRos2.h

+#pragma once
+
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+
+#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp"
+#include "tier4_perception_msgs/msg/detected_object_with_feature.hpp"
+#include "autoware_auto_perception_msgs/msg/object_classification.hpp"
+#include "TrackingObj.h"
+//#define ONLINE
+
+namespace juefx_tracking {
+
+
+/**
+ * 
+ *
+ */
+class TrackingRos2 : public rclcpp::Node {
+   private:
+
+
+   public:
+    TrackingRos2(const rclcpp::NodeOptions &node_options);
+    ~TrackingRos2();
+
+   private:
+
+    rclcpp::Subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr m_fusion_res;
+    rclcpp::Publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr m_tracking_res;
+
+    void ReceiveFusionResMsg(const tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg);
+
+    void TrackingPorcess(objTrackListPtr objsPtr);
+
+
+    TrackingObj m_tracking;
+};
+
+}  // namespace juefx_tracking
+ 

ros2/main.cpp

+#include "SensorsFusion.h"
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    auto fusion = std::make_shared<juefx_fusion::SensorsFusion>(rclcpp::NodeOptions{});
+    rclcpp::spin(fusion);
+    rclcpp::shutdown();
+
+    return 0;
+}