Merge pull request #9 from incluit/feature/mauriaguilar_OP-160_Driver…

…-Detection-Mode

[OP-160] Urban Driving - Flag -dm to force driving mode

BlindspotAssistance/src/common/classes.hpp

@@ -19,8 +19,6 @@ class Vehicle
         float getCruiseControl() {return cruiseControl; }
         void calc_mocked_status(){
             int elapsed_time = time(NULL) - current_time;
-            if (elapsed_time % 10 == 0) 
-                std::cout << elapsed_time << " seconds has passed." << std::endl;
 
             if (elapsed_time < 12)
                 parkingBrakeON = true;

BlindspotAssistance/src/common/multichannel_params.hpp

@@ -35,6 +35,7 @@ static const char utilization_monitors_message[] = "Optional. List of monitors t
 static const char calibration_message[] = "Optional. Camera calibration.";
 static const char show_calibration_message[] = "Optional. Show camera calibration.";
 static const char alerts_message[] = "Optional. Send alerts to AlertManager.";
+static const char driver_mode[] = "Optional. Force a specific driver mode.";
 
 DEFINE_bool(h, false, help_message);
 DEFINE_string(m, "", model_path_message);
@@ -59,3 +60,4 @@ DEFINE_string(u, "", utilization_monitors_message);
 DEFINE_bool(calibration, false, calibration_message);
 DEFINE_bool(show_calibration, false, show_calibration_message);
 DEFINE_bool(alerts, false, alerts_message);
+DEFINE_string(dm, "", driver_mode);

BlindspotAssistance/src/common/vehicle_status.hpp

@@ -1,6 +1,7 @@
 #include <iostream>
 #include <string>
 #include "classes.hpp"
+#include "multichannel_params.hpp"
 
 enum class Modes {
     unknown,
@@ -11,12 +12,30 @@ enum class Modes {
     highway
 };
 
-class vehicle_status
+class VehicleStatus
 {
     private:
-        Modes mode = Modes::surveillance;
+        Modes mode = Modes::unknown;
         bool engine_on, trailer_on, cruise_control_on;
         Vehicle vehicle;
+        void setModeByFlag(){
+            std::string driver_mode = FLAGS_dm;
+            // driver_mode to lowercase
+            std::transform(driver_mode.begin(), driver_mode.end(), driver_mode.begin(),
+                            [](unsigned char c){ return std::tolower(c); });
+            if (driver_mode == "parking")
+                mode = Modes::parking;
+            else if (driver_mode == "reverse")
+                mode = Modes::reverse;
+            else if (driver_mode == "surveillance")
+                mode = Modes::surveillance;
+            else if (driver_mode == "urban")
+                mode = Modes::urban_driving;
+            else if (driver_mode == "highway")
+                mode = Modes::highway;
+            else
+                mode = Modes::unknown;
+        }
         void calc_mode(){
             if (vehicle.getParkingBrake())
                 mode = Modes::parking;
@@ -30,13 +49,18 @@ class vehicle_status
                 mode = Modes::highway;
         }
     public:
+        void find_mode(){
+            if (!FLAGS_dm.empty())
+                setModeByFlag();
+            else{
+                vehicle.calc_mocked_status();
+                calc_mode();
+            }
+        }
         Modes get_mode(){
-            calc_mode();
             return mode;
         }
         std::string get_mode_to_string(){
-            vehicle.calc_mocked_status();
-            calc_mode();
             if( mode == Modes::parking )
                 return "Parking";
             if( mode == Modes::reverse )

BlindspotAssistance/src/main.cpp

@@ -80,6 +80,7 @@ namespace
         std::cout << "    -calibration                 " << calibration_message << std::endl;
         std::cout << "    -show_calibration            " << show_calibration_message << std::endl;
         std::cout << "    -alerts                      " << alerts_message << std::endl;
+        std::cout << "    -dm                          " << driver_mode << std::endl;
     }
 
     bool ParseAndCheckCommandLine(int argc, char *argv[])
@@ -159,23 +160,20 @@ namespace
         }
     }
 
-    void alertHandler(size_t i, const Detection &f){
+    void alertHandler(size_t i, const Detection &f, VehicleStatus *vehicle){
 
-        std::string payload = std::to_string(i)+","+std::to_string(f.label)+","+std::to_string(f.confidence);
+        vehicle->find_mode();
+        std::string payload = std::to_string(i)+","+std::to_string(f.label)+","+std::to_string(f.confidence)+","+vehicle->get_mode_to_string();
 
-        int n = payload.length();
-
-        // declaring character array
-        char char_array[n + 1];
-
-        // copying the contents of the
-        // string to char array
+        // copying the contents of the string to char array
+        char char_array[payload.length() + 1];
         strcpy(char_array, payload.c_str());
+
         alertPublisher.sendAlert(char_array);
 
     }
 
-    int areaDetectionCount(cv::Mat &img, const std::vector<Detection> &detections, size_t i, cv::Rect2d roi)
+    int areaDetectionCount(cv::Mat &img, const std::vector<Detection> &detections, size_t i, cv::Rect2d roi, VehicleStatus *vehicle)
     {
         int count = 0;
 
@@ -194,7 +192,7 @@ namespace
 
                     // Send alert if enable
                     if (FLAGS_alerts){
-                        alertHandler(i+1, f);
+                        alertHandler(i+1, f, vehicle);
                     } 
                 }
             }
@@ -279,10 +277,9 @@ namespace
     }
 
     void displayNSources(const std::vector<std::shared_ptr<VideoFrame>> &data,
-                         float time,
-                         const std::string &stats,
-                         DisplayParams params,
-                         Presenter &presenter)
+                         float time, const std::string &stats,
+                         DisplayParams params, Presenter &presenter,
+                         VehicleStatus *vehicle)
     {
         cv::Mat windowImage = cv::Mat::zeros(params.windowSize, CV_8UC3);
         auto loopBody = [&](size_t i) {
@@ -296,7 +293,7 @@ namespace
                 {
                     drawDetections(windowPart, elem->detections.get<std::vector<Detection>>());
                 }
-                camDetections[i] = areaDetectionCount(windowPart, elem->detections.get<std::vector<Detection>>(), i, roi[i]);
+                camDetections[i] = areaDetectionCount(windowPart, elem->detections.get<std::vector<Detection>>(), i, roi[i], vehicle);
             }
         };
 
@@ -377,7 +374,7 @@ int main(int argc, char *argv[])
 {
     try
     {
-        vehicle_status vehicle;
+        VehicleStatus vehicle;
 #if USE_TBB
         TbbArenaWrapper arena;
 #endif
@@ -534,7 +531,7 @@ int main(int argc, char *argv[])
                                    std::unique_lock<std::mutex> lock(statMutex);
                                    str = statStream.str();
                                }
-                               displayNSources(result, averageFps, str, params, presenter);
+                               displayNSources(result, averageFps, str, params, presenter, &vehicle);
                                int key = cv::waitKey(1);
                                presenter.handleKey(key);