init repo

CMakeLists.txt

+cmake_minimum_required(VERSION 3.0.2)
+project(jfx_rvfusion_cx VERSION 1.0)
+
+##############
+## compiler ##
+##############
+
+add_compile_options(-std=c++14)
+
+set(CMAKE_C_FLAGS "-fPIC -Wno-psabi -Wno-write-strings -Wpointer-arith  -pthread")
+set(CMAKE_CXX_FLAGS "-fPIC -Wno-psabi -Wno-write-strings -Wpointer-arith  -pthread")
+
+IF (NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE)
+ENDIF()
+
+message("${PROJECT_NAME} Build type: ${CMAKE_BUILD_TYPE}")
+
+if(CMAKE_BUILD_TYPE STREQUAL "Debug")
+    add_definitions(-D_DEBUG)
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -g -O0")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0")
+elseif(CMAKE_BUILD_TYPE STREQUAL "Release")
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
+endif()
+
+set(JFX_COMMON_LIBS_PATH ${PROJECT_SOURCE_DIR}/../jfx_common_libs)
+
+message("JFX_COMMON_LIBS_PATH" ${JFX_COMMON_LIBS_PATH})
+
+###################################
+## catkin specific configuration ##
+###################################
+#find_package(yaml-cpp 0.5.3 REQUIRED)
+find_package(OpenCV 3.2.0 REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+	roscpp
+	rospy
+	std_msgs
+	message_generation
+	jfx_common_msgs
+)
+
+catkin_package(
+	CATKIN_DEPENDS roscpp rospy std_msgs jfx_common_msgs
+	DEPENDS OpenCV
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+	${PROJECT_SOURCE_DIR}/incl
+	${catkin_INCLUDE_DIRS}
+	${JFX_COMMON_LIBS_PATH}/core
+	${JFX_COMMON_LIBS_PATH}/utils
+	${JFX_COMMON_LIBS_PATH}/coordinate
+)
+
+file(GLOB_RECURSE RVFUSION_SRC_LIST
+	src/*.cpp
+)
+
+file(GLOB_RECURSE COMMON_SRC_LIST
+	${JFX_COMMON_LIBS_PATH}/core/udp_client.cpp
+	${JFX_COMMON_LIBS_PATH}/utils/*.cpp
+	${JFX_COMMON_LIBS_PATH}/coordinate/*.cpp
+)
+
+add_executable(${PROJECT_NAME}
+	${RVFUSION_SRC_LIST}
+	${COMMON_SRC_LIST}
+)
+
+add_dependencies(${PROJECT_NAME} jfx_common_msgs_gencpp)
+
+target_link_libraries(${PROJECT_NAME}
+	uuid
+	glog
+	pthread
+	yaml-cpp
+	${OpenCV_LIBS}
+	${catkin_LIBRARIES}
+)
+
+add_executable(rvfusion_test
+    ${PROJECT_SOURCE_DIR}/src/fusionAlgo.cpp
+	${PROJECT_SOURCE_DIR}/src/fusionCheckNearTask.cpp
+	${PROJECT_SOURCE_DIR}/src/fusionCsv.cpp
+	${PROJECT_SOURCE_DIR}/src/fusionData.cpp
+	${PROJECT_SOURCE_DIR}/src/fusionDistribTask.cpp
+	${PROJECT_SOURCE_DIR}/src/fusionObject.cpp
+	${PROJECT_SOURCE_DIR}/src/fusionTrajectionTask.cpp
+	${PROJECT_SOURCE_DIR}/src/trafficObject.cpp
+	${PROJECT_SOURCE_DIR}/test/test.cpp
+	${COMMON_SRC_LIST}
+)
+
+target_link_libraries(rvfusion_test
+	uuid
+	glog
+	pthread
+	yaml-cpp
+	${OpenCV_LIBS}
+	${catkin_LIBRARIES}
+)
+
+add_dependencies(rvfusion_test jfx_common_msgs_gencpp)
+
+## 添加可执行文件或者库文件，Mark executables and/or libraries for installation
+install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## 添加资源文件．Mark other files for installation (e.g. launch and bag files, etc.)
+install(DIRECTORY
+    launch
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
\ No newline at end of file

README.md

+1、activeRadius，再输出的时候判断
+2、交叉遇到的被融合了咋办: 通过farObj和count > 3/4解决；
+3、如果先近后far怎么解决： far三次断开关系；
+4、和fusionObj其他目标（mainObj）距离过大，移出fusionObj;
+5、fusiobObj里的目标在obj的farObj里，移出fusionObj;
+6、时间延迟，识别的同一个目标前后的情况导致匹配点少；
+7、目标输出的逻辑规则；
+8、如果mainObj的时间比当前目标时间早很多，怎么处理？
+9、出现在路口，活动范围不超过10米的，不输出
+10、如果和mainObj距离超过15米，应该是融合错了，怎么脱离？
\ No newline at end of file

incl/fusionAlgo.h

+#pragma once
+
+#include <memory>
+
+#include "fusionDef.h"
+#include "trafficObject.h"
+#include "fusionObject.h"
+#include "jfx_common_msgs/OutFusionObj.h"
+#include "jfx_common_msgs/OutFusionObjs.h"
+
+class FusionAlgo {
+public:
+    FusionAlgo();
+    ~FusionAlgo();
+
+public:
+    static int forecastTObjRealPoint(TOBJPtr objPtr, uint64_t realTimeMS);
+
+    static double geoGetHeading(double lon1, double lat1, double lon2, double lat2);
+
+    static double calcuObjCurDist(int pos, TOBJPtr obj1, TOBJPtr obj2, uint64_t& obj2MS, uint64_t& dms);
+
+    static double calcuObjCurDist2(int pos, TOBJPtr obj1, TOBJPtr obj2);
+
+    static int ifCanCheckNear(TOBJPtr& tObjPtr, TOBJPtr& o);
+
+    static bool isTwoObjNear(TOBJPtr& tObjPtr, TOBJPtr& o, double dist);
+
+    static void updateNearInfo(TOBJPtr& tObjPtr, TOBJPtr& o, double dist, uint64_t dms, uint64_t oms);
+
+    static void fitFObjNextPoint(FOBJPtr& fObj, jfx_common_msgs::OutFusionObj& obj);
+};
\ No newline at end of file

incl/fusionCheckNearTask.h

+#pragma once
+
+#include <atomic>
+#include <chrono>
+#include <list>
+#include <map>
+#include <mutex>
+#include <thread>
+#include <unordered_map>
+
+#include "fusionDef.h"
+#include "trafficObject.h"
+
+using namespace std;
+
+class FusionCheckNearTask {
+public:
+    FusionCheckNearTask();
+    ~FusionCheckNearTask();
+
+public:
+    int init(int taskID);
+
+    int pushTrafficObj(STCheckNearItem& item);
+
+    void run();
+
+    int checkNear(STCheckNearItem& item);
+
+    bool checkTObjLost(TOBJPtr& tObjPtr);
+
+public:
+    int mTaskID = 0;
+    std::thread* mRunThread = nullptr;
+    FusionCheckNearTask* mNextTask = nullptr;
+    uint64_t mLastCheckDataMS = 0;
+
+    std::map<uint64_t, uint64_t> mDeviceIDMap;
+    std::unordered_map<uint64_t, TOBJPtr> mTrafficObjMap;
+
+    std::mutex mBufCheckItemListMutex;
+    std::list<STCheckNearItem> mBufCheckItemList;
+};

incl/fusionConfig.h

+#pragma once
+
+#include "fusionDef.h"
+#include "gaussian.h"
+#include "single_ton.hpp"
+#include <ros/ros.h>
+#include <yaml-cpp/yaml.h>
+
+using namespace std;
+
+class FusionConfig : public Singleton<FusionConfig> {
+public:
+    FusionConfig() {};
+    ~FusionConfig() {};
+
+public:
+    int init(string yamlFile)
+    {
+        try {
+            YAML::Node conf = YAML::LoadFile(yamlFile);
+            mMecID = conf["GlobalConfig"]["mec_id"].as<string>();
+            mClusterThreadNum = conf["FusionConfig"]["thread"].as<int>();
+            mCrossCenterX = conf["FusionConfig"]["center_lon"].as<double>();
+            mCrossCenterY = conf["FusionConfig"]["center_lat"].as<double>();
+            mBFusionStdout = conf["FusionConfig"]["fusion_stdout"].as<bool>();
+            mBLogInData = conf["FusionConfig"]["log_indata"].as<bool>();
+            mLogPath = conf["FusionConfig"]["log_path"].as<string>();
+
+            mFilterMaxRadius = conf["FusionConfig"]["filter_radius"].as<int>();
+            mFusionRadius = conf["FusionConfig"]["fusion_radius"].as<int>();
+            mVisionVehicleOutRadius = conf["FusionConfig"]["vision_vehicle_out_radius"].as<int>();
+            mVisionUnVehicleOutRadius = conf["FusionConfig"]["vision_unvehicle_out_radius"].as<int>();
+
+            string modeStr = conf["FusionConfig"]["first_mode"].as<string>();
+            if (modeStr == "v")
+                mFirstOutMode = enTSModeVideo;
+            else if (modeStr == "r")
+                mFirstOutMode = enTSModeRadar;
+            mCreateFusionObjMode = conf["FusionConfig"]["create_fobj_mode"].as<string>();
+
+            mIfFilterRadarNonVehicle = conf["FusionConfig"]["filter_radar_nonvehicle"].as<bool>();
+            mIfCheckObjType = conf["FusionConfig"]["check_objtype"].as<bool>();
+            mIfFilterRadarVehicle = conf["FusionConfig"]["filter_radar_vehicle"].as<bool>();
+            mIfFilterVisionNonVehicle = conf["FusionConfig"]["filter_vision_nonvehicle"].as<bool>();
+            mIfFilterVisionVehicle = conf["FusionConfig"]["filter_vision_vehicle"].as<bool>();
+            mIfDynamicChangeDropFrameStep = conf["FusionConfig"]["dynamicDropFrame"].as<bool>();
+
+            // filter device id
+            std::vector<string> vecR = conf["FusionConfig"]["filter_devid_r"].as<std::vector<string>>();
+            for (auto const& n : vecR) {
+                if (n.size() > 0) {
+                    mFilterDevID[n] = enTSModeRadar;
+                }
+            }
+            std::vector<string> vecV = conf["FusionConfig"]["filter_devid_v"].as<std::vector<string>>();
+            for (auto const& n : vecV) {
+                if (n.size() > 0) {
+                    mFilterDevID[n] = enTSModeVideo;
+                }
+            }
+
+            // heading keep
+            auto itrHeadingKeep = conf["FusionConfig"]["heading_keep"].begin();
+            for (; itrHeadingKeep != conf["FusionConfig"]["heading_keep"].end(); ++itrHeadingKeep) {
+                std::vector<double> vecHK = itrHeadingKeep->as<std::vector<double>>();
+                if (vecHK.size() != 7 && vecHK.size() != 6) {
+                    continue;
+                }
+                setHeadingKeepArea(vecHK);
+            }
+        } catch (...) {
+            ROS_WARN("load fusion config fail: %s", yamlFile.c_str());
+            return -1;
+        }
+
+        ROS_INFO("load fusion config ok: %s", yamlFile.c_str());
+
+        return 0;
+    }
+
+    void setHeadingKeepArea(std::vector<double>& vec)
+    {
+        if (vec.size() < 6 || (vec[0] == 0 && vec[3] == 0)) {
+            return;
+        }
+
+        STHeadingKeepArea from, to;
+        from.radius = vec[0];
+        from.lon = vec[1];
+        from.lat = vec[2];
+        to.radius = vec[3];
+        to.lon = vec[4];
+        to.lat = vec[5];
+
+        double direct = jfx::GetDirectionXS(from.lon, from.lat, to.lon, to.lat) + 1.26;
+        direct = direct > 360 ? (direct - 360) : direct;
+        if (vec.size() == 6) {
+            from.heading = to.heading = direct;
+        } else {
+            from.heading = to.heading = vec[6];
+        }
+
+        mHeadingKeepArea.push_back(from);
+
+        double dist = jfx::GetDistance(from.lon, from.lat, to.lon, to.lat);
+        double radiusStep = (to.radius - from.radius + 0.1) / (dist * 2 / (from.radius + to.radius));
+        while (dist > ((from.radius + to.radius) / 2)) {
+            from.radius += radiusStep;
+            jfx::Array point = jfx::GetPoint(from.lon, from.lat, from.radius, direct);
+            from.lon = point[0];
+            from.lat = point[1];
+            mHeadingKeepArea.push_back(from);
+            dist = jfx::GetDistance(from.lon, from.lat, to.lon, to.lat);
+        }
+
+        mHeadingKeepArea.push_back(to);
+    }
+
+public:
+    bool mBAppExit = false;
+    bool mBLogInData = true;
+    bool mBFusionStdout = false;
+    string mLogPath = "";
+
+    string mMecID;
+    int mClusterThreadNum = 1;
+
+    double mCrossCenterX = 0;
+    double mCrossCenterY = 0;
+
+    std::map<string, int> mFilterDevID;
+
+    int mDropFrameStep = 50;
+    bool mIfDynamicChangeDropFrameStep = true;
+
+    ENTrafficSensMode mFirstOutMode = enTSModeVideo;
+    string mCreateFusionObjMode = "rv";
+
+    int mObjRetainMS = 5000;
+    int mFilterMaxRadius = 250;
+    int mFusionRadius = 250;
+    int mVisionVehicleOutRadius = 100;
+    int mVisionUnVehicleOutRadius = 100;
+
+    // 不同Mode间车辆距离门限
+    double mDistThresholdVehicleDiffMode = 2.8;
+
+    // 不同Mode间非车辆距离门限
+    double mDistThresholdNonVehicleDiffMode = 2;
+
+    // 相同Mode间车辆距离门限
+    double mDistThresholdVehicleSameMode = 1;
+
+    int mTrajNearMinPoint = 4;
+    int mObjFusionMinPoint = 10;
+    int mObjFusionMaxPoint = 500;
+
+    bool mIfFilterRadarNonVehicle = true;
+    bool mIfFilterVisionNonVehicle = true;
+    bool mIfFilterRadarVehicle = true;
+    bool mIfFilterVisionVehicle = true;
+    bool mIfForecastInputObj = false;
+    bool mIfSmoothTrajectory = false;
+    bool mIfCorrectTrajectory = false;
+
+    bool mIfCheckObjType = true;
+    bool mIfCheckObjHeading = false;
+
+    std::vector<STHeadingKeepArea> mHeadingKeepArea;
+};
+
+#define FConfPtr (FusionConfig::Instance())

incl/fusionCsv.h

+#pragma once
+
+#include <string>
+#include <fstream>
+
+#include "trafficObject.h"
+
+using namespace std;
+
+class FusionCsvFile {
+public:
+    FusionCsvFile() {};
+    ~FusionCsvFile() {};
+
+public:
+    int openRead(string fileName);
+
+    TOBJPtr readLine();
+
+public:
+    ifstream mInStream;
+};
\ No newline at end of file

incl/fusionData.h

+#pragma once
+
+#include <ros/ros.h>
+#include <memory>
+#include <mutex>
+#include <unordered_map>
+
+#include "fusionDef.h"
+#include "fusionObject.h"
+#include "sensDevice.h"
+#include "single_ton.hpp"
+#include "trafficObject.h"
+
+using namespace std;
+
+class FusionData : public Singleton<FusionData> {
+public:
+    FusionData() {};
+    ~FusionData() {};
+
+public:
+    TOBJPtr setTrafficObj(TOBJPtr& tObjPtr);
+    TOBJPtr getTrafficObj(uint64_t key);
+    void eraseTrafficObj(uint64_t key);
+
+    SDEVPtr genSensDevice(TOBJPtr& tObjPtr);
+    SDEVPtr getSensDevice(uint64_t did);
+
+    FOBJPtr getFusionObj(uint64_t fid);
+    FOBJPtr setFusionObj(FOBJPtr& fObjPtr);
+    void eraseFusionObj(uint64_t fid);
+
+    uint64_t genSensDevID(string mode, string devIDStr);
+    uint64_t genTfcObjKey(uint64_t devID, uint64_t objOriginID);
+    uint64_t genFusionObjID();
+
+    ENTrafficSensMode modeStrToID(string modeStr);
+    ENTrafficObjectType typeStr2ID(string typeStr);
+
+    string getStateStr();
+
+public:
+    std::mutex mTrafficObjMapMainMutex;
+    std::unordered_map<uint64_t, TOBJPtr> mTrafficObjMainMap;
+    std::mutex mFusionObjMapMutex;
+    std::unordered_map<uint64_t, FOBJPtr> mFusionObjMap;
+    std::mutex mSensDeviceMapMutex;
+    std::unordered_map<uint64_t, SDEVPtr> mSensDeviceMap;
+
+    std::mutex mSensDeviceIDMapMutex;
+    std::unordered_map<string, uint64_t> mSensDeviceIDStr2Int64;
+
+    STFusionState mFusionState;
+
+    uint64_t mSensDevIdx = 1;
+    uint64_t mFusionObjIdx = 1;
+    uint64_t mTfcObjIdx = 1;
+    uint64_t mDataMSMax = 0;
+
+    uint64_t mDistribListSize = 0;
+    uint64_t mFirstCheckNearListSize = 0;
+    uint64_t mTObjMapSize = 0;
+    uint64_t mFObjMapSize = 0;
+    uint64_t mTrajTaskListSize = 0;
+
+    uint64_t mCountIn = 0;
+    uint64_t mCountNear = 0;
+    uint64_t mCountTraj = 0;
+};
+
+#define FDataPtr (FusionData::Instance())
\ No newline at end of file

incl/fusionDataLog.h

+#pragma once
+
+#include <map>
+#include <string.h>
+#include <string>
+#include <vector>
+
+#include "single_ton.hpp"
+#include "spin_mutex.h"
+#include "trafficObject.h"
+#include "fusionObject.h"
+#include "utils.h"
+
+using namespace std;
+
+class FusionDataLog : public Singleton<FusionDataLog> {
+public:
+    FusionDataLog()
+    {
+        mFilePath = "/workspace/log";
+        mPCueFile = nullptr;
+        mCurFileSize = 0;
+    };
+
+public:
+    void init(string filePath, string dataType)
+    {
+        mFilePath = filePath;
+        mDataType = dataType;
+    }
+
+    void close()
+    {
+        if (mPCueFile) {
+            fclose(mPCueFile);
+            mPCueFile = nullptr;
+            mCurFileSize = 0;
+        }
+    }
+
+    void save(TOBJPtr& tObjPtr)
+    {
+        if (!tObjPtr) {
+            return;
+        }
+
+        std::lock_guard<SpinMutex> lock(mFileWLock);
+        saveOneTObj(tObjPtr);
+    }
+
+    void save(jfx_common_msgs::OutFusionObj& fobj)
+    {
+        std::lock_guard<SpinMutex> lock(mFileWLock);
+        saveOneFObj(fobj);
+    }
+
+protected:
+    int saveOneTObj(TOBJPtr& tObjPtr)
+    {
+        string buf(2048, 0);
+        sprintf((char*)buf.c_str(), "%ld,%ld,%s,%.10lf,%.10lf,%.10lf,%s,0,%.2lf,%.2lf,%s,%ld\n",
+            tObjPtr->mMS, tObjPtr->mID, tObjPtr->mTypeStr.c_str(), tObjPtr->mHeading, tObjPtr->mLon, tObjPtr->mLat,
+            tObjPtr->mDevIDStr.c_str(), tObjPtr->mLength, tObjPtr->mWidth, tObjPtr->mModeStr.c_str(),
+            mills_UTC() - tObjPtr->mMS);
+        saveData(buf.c_str());
+
+        return 0;
+    }
+
+    int saveOneFObj(jfx_common_msgs::OutFusionObj& fobj)
+    {
+        string buf(2048, 0);
+        sprintf((char*)buf.c_str(), "%ld,%ld,%s,%.10lf,%.10lf,%.10lf,%s,%.2lf,%.2lf,%.2lf\n",
+            fobj.time, fobj.all_id, fobj.type.c_str(), fobj.heading, fobj.lon, fobj.lat,
+            fobj.inner_pos.c_str(), fobj.obj_l, fobj.obj_w, fobj.speed);
+        saveData(buf.c_str());
+
+        return 0;
+    }
+
+    int saveData(const char* pData)
+    {
+        if (!pData) {
+            return -1;
+        }
+
+        if (mCurFileSize > 200 * 1024 * 1024) {
+            if (mPCueFile) {
+                fclose(mPCueFile);
+                mPCueFile = nullptr;
+                mCurFileSize = 0;
+            }
+        }
+
+        if (!mPCueFile) {
+            mPCueFile = createFile(mDataType.c_str());
+        }
+
+        writeFile(pData, strlen(pData));
+
+        return 0;
+    }
+
+    FILE* createFile(const char* type)
+    {
+        char szFileName[256] = { 0 };
+
+        sprintf(szFileName, "%s/%s_%ld.dat", mFilePath.c_str(), type, mills_UTC());
+        FILE* pFile = fopen(szFileName, "w");
+
+        return pFile;
+    }
+
+    void writeFile(const char* pData, int len)
+    {
+        if (mPCueFile) {
+            fwrite(pData, len, 1, mPCueFile);
+            fflush(mPCueFile);
+            mCurFileSize += len;
+        }
+    }
+
+public:
+    // 文件路径
+    string mFilePath;
+
+    // 数据类型-文件名前缀
+    string mDataType;
+
+    // 当前文件句柄
+    FILE* mPCueFile;
+
+    // 当前文件写位置
+    uint64_t mCurFileSize;
+
+    // 批次
+    uint64_t mBatchIdx = 0;
+
+    SpinMutex mFileWLock;
+};

incl/fusionDef.h

+#pragma once
+
+#include <stdint.h>
+
+typedef enum _tagENTrafficSensMode {
+    enTSModeUnknown = 0,
+    enTSModeVideo = 1,
+    enTSModeRadar = 2,
+    enTSModeLidar = 3
+} ENTrafficSensMode;
+
+typedef enum _tagENTrafficObjectType {
+    enTOTypeUnknown = 0,
+    enTOTypePerson = 1,
+    enTOTypeBicycle = 2,
+    enTOTypeMotorbike = 3,
+    enTOTypeCar = 4,
+    enTOTypeTruck = 5,
+    enTOTypeBus = 6
+} ENTrafficObjectType;
+
+typedef struct _tagSTObjTrajPoint {
+    uint64_t ms = 0;
+    double lon = 0;
+    double lat = 0;
+    double heading = 0;
+    double headingReal = 0;
+    bool isUseHeadingKeep = false;
+    double speed = 0;
+    int laneseqnum = 0;
+    _tagSTObjTrajPoint() { }
+    _tagSTObjTrajPoint(uint64_t _ms, double _lon, double _lat,
+        double _heading, double _headingReal, double _speed, int _lane)
+    {
+        ms = _ms;
+        lon = _lon;
+        lat = _lat;
+        heading = _heading;
+        speed = _speed;
+        laneseqnum = _lane;
+        headingReal = _headingReal;
+    }
+} STObjTrajPoint;
+
+typedef struct _tagSTFusionState {
+    uint64_t inQueueSize = 0;
+    uint64_t totalRecvCount = 0;
+    uint64_t firstClusterQueueSize = 0;
+    uint64_t filteredCount = 0;
+} STFusionState;
+
+typedef struct _tagSTFusionNearObject {
+    uint64_t key = 0;
+    std::string devIdStr = "";
+    uint64_t id = 0;
+    uint64_t count = 0;
+    // 最近连续未匹配上的次数
+    uint64_t lastMissCount = 0;
+    double totalDist = 0;
+    uint64_t totalDMS = 0;
+    double score = 0;
+    double lastMS = 0;
+    double lastLon = 0;
+    double lastLat = 0;
+} STNearObj;
+
+typedef struct _tagSTFusionFarObject {
+    uint64_t key = 0;
+    std::string devIdStr = "";
+    uint64_t id = 0;
+    uint64_t count = 0;
+    double totalDist = 0;
+    double maxDist = 0;
+} STFarObj;
+
+typedef struct _tagSTHeadingKeepArea {
+    double radius = 0;
+    double lon = 0;
+    double lat = 0;
+    double heading = 0;
+} STHeadingKeepArea;
\ No newline at end of file

incl/fusionDistribTask.h

+#pragma once
+
+#include <atomic>
+#include <chrono>
+#include <list>
+#include <map>
+#include <mutex>
+#include <ros/ros.h>
+#include <thread>
+#include <unordered_map>
+
+#include "single_ton.hpp"
+#include "fusionDef.h"
+#include "jfx_common_msgs/OutFusionObj.h"
+#include "jfx_common_msgs/OutFusionObjs.h"
+#include "jfx_common_msgs/RadarData.h"
+#include "jfx_common_msgs/RadarObj.h"
+#include "jfx_common_msgs/det_tracking.h"
+#include "jfx_common_msgs/det_tracking_array.h"
+#include "trafficObject.h"
+#include "fusionCheckNearTask.h"
+#include "fusionCsv.h"
+#include "sensDevice.h"
+#include "fusionDataLog.h"
+
+using namespace std;
+
+class FusionDistributeTask : public Singleton<FusionDistributeTask> {
+public:
+    FusionDistributeTask();
+    ~FusionDistributeTask();
+
+public:
+    int init(ros::NodeHandle& nh, string replayCsv = "");
+
+    void onRadarData(const jfx_common_msgs::RadarData& data);
+    void onVisionData(const jfx_common_msgs::det_tracking_array& data);
+
+    void run();
+
+    std::shared_ptr<TrafficObject> doTObj(std::shared_ptr<TrafficObject>& tObjPtr, uint64_t nextQueueSize = 0);
+
+    bool ifDropFrame(SDEVPtr& sensDevPtr, int checkNearQueueLen);
+
+    bool filter(std::shared_ptr<TrafficObject>& tObjPtr);
+
+    void pubState();
+
+public:
+    std::thread* mRunThread = nullptr;
+
+    FusionDataLog mOriginDataLog;
+
+    FusionCsvFile mCsvFile;
+    std::string mInCsvFileName;
+    ros::Subscriber mRadarDataSub;
+    ros::Subscriber mVisionDataSub;
+
+    std::mutex mBufTrafficObjListMutex;
+    std::list<std::shared_ptr<TrafficObject>> mBufTrafficObjList;
+
+    std::vector<FusionCheckNearTask*> mClusterTask;
+};
+
+#define FDistribTaskPtr (FusionDistributeTask::Instance())
\ No newline at end of file

incl/fusionGeo.h

+#pragma once
+
+class FusionGeo {
+
+};
\ No newline at end of file

incl/fusionObject.h

+#pragma once
+
+#include <memory>
+#include <map>
+#include <mutex>
+#include <list>
+
+#include "jfx_common_msgs/OutFusionObj.h"
+#include "jfx_common_msgs/OutFusionObjs.h"
+#include "trafficObject.h"
+
+using namespace std;
+
+class FusionObject {
+public:
+    FusionObject() {};
+    ~FusionObject() {};
+
+public:
+    void setTObj(TOBJPtr& tObjPtr);
+
+    void removeTObj(TOBJPtr& tObjPtr);
+
+    bool isEmpty();
+
+    void setInValid();
+
+    void setHisPoint(jfx_common_msgs::OutFusionObj& obj);
+
+    std::string getTrueTypeStr();   
+
+    double getHeading(double curLon, double curLat);
+
+public:
+    uint64_t mID = 0;
+    uint64_t mMS = 0;
+    ENTrafficObjectType mType = enTOTypeUnknown;
+
+    double mLon = 0;
+    double mLat = 0;
+    double mHeading = 0;
+    int mLaneSeqNum = 0;
+
+    TOBJPtr mMainObj = nullptr;
+    uint64_t mLastOutMS = 0;
+    uint64_t mLastOutMainObjMS = 0;
+
+    std::mutex mTObjMapMutex;
+    std::map<uint64_t, TOBJPtr> mRTObjMap;
+    std::map<uint64_t, TOBJPtr> mLTObjMap;
+    std::map<uint64_t, TOBJPtr> mVTObjMap;
+
+    std::vector<STObjTrajPoint> mFusionHisData;
+    
+    double mLastCalcuHeadingLon = 0;
+    double mLastCalcuHeadingLat = 0;
+};
+
+#define FOBJPtr std::shared_ptr<FusionObject>

incl/fusionPubTask.h

+#pragma once
+
+#include <map>
+#include <mutex>
+#include <ros/ros.h>
+#include <thread>
+#include <std_msgs/String.h>
+
+#include "fusionConfig.h"
+#include "fusionObject.h"
+#include "jfx_common_msgs/OutFusionObj.h"
+#include "jfx_common_msgs/OutFusionObjs.h"
+#include "single_ton.hpp"
+#include "utils.h"
+
+using namespace std;
+
+typedef struct _tagSTFusionOutItem {
+    uint64_t ms = 0;
+    jfx_common_msgs::OutFusionObj obj;
+} STFusionOutItem;
+
+class FusionObjPubTask : public Singleton<FusionObjPubTask> {
+public:
+    FusionObjPubTask() {};
+    ~FusionObjPubTask() {};
+
+public:
+    std::thread* mRunThread = nullptr;
+
+    ros::Publisher mFusionDataPub;
+    ros::Publisher mStateDataPub;
+
+    std::mutex mOutItemMapMutex;
+    std::map<uint64_t, STFusionOutItem> mOutItemMap;
+
+    uint64_t mLastDataMS = 0;
+
+public:
+    int init(ros::NodeHandle& nh)
+    {
+        mRunThread = new std::thread(&FusionObjPubTask::run, this);
+        if (!mRunThread) {
+            ROS_WARN("new std::thread fail");
+            return -1;
+        }
+
+        mFusionDataPub = nh.advertise<jfx_common_msgs::OutFusionObjs>("/FusionData", 100);
+        mStateDataPub = nh.advertise<std_msgs::String>("/node/state", 100);
+
+        return true;
+    }
+
+    int pushFusionObj(jfx_common_msgs::OutFusionObj& obj)
+    {
+        std::lock_guard<std::mutex> guard(mOutItemMapMutex);
+        STFusionOutItem item;
+        item.ms = mills_UTC();
+        item.obj = obj;
+        mOutItemMap[obj.all_id] = item;
+
+        mLastDataMS = obj.time;
+
+        return mOutItemMap.size();
+    }
+
+    void run()
+    {
+        while (FConfPtr->mBAppExit == false) {
+            uint64_t tCur = mills_UTC();
+
+            if (mOutItemMap.size() <= 0) {
+                std::this_thread::sleep_for(std::chrono::milliseconds(1));
+                continue;
+            }
+
+            mOutItemMapMutex.lock();
+
+            jfx_common_msgs::OutFusionObjs frame;
+            auto itr = mOutItemMap.begin();
+            for (; itr != mOutItemMap.end();) {
+                if ((tCur - itr->second.ms) > 500) {
+                    mOutItemMap.erase(itr++);
+                    continue;
+                }
+                itr->second.obj.time = mLastDataMS - 100;
+                frame.objs.push_back(itr->second.obj);
+                itr++;
+            }
+
+            mOutItemMapMutex.unlock();
+
+            mFusionDataPub.publish(frame);
+
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
+    }
+};
+
+#define FPubTaskPtr (FusionObjPubTask::Instance())

incl/fusionTrajectionTask.h

+#pragma once
+
+#include <mutex>
+#include <map>
+#include <thread>
+
+#include "fusionObject.h"
+#include "trafficObject.h"
+#include "single_ton.hpp"
+#include "fusionDataLog.h"
+
+using namespace std;
+
+class FusionTrajectionTask : public Singleton<FusionTrajectionTask> {
+public:
+    FusionTrajectionTask() {};
+    ~FusionTrajectionTask() {};
+
+public:
+    int init();
+
+    void pushTrafficObj(STCheckNearItem& item);
+
+    void run();
+
+    FOBJPtr genFusionObj(STCheckNearItem& item);
+
+    void outFusionObj(int pos, FOBJPtr& fObjPtr, TOBJPtr& tObjPtr);
+
+    void outFusionObj1(int pos, FOBJPtr& fObjPtr, TOBJPtr& tObjPtr);
+
+    void outFusionObj2(int pos, FOBJPtr& fObjPtr, TOBJPtr& tObjPtr);
+
+    void outFusionObj3(int pos, FOBJPtr& fObjPtr, TOBJPtr& tObjPtr);
+
+    void cleatTimeoutFObj();
+
+public:
+    std::thread* mRunThread = nullptr;
+
+    std::mutex mLastFusionDataMSMutex;
+    uint64_t mLastFusionDataMS = 0;
+
+    std::mutex mBufTItemListMutex;
+    std::list<STCheckNearItem> mBufTItemList;
+
+    FusionDataLog mFusionDataLog;
+};
+
+#define FTrajTaskPtr (FusionTrajectionTask::Instance())

incl/sensDevice.h

+#pragma once
+
+#include <string>
+#include <memory>
+
+#include "fusionDef.h"
+
+using namespace std;
+
+class SensorDevice {
+public:
+    SensorDevice() {};
+    ~SensorDevice() {};
+
+public:
+    string mIDStr;
+    uint64_t mID = 0;
+    string mModeStr = "";
+    ENTrafficSensMode mMode = enTSModeUnknown;
+
+    uint64_t mFrameIdx = 0;
+    uint64_t mDataDelayMS = 0;
+    uint64_t mLastDataMS = 0;
+    uint64_t mLastDataLocalMS = 0;
+
+    uint64_t mLLastDataMS = 0;
+};
+
+#define SDEVPtr std::shared_ptr<SensorDevice>
\ No newline at end of file

incl/simpleKalman.h

+#pragma once
+
+#include <math.h>
+
+class SimpleKalman {
+public:
+    SimpleKalman() {};
+    ~SimpleKalman() {};
+
+public:
+    int mCount = 0;
+    double p_dx = 0;
+    double mData[4] = { 0 };
+
+public:
+    double filter(double in)
+    {
+        if (mCount < 4) {
+            mData[3 - mCount] = in;
+            return in;
+        }
+
+        double prd0 = 0;
+        double prd1 = 0;
+        double result = 0;
+        double de = 0;
+        double gz = 0;
+        double kg = 0;
+
+        prd1 = mData[1] - mData[2] + mData[1];
+        de = mData[0] = prd1;
+        prd0 = mData[0] - mData[1] + mData[0];
+
+        gz = sqrt(p_dx * p_dx + de * de + 0.00000001);
+        kg = gz * gz / (gz * gz + de * de + 0.00000001);
+        result = prd0 + kg * (in - prd0);
+        p_dx = sqrt((1.0 - kg) * gz * gz);
+
+        mData[3] = mData[2];
+        mData[2] = mData[1];
+        mData[1] = mData[0];
+        mData[0] = result;
+
+        return result;
+    }
+};

incl/trafficObject.h

+#pragma once
+
+#include <string>
+#include <map>
+#include <unordered_map>
+#include <memory>
+#include <list>
+#include <vector>
+#include <mutex>
+
+#include "fusionDef.h"
+
+using namespace std;
+
+class TrafficObject {
+public:
+    TrafficObject() {};
+    TrafficObject(ENTrafficSensMode mode);
+    ~TrafficObject() {};
+
+public:
+    ENTrafficObjectType setType(ENTrafficObjectType type);
+
+    double genHeading(int steps = 10);
+
+    double genSpeed(STObjTrajPoint& p1, STObjTrajPoint& p2);
+
+    double genSpeed(int steps = 5);
+
+    double genSpeed(int hisPos1, int hisPos2);
+
+    double genSpeedX();
+
+    bool isVehicle();
+
+    bool isSameType(std::shared_ptr<TrafficObject>& tObjPtr);
+
+    void setFarObj(std::shared_ptr<TrafficObject>& tObjPtr, double dist);
+
+    bool isFarObj(std::shared_ptr<TrafficObject>& tObjPtr);
+
+    void removeNearObj(uint64_t key);
+
+    /// Reset max near score
+    void resetMaxScoreObj();
+
+    /// Get the TrajPoint closest to this time
+    STObjTrajPoint* getTrajPointClosestToMS(uint64_t ms);
+
+    bool isNearStillNear(STNearObj& nearInfo);
+
+public:
+    string mKeyStr = "";
+    string mDevIDStr = "";
+    string mIDStr = "";
+    string mModeStr = "";
+    string mTypeStr = "";
+
+    uint64_t mDevID = 0;
+    uint64_t mID = 0;
+    uint64_t mKey = 0;
+    uint64_t mIdx = 0;
+    uint64_t mFID = 0;
+    int mClusterID = -1;
+
+    ENTrafficSensMode mMode = enTSModeUnknown;
+    ENTrafficObjectType mType = enTOTypeUnknown;
+    int mSubType = 0;
+    uint64_t mMS = 0;
+    uint64_t mFusionMS = 0;
+    double mLon = 0;
+    double mLat = 0;
+    double mAlt = 0;
+    double mHeading = 0;
+    double mSpeed = 0;
+    double mLength = 0;
+    double mWidth = 0;
+    double mHeight = 0;
+    int mLaneSeqNum = 0;
+
+    double mXPos = 0;
+    double mYPos = 0;
+    int mProb = 0;
+
+    uint64_t mCount = 0;
+    uint64_t mFMS = 0;
+    bool mBLost = false;
+    bool mValid = true;
+    double mDist2Center = 0;
+    double mLastCalcuHeadingLon = 0;
+    double mLastCalcuHeadingLat = 0;
+
+    double mMaxNearScore = 0;
+    uint64_t mMaxNearKey = 0;
+
+    std::mutex mNearObjMapMutex;
+    std::unordered_map<uint64_t, STNearObj> mNearObjMap; 
+
+    std::mutex mFarObjMapMutex;
+    std::unordered_map<uint64_t, STFarObj> mFarObjMap; 
+
+    std::unordered_map<ENTrafficObjectType, uint64_t> mTypeCountMap; 
+
+    // 历史数据，理论上需要锁定，但不需要锁定
+    std::vector<STObjTrajPoint> mHisDataReal;
+    std::vector<STObjTrajPoint> mHisData;
+
+    bool mIsUseHeadingKeep = false;
+    int mCheckNearPos = 0;
+    int mOutPos = 0;
+};
+
+#define TOBJPtr std::shared_ptr<TrafficObject>
+
+typedef struct _tagSTCheckNearItem {
+    int pos = 0;
+    TOBJPtr tObjPtr = nullptr;
+    _tagSTCheckNearItem() {};
+    _tagSTCheckNearItem(TOBJPtr& _tObjPtr)
+    {
+        if (_tObjPtr) {
+            tObjPtr = _tObjPtr;
+            pos = tObjPtr->mHisData.size() - 1;
+        }
+    }
+} STCheckNearItem;
\ No newline at end of file

launch/fusion.launch

+<launch>
+   <arg name="rvfusion_config_path" default="/workspace/etc/rvfusion_config.yaml" />
+   <node pkg="jfx_rvfusion_cx" type="jfx_rvfusion_cx" name="jfx_rvfusion_cx" output="screen" respawn="true" >
+       <param name="config_file" value="$(arg rvfusion_config_path)" />
+   </node>
+</launch>

package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>jfx_rvfusion_cx</name>
+  <version>0.0.0</version>
+  <description>The jfx_rvfusion_cx package</description>
+
+  <maintainer email="like@juefx.com">like</maintainer>
+
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>jfx_common_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>jfx_common_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>jfx_common_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
\ No newline at end of file

src/fusionAlgo.cpp

+#include <ros/ros.h>
+
+#include "fusionAlgo.h"
+#include "fusionConfig.h"
+#include "gaussian.h"
+
+FusionAlgo::FusionAlgo()
+{
+}
+
+FusionAlgo::~FusionAlgo()
+{
+}
+
+int FusionAlgo::forecastTObjRealPoint(TOBJPtr objPtr, uint64_t realTimeMS)
+{
+    return 0;
+}
+
+double FusionAlgo::geoGetHeading(double lng1, double lat1, double lng2, double lat2)
+{
+    return jfx::GetDirectionXS(lng1, lat1, lng2, lat2);
+}
+
+double FusionAlgo::calcuObjCurDist(int pos, TOBJPtr obj1, TOBJPtr obj2, uint64_t& obj2MS, uint64_t& dms)
+{
+    if (!obj1 || !obj2) {
+        return 999;
+    }
+
+    if (pos >= obj1->mHisData.size() || obj1->mHisData.size() <= 0 || obj2->mHisData.size() <= 0) {
+        return 999;
+    }
+
+    if (pos < 0) {
+        pos = 0;
+    }
+
+    // ToDO: 反过来也要处理
+    uint64_t backOffMS = 0;
+    if (obj1->mMode == enTSModeVideo && obj2->mMode == enTSModeRadar) {
+        backOffMS = 300;
+    }
+
+    // Calculate p1,p2,p3 of object1
+    STObjTrajPoint p1 = obj1->mHisData[pos];
+    double offset1 = obj1->mHeading == 0 ? 0 : obj1->mLength / 2 + p1.speed / 3;
+
+    // Select object2`s trajection point that time closest to object1
+    STObjTrajPoint* obj2TrajPointPtr = obj2->getTrajPointClosestToMS(p1.ms - backOffMS);
+    if (obj2TrajPointPtr == nullptr) {
+        return 999;
+    }
+    STObjTrajPoint p2 = *obj2TrajPointPtr;
+    double offset2 = obj2->mHeading == 0 ? 0 : obj2->mLength / 2 + p2.speed / 3;
+
+    obj2MS = p2.ms;
+    dms = abs((int)(p2.ms - p1.ms));
+
+    if (obj1->mDevID == obj2->mDevID && dms >= 150) {
+        return 999;
+    }
+
+    if (obj1->mMode == obj2->mMode && dms >= 800) {
+        return 999;
+    }
+
+    if (obj1->mMode != obj2->mMode && dms >= 1000) {
+        return 999;
+    }
+
+    //
+    int offsetPointCount = 2;
+
+    std::vector<jfx::Array> pVec1;
+    pVec1.push_back({ p1.lon, p1.lat });
+    for (int i = 1; i <= offsetPointCount; i++) {
+        pVec1.push_back(jfx::GetPoint(p1.lon, p1.lat, offset1 * i / offsetPointCount, p1.headingReal));
+        pVec1.push_back(jfx::GetPoint(p1.lon, p1.lat, -1 * offset1 * i / offsetPointCount, p1.headingReal));
+    }
+
+    std::vector<jfx::Array> pVec2;
+    pVec2.push_back({ p2.lon, p2.lat });
+    for (int i = 1; i <= offsetPointCount; i++) {
+        pVec2.push_back(jfx::GetPoint(p2.lon, p2.lat, offset2 * i / offsetPointCount, p2.headingReal));
+        pVec2.push_back(jfx::GetPoint(p2.lon, p2.lat, -1 * offset2 * i / offsetPointCount, p2.headingReal));
+    }
+
+    // 
+    double minDist = 999;
+    for (auto& pa1 : pVec1) {
+        for (auto& pa2: pVec2) {
+            double dist = jfx::GetDistance(pa1[0], pa1[1], pa2[0], pa2[1]);
+            if (dist < minDist) {
+                minDist = dist;
+            }
+        }
+    }
+
+    fflush(stdout);
+
+    return minDist;
+}
+
+double FusionAlgo::calcuObjCurDist2(int pos, TOBJPtr obj1, TOBJPtr obj2) {
+    uint64_t obj2MS = 0, dms = 0;
+    return calcuObjCurDist(pos, obj1, obj2, obj2MS, dms);
+}   
+
+int FusionAlgo::ifCanCheckNear(TOBJPtr& tObjPtr, TOBJPtr& o)
+{
+    if (!tObjPtr || !o) {
+        return -1;
+    }
+
+    if (o->mBLost || o->mValid == false) {
+        return -2;
+    }
+
+    if (tObjPtr->mKey == o->mKey) {
+        return -3;
+    }
+
+    if (FConfPtr->mCreateFusionObjMode.find(o->mModeStr) == std::string::npos) {
+        return -1;
+    }
+    /*
+        if (tObjPtr->mDevID == o->mDevID && abs((int)(tObjPtr->mMS - o->mMS)) >= 150) {
+            return false;
+        }
+
+        if (tObjPtr->mMode == o->mMode && abs((int)(tObjPtr->mMS - o->mMS)) >= 1000) {
+            return false;
+        }
+
+        if (tObjPtr->mMode != o->mMode && abs((int)(tObjPtr->mMS - o->mMS)) >= 1500) {
+            return false;
+        }
+    */
+
+    if (tObjPtr->mIdx < o->mIdx) {
+        return -4;
+    }
+
+    if (FConfPtr->mIfCheckObjType && tObjPtr->isSameType(o) == false) {
+        return -5;
+    }
+
+    if (tObjPtr->isFarObj(o)) {
+        return -6;
+    }
+
+    /*
+    if (FConfPtr->mIfCheckObjHeading && jfx::GetHeadingChange(tObjPtr->mHeading, o->mHeading) > 90) {
+        return false;
+    }*/
+
+    return 0;
+}
+
+bool FusionAlgo::isTwoObjNear(TOBJPtr& tObjPtr, TOBJPtr& o, double dist)
+{
+    if (tObjPtr->mDevID == o->mDevID) {
+        if (tObjPtr->isVehicle()) {
+            if (dist > 1) {
+                return false;
+            }
+        } else {
+            return false;
+        }
+    } else {
+        if (tObjPtr->isVehicle() || o->isVehicle()) {
+            if (dist > max(2.8, tObjPtr->mWidth)) {
+                return false;
+            }
+        } else {
+            if (dist > 2) {
+                return false;
+            }
+        }
+    }
+
+    return true;
+}
+
+void FusionAlgo::updateNearInfo(TOBJPtr& tObjPtr, TOBJPtr& o, double dist, uint64_t dms, uint64_t oms)
+{
+    auto itrNear = tObjPtr->mNearObjMap.find(o->mKey);
+    if (itrNear == tObjPtr->mNearObjMap.end()) {
+        STNearObj nearObj;
+        nearObj.key = o->mKey;
+        nearObj.devIdStr = o->mDevIDStr;
+        nearObj.id = o->mID;
+        tObjPtr->mNearObjMap[nearObj.key] = nearObj;
+        itrNear = tObjPtr->mNearObjMap.find(o->mKey);
+    }
+
+    if (itrNear->second.lastMS == oms) {
+        return;
+    }
+
+    itrNear->second.count++;
+    itrNear->second.lastMissCount = 0;
+    itrNear->second.totalDist += dist;
+    itrNear->second.totalDMS += dms;
+    itrNear->second.lastLon = o->mLon;
+    itrNear->second.lastLat = o->mLat;
+
+    if (itrNear->second.count >= FConfPtr->mTrajNearMinPoint) {
+        if (itrNear->second.totalDist > 0 && itrNear->second.count > 0) {
+            double aveDist = itrNear->second.totalDist / itrNear->second.count;
+            double aveDMS = itrNear->second.totalDMS / itrNear->second.count;
+            itrNear->second.score = sqrt(itrNear->second.count) / (aveDist * log10(aveDMS + 10));
+        }
+
+        if (tObjPtr->mMaxNearScore < itrNear->second.score) {
+            tObjPtr->mMaxNearScore = itrNear->second.score;
+            tObjPtr->mMaxNearKey = itrNear->second.key;
+        }
+
+        //
+        itrNear = tObjPtr->mNearObjMap.find(tObjPtr->mMaxNearKey);
+        if (itrNear != tObjPtr->mNearObjMap.end()) {
+            tObjPtr->mMaxNearScore = itrNear->second.score;
+        }
+    }
+}
+
+void FusionAlgo::fitFObjNextPoint(FOBJPtr& fObj, jfx_common_msgs::OutFusionObj& obj)
+{
+    if (fObj->mFusionHisData.size() < 1 || fObj->mMainObj == nullptr) {
+        return;
+    }
+
+    int pos = fObj->mFusionHisData.size() > 4 ? fObj->mFusionHisData.size() - 4 : 0;
+    double lon = obj.lon, lat = obj.lat;
+    int count = 1;
+    for (int i = pos; i < (int)fObj->mFusionHisData.size(); i++) {
+        if (fObj->mFusionHisData[i].lon > DBL_EPSILON) {
+            lon += fObj->mFusionHisData[i].lon;
+            lat += fObj->mFusionHisData[i].lat;
+            count++;
+        }
+    }
+
+    obj.lon = lon / count;
+    obj.lat = lat / count;
+
+    /*
+    // 轨迹倒退或横向调整大的问题
+    auto p1 = fObj->mFusionHisData[0];
+    auto p4 = fObj->mFusionHisData[3];
+    double headingHis = jfx::GetDirectionXS(p1.lon, p1.lat, p4.lon, p4.lat);
+    double headingNow = jfx::GetDirectionXS(p4.lon, p4.lat, obj.lon, obj.lat);
+    double headingChange1 = jfx::GetHeadingChange(headingHis, headingNow);
+    if (headingHis != 0 && abs(headingChange1) > 90) {
+        obj.lon = p4.lon;
+        obj.lat = p4.lat;
+        return;
+    }
+
+    auto lastPoint = fObj->mFusionHisData[fObj->mFusionHisData.size() - 1];
+    double headingChange = abs(jfx::GetHeadingChange(lastPoint.heading, obj.heading));
+    if (headingChange > 30) {
+        return;
+    }
+
+    double totalDist = 0;
+    double count = 0;
+    double totalMS = obj.time - fObj->mFusionHisData.begin()->ms;
+    for (int i = 0; i < (fObj->mFusionHisData.size() - 1); i++) {
+        totalDist += jfx::GetDistance(fObj->mFusionHisData[i].lon, fObj->mFusionHisData[i].lat,
+            fObj->mFusionHisData[i + 1].lon, fObj->mFusionHisData[i + 1].lat);
+        count++;
+    }
+
+    double lastStep = jfx::GetDistance(lastPoint.lon, lastPoint.lat, obj.lon, obj.lat);
+    totalDist += lastStep;
+    count++;
+
+    double avgStep = totalDist / count;
+    if (lastStep <= (avgStep * 1.1) || avgStep == 0) {
+        return;
+    }
+
+    double lastHeading = jfx::GetDirectionXS(lastPoint.lon, lastPoint.lat, obj.lon, obj.lat) + 1.26;
+    lastHeading = lastHeading > 360 ? (lastHeading - 360) : lastHeading;
+    jfx::Array toPoint = jfx::GetPoint(lastPoint.lon, lastPoint.lat, avgStep, lastHeading);
+    obj.lon = toPoint[0];
+    obj.lat = toPoint[1];*/
+}
\ No newline at end of file

src/fusionCheckNearTask.cpp

+#include <algorithm>
+#include <math.h>
+#include <ros/ros.h>
+
+#include "fusionAlgo.h"
+#include "fusionCheckNearTask.h"
+#include "fusionConfig.h"
+#include "fusionData.h"
+#include "fusionTrajectionTask.h"
+#include "gaussian.h"
+#include "utils.h"
+
+FusionCheckNearTask::FusionCheckNearTask()
+{
+}
+
+FusionCheckNearTask::~FusionCheckNearTask()
+{
+}
+
+int FusionCheckNearTask::init(int taskID)
+{
+    mTaskID = taskID;
+    mRunThread = new std::thread(&FusionCheckNearTask::run, this);
+    return 0;
+}
+
+int FusionCheckNearTask::pushTrafficObj(STCheckNearItem& item)
+{
+    int nRet = 0;
+
+    mBufCheckItemListMutex.lock();
+    mBufCheckItemList.push_back(item);
+    nRet = mBufCheckItemList.size();
+    mBufCheckItemListMutex.unlock();
+
+    return nRet;
+}
+
+void FusionCheckNearTask::run()
+{
+    FusionAlgo algo;
+    std::shared_ptr<TrafficObject> tObjPtr = nullptr;
+    STCheckNearItem item;
+
+    ROS_INFO("cluster task %d started", mTaskID);
+
+    while (FConfPtr->mBAppExit == false) {
+        if (mBufCheckItemList.size() <= 0) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            continue;
+        }
+
+        // Get Obj
+        mBufCheckItemListMutex.lock();
+        item = mBufCheckItemList.front();
+        mBufCheckItemList.pop_front();
+#ifndef _DEBUG
+        if (mBufCheckItemList.size() > 500) {
+            mBufCheckItemList.clear();
+        }
+#endif // _DEBUG
+        mBufCheckItemListMutex.unlock();
+
+        if (mTaskID == 0) {
+            FDataPtr->mCountNear ++;
+        }
+
+        if (checkNear(item) != 0) {
+            continue;
+        }
+
+        if (mNextTask) {
+            mNextTask->pushTrafficObj(item);
+        } else if (item.pos >= FConfPtr->mObjFusionMinPoint) {
+            item.tObjPtr->mCheckNearPos = item.pos;
+            FTrajTaskPtr->pushTrafficObj(item);
+        }
+    }
+}
+
+int FusionCheckNearTask::checkNear(STCheckNearItem& item)
+{
+    auto tObjPtr = item.tObjPtr;
+
+    if (!tObjPtr) {
+        return -1;
+    }
+
+    if (tObjPtr->mDist2Center > FConfPtr->mFusionRadius) {
+        return 0;
+    }
+
+    auto checkPoint = tObjPtr->mHisData[item.pos];
+    mLastCheckDataMS = checkPoint.ms;
+
+    if (tObjPtr->mClusterID == mTaskID) {
+        mTrafficObjMap[tObjPtr->mKey] = tObjPtr;
+    }
+
+    if (tObjPtr->mHisData.size() <= 0) {
+        return -1;
+    }
+
+    if (item.pos >= tObjPtr->mHisData.size() || item.pos < 0) {
+        ROS_WARN("bad item pos value: %d", item.pos);
+        return -1;
+    }
+
+    if (item.pos > FConfPtr->mObjFusionMaxPoint) {
+        return 0;
+    }
+
+    std::unordered_map<uint64_t, TOBJPtr>::iterator itr = mTrafficObjMap.begin();
+    for (; itr != mTrafficObjMap.end();) {
+        if (itr->second->mValid == false) {
+            FDataPtr->eraseTrafficObj(itr->second->mKey);
+            mTrafficObjMap.erase(itr++);
+            continue;
+        } else {
+            if (itr->second->mFID == 0) {
+                if (mLastCheckDataMS > itr->second->mMS && (mLastCheckDataMS - itr->second->mMS) > 3 * FConfPtr->mObjRetainMS) {
+                    itr->second->mValid = false;
+                }
+            } else {
+                if (mLastCheckDataMS > itr->second->mMS && (mLastCheckDataMS - itr->second->mMS) > 20 * FConfPtr->mObjRetainMS) {
+                    itr->second->mValid = false;
+                }
+            }
+            checkTObjLost(itr->second);
+        }
+
+        TOBJPtr o = itr->second;
+        itr++;
+
+        if (item.pos >= item.tObjPtr->mHisData.size() || item.tObjPtr->mHisData.size() <= 0 || o->mHisData.size() <= 0) {
+            continue;
+        }
+
+        if (FusionAlgo::ifCanCheckNear(tObjPtr, o) != 0) {
+            continue;
+        }
+
+        uint64_t oms = 0, dms = 0;
+        double dist = FusionAlgo::calcuObjCurDist(item.pos, tObjPtr, o, oms, dms);
+        if (FusionAlgo::isTwoObjNear(tObjPtr, o, dist) == false) {
+            tObjPtr->setFarObj(o, dist);
+            continue;
+        }
+
+        tObjPtr->mNearObjMapMutex.lock();
+        FusionAlgo::updateNearInfo(tObjPtr, o, dist, dms, oms);
+        tObjPtr->mNearObjMapMutex.unlock();
+    }
+
+    return 0;
+}
+
+bool FusionCheckNearTask::checkTObjLost(TOBJPtr& tObjPtr)
+{
+    if (!tObjPtr || tObjPtr->mBLost) {
+        return true;
+    }
+
+    auto devPtr = FDataPtr->getSensDevice(tObjPtr->mDevID);
+    if (!devPtr) {
+        return true;
+    }
+
+    if (devPtr->mLLastDataMS > tObjPtr->mMS && (tObjPtr->mCheckNearPos + 1) >= tObjPtr->mHisData.size()) {
+        tObjPtr->mBLost = true;
+        return true;
+    }
+
+    return false;
+}

src/fusionCsv.cpp

+#include <unistd.h>
+#include <string.h>
+
+#include "fusionCsv.h"
+
+int FusionCsvFile::openRead(string fileName) {
+    if (access(fileName.c_str(), 0) != 0) {
+        return -1;
+    }
+
+    mInStream.open(fileName);
+    
+    return 0;
+}
+
+TOBJPtr FusionCsvFile::readLine() {
+    string buf(2048, 0);
+
+    if (!mInStream.is_open() || mInStream.eof()) {
+        return nullptr;
+    }
+
+    TOBJPtr obj = std::make_shared<TrafficObject>();
+
+    mInStream.getline((char*)buf.c_str(), 2048);
+
+    char* data = (char*)buf.c_str();
+    char* pos = strstr(data, ",");
+    if (pos) {
+        pos[0] = 0;
+        obj->mMS = atoll(data);
+        data += pos - data + 1;
+    }
+    
+    pos = strstr(data, ",");
+    if (pos) {
+        pos[0] = 0;
+        obj->mID = atoll(data);
+        data += pos - data + 1;
+    }
+
+    pos = strstr(data, ",");
+    if (pos) {
+        pos[0] = 0;
+        obj->mTypeStr = data;
+        data += pos - data + 1;
+    }
+
+    pos = strstr(data, ",");
+    if (pos) {
+        pos[0] = 0;
+        obj->mHeading = atof(data);
+        data += pos - data + 1;
+    }
+
+    pos = strstr(data, ",");
+    if (pos) {
+        pos[0] = 0;
+        obj->mLon = atof(data);
+        data += pos - data + 1;
+    }
+
+    pos = strstr(data, ",");
+    if (pos) {
+        pos[0] = 0;
+        obj->mLat = atof(data);
+        data += pos - data + 1;
+    }
+
+    pos = strstr(data, ",");
+    if (pos) {
+        pos[0] = 0;
+        obj->mDevIDStr = data;
+        data += pos - data + 1;
+    }
+    
+    pos = strstr(data, ",");
+    if (pos) {
+        pos[0] = 0;
+        data += pos - data + 1;
+    }
+
+    pos = strstr(data, ",");
+    if (pos) {
+        pos[0] = 0;
+        obj->mLength = atof(data);
+        data += pos - data + 1;
+    }
+
+    pos = strstr(data, ",");
+    if (pos) {
+        pos[0] = 0;
+        obj->mWidth = atof(data);
+        data += pos - data + 1;
+    }
+
+    pos = strstr(data, ",");
+    if (pos) {
+        pos[0] = 0;
+        obj->mModeStr = data;
+        data += pos - data + 1;
+    }
+
+    return obj;
+}
\ No newline at end of file

src/fusionData.cpp

+#include <ros/ros.h>
+#include <sstream>
+
+#include "fusionConfig.h"
+#include "fusionData.h"
+#include "gaussian.h"
+#include "utils.h"
+
+TOBJPtr FusionData::setTrafficObj(TOBJPtr& tObjPtr)
+{
+    if (!tObjPtr || tObjPtr->mID <= 0) {
+        return nullptr;
+    }
+
+    std::lock_guard<std::mutex> guard(mTrafficObjMapMainMutex);
+
+    if (mDataMSMax < tObjPtr->mMS) {
+        mDataMSMax = tObjPtr->mMS;
+    }
+
+    // 查找存储的目标对象
+    uint64_t objKey = genTfcObjKey(tObjPtr->mDevID, tObjPtr->mID);
+    std::unordered_map<uint64_t, TOBJPtr>::iterator itr = mTrafficObjMainMap.find(objKey);
+    if (itr == mTrafficObjMainMap.end()) {
+        tObjPtr->mKey = objKey;
+        tObjPtr->mFMS = tObjPtr->mMS;
+        tObjPtr->mIdx = mTfcObjIdx++;
+        mTrafficObjMainMap[objKey] = tObjPtr;
+        itr = mTrafficObjMainMap.find(objKey);
+    }
+
+    if (itr == mTrafficObjMainMap.end() || itr->second == nullptr) {
+        return nullptr;
+    }
+
+    // 更新目标属性
+    itr->second->mLon = tObjPtr->mLon;
+    itr->second->mLat = tObjPtr->mLat;
+    itr->second->mMS = tObjPtr->mMS;
+    itr->second->mLength = tObjPtr->mLength;
+    itr->second->mWidth = tObjPtr->mWidth;
+    itr->second->mHeight = tObjPtr->mHeight;
+    itr->second->mLaneSeqNum = tObjPtr->mLaneSeqNum;
+
+    itr->second->mType = tObjPtr->mType;
+
+    itr->second->mCount++;
+    itr->second->mFusionMS = mDataMSMax;
+    itr->second->mBLost = false;
+    itr->second->mValid = true;
+
+    itr->second->mHeading = itr->second->genHeading();
+    itr->second->mSpeed = itr->second->genSpeedX();
+    itr->second->mDist2Center = tObjPtr->mDist2Center;
+
+    double headingReal = itr->second->genHeading(3);
+
+    // 更新目标历史轨迹
+    STObjTrajPoint hisData(itr->second->mMS, itr->second->mLon, itr->second->mLat,
+        itr->second->mHeading, headingReal, itr->second->mSpeed, itr->second->mLaneSeqNum);
+    hisData.isUseHeadingKeep = itr->second->mIsUseHeadingKeep;
+    itr->second->mHisDataReal.push_back(hisData);
+    itr->second->mHisData.push_back(hisData);
+
+    return itr->second;
+}
+
+TOBJPtr FusionData::getTrafficObj(uint64_t key)
+{
+    auto itr = mTrafficObjMainMap.find(key);
+    if (itr != mTrafficObjMainMap.end()) {
+        return itr->second;
+    }
+    return nullptr;
+}
+
+void FusionData::eraseTrafficObj(uint64_t key)
+{
+    std::lock_guard<std::mutex> guard(mTrafficObjMapMainMutex);
+    mTrafficObjMainMap.erase(key);
+}
+
+std::shared_ptr<SensorDevice> FusionData::genSensDevice(TOBJPtr& tObjPtr)
+{
+    if (!tObjPtr || tObjPtr->mModeStr.size() < 0 || tObjPtr->mDevIDStr.size() <= 0) {
+        ROS_WARN("SensorDevice has no ID!");
+        return nullptr;
+    }
+
+    uint64_t devID = genSensDevID(tObjPtr->mModeStr, tObjPtr->mDevIDStr);
+
+    mSensDeviceMapMutex.lock();
+    std::unordered_map<uint64_t, std::shared_ptr<SensorDevice>>::iterator itr = mSensDeviceMap.find(devID);
+    if (itr == mSensDeviceMap.end()) {
+        std::shared_ptr<SensorDevice> sensDevPtr = std::make_shared<SensorDevice>();
+        sensDevPtr->mMode = modeStrToID(tObjPtr->mModeStr);
+        sensDevPtr->mModeStr = tObjPtr->mModeStr;
+        sensDevPtr->mIDStr = tObjPtr->mDevIDStr;
+        sensDevPtr->mID = devID;
+        sensDevPtr->mLLastDataMS = tObjPtr->mMS;
+        sensDevPtr->mLastDataMS = tObjPtr->mMS;
+        sensDevPtr->mFrameIdx = 1;
+        mSensDeviceMap[devID] = sensDevPtr;
+        itr = mSensDeviceMap.find(devID);
+    }
+    mSensDeviceMapMutex.unlock();
+
+    if (itr == mSensDeviceMap.end()) {
+        return nullptr;
+    }
+
+    if (itr->second->mLastDataMS < tObjPtr->mMS) {
+        itr->second->mFrameIdx++;
+        itr->second->mLLastDataMS = itr->second->mLastDataMS;
+    }
+    itr->second->mLastDataMS = tObjPtr->mMS;
+    itr->second->mLastDataLocalMS = mills_UTC();
+    itr->second->mDataDelayMS = itr->second->mLastDataLocalMS - tObjPtr->mMS;
+
+    return itr->second;
+}
+
+SDEVPtr FusionData::getSensDevice(uint64_t did)
+{
+    std::lock_guard<std::mutex> guard(mSensDeviceMapMutex);
+    auto itr = mSensDeviceMap.find(did);
+    if (itr != mSensDeviceMap.end()) {
+        return itr->second;
+    }
+    return nullptr;
+}
+
+FOBJPtr FusionData::getFusionObj(uint64_t fid)
+{
+    std::lock_guard<std::mutex> guard(mFusionObjMapMutex);
+    std::unordered_map<uint64_t, FOBJPtr>::iterator itr = mFusionObjMap.find(fid);
+    if (itr != mFusionObjMap.end()) {
+        return itr->second;
+    }
+
+    return nullptr;
+}
+
+FOBJPtr FusionData::setFusionObj(FOBJPtr& fObjPtr)
+{
+    if (fObjPtr) {
+        std::lock_guard<std::mutex> guard(mFusionObjMapMutex);
+        mFusionObjMap[fObjPtr->mID] = fObjPtr;
+        return fObjPtr;
+    }
+    return nullptr;
+}
+
+void FusionData::eraseFusionObj(uint64_t fid)
+{
+    std::lock_guard<std::mutex> guard(mFusionObjMapMutex);
+    std::unordered_map<uint64_t, FOBJPtr>::iterator itr = mFusionObjMap.find(fid);
+    if (itr == mFusionObjMap.end()) {
+        return;
+    }
+
+    mFusionObjMap.erase(fid);
+}
+
+uint64_t FusionData::genSensDevID(string mode, string devIDStr)
+{
+    uint64_t devID = 0;
+
+    mSensDeviceIDMapMutex.lock();
+
+    std::string key = mode + "-" + devIDStr;
+    std::unordered_map<string, uint64_t>::iterator itr = mSensDeviceIDStr2Int64.find(key);
+    if (itr != mSensDeviceIDStr2Int64.end()) {
+        devID = itr->second;
+    } else {
+        devID = mSensDevIdx++;
+        mSensDeviceIDStr2Int64[key] = devID;
+    }
+
+    mSensDeviceIDMapMutex.unlock();
+
+    return devID;
+}
+
+uint64_t FusionData::genTfcObjKey(uint64_t devID, uint64_t objOriginID)
+{
+    return (devID << 48) + objOriginID;
+}
+
+uint64_t FusionData::genFusionObjID()
+{
+    uint64_t fObjID = mills_UTC();
+    return fObjID * 10000 + mFusionObjIdx++;
+}
+
+ENTrafficSensMode FusionData::modeStrToID(string modeStr)
+{
+    ENTrafficSensMode enMode = enTSModeUnknown;
+    if (modeStr == "r") {
+        enMode = enTSModeRadar;
+    } else if (modeStr == "v") {
+        enMode = enTSModeVideo;
+    }
+
+    return enMode;
+}
+
+ENTrafficObjectType FusionData::typeStr2ID(string typeStr)
+{
+    ENTrafficObjectType enType = enTOTypeUnknown;
+
+    if (typeStr == "Person") {
+        enType = enTOTypePerson;
+    } else if (typeStr == "Bicycle") {
+        enType = enTOTypeBicycle;
+    } else if (typeStr == "Motorbike") {
+        enType = enTOTypeMotorbike;
+    } else if (typeStr == "Car") {
+        enType = enTOTypeCar;
+    } else if (typeStr == "Truck") {
+        enType = enTOTypeTruck;
+    } else if (typeStr == "Bus") {
+        enType = enTOTypeBus;
+    }
+
+    return enType;
+}
+
+string FusionData::getStateStr()
+{
+    std::lock_guard<std::mutex> guard(mSensDeviceMapMutex);
+
+    std::stringstream ss;
+    ss << "{\"node\": \"\","
+       << "\"type\": \"fusion\","
+       << "\"state\": [";
+    std::unordered_map<uint64_t, SDEVPtr>::iterator itr = mSensDeviceMap.begin();
+    for (; itr != mSensDeviceMap.end(); ++itr) {
+        if (itr != mSensDeviceMap.begin()) {
+            ss << ",";
+        }
+        ss << "{"
+           << "\"devid\": \"" << itr->second->mIDStr << "\","
+           << "\"mode\": \"" << itr->second->mModeStr << "\","
+           << "\"delay\": " << itr->second->mDataDelayMS << ","
+           << "\"count\": " << itr->second->mFrameIdx << ","
+           << "\"ms\": " << itr->second->mLastDataMS << "}";
+    }
+    ss << "]}";
+
+    return ss.str();
+}

src/fusionObject.cpp

+
+#include "fusionObject.h"
+#include "gaussian.h"
+#include <ros/ros.h>
+
+void FusionObject::setTObj(TOBJPtr& tObjPtr)
+{
+    if (!tObjPtr) {
+        return;
+    }
+
+    std::lock_guard<std::mutex> guard(mTObjMapMutex);
+
+    if (tObjPtr->mMode == enTSModeLidar) {
+        mLTObjMap[tObjPtr->mKey] = tObjPtr;
+    } else if (tObjPtr->mMode == enTSModeRadar) {
+        mRTObjMap[tObjPtr->mKey] = tObjPtr;
+    } else if (tObjPtr->mMode == enTSModeVideo) {
+        mVTObjMap[tObjPtr->mKey] = tObjPtr;
+    }
+
+    mMS = tObjPtr->mMS;
+
+    tObjPtr->mFID = mID;
+}
+
+void FusionObject::removeTObj(TOBJPtr& tObjPtr)
+{
+    std::lock_guard<std::mutex> guard(mTObjMapMutex);
+
+    if (tObjPtr->mMode == enTSModeLidar) {
+        mLTObjMap.erase(tObjPtr->mKey);
+    } else if (tObjPtr->mMode == enTSModeRadar) {
+        mRTObjMap.erase(tObjPtr->mKey);
+    } else if (tObjPtr->mMode == enTSModeVideo) {
+        mVTObjMap.erase(tObjPtr->mKey);
+    }
+
+    tObjPtr->mFID = 0;
+}
+
+bool FusionObject::isEmpty()
+{
+    auto objCount = mLTObjMap.size() + mRTObjMap.size() + mVTObjMap.size();
+    return objCount == 0;
+}
+
+void FusionObject::setInValid()
+{
+    std::map<uint64_t, TOBJPtr>::iterator itrR = mRTObjMap.begin();
+    for (; itrR != mRTObjMap.end(); ++itrR) {
+        itrR->second->mValid = false;
+        itrR->second->mFID = 0;
+    }
+
+    std::map<uint64_t, TOBJPtr>::iterator itrL = mLTObjMap.begin();
+    for (; itrL != mLTObjMap.end(); ++itrL) {
+        itrL->second->mValid = false;
+        itrL->second->mFID = 0;
+    }
+
+    std::map<uint64_t, TOBJPtr>::iterator itrV = mVTObjMap.begin();
+    for (; itrV != mVTObjMap.end(); ++itrV) {
+        itrV->second->mValid = false;
+        itrV->second->mFID = 0;
+    }
+}
+
+void FusionObject::setHisPoint(jfx_common_msgs::OutFusionObj& obj)
+{
+    STObjTrajPoint p;
+    p.heading = obj.heading;
+    p.speed = obj.speed;
+    p.lon = obj.lon;
+    p.lat = obj.lat;
+    p.ms = obj.time;
+
+    mFusionHisData.push_back(p);
+    if (mFusionHisData.size() > 10) {
+        mFusionHisData.erase(mFusionHisData.begin());
+    }
+}
+
+std::string FusionObject::getTrueTypeStr()
+{
+    std::map<uint64_t, TOBJPtr>::iterator itr = mVTObjMap.begin();
+    if (itr != mVTObjMap.end()) {
+        return itr->second->mTypeStr;
+    }
+
+    itr = mRTObjMap.begin();
+    if (itr != mRTObjMap.end()) {
+        return itr->second->mTypeStr;
+    }
+
+    return "Person";
+}
+
+double FusionObject::getHeading(double curLon, double curLat)
+{
+    if (mFusionHisData.empty()) {
+        return mHeading;
+    }
+
+    double dist = jfx::GetDistance(mLastCalcuHeadingLon, mLastCalcuHeadingLat, curLon, curLat);
+    if (dist < 3) {
+        return mHeading;
+    }
+
+    int pos = mFusionHisData.size() > 5 ? mFusionHisData.size() - 5 : 0;
+    STObjTrajPoint& hisPoint = mFusionHisData[pos];
+
+    mHeading = jfx::GetDirectionXS(hisPoint.lon, hisPoint.lat, curLon, curLat);
+
+    mLastCalcuHeadingLon = curLon;
+    mLastCalcuHeadingLat = curLat;
+
+    return mHeading;
+}
\ No newline at end of file

src/main.cpp

+#include "fusionAlgo.h"
+#include "fusionConfig.h"
+#include "fusionData.h"
+#include "fusionDef.h"
+#include "fusionDistribTask.h"
+#include "fusionGeo.h"
+#include "fusionObject.h"
+#include "fusionPubTask.h"
+#include "fusionTrajectionTask.h"
+#include "sensDevice.h"
+#include "trafficObject.h"
+#include "fusionDataLog.h"
+
+int main(int argc, char* argv[])
+{
+    ros::init(argc, argv, "fusion_cx");
+    ros::NodeHandle nh("~");
+
+    std::string confFile;
+    nh.param<std::string>("config_file", confFile, "/workspace/etc/rvfusion_config.yaml");
+
+    FConfPtr->init(confFile);
+    FPubTaskPtr->init(nh);
+    FTrajTaskPtr->init();
+
+    if (FDistribTaskPtr->init(nh, "/workspace/data/xishan/rv.csv") != 0) {
+    //if (FDistribTaskPtr->init(nh) != 0) {
+        return -1;
+    }
+
+    ROS_INFO("mec id: %s", FConfPtr->mMecID.c_str());
+    ROS_INFO("log path: %s", FConfPtr->mLogPath.c_str());
+
+    ros::spin();
+    return 0;
+}
\ No newline at end of file

src/trafficObject.cpp

+#include "trafficObject.h"
+#include "fusionAlgo.h"
+#include "fusionConfig.h"
+#include "gaussian.h"
+#include "fusionData.h"
+#include <algorithm>
+
+TrafficObject::TrafficObject(ENTrafficSensMode mode)
+{
+    mMode = mode;
+}
+
+ENTrafficObjectType TrafficObject::setType(ENTrafficObjectType type)
+{
+    return enTOTypeUnknown;
+}
+
+double TrafficObject::genHeading(int steps/* = 10*/)
+{
+    if (mHisDataReal.size() <= 1) {
+        return mHeading;
+    }
+
+    double dist = jfx::GetDistance(mLon, mLat, mLastCalcuHeadingLon, mLastCalcuHeadingLat);
+    if (dist < 2) {
+        return mHeading;
+    }
+
+    mLastCalcuHeadingLon = mLon;
+    mLastCalcuHeadingLat = mLat;
+
+    // 根据航向保持配置保持航向
+    if (isVehicle()) {
+        auto itr = FConfPtr->mHeadingKeepArea.begin();
+        for (; itr != FConfPtr->mHeadingKeepArea.end(); ++itr) {
+            dist = jfx::GetDistance(mLon, mLat, itr->lon, itr->lat);
+            if (dist <= itr->radius) {
+                mIsUseHeadingKeep = true;
+                return itr->heading;
+            }
+        }
+    }
+    mIsUseHeadingKeep = false;
+
+    int hisCount = (int)mHisDataReal.size();
+    int pos = min(hisCount, steps);
+
+    STObjTrajPoint hisData1 = mHisDataReal[hisCount - pos];
+    return FusionAlgo::geoGetHeading(hisData1.lon, hisData1.lat, mLon, mLat);
+}
+
+double TrafficObject::genSpeed(STObjTrajPoint& p1, STObjTrajPoint& p2)
+{
+    double dts = abs((int)(p1.ms - p2.ms));
+    if (dts == 0) {
+        return 0;
+    }
+    double dist = jfx::GetDistance(p1.lon, p1.lat, p2.lon, p2.lat);
+    return abs((dist * 1000) / dts);
+}
+
+double TrafficObject::genSpeed(int steps)
+{
+    if (steps <= 1) {
+        return mSpeed;
+    }
+
+    int hisCount = (int)mHisDataReal.size();
+    if (hisCount <= 1) {
+        return mSpeed;
+    }
+
+    steps = min(steps, hisCount);
+    STObjTrajPoint p1 = mHisDataReal[hisCount - steps];
+    STObjTrajPoint p2 = mHisDataReal[hisCount - 1];
+
+    return genSpeed(p1, p2);
+}
+
+double TrafficObject::genSpeed(int hisPos1, int hisPos2)
+{
+    int hisCount = (int)mHisDataReal.size();
+    if (hisPos1 >= hisCount || hisPos1 >= hisPos2) {
+        return 0;
+    }
+
+    int fromPos = max(0, hisPos1);
+    int toPos = min(hisPos2, hisCount - 1);
+
+    STObjTrajPoint p1 = mHisDataReal[fromPos];
+    STObjTrajPoint p2 = mHisDataReal[toPos];
+
+    return genSpeed(p1, p2);
+}
+
+double TrafficObject::genSpeedX()
+{
+    double speed = 0;
+
+    int hisCount = (int)mHisDataReal.size();
+    if (hisCount <= 6) {
+        speed = genSpeed(6);
+    } else {
+        int round = min(hisCount - 5, 5);
+        double speedAll = 0;
+        double count = 0;
+        for (int i = 0; i < round; i++) {
+            speedAll += genSpeed(hisCount - 5 - i, hisCount - 1 - i);
+            count++;
+        }
+        if (count) {
+            speed = speedAll / count;
+        }
+    }
+
+    if (speed < 0) {
+        speed = 0;
+    } else if (speed > 45) {
+        speed = 45;
+    }
+
+    return speed;
+}
+
+bool TrafficObject::isVehicle()
+{
+    if (mType == enTOTypePerson || mType == enTOTypeBicycle || mType == enTOTypeMotorbike) {
+        return false;
+    }
+    return true;
+}
+
+bool TrafficObject::isSameType(std::shared_ptr<TrafficObject>& tObjPtr)
+{
+    return isVehicle() == tObjPtr->isVehicle();
+}
+
+void TrafficObject::setFarObj(std::shared_ptr<TrafficObject>& tObjPtr, double dist)
+{
+    std::lock_guard<std::mutex> guardFarObj(mFarObjMapMutex);
+
+    auto nearObjItr = mNearObjMap.find(tObjPtr->mKey);
+    if (nearObjItr != mNearObjMap.end()) {
+        nearObjItr->second.lastMissCount ++;
+    }
+
+    // More than 10 meters is the far target
+    if (dist < 10) {
+        return;
+    }
+
+    // Update far object map
+    auto farObjItr = mFarObjMap.find(tObjPtr->mKey);
+    if (farObjItr == mFarObjMap.end()) {
+        STFarObj farObj;
+        farObj.key = tObjPtr->mKey;
+        farObj.devIdStr = tObjPtr->mDevIDStr;
+        farObj.id = tObjPtr->mID;
+
+        mFarObjMap[farObj.key] = farObj;
+        farObjItr = mFarObjMap.find(tObjPtr->mKey);
+    }
+
+    farObjItr->second.count++;
+    farObjItr->second.totalDist += dist;
+    if (dist > farObjItr->second.maxDist) {
+        farObjItr->second.maxDist = dist;
+    }
+
+    // ToDO: Remove far object from near object
+    double avgDist = farObjItr->second.count > 0 ? farObjItr->second.totalDist / farObjItr->second.count : 0;
+    if (farObjItr->second.count >= 5 || avgDist >= 10) {
+        auto nearObjItr = mNearObjMap.find(tObjPtr->mKey);
+        if (nearObjItr != mNearObjMap.end()) {
+            // ToDO:
+        }
+    }
+}
+
+bool TrafficObject::isFarObj(std::shared_ptr<TrafficObject>& tObjPtr)
+{
+    auto farObjItr = mFarObjMap.find(tObjPtr->mKey);
+    if (farObjItr == mFarObjMap.end()) {
+        return false;
+    }
+
+    double avgDist = farObjItr->second.count > 0 ? farObjItr->second.totalDist / farObjItr->second.count : 0;
+    if (farObjItr->second.count < 5 || avgDist < 10) {
+        return false;
+    }
+
+    return true;
+}
+
+void TrafficObject::removeNearObj(uint64_t key)
+{
+    std::lock_guard<std::mutex> guardNearObj(mNearObjMapMutex);
+    mNearObjMap.erase(key);
+}
+
+/// Reset max near score
+void TrafficObject::resetMaxScoreObj()
+{
+    std::lock_guard<std::mutex> guardNearObj(mNearObjMapMutex);
+
+    mMaxNearScore = 0;
+    mMaxNearKey = 0;
+    for (auto itr : mNearObjMap) {
+        if (itr.second.score > mMaxNearScore && mCount > 0 && ((double)itr.second.count / mCount) >= 0.7) {
+            mMaxNearScore = itr.second.score;
+            mMaxNearKey = itr.second.key;
+        }
+    }
+}
+
+/// Get the hisdata closest to this time
+STObjTrajPoint* TrafficObject::getTrajPointClosestToMS(uint64_t ms)
+{
+    if (mHisData.empty()) {
+        return nullptr;
+    }
+
+    // Default use last one
+    STObjTrajPoint* trajPointPtr = &(mHisData[mHisData.size() - 1]);
+
+    // ToDO: 防止过度循环浪费时间
+    int maxMSDiff = abs((int)(trajPointPtr->ms - ms));
+    int count = mHisData.size() - 1;
+    for (int i = count; i >= 0; i--) {
+        int msDiff = abs((int)(mHisData[i].ms - ms));
+        if (msDiff <= maxMSDiff) {
+            trajPointPtr = &(mHisData[i]);
+            maxMSDiff = msDiff;
+        } else {
+            break;
+        }
+    }
+
+    return trajPointPtr;
+}
+
+bool TrafficObject::isNearStillNear(STNearObj& nearInfo) {
+    if (mCount <= 0) {
+        return false;
+    }
+
+    // At least 70% point near( 主要是时间延迟，车前后的情况导致匹配点少 )
+    if (((double)nearInfo.count / mCount) < 0.7 && nearInfo.lastMissCount > 10) {
+        return false;
+    }
+
+    if (((double)nearInfo.count / mCount) < 0.5 && nearInfo.lastMissCount > 5) {
+        return false;
+    }
+
+    if (((double)nearInfo.count / mCount) < 0.2) {
+        return false;
+    }
+
+    if (nearInfo.lastMissCount >= 20) {
+        return false;
+    }
+
+    // ToDO: if null?
+    auto tObjNear = FDataPtr->getTrafficObj(nearInfo.key);
+    if (tObjNear) {
+        double dist = jfx::GetDistance(mLon, mLat, tObjNear->mLon, tObjNear->mLat);
+        if (dist > 15) {
+            return false;
+        }
+    }
+
+    return true;
+}
\ No newline at end of file

test/test.cpp

+#include "fusionCheckNearTask.h"
+#include "fusionConfig.h"
+#include "fusionCsv.h"
+#include "fusionData.h"
+#include "fusionDistribTask.h"
+#include "fusionTrajectionTask.h"
+
+int main(int argc, char* argv[])
+{
+    FusionDistributeTask dist;
+    FusionCheckNearTask near;
+    FusionTrajectionTask traj;
+
+    FusionData* pData = FDataPtr;
+    FusionConfig* pConfig = FConfPtr;
+
+    if (pConfig->init("/workspace/etc/rvfusion_config.yaml") != 0) {
+        return -1;
+    }
+
+    traj.init();
+
+    FusionCsvFile csv;
+    if (csv.openRead(argv[1]) != 0) {
+        return -1;
+    }
+
+    TOBJPtr tObj = csv.readLine();
+    while (tObj) {
+        if (tObj->mMS >= 1655280347967) {
+            int i = 0;
+            i = 2;
+        }
+        if (tObj->mID == 22763425) {
+            int i = 0;
+            i = 2;
+        }
+        if (tObj->mID == 362 && tObj->mDevIDStr == "xianfengchunfengd") {
+            int i = 0;
+            i = 2;
+        }
+
+        TOBJPtr tStoreObj = dist.doTObj(tObj);
+        if (tStoreObj) {
+            STCheckNearItem item(tStoreObj);
+            if (near.checkNear(item) == 0) {
+                FOBJPtr fObj = traj.genFusionObj(item);
+                if (fObj) {
+                    if (tStoreObj->mMS > traj.mLastFusionDataMS) {
+                        traj.mLastFusionDataMS = tStoreObj->mMS;
+                    }
+                    tStoreObj->mOutPos = item.pos;
+                    traj.outFusionObj(item.pos, fObj, tStoreObj);
+                }
+            }
+        }
+
+        tObj = csv.readLine();
+    }
+
+    return 0;
+}
\ No newline at end of file