Merge branch 'improve-aeb-static-config' into 'develop'

#182 - Improve Algorithm_AEB to be also statically configured

See merge request !103

sim/src/common/sensorFusionQuery.h

@@ -2,6 +2,7 @@
  * Copyright (c) 2020 HLRS, University of Stuttgart
  *               2018-2020 in-tech GmbH
  *               2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+ *               2023 Hexad GmbH
  *
  * This program and the accompanying materials are made available under the
  * terms of the Eclipse Public License 2.0 which is available at
@@ -25,15 +26,20 @@
 
 namespace SensorFusionHelperFunctions
 {
-    static std::vector<osi3::DetectedMovingObject> RetrieveMovingObjectsBySensorId(const std::vector<int>& sensorIds, const osi3::SensorData &sensorData)
+    static std::vector<osi3::DetectedMovingObject> RetrieveMovingObjectsBySensorId(const std::vector<int> &sensorIds, const osi3::SensorData &sensorData)
     {
         std::vector<osi3::DetectedMovingObject> result;
         auto detectedMovingObjects = sensorData.moving_object();
-        for (const auto& object : detectedMovingObjects)
+        for (const auto &object : detectedMovingObjects)
         {
+            if (sensorIds.empty())
+            {
+                result.push_back(object);
+                continue;
+            }
             for (auto sensorId : object.header().sensor_id())
             {
-                if(std::count(sensorIds.cbegin(), sensorIds.cend(), sensorId.value()) > 0)
+                if (std::count(sensorIds.cbegin(), sensorIds.cend(), sensorId.value()) > 0)
                 {
                     result.push_back(object);
                     break;
@@ -43,15 +49,20 @@ namespace SensorFusionHelperFunctions
         return result;
     }
 
-    static std::vector<osi3::DetectedStationaryObject> RetrieveStationaryObjectsBySensorId(const std::vector<int>& sensorIds, const osi3::SensorData &sensorData)
+    static std::vector<osi3::DetectedStationaryObject> RetrieveStationaryObjectsBySensorId(const std::vector<int> &sensorIds, const osi3::SensorData &sensorData)
     {
         std::vector<osi3::DetectedStationaryObject> result;
         auto detectedStationaryObjects = sensorData.stationary_object();
-        for (const auto& object : detectedStationaryObjects)
+        for (const auto &object : detectedStationaryObjects)
         {
+            if (sensorIds.empty())
+            {
+                result.push_back(object);
+                continue;
+            }
             for (auto sensorId : object.header().sensor_id())
             {
-                if(std::count(sensorIds.cbegin(), sensorIds.cend(), sensorId.value()) > 0)
+                if (std::count(sensorIds.cbegin(), sensorIds.cend(), sensorId.value()) > 0)
                 {
                     result.push_back(object);
                     break;

sim/src/components/Algorithm_AEB/src/autonomousEmergencyBraking.cpp

 /********************************************************************************
  * Copyright (c) 2019-2020 in-tech GmbH
+ *               2023 Hexad GmbH
  *
  * This program and the accompanying materials are made available under the
  * terms of the Eclipse Public License 2.0 which is available at
@@ -74,17 +75,19 @@ AlgorithmAutonomousEmergencyBrakingImplementation::AlgorithmAutonomousEmergencyB
 
 void AlgorithmAutonomousEmergencyBrakingImplementation::ParseParameters(const ParameterInterface *parameters)
 {
-    ttcBrake = parameters->GetParametersDouble().at("TTC");
-    brakingAcceleration = parameters->GetParametersDouble().at("Acceleration");
-    collisionDetectionLongitudinalBoundary = parameters->GetParametersDouble().at("CollisionDetectionLongitudinalBoundary");
-    collisionDetectionLateralBoundary = parameters->GetParametersDouble().at("CollisionDetectionLateralBoundary");
+    collisionDetectionLongitudinalBoundary = parameters->GetParametersDouble().count("CollisionDetectionLongitudinalBoundary") == 1 ? parameters->GetParametersDouble().at("CollisionDetectionLongitudinalBoundary") : parameters->GetParametersDouble().at("0");
+    collisionDetectionLateralBoundary = parameters->GetParametersDouble().count("CollisionDetectionLateralBoundary") == 1 ? parameters->GetParametersDouble().at("CollisionDetectionLateralBoundary") : parameters->GetParametersDouble().at("1");
+    ttcBrake = parameters->GetParametersDouble().count("TTC") == 1 ? parameters->GetParametersDouble().at("TTC") : parameters->GetParametersDouble().at("2");
+    brakingAcceleration = parameters->GetParametersDouble().count("Acceleration") == 1 ? parameters->GetParametersDouble().at("Acceleration") : parameters->GetParametersDouble().at("3");
 
-    const auto &sensorList = parameters->GetParameterLists().at("SensorLinks");
-    for (const auto &sensorLink : sensorList)
-    {
-        if (sensorLink->GetParametersString().at("InputId") == "Camera")
+    if(parameters->GetParametersDouble().count("SensorLinks") != 0){
+        const auto &sensorList = parameters->GetParameterLists().at("SensorLinks");
+        for (const auto &sensorLink : sensorList)
         {
-            sensors.push_back(sensorLink->GetParametersInt().at("SensorId"));
+            if (sensorLink->GetParametersString().at("InputId") == "Camera")
+            {
+                sensors.push_back(sensorLink->GetParametersInt().at("SensorId"));
+            }
         }
     }
 }