debug for new version

incl/fusionDistribTask.h

@@ -36,6 +36,7 @@ public:
 
     void onRadarData(const jfx_common_msgs::RadarData& data);
     void onVisionData(const jfx_common_msgs::det_tracking_array& data);
+    void onLidarData(const jfx_common_msgs::det_tracking_array& data);
 
     void udpSrv();
     void run();
@@ -58,6 +59,7 @@ public:
     std::string mInCsvFileName;
     ros::Subscriber mRadarDataSub;
     ros::Subscriber mVisionDataSub;
+    ros::Subscriber mLidarDataSub;
 
     std::mutex mBufTrafficObjListMutex;
     std::list<std::shared_ptr<TrafficObject>> mBufTrafficObjList;

src/fusionDistribTask.cpp

@@ -68,9 +68,11 @@ int FusionDistributeTask::init(ros::NodeHandle& nh, string replayCsv)
 
     mRadarDataSub = nh.subscribe("/RadarData", 100, &FusionDistributeTask::onRadarData, this);
     mVisionDataSub = nh.subscribe("/TrackingRes", 100, &FusionDistributeTask::onVisionData, this);
+    mLidarDataSub = nh.subscribe("/tracking_res", 1000, &FusionDistributeTask::onLidarData, this);
 
     ROS_INFO("subscribe /RadarData ok");
     ROS_INFO("subscribe /TrackingRes ok");
+    ROS_INFO("subscribe /tracking_res ok")
 
     return 0;
 }
@@ -160,6 +162,40 @@ void FusionDistributeTask::onVisionData(const jfx_common_msgs::det_tracking_arra
     }
 }
 
+void FusionDistributeTask::onLidarData(const jfx_common_msgs::det_tracking_array& data)
+{
+    if (data.array.size() <= 0) {
+        return;
+    }
+
+    for (const auto& obj : data.array) {
+        std::shared_ptr<TrafficObject> tObjPtr = std::make_shared<TrafficObject>(enTSModeRadar);
+        if (!tObjPtr) {
+            return;
+        }
+
+        tObjPtr->mDevIDStr = obj.color_name;
+        tObjPtr->mID = obj.obj_id;
+        tObjPtr->mModeStr = "r";
+        tObjPtr->mMS = obj.frame;
+        tObjPtr->mLon = obj.Long;
+        tObjPtr->mLat = obj.Lat;
+        tObjPtr->mAlt = 0;
+        tObjPtr->mHeading = 0;
+        tObjPtr->mSpeed = 0;
+        tObjPtr->mLength = obj.l;
+        tObjPtr->mWidth = obj.w;
+        tObjPtr->mHeight = obj.h;
+        tObjPtr->mXPos = obj.x;
+        tObjPtr->mYPos = obj.y;
+        tObjPtr->mTypeStr = obj.name;
+        tObjPtr->mSubType = 0;
+
+        mBufTrafficObjListMutex.lock();
+        mBufTrafficObjList.push_back(tObjPtr);
+        mBufTrafficObjListMutex.unlock();
+    }
+}
 
 void FusionDistributeTask::udpSrv()
 {