上传新的跟踪逻辑

BaseTracker/BaseTracker.h

 #pragma once
 
 
+#include <chrono>
 #include <iostream>
 //#include <eigen3/Eigen/Dense>
 //#include "kalman_filter.h"

TrackingRosAbs.h

+#pragma once
+
+#include <vector>
+#include "BaseTracker.h"
+#include "BaseTrackerEx.h"
+#include "Track3D.h"
+#include "TrackEx.hpp"
+#ifdef _QICHECHENG_
+#include "jfxrosperceiver/det_tracking_array.h"
+#define jfx_common_msgs jfxrosperceiver
+#else
+#include "jfx_common_msgs/det_tracking_array.h"
+#endif
+#include "SafeQueue.hpp"
+//#include "TrackingMsg.h"
+#include "ros/ros.h"
+#include "coordinate_convert.h"
+#include <yaml-cpp/yaml.h>
+
+using objTrackListPtr = std::shared_ptr<jfx_common_msgs::det_tracking_array>;
+
+
+class TrackingRos
+{
+private:
+    ros::NodeHandle private_nh;
+public:
+    // Constructor
+    TrackingRos() {}
+    ~TrackingRos();
+
+    void TrackingCallBackFunc(const jfx_common_msgs::det_tracking_arrayConstPtr& msg);//接受ros发过来的消息
+
+    void Init(ros::NodeHandle& nh);
+
+    void ThreadTrackingProcess();
+    void ThreadTrackingProcessEx();
+
+
+public:
+
+    SafeQueue<objTrackListPtr> m_objsQueue;
+
+    BaseTracker<Track3D> m_tracker;
+    BaseTrackerEx<TrackEx> m_trackerEx;
+
+    bool m_isTrackingRun = false;//记录是否在运行
+    std::thread m_trackingThread;//运行事件线程
+    ros::Publisher m_pub;//发送所有物体信息的publisher
+    ros::Publisher m_pubCloud;//发送点云数据
+    ros::Publisher m_pubBoundingBoxes;//发送3D框信息
+    ros::Publisher m_pubMarkerArray;//发送marker框信息
+    ros::Publisher m_pubMarkerArrow;//发送marker框信息
+    ros::Subscriber m_subFusionRes;
+#ifdef _SEND_ABUZHABI_
+    ros::Publisher m_pubAbuzhabi;//阿布扎比发送的数据
+#endif
+
+    std::vector<std::vector<double>> m_trans;
+    std::vector<std::vector<double>> m_kitti2origin;
+#ifdef _ABUZHABI_
+    std::vector<double> m_UTM_station;
+    std::vector<double> m_wgs84_station; 
+    std::vector<std::vector<double>> m_trans2station;
+#endif
+
+    int m_frameNum = 0;
+
+    float m_lidar_x_angle = 0.0f;//修改rot_y的参数，从配置里读取
+
+    std::string m_root_dir;//存储统一的根目录路径
+    std::string m_nodeName;//点位信息，现在就是10-1，1-1等信息
+    YAML::Node m_config;//配置的yaml文件
+    int mark_type_text = 0;//0-type,1-object_id,2-object-type,3-text...
+
+    double m_cross_center_lon = 0;
+    double m_cross_center_lat = 0;
+    double m_clear_distance = 0;
+};

jfx_tracking_abs.cpp

+#include <ros/ros.h>
+//#include "image_synchronizer.h"
+//#include <yaml-cpp/yaml.h>
+
+//#include "cv_config.h"
+#include "Track3D.h"
+#include "Track2D.h"
+#include "BaseTracker.h"
+#include "Iou.h"
+#include "jfx_log.h"
+#include "LogBase.h"
+#include "TrackingRosAbs.h"
+
+//using namespace jfx_vision;
+
+//std::unique_ptr<VisionConfig>  jfx_vision::g_visionConfig;
+
+int main(int argc, char*argv[]) {
+    //ROS_INFO("START JFX_VISION_NODE");
+
+
+    InitLog(argv[0]);
+
+    SDK_LOG(SDK_INFO, "main function");
+
+
+    ros::init(argc, argv, "jfx_tracking");
+    ros::NodeHandle nh("~");
+
+
+    //BaseTracker<Track3D> tracker3d;
+    //BaseTracker<Track2D> tracker2d;
+
+    //std::map<uint64_t, std::shared_ptr<Track3D> >& a = tracker3d.GetStates();
+    //std::map<uint64_t, std::shared_ptr<Track2D> >& b = tracker2d.GetStates();
+    
+
+    //std::vector<float> truth_poses{ -0.4875, -0.798913, -1.125, 0, 0, -1.27409, 0.240477, 0.235315, 0.375 };
+    //std::vector<float> truth_poses{ -0.1875, -0.798913, -1.0, 0, 0, -1.27409, 0.240477, 0.235315, 0.375 };
+    ////std::vector<float> landmark_poses{ -0.506616, -0.796303, -1.20499, 0, 0, -0.345443, 0.22,  0.22, 0.4 };
+    //std::vector<float> landmark_poses{ -0.4875, -0.798913, -1.125, 0, 0, -1.27409, 0.240477, 0.47063, 0.375 };
+    //double iou_3d = CuboidIoU(truth_poses, landmark_poses);
+    //std::cout<<"the iou of two cuboid is: "<< iou_3d << std::endl;
+
+    //std::string config_yaml = "/data/catkin_ws_xishan/src/rosrv2x/jfx_rvconfig/param/vision_config.yaml";
+    //nh.param<std::string>("vision_config_path", config_yaml, "");
+
+    //g_visionConfig.reset(new VisionConfig(config_yaml));
+
+    //int cam_size = g_visionConfig->globalConf.camera_size;
+    //ROS_INFO("cam_size:%d", cam_size);
+
+    // ROS_INFO("CAM[0] roi: h %d,w %d,x %d,y %d", g_visionConfig->cameraParamConfigs_map[0].roiRect.height, g_visionConfig->cameraParamConfigs_map[0].roiRect.width, 
+    // g_visionConfig->cameraParamConfigs_map[0].roiRect.x, g_visionConfig->cameraParamConfigs_map[0].roiRect.y);
+
+    //MultiCamImageSynchronizer<sensor_msgs::Image> sync(nh, cam_size); 
+
+    TrackingRos jfx_tracking;
+    jfx_tracking.Init(nh);
+    // ros::Subscriber track_sub = nh.subscribe<jfx_common_msgs::det_tracking_array>("/fusion_res", 1000, &TrackingRos::TrackingCallBackFunc, &jfx_tracking);
+    //rospy.Subscriber("/detection/lidar_detector/objects", det_tracking_array, callback)
+
+    ros::spin();
+
+    return 0;
+}