Skip to content
Snippets Groups Projects
Commit cfc39bf0 authored by Victor Hexad's avatar Victor Hexad
Browse files

added new behavior to sensorFusionQuery and autonomousEmergencyBraking.cpp

parent 79f0b9b9
No related branches found
No related tags found
2 merge requests!133Merge v0.11,!103#182 - Improve Algorithm_AEB to be also statically configured
......@@ -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;
......
/********************************************************************************
* 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"));
}
}
}
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment