Untested code of the tracking server

CMakeLists.txt

@@ -36,6 +36,7 @@ add_library(ball_tracking SHARED
   src/img_proc.cpp
   src/utils.cpp
   src/tracker.cpp
+  src/server.cpp
   ${GPU_CPP_SRC}
   )
 target_link_libraries(ball_tracking

src/server.cpp

@@ -6,6 +6,7 @@
 #include <string>
 #include <unordered_map>
 #include <thread>
+#include <chrono>
 #include <camera.hpp>
 
 #include <boost/log/core.hpp>
@@ -17,6 +18,7 @@
 
 namespace logging = boost::log;
 using namespace std;
+using namespace std::chrono;
 using json = nlohmann::json;
 using namespace camera;
 
@@ -28,18 +30,26 @@ namespace ball_tracking {
       ThreadedListener tlistener;
       zmqpp::socket position_pub;
       CameraSet cams;
+      high_resolution_clock::time_point start_time;
 
       void start_trackers(const json& conf) {
         for (auto tracker : conf) {
           unsigned int ID = tracker.at("ID");
+          BOOST_LOG_TRIVIAL(info) << "Starting ball tracker with ID " << ID;
           if (tracker.count("file")) {
-            json tconf = load_json(tracker.at("file"));
+            string fname = tracker.at("file");
+            BOOST_LOG_TRIVIAL(info) << "Reading configuration for ID " << ID << " in " << fname;
+            json tconf = load_json(fname);
+            trackers[ID] = Tracker(tconf);
+          } else {
+            throw std::logic_error("Configuration for the tracking object expected but not given");
           }
         }
       }
 
       void start_server(const json& conf) {
         if (conf.count("log")) set_log_config(conf.at("log"));
+        start_time = high_resolution_clock::now();
 
         //1) Start the networking sockets
         const json& srv_conf = conf.at("servers");
@@ -47,15 +57,50 @@ namespace ball_tracking {
         auto socket_type = zmqpp::socket_type::pub;
         position_pub = zmqpp::socket(context, socket_type);
         const string& pp_url = srv_conf.at("position_publisher");
-        BOOST_LOG_TRIVIAL(info) << "Binding to " << pp_url;
+        BOOST_LOG_TRIVIAL(info) << "Starting 2D position publisher server in " << pp_url;
         position_pub.bind(pp_url);
 
         //2) Start the tracking pipeline
         start_trackers(conf.at("trackers"));        
 
-        //3) Start the cameras
+        //3) Start the cameras and call backs
+        BOOST_LOG_TRIVIAL(info) << "Starting the cameras...";
         cams = CameraSet(conf.at("cameras"));
+        for (auto& p: trackers) {
+          unsigned int ID = p.first;
 
+          //3.1) Create a call-back
+          auto call_back = [ID, this](const Frame& frame) -> void {
+            duration<double, std::milli> obs_time = frame.time - this->start_time;
+            BOOST_LOG_TRIVIAL(debug) << "Obs2D call-back called on camera " << ID << 
+              " Frame: {cam_id: " << frame.cam_id << ", num:" << frame.num << 
+              ", time: " << obs_time.count() << "}";
+            const string topic = "a";
+            Tracker track = this->trackers.at(ID);
+            vector<cv::KeyPoint> obs2d = track(frame.img);
+            json obs;
+            if (obs2d.size() != 0) {
+              obs.push_back(obs2d[0].pt.x);
+              obs.push_back(obs2d[0].pt.y);
+            }
+            json jframe {
+              {"cam_id", frame.cam_id},
+              {"num", frame.num},
+              {"time", obs_time.count()},
+              {"obs", obs}
+            };
+            BOOST_LOG_TRIVIAL(debug) << "Sending message: " << jframe.dump();
+            zmqpp::message msg;
+            msg << topic << jframe.dump();
+            this->position_pub.send(msg);
+          };
+
+          //3.2) Add the call-back as a new thread
+          BOOST_LOG_TRIVIAL(debug) << "Adding a threaded call-back for camera " << ID;
+          tlistener.add_listener(ID, call_back);
+        }
+        BOOST_LOG_TRIVIAL(debug) << "Starting all the camera listeners";
+        cams.start(tlistener);
       }
   };
 };