add udp server

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 udpSrv();
     void run();
 
     std::shared_ptr<TrafficObject> doTObj(std::shared_ptr<TrafficObject>& tObjPtr, uint64_t nextQueueSize = 0);
@@ -48,6 +49,7 @@ public:
 
 public:
     std::thread* mRunThread = nullptr;
+    std::thread* mUdpSrvThread = nullptr;
 
     FusionDataLog mOriginDataLog;
 

src/fusionDistribTask.cpp

@@ -7,6 +7,12 @@
 #include "fusionTrajectionTask.h"
 #include "gaussian.h"
 #include "utils.h"
+#include <ctype.h>
+#include <netinet/in.h>
+#include <stdio.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <unistd.h>
 
 FusionDistributeTask::FusionDistributeTask()
 {
@@ -54,6 +60,12 @@ int FusionDistributeTask::init(ros::NodeHandle& nh, string replayCsv)
         return -1;
     }
 
+    mUdpSrvThread = new std::thread(&FusionDistributeTask::udpSrv, this);
+    if (!mUdpSrvThread) {
+        ROS_WARN("new std::thread fail");
+        return -1;
+    }
+
     mRadarDataSub = nh.subscribe("/RadarData", 100, &FusionDistributeTask::onRadarData, this);
     mVisionDataSub = nh.subscribe("/TrackingRes", 100, &FusionDistributeTask::onVisionData, this);
 
@@ -148,6 +160,54 @@ void FusionDistributeTask::onVisionData(const jfx_common_msgs::det_tracking_arra
     }
 }
 
+// ToDO: 用实际的结构代替这个
+typedef struct _tagSTTempData {
+} STTempData;
+void FusionDistributeTask::udpSrv()
+{
+    int socketHandle;
+    struct sockaddr_in server, client;
+    STTempData data;
+
+    socketHandle = socket(AF_INET, SOCK_DGRAM, 0);
+    if (-1 == socketHandle) {
+        ROS_WARN("udpSrv creat socket fail");
+        return;
+    }
+
+    server.sin_family = AF_INET;
+    server.sin_port = htons(9009);
+    server.sin_addr.s_addr = htonl(INADDR_ANY);
+
+    int b = bind(socketHandle, (struct sockaddr*)&server, sizeof(server));
+    if (-1 == b) {
+        ROS_WARN("udpSrv bind port fail");
+        return;
+    }
+
+    char buf[10240] = { 0 };
+    while (FConfPtr->mBAppExit == false) {
+        int cliLen = sizeof(client);
+        int ret = recvfrom(socketHandle, (void*)&data, (size_t)sizeof(data), 0, (struct sockaddr*)&client, (socklen_t*)&cliLen);
+        if (ret != sizeof(data)) {
+            continue;
+        }
+        
+        std::shared_ptr<TrafficObject> tObjPtr = std::make_shared<TrafficObject>(enTSModeRadar);
+        if (!tObjPtr) {
+            continue;
+        }
+
+        // ToDO: set tObjPtr value
+
+        mBufTrafficObjListMutex.lock();
+        mBufTrafficObjList.push_back(tObjPtr);
+        mBufTrafficObjListMutex.unlock();
+    }
+
+    close(socketHandle);
+}
+
 void FusionDistributeTask::run()
 {
     FusionAlgo algo;
@@ -191,7 +251,7 @@ void FusionDistributeTask::run()
             continue;
         }
 
-        FDataPtr->mCountIn ++;
+        FDataPtr->mCountIn++;
 
         if (FConfPtr->mBLogInData) {
             mOriginDataLog.save(tObjPtr);