From 86da302163f782490dc1f8818e89342455fc0231 Mon Sep 17 00:00:00 2001
From: mive93 <micaelaverucchi@gmail.com>
Date: Tue, 14 May 2019 08:38:52 +0200
Subject: [PATCH] added send of trackers infos

---
 demo/demo/demo.cpp   | 22 +++++++++++--------
 include/classutils.h | 51 +++++++++++++++++++++++++++++++++++++++++---
 tracker_CLASS        |  2 +-
 3 files changed, 62 insertions(+), 13 deletions(-)

diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp
index 5472bc1..c59f426 100644
--- a/demo/demo/demo.cpp
+++ b/demo/demo/demo.cpp
@@ -126,6 +126,8 @@ int main(int argc, char *argv[])
     Communicator Comm(SOCK_DGRAM);
     Comm.open_client_socket("127.0.0.1", 8888);
     Message *m = new Message;
+    m->cam_idx = CAM_IDX;
+    m->lights.clear();
 
     /*Conversion for tracker, from gps to meters and viceversa*/
     geodetic_converter::GeodeticConverter gc;
@@ -152,7 +154,6 @@ int main(int argc, char *argv[])
     cv::Size S = cv::Size((int)cap.get(cv::CAP_PROP_FRAME_WIDTH), //Acquire input size
                           (int)cap.get(cv::CAP_PROP_FRAME_HEIGHT));
     outputVideo.open("test.avi", static_cast<int>(cap.get(cv::CAP_PROP_FOURCC)), cap.get(cv::CAP_PROP_FPS), S, true);*/
-    
 
     while (gRun)
     {
@@ -202,8 +203,6 @@ int main(int argc, char *argv[])
 
                 if (objClass < 6)
                 {
-                    
-                    
                     convert_coords(coords, x0 + b.w / 2, y1, objClass, H, adfGeoTransform, frame_nbr);
 
                     //std::cout<<objClass<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
@@ -226,7 +225,7 @@ int main(int argc, char *argv[])
         for (size_t i = 0; i < coords.size(); i++)
         {
             gc.geodetic2Enu(coords[i].lat_, coords[i].long_, 0, &east, &north, &up);
-            cur_frame.push_back(Data(east, north, frame_nbr));
+            cur_frame.push_back(Data(east, north, frame_nbr, coords[i].class_));
         }
         if (frame_nbr == 0)
         {
@@ -241,6 +240,15 @@ int main(int argc, char *argv[])
         TIMER_STOP
         std::cout << "There are " << trackers.size() << " trackers" << std::endl;
 
+
+        //prepare message with tracker info
+        addRoadUserfromTracker(trackers, m, gc);
+        //prepare the message with detection info
+        //prepare_message(m, coords, CAM_IDX);
+        //send message
+        Comm.send_message(m);
+
+
         if (to_show)
         {
             frame_top = original_frame_top.clone();
@@ -280,8 +288,6 @@ int main(int argc, char *argv[])
             }
         }
 
-
-
         if (to_show)
         {
             sem.lock();
@@ -302,9 +308,7 @@ int main(int argc, char *argv[])
         }
 
         frame_nbr++;
-        prepare_message(m, coords, CAM_IDX);
-        Comm.send_message(m);
-
+        
     }
 
     free(adfGeoTransform);
diff --git a/include/classutils.h b/include/classutils.h
index ce21f6b..31f04c0 100644
--- a/include/classutils.h
+++ b/include/classutils.h
@@ -17,6 +17,8 @@
 #include "gdal/gdal_priv.h"
 #include "gdal/cpl_conv.h"
 
+#include "tracker.h"
+
 #include <yaml-cpp/yaml.h>
 
 #include "../masa_protocol/include/send.hpp"
@@ -76,8 +78,6 @@ void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform
 
     lon = a * x + b * y + xoff;
     lat = d * x + e * y + yoff;
-
-    
 }
 void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform)
 {
@@ -176,6 +176,51 @@ unsigned long long time_in_ms()
     return t_stamp_ms;
 }
 
+void addRoadUserfromTracker(const std::vector<Tracker>& trackers, Message *m, geodetic_converter::GeodeticConverter& gc)
+{
+    m->t_stamp_ms = time_in_ms();
+    m->num_objects = trackers.size();
+    m->objects.clear();
+    double lat, lon, alt;
+
+    for (auto t : trackers)
+    {
+        if (t.pred_list_.size() > 0)
+        {
+            Categories cat;
+            switch (t.class_)
+            {
+            case 0:
+                cat = Categories::C_person;
+                break;
+            case 1:
+                cat = Categories::C_car;
+                break;
+            case 2:
+                cat = Categories::C_car;
+                break;
+            case 3:
+                cat = Categories::C_bus;
+                break;
+            case 4:
+                cat = Categories::C_motorbike;
+                break;
+            case 5:
+                cat = Categories::C_bycicle;
+                break;
+            }
+            //std::cout << t.pred_list_.size() << std::endl;
+            gc.enu2Geodetic(t.pred_list_.back().x_, t.pred_list_.back().y_, 0, &lat, &lon, &alt);
+            //std::cout << "lat: " << lat << " lon: " << lon << std::endl;
+            uint8_t velocity = uint8_t(std::abs(t.pred_list_.back().vel_ * 3.6 / 2));
+            uint8_t orientation = uint8_t((int((t.pred_list_.back().yaw_ * 57.29 + 360)) % 360) * 17 / 24);
+            RoadUser r{static_cast<float>(lat), static_cast<float>(lon), velocity, orientation, cat};
+            std::cout << std::setprecision(10) << r.latitude << " , " << r.longitude << " " << int(r.speed) << " " << int(r.orientation) << " " << r.category << std::endl;
+            m->objects.push_back(r);
+        }
+    }
+}
+
 void prepare_message(Message *m, const std::vector<ObjCoords> &coords, int idx)
 {
     m->cam_idx = idx;
@@ -209,7 +254,7 @@ void prepare_message(Message *m, const std::vector<ObjCoords> &coords, int idx)
             break;
         }
         RoadUser r{static_cast<float>(coords[i].lat_), static_cast<float>(coords[i].long_), 0, 1, cat};
-        std::cout<<std::setprecision(10) <<r.latitude<<" , "<<r.longitude << " " << cat << std::endl;
+        std::cout << std::setprecision(10) << r.latitude << " , " << r.longitude << " " << cat << std::endl;
         m->objects.push_back(r);
     }
 
diff --git a/tracker_CLASS b/tracker_CLASS
index 9d2e8d3..38b1c67 160000
--- a/tracker_CLASS
+++ b/tracker_CLASS
@@ -1 +1 @@
-Subproject commit 9d2e8d3bcc059cc4f33c6d9497e22fb56aec3940
+Subproject commit 38b1c670dc54b10cb18d15c49849a8d8fe672261