merge of modular-driver v.0.7-update to hlrs branch

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/Algorithm_ModularDriver.cpp

@@ -9,7 +9,7 @@
 ******************************************************************************/
 
 #include "Algorithm_ModularDriver.h"
-#include "Algorithm_ModularDriver_implementation.h"
+#include "src/Algorithm_ModularDriver_implementation.h"
 
 const std::string Version = "0.0.1";
 static const CallbackInterface *Callbacks = nullptr;
@@ -29,7 +29,7 @@ extern "C" ALGORITHM_MODULARDRIVER_SHARED_EXPORT ModelInterface *OpenPASS_Create
         StochasticsInterface *stochastics,
         WorldInterface *world,
         const ParameterInterface *parameters,
-        const std::map<int, ObservationInterface*> *observations,
+        PublisherInterface *const publisher,
         AgentInterface *agent,
         const CallbackInterface *callbacks)
 {
@@ -47,7 +47,7 @@ extern "C" ALGORITHM_MODULARDRIVER_SHARED_EXPORT ModelInterface *OpenPASS_Create
                                      stochastics,
                                      world,
                                      parameters,
-                                     observations,
+                                     publisher,
                                      callbacks,
                                      agent));
     }
@@ -171,10 +171,10 @@ extern "C" ALGORITHM_MODULARDRIVER_SHARED_EXPORT bool OpenPASS_Trigger(
         if(Callbacks != nullptr)
         {
 #ifndef _MSC_VER
-			std::exception_ptr p = std::current_exception();
-			const std::string exType = p ? p.__cxa_exception_type()->name() : "null";
+            std::exception_ptr p = std::current_exception();
+            const std::string exType = p ? p.__cxa_exception_type()->name() : "null";
 #else
-			const std::string exType = "null";
+            const std::string exType = "null";
 #endif
 
             Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, "unexpected exception - type: " + exType);

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/Algorithm_ModularDriver.h

@@ -15,7 +15,7 @@
 
 #pragma once
 
-#include "Algorithm_ModularDriver_global.h"
-#include "modelInterface.h"
+#include "src/Algorithm_ModularDriver_global.h"
+#include "include/modelInterface.h"
 
 

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/Algorithm_ModularDriver.pro

@@ -11,13 +11,13 @@ CONFIG += OPENPASS_LIBRARY
 include(../../../global.pri)
 
 SUBDIRS +=  . \
-            ../Sensor_Modular_Driver/Signals \
-            ../Sensor_Modular_Driver/Container \
-            ActionDeductionMethods \
-            SituationAssessmentMethods \
+            ../Sensor_Modular_Driver/src/Signals \
+            ../Sensor_Modular_Driver/src/Container \
+            src/ActionDeductionMethods \
+            src/SituationAssessmentMethods \
             ../../Common \
             ../../Interfaces \
-            ../Sensor_Driver/Signals \
+            ../Sensor_Driver/src/Signals \
             ..
 
 INCLUDEPATH += $$SUBDIRS \
@@ -29,9 +29,9 @@ SOURCES += \
     $$getFiles(SUBDIRS, cpp) \
     $$getFiles(SUBDIRS, cc) \
     $$getFiles(SUBDIRS, c) \
-    ../Algorithm_Longitudinal/algorithm_longitudinalCalculations.cpp \
+    ../Algorithm_Longitudinal/src/algorithm_longitudinalCalculations.cpp \
 
 HEADERS += \
     $$getFiles(SUBDIRS, hpp) \
     $$getFiles(SUBDIRS, h) \
-    ../Algorithm_Longitudinal/algorithm_longitudinalCalculations.h \
+    ../Algorithm_Longitudinal/src/algorithm_longitudinalCalculations.h \

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/CMakeLists.txt

@@ -16,43 +16,39 @@ add_openpass_target(
   NAME ${COMPONENT_NAME} TYPE library LINKAGE shared COMPONENT core
 
   HEADERS
-	AlgorithmSituationAssessment.h
-	AlgorithmActionExecution.h
-	Algorithm_ModularDriver.h
-	Algorithm_ModularDriver_implementation.h
-	Algorithm_ModularDriver_global.h
-	AlgorithmActionDeduction.h
-	AlgorithmInformationAcquisition.h
-	AlgorithmMentalModel.h
-	ActionDeductionMethods/CFModel.h
-	ActionDeductionMethods/LCM_LC2013.h
-	ActionDeductionMethods/AbstractLaneChangeModel.h
-	ActionDeductionMethods/TargetBraking.h
-	ActionDeductionMethods/CFModel_Daniel1.h
-	SituationAssessmentMethods/SituationCalculation.h
-	SituationAssessmentMethods/SituationLogging.h
-	SituationAssessmentMethods/boundingBoxCalculation.h
-    ../Algorithm_Longitudinal/algorithm_longitudinalCalculations.h
+    Algorithm_ModularDriver.h
+    src/Algorithm_ModularDriver_global.h
+    src/Algorithm_ModularDriver_implementation.h
+    src/AlgorithmActionDeduction.h
+    src/AlgorithmActionExecution.h
+    src/AlgorithmInformationAcquisition.h
+    src/AlgorithmMentalModel.h
+    src/AlgorithmSituationAssessment.h
+    src/ActionDeductionMethods/AbstractLaneChangeModel.h
+    src/ActionDeductionMethods/CFModel.h
+    src/ActionDeductionMethods/CFModel_Daniel1.h
+    src/ActionDeductionMethods/LCM_LC2013.h
+    src/ActionDeductionMethods/TargetBraking.h
+    src/SituationAssessmentMethods/boundingBoxCalculation.h
+    src/SituationAssessmentMethods/SituationCalculation.h
+    src/SituationAssessmentMethods/SituationLogging.h
 
   SOURCES
-	AlgorithmSituationAssessment.cpp
-	AlgorithmActionExecution.cpp
-	Algorithm_ModularDriver.cpp
-	AlgorithmInformationAcquisition.cpp
-	Algorithm_ModularDriver_implementation.cpp
-	AlgorithmMentalModel.cpp
-	AlgorithmActionDeduction.cpp
-	ActionDeductionMethods/AbstractLaneChangeModel.cpp
-	ActionDeductionMethods/TargetBraking.cpp
-	ActionDeductionMethods/CFModel.cpp
-	ActionDeductionMethods/LCM_LC2013.cpp
-	ActionDeductionMethods/CFModel_Daniel1.cpp
-	SituationAssessmentMethods/SituationLogging.cpp
-	SituationAssessmentMethods/SituationCalculation.cpp
-	SituationAssessmentMethods/boundingBoxCalculation.cpp
-    ../Algorithm_Longitudinal/algorithm_longitudinalCalculations.cpp
-	../Sensor_Modular_Driver/Container/agent_representation.cpp
-	../Sensor_Modular_Driver/Container/MentalModelLane.cpp
+    Algorithm_ModularDriver.cpp
+    src/Algorithm_ModularDriver_implementation.cpp
+    src/AlgorithmActionDeduction.cpp
+    src/AlgorithmActionExecution.cpp
+    src/AlgorithmInformationAcquisition.cpp
+    src/AlgorithmMentalModel.cpp
+    src/AlgorithmSituationAssessment.cpp
+    src/ActionDeductionMethods/AbstractLaneChangeModel.cpp
+    src/ActionDeductionMethods/CFModel.cpp
+    src/ActionDeductionMethods/CFModel_Daniel1.cpp
+    src/ActionDeductionMethods/LCM_LC2013.cpp
+    src/ActionDeductionMethods/TargetBraking.cpp
+    src/SituationAssessmentMethods/boundingBoxCalculation.cpp
+    src/SituationAssessmentMethods/SituationCalculation.cpp
+    src/SituationAssessmentMethods/SituationLogging.cpp
 
   LIBRARIES
     Qt5::Core

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/ActionDeductionMethods/AbstractLaneChangeModel.h → OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/src/ActionDeductionMethods/AbstractLaneChangeModel.h

@@ -36,7 +36,7 @@
 #include <map>
 #include <vector>
 #include <utility>
-#include "Container/ContainerStructures.h"
+#include "../../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
 
 class Lane;
 

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/ActionDeductionMethods/TargetBraking.cpp → OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/src/ActionDeductionMethods/TargetBraking.cpp

@@ -24,7 +24,7 @@
 */
 
 #include "TargetBraking.h"
-#include "Container/ContainerStructures.h"
+#include "../../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
 
 TargetBraking::TargetBraking(int cycleTime, StochasticsInterface *stochastics):
     cycleTime(cycleTime),

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/ActionDeductionMethods/TargetBraking.h → OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/src/ActionDeductionMethods/TargetBraking.h

@@ -18,8 +18,8 @@
 //-----------------------------------------------------------------------------
 
 #pragma once
-#include "Interfaces/stochasticsInterface.h"
-#include "Container/ContainerStructures.h"
+#include "include/stochasticsInterface.h"
+#include "../../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
 #include "CFModel.h"
 
 //! \ingroup Algorithm_Action_Deduction

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/AlgorithmActionDeduction.cpp → OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/src/AlgorithmActionDeduction.cpp

@@ -36,7 +36,7 @@
 //#include <limits>
 #include <iostream>
 #include "AlgorithmActionDeduction.h"
-#include <Common/commonTools.h>
+#include "common/commonTools.h"
 
 ActionDeduction::ActionDeduction(int cycletime, StochasticsInterface *stochastics, ObservationInterface *observer) :
     CycleTime(cycletime),
@@ -103,7 +103,19 @@ void ActionDeduction::CalcAccelerationWish(ActionDeduction_Output_BU &AD_Output_
             CarFollowingModel->setHeadwayTime(*AD_Input->SA_O_BU->Ego->GetDriverInformation()->thw_Wish);
             double delta_s_front = *Leader.GetDistanceToEgo() - *DriverInformation->MinGap;
             double v_long_Front = Leader.GetState()->velocity_long;
-            double netttc = TrafficHelperFunctions::CalculateNetTTC(v_long, v_long_Front, delta_s_front);
+            // Calculation not in TrafficHelperFunctions -> Calculation previously done in function is done here now
+            // TODO Decide where it should be
+//            double netttc = TrafficHelperFunctions::CalculateNetTTC(v_long, v_long_Front, delta_s_front);
+            double netttc {-99.};
+            if (delta_s_front < 0.)
+            {
+                netttc = -1.;
+            }
+            else
+            {
+                double deltaV {v_long - v_long_Front};
+                netttc = deltaV < 1e-6 ? 99. : delta_s_front / deltaV;
+            }
 
             if (netttc < *AD_Input->SA_O_BU->Ego->GetDriverInformation()->ttc_threshold)
                 decel = vehicleParameters->maxDeceleration;
@@ -625,29 +637,31 @@ void ActionDeduction::Initialize()
 void ActionDeduction::LogSetValues(int time)
 {
     Logged = true;
-    observation->Insert(time,
-                        AD_Input->MM_O->Ego->GetState()->id,
-                        LoggingGroup::Driver,
-                        "MinComfortDeceleration",
-                        std::to_string(minComfortDeceleration));
-    observation->Insert(time,
-                        AD_Input->MM_O->Ego->GetState()->id,
-                        LoggingGroup::Driver,
-                        "MaxComfortAcceleration",
-                        std::to_string(maxComfortAcceleration));
-    observation->Insert(time,
-                        AD_Input->MM_O->Ego->GetState()->id,
-                        LoggingGroup::Driver,
-                        "SpeedGain",
-                        std::to_string(SpeedGain));
-    observation->Insert(time,
-                        AD_Input->MM_O->Ego->GetState()->id,
-                        LoggingGroup::Driver,
-                        "KeepRight",
-                        std::to_string(KeepRight));
-    observation->Insert(time,
-                        AD_Input->MM_O->Ego->GetState()->id,
-                        LoggingGroup::Driver,
-                        "Cooperative",
-                        std::to_string(Cooperative));
+      //!!!
+//    observation->Insert(time,
+//                        AD_Input->MM_O->Ego->GetState()->id,
+//                        LoggingGroup::Driver,
+//                        "MinComfortDeceleration",
+//                        std::to_string(minComfortDeceleration));
+//    observation->Insert(time,
+//                        AD_Input->MM_O->Ego->GetState()->id,
+//                        LoggingGroup::Driver,
+//                        "MaxComfortAcceleration",
+//                        std::to_string(maxComfortAcceleration));
+//    observation->Insert(time,
+//                        AD_Input->MM_O->Ego->GetState()->id,
+//                        LoggingGroup::Driver,
+//                        "SpeedGain",
+//                        std::to_string(SpeedGain));
+//    observation->Insert(time,
+//                        AD_Input->MM_O->Ego->GetState()->id,
+//                        LoggingGroup::Driver,
+//                        "KeepRight",
+//                        std::to_string(KeepRight));
+//    observation->Insert(time,
+//                        AD_Input->MM_O->Ego->GetState()->id,
+//                        LoggingGroup::Driver,
+//                        "Cooperative",
+//                        std::to_string(Cooperative));
+      //!!!
 }

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/AlgorithmActionDeduction.h → OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/src/AlgorithmActionDeduction.h

@@ -29,9 +29,9 @@
 #pragma once
 
 //#include <vector>
-#include "Interfaces/stochasticsInterface.h"
-#include "Interfaces/observationInterface.h"
-#include "Container/ContainerStructures.h"
+#include "include/stochasticsInterface.h"
+#include "include/observationInterface.h"
+#include "../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
 #include "ActionDeductionMethods/CFModel.h"
 #include "ActionDeductionMethods/CFModel_Daniel1.h"
 #include "ActionDeductionMethods/AbstractLaneChangeModel.h"

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/AlgorithmActionExecution.cpp → OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/src/AlgorithmActionExecution.cpp

@@ -31,7 +31,8 @@
 #include <map>
 #include <iostream>
 #include "AlgorithmActionExecution.h"
-#include <Common/commonTools.h>
+#include "common/commonTools.h"
+#include "../../Algorithm_Longitudinal/src/longCalcs.cpp"
 
 
 ActionExecution::ActionExecution(StochasticsInterface *stochastics):

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/AlgorithmActionExecution.h → OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/src/AlgorithmActionExecution.h

@@ -23,9 +23,9 @@
 #pragma once
 
 //#include <vector>
-#include "Container/ContainerStructures.h"
-#include "../Algorithm_Longitudinal/algorithm_longitudinalCalculations.h"
-#include "Interfaces/stochasticsInterface.h"
+#include "../../Sensor_Modular_Driver/src/Container/ContainerStructures.h"
+#include "../../Algorithm_Longitudinal/src/longCalcs.h"
+#include "include/stochasticsInterface.h"
 
 //! \brief This class contains the data calculations for the execution of actions from the previous cognition.
 //! Singleton such that data calculations is generated only once and not seperately

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/AlgorithmInformationAcquisition.cpp → OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/src/AlgorithmInformationAcquisition.cpp

@@ -138,34 +138,35 @@ bool InformationAcquisition::IsReactive()
 
 void InformationAcquisition::LogSetValues(int time)
 {
-    _observation->Insert(time,
-                        IA_Input_BU->EgoData.GetState()->id,
-                        LoggingGroup::Driver,
-                        "CurrentAvertViewTime",
-                        std::to_string(currentReactionTime));
-
-    if (IA_Input_BU->EgoData.GetState()->collisionpartners.size()>0)
-    {
-        std::string partners;
-
-        auto collisionPartners = IA_Input_BU->EgoData.GetState()->collisionpartners;
-        for (const auto &partner : collisionPartners)
-        {
-            partners += std::to_string(partner.second) + ", ";
-        }
-
-        std::string out =   "CollisionAgentId: " + std::to_string(IA_Input_BU->EgoData.GetState()->id) +
-                            ", CollisionPartners: " + partners +
-                            "CurrAvertView: " + std::to_string(currentReactionTime) +
-                            ", CurrAvertTrigger: " + std::to_string(reactionTimeTrigger);
-
-        std::shared_ptr<BasicEvent> event = std::make_shared<BasicEvent>(time,
-                                                                         out,
-                                                                         "sequenceName",
-                                                                         EventDefinitions::EventType::ComponentStateChange);
-
-        _observation->InsertEvent(event);
-    }
-
+      //!!!
+//    _observation->Insert(time,
+//                        IA_Input_BU->EgoData.GetState()->id,
+//                        LoggingGroup::Driver,
+//                        "CurrentAvertViewTime",
+//                        std::to_string(currentReactionTime));
+
+//    if (IA_Input_BU->EgoData.GetState()->collisionpartners.size()>0)
+//    {
+//        std::string partners;
+
+//        auto collisionPartners = IA_Input_BU->EgoData.GetState()->collisionpartners;
+//        for (const auto &partner : collisionPartners)
+//        {
+//            partners += std::to_string(partner.second) + ", ";
+//        }
+
+//        std::string out =   "CollisionAgentId: " + std::to_string(IA_Input_BU->EgoData.GetState()->id) +
+//                            ", CollisionPartners: " + partners +
+//                            "CurrAvertView: " + std::to_string(currentReactionTime) +
+//                            ", CurrAvertTrigger: " + std::to_string(reactionTimeTrigger);
+
+//        std::shared_ptr<BasicEvent> event = std::make_shared<BasicEvent>(time,
+//                                                                         out,
+//                                                                         "sequenceName",
+//                                                                         EventDefinitions::EventType::ComponentStateChange);
+
+//        _observation->InsertEvent(event);
+//    }
+      //!!!
 }
 

OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/AlgorithmInformationAcquisition.h → OpenPass_Source_Code/openPASS/Components/Algorithm_ModularDriver/src/AlgorithmInformationAcquisition.h

@@ -24,11 +24,9 @@
 
 //#include <vector>
 #include <iostream>
-#include "Container/ContainerStructures.h"
-#include "Interfaces/stochasticsInterface.h"
-#include "Interfaces/observationInterface.h"
-
-#include "Common/vehicleComponentEvent.h"
+#include "components/Sensor_Modular_Driver/src/Container/ContainerStructures.h"
+#include "include/stochasticsInterface.h"
+#include "include//observationInterface.h"
 
 //! \brief This class contains the acquisition of informations like visual and auditive.
 //! Singleton such that data calculations is generated only once and not seperately