diff --git a/sim/src/common/sensorFusionQuery.h b/sim/src/common/sensorFusionQuery.h index a144a14333591a2b751765922234c121e8f7c621..6e1da405fe252e42538ec7204d8c0190d181b678 100644 --- a/sim/src/common/sensorFusionQuery.h +++ b/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; diff --git a/sim/src/components/Algorithm_AEB/src/autonomousEmergencyBraking.cpp b/sim/src/components/Algorithm_AEB/src/autonomousEmergencyBraking.cpp index 5a24e0d2255d8975aefb764d1ddb80f9a2ec3cd5..d20650a9bd9b0eb3e00cec142e908c4b14e3fbca 100644 --- a/sim/src/components/Algorithm_AEB/src/autonomousEmergencyBraking.cpp +++ b/sim/src/components/Algorithm_AEB/src/autonomousEmergencyBraking.cpp @@ -1,5 +1,6 @@ /******************************************************************************** * 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")); + } } } }