From cfc39bf02e78032a0c11ad56a97b0a3d66d7972b Mon Sep 17 00:00:00 2001
From: Victor Baena <victor.baena@hexad.de>
Date: Thu, 12 Jan 2023 14:24:54 +0100
Subject: [PATCH] added new behavior to sensorFusionQuery and
autonomousEmergencyBraking.cpp
---
sim/src/common/sensorFusionQuery.h | 23 ++++++++++++++-----
.../src/autonomousEmergencyBraking.cpp | 21 +++++++++--------
2 files changed, 29 insertions(+), 15 deletions(-)
diff --git a/sim/src/common/sensorFusionQuery.h b/sim/src/common/sensorFusionQuery.h
index a144a1433..130efecd4 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)
+ * 2022-2025 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 5a24e0d22..bc5db80f0 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
+ * 2022-2025 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"));
+ }
}
}
}
--
GitLab