From 3f1544e8c90e3488eea3c5eb31ef1c91dad620b5 Mon Sep 17 00:00:00 2001 From: Reinhard Biegel Date: Wed, 1 Sep 2021 20:09:09 +0200 Subject: [PATCH 1/3] fix(SensorFusionErrorless_OSI): Also copy SensorViews Signed-off-by: Reinhard Biegel --- .../src/sensorAggregationImpl.cpp | 3 +- .../src/sensorFusionImpl.cpp | 11 ++- .../Sensor_OSI/src/objectDetectorBase.cpp | 3 +- .../sensorAggregationOSI_Tests.cpp | 88 ++++++++++++++++++- .../sensorFusionErrorless_Tests.cpp | 45 +++++++++- 5 files changed, 139 insertions(+), 11 deletions(-) diff --git a/sim/src/components/SensorAggregation_OSI/src/sensorAggregationImpl.cpp b/sim/src/components/SensorAggregation_OSI/src/sensorAggregationImpl.cpp index 37654892..528b569f 100644 --- a/sim/src/components/SensorAggregation_OSI/src/sensorAggregationImpl.cpp +++ b/sim/src/components/SensorAggregation_OSI/src/sensorAggregationImpl.cpp @@ -80,8 +80,7 @@ void SensorAggregationImplementation::UpdateOutput(int localLinkId, std::shared_ // to any ADAS try { - data = std::make_shared( - out_sensorData); + data = std::make_shared(out_sensorData); } catch(const std::bad_alloc&) { diff --git a/sim/src/components/SensorFusionErrorless_OSI/src/sensorFusionImpl.cpp b/sim/src/components/SensorFusionErrorless_OSI/src/sensorFusionImpl.cpp index 2d639d4b..4ce0685c 100644 --- a/sim/src/components/SensorFusionErrorless_OSI/src/sensorFusionImpl.cpp +++ b/sim/src/components/SensorFusionErrorless_OSI/src/sensorFusionImpl.cpp @@ -73,8 +73,7 @@ void SensorFusionErrorlessImplementation::UpdateOutput(int localLinkId, std::sha // to any ADAS try { - data = std::make_shared( - out_sensorData); + data = std::make_shared(out_sensorData); } catch(const std::bad_alloc&) { @@ -98,7 +97,10 @@ void SensorFusionErrorlessImplementation::Trigger(int) void SensorFusionErrorlessImplementation::MergeSensorData(const osi3::SensorData& in_SensorData) { out_sensorData = {}; - for (auto& movingObject : in_SensorData.moving_object()) + + out_sensorData.mutable_sensor_view()->MergeFrom(in_SensorData.sensor_view()); + + for (const auto& movingObject : in_SensorData.moving_object()) { auto existingObject = std::find_if(out_sensorData.mutable_moving_object()->begin(), out_sensorData.mutable_moving_object()->end(), [&](const auto& object){return movingObject.header().ground_truth_id(0).value() == object.header().ground_truth_id(0).value();}); @@ -111,7 +113,8 @@ void SensorFusionErrorlessImplementation::MergeSensorData(const osi3::SensorData out_sensorData.add_moving_object()->CopyFrom(movingObject); } } - for (auto& stationaryObject : in_SensorData.stationary_object()) + + for (const auto& stationaryObject : in_SensorData.stationary_object()) { auto existingObject = std::find_if(out_sensorData.mutable_stationary_object()->begin(), out_sensorData.mutable_stationary_object()->end(), [&](const auto& object){return stationaryObject.header().ground_truth_id(0).value() == object.header().ground_truth_id(0).value();}); diff --git a/sim/src/components/Sensor_OSI/src/objectDetectorBase.cpp b/sim/src/components/Sensor_OSI/src/objectDetectorBase.cpp index ddc1e160..43c3e6f9 100644 --- a/sim/src/components/Sensor_OSI/src/objectDetectorBase.cpp +++ b/sim/src/components/Sensor_OSI/src/objectDetectorBase.cpp @@ -86,8 +86,7 @@ void ObjectDetectorBase::UpdateOutput(int localLinkId, std::shared_ptr( - sensorData); + data = std::make_shared(sensorData); } catch (const std::bad_alloc&) { diff --git a/sim/tests/unitTests/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.cpp b/sim/tests/unitTests/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.cpp index cd430e0c..35f972b1 100644 --- a/sim/tests/unitTests/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.cpp +++ b/sim/tests/unitTests/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.cpp @@ -73,7 +73,6 @@ TEST(SensorAggregationOSI_Unittest, TestAppendingDetectedObjectsWithinTheSameTim ASSERT_THAT(resultMovingObjects.Get(2).header().ground_truth_id(0).value(), Eq(12)); } - TEST(SensorAggregationOSI_Unittest, TestResettingDetectedObjectsInNewTimestamp) { osi3::SensorData sensorData1; @@ -121,3 +120,90 @@ TEST(SensorAggregationOSI_Unittest, TestResettingDetectedObjectsInNewTimestamp) ASSERT_THAT(resultMovingObjects.Get(0).header().sensor_id(0).value(), Eq(2)); ASSERT_THAT(resultMovingObjects.Get(0).header().ground_truth_id(0).value(), Eq(12)); } + +TEST(SensorAggregationOSI_Unittest, TestAppendingSensorViewsWithinTheSameTimestamp) +{ + unsigned int idSensor1 = 101; + unsigned int idSensor2 = 102; + + osi3::SensorData sensorData1; + osi3::SensorData sensorData2; + auto sensorView1 = sensorData1.add_sensor_view(); + auto sensorView2 = sensorData2.add_sensor_view(); + sensorView1->mutable_sensor_id()->set_value(idSensor1); + sensorView2->mutable_sensor_id()->set_value(idSensor2); + + std::shared_ptr fakeSignal1 = std::make_shared(sensorData1); + std::shared_ptr fakeSignal2 = std::make_shared(sensorData2); + + NiceMock fakeAgent; + ON_CALL(fakeAgent, GetId()).WillByDefault(Return(0)); + + SensorAggregationImplementation sensorFusion("", + false, + 0, + 0, + 0, + 0, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + &fakeAgent); + + sensorFusion.UpdateInput(0, fakeSignal1, 100); + sensorFusion.UpdateInput(1, fakeSignal2, 100); + + std::shared_ptr response; + sensorFusion.UpdateOutput(0, response, 100); + + const std::shared_ptr result = std::dynamic_pointer_cast(response); + auto resultSensorData = result->sensorData; + ASSERT_THAT(resultSensorData.sensor_view_size(), Eq(2)); + ASSERT_THAT(resultSensorData.sensor_view(0).sensor_id().value(), Eq(idSensor1)); + ASSERT_THAT(resultSensorData.sensor_view(1).sensor_id().value(), Eq(idSensor2)); +} + +TEST(SensorAggregationOSI_Unittest, TestResettingSensorViewsInNewTimestamp) +{ + unsigned int idSensor1 = 101; + unsigned int idSensor2 = 102; + + osi3::SensorData sensorData1; + osi3::SensorData sensorData2; + auto sensorView1 = sensorData1.add_sensor_view(); + auto sensorView2 = sensorData2.add_sensor_view(); + sensorView1->mutable_sensor_id()->set_value(idSensor1); + sensorView2->mutable_sensor_id()->set_value(idSensor2); + + std::shared_ptr fakeSignal1 = std::make_shared(sensorData1); + std::shared_ptr fakeSignal2 = std::make_shared(sensorData2); + + NiceMock fakeAgent; + ON_CALL(fakeAgent, GetId()).WillByDefault(Return(0)); + + SensorAggregationImplementation sensorFusion("", + false, + 0, + 0, + 0, + 0, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + &fakeAgent); + + sensorFusion.UpdateInput(0, fakeSignal1, 100); + sensorFusion.UpdateInput(1, fakeSignal2, 200); + + std::shared_ptr response; + sensorFusion.UpdateOutput(0, response, 200); + + const std::shared_ptr result = std::dynamic_pointer_cast(response); + auto resultSensorData = result->sensorData; + ASSERT_THAT(resultSensorData.sensor_view_size(), Eq(1)); + ASSERT_THAT(resultSensorData.sensor_view(0).sensor_id().value(), Eq(idSensor2)); +} diff --git a/sim/tests/unitTests/components/SensorFusionErrorless_OSI/sensorFusionErrorless_Tests.cpp b/sim/tests/unitTests/components/SensorFusionErrorless_OSI/sensorFusionErrorless_Tests.cpp index 1d06c830..9b8c7ba6 100644 --- a/sim/tests/unitTests/components/SensorFusionErrorless_OSI/sensorFusionErrorless_Tests.cpp +++ b/sim/tests/unitTests/components/SensorFusionErrorless_OSI/sensorFusionErrorless_Tests.cpp @@ -16,10 +16,11 @@ #include "sensorFusionImpl.h" using ::testing::Eq; +using ::testing::NiceMock; TEST(SensorFusionErrorless_Tests, SensorDataWithMovingObjects_IsMergedAppropriately) { - FakeAgent fakeAgent; + NiceMock fakeAgent; auto sensorFusion = SensorFusionErrorlessImplementation("", false, @@ -94,7 +95,7 @@ TEST(SensorFusionErrorless_Tests, SensorDataWithMovingObjects_IsMergedAppropriat TEST(SensorFusionErrorless_Tests, SensorDataWithStationaryObjects_IsMergedAppropriately) { - FakeAgent fakeAgent; + NiceMock fakeAgent; auto sensorFusion = SensorFusionErrorlessImplementation("", false, @@ -166,3 +167,43 @@ TEST(SensorFusionErrorless_Tests, SensorDataWithStationaryObjects_IsMergedApprop ASSERT_THAT(outSensorData.stationary_object(2).base().position().x(), Eq(30)); ASSERT_THAT(outSensorData.stationary_object(2).base().position().y(), Eq(31)); } + +TEST(SensorFusionErrorless_Tests, SensorDataWithSensorView_IsMergedAppropriately) +{ + NiceMock fakeAgent; + + auto sensorFusion = SensorFusionErrorlessImplementation("", + false, + 0, + 0, + 0, + 100, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + &fakeAgent); + unsigned int idSensor1 = 101; + unsigned int idSensor2 = 102; + + osi3::SensorData sensorData; + auto sensorView1 = sensorData.add_sensor_view(); + auto sensorView2 = sensorData.add_sensor_view(); + sensorView1->mutable_sensor_id()->set_value(idSensor1); + sensorView2->mutable_sensor_id()->set_value(idSensor2); + + auto signal = std::make_shared(sensorData); + sensorFusion.UpdateInput(0, signal, 0); + + sensorFusion.Trigger(0); + + std::shared_ptr output; + sensorFusion.UpdateOutput(0, output, 0); + auto outSensorDataSignal = std::dynamic_pointer_cast(output); + auto outSensorData = outSensorDataSignal->sensorData; + + ASSERT_THAT(outSensorData.sensor_view_size(), Eq(2)); + ASSERT_THAT(outSensorData.sensor_view(0).sensor_id().value(), Eq(idSensor1)); + ASSERT_THAT(outSensorData.sensor_view(1).sensor_id().value(), Eq(idSensor2)); +} -- GitLab From 3bc67ef2b95cb92822d42e7060d9c8722d2fe3e4 Mon Sep 17 00:00:00 2001 From: Weiss David Date: Tue, 7 Sep 2021 12:49:28 +0200 Subject: [PATCH 2/3] fix(Sensor_OSI): Remove references to old SensorObjectDetector Signed-off-by: Weiss David --- sim/src/components/Sensor_OSI/CMakeLists.txt | 2 +- sim/src/components/Sensor_OSI/Sensor_OSI.pro | 2 +- sim/src/components/Sensor_OSI/sensorOSI.cpp | 14 ++++++------- sim/src/components/Sensor_OSI/sensorOSI.h | 6 +++--- .../Sensor_OSI/src/objectDetectorBase.h | 20 +++++++++---------- .../Sensor_OSI/src/sensorGeometric2D.h | 2 +- 6 files changed, 23 insertions(+), 23 deletions(-) diff --git a/sim/src/components/Sensor_OSI/CMakeLists.txt b/sim/src/components/Sensor_OSI/CMakeLists.txt index b28a27eb..f62b8ac6 100644 --- a/sim/src/components/Sensor_OSI/CMakeLists.txt +++ b/sim/src/components/Sensor_OSI/CMakeLists.txt @@ -1,6 +1,6 @@ set(COMPONENT_NAME Sensor_OSI) -add_compile_definitions(SENSOR_OBJECT_DETECTOR_LIBRARY) +add_compile_definitions(SENSOR_OSI_LIBRARY) add_openpass_target( NAME ${COMPONENT_NAME} TYPE library LINKAGE shared COMPONENT core diff --git a/sim/src/components/Sensor_OSI/Sensor_OSI.pro b/sim/src/components/Sensor_OSI/Sensor_OSI.pro index 126cf0d2..61030329 100644 --- a/sim/src/components/Sensor_OSI/Sensor_OSI.pro +++ b/sim/src/components/Sensor_OSI/Sensor_OSI.pro @@ -14,7 +14,7 @@ # module Sensor_OSI #-----------------------------------------------------------------------------/ -DEFINES += SENSOR_OBJECT_DETECTOR_LIBRARY +DEFINES += SENSOR_OSI_LIBRARY CONFIG += OPENPASS_LIBRARY include(../../../global.pri) diff --git a/sim/src/components/Sensor_OSI/sensorOSI.cpp b/sim/src/components/Sensor_OSI/sensorOSI.cpp index aafc861c..58762ea8 100644 --- a/sim/src/components/Sensor_OSI/sensorOSI.cpp +++ b/sim/src/components/Sensor_OSI/sensorOSI.cpp @@ -9,7 +9,7 @@ *******************************************************************************/ //----------------------------------------------------------------------------- -/** \brief SensorObjectDetectorFactory.cpp */ +/** \brief SensorOSI.cpp */ //----------------------------------------------------------------------------- #include "include/parameterInterface.h" @@ -19,12 +19,12 @@ const std::string Version = "0.0.1"; static const CallbackInterface *Callbacks = nullptr; -extern "C" SENSOR_OBJECT_DETECTOR_SHARED_EXPORT const std::string &OpenPASS_GetVersion() +extern "C" SENSOR_OSI_SHARED_EXPORT const std::string &OpenPASS_GetVersion() { return Version; } -extern "C" SENSOR_OBJECT_DETECTOR_SHARED_EXPORT ModelInterface *OpenPASS_CreateInstance( +extern "C" SENSOR_OSI_SHARED_EXPORT ModelInterface *OpenPASS_CreateInstance( std::string componentName, bool isInit, int priority, @@ -90,12 +90,12 @@ extern "C" SENSOR_OBJECT_DETECTOR_SHARED_EXPORT ModelInterface *OpenPASS_CreateI } } -extern "C" SENSOR_OBJECT_DETECTOR_SHARED_EXPORT void OpenPASS_DestroyInstance(ModelInterface *implementation) +extern "C" SENSOR_OSI_SHARED_EXPORT void OpenPASS_DestroyInstance(ModelInterface *implementation) { delete implementation; } -extern "C" SENSOR_OBJECT_DETECTOR_SHARED_EXPORT bool OpenPASS_UpdateInput(ModelInterface *implementation, +extern "C" SENSOR_OSI_SHARED_EXPORT bool OpenPASS_UpdateInput(ModelInterface *implementation, int localLinkId, const std::shared_ptr &data, int time) @@ -126,7 +126,7 @@ extern "C" SENSOR_OBJECT_DETECTOR_SHARED_EXPORT bool OpenPASS_UpdateInput(ModelI return true; } -extern "C" SENSOR_OBJECT_DETECTOR_SHARED_EXPORT bool OpenPASS_UpdateOutput(ModelInterface *implementation, +extern "C" SENSOR_OSI_SHARED_EXPORT bool OpenPASS_UpdateOutput(ModelInterface *implementation, int localLinkId, std::shared_ptr &data, int time) @@ -157,7 +157,7 @@ extern "C" SENSOR_OBJECT_DETECTOR_SHARED_EXPORT bool OpenPASS_UpdateOutput(Model return true; } -extern "C" SENSOR_OBJECT_DETECTOR_SHARED_EXPORT bool OpenPASS_Trigger(ModelInterface *implementation, +extern "C" SENSOR_OSI_SHARED_EXPORT bool OpenPASS_Trigger(ModelInterface *implementation, int time) { try diff --git a/sim/src/components/Sensor_OSI/sensorOSI.h b/sim/src/components/Sensor_OSI/sensorOSI.h index 88e99517..e2d42351 100644 --- a/sim/src/components/Sensor_OSI/sensorOSI.h +++ b/sim/src/components/Sensor_OSI/sensorOSI.h @@ -12,10 +12,10 @@ #include -#if defined(SENSOR_OBJECT_DETECTOR_LIBRARY) -# define SENSOR_OBJECT_DETECTOR_SHARED_EXPORT Q_DECL_EXPORT +#if defined(SENSOR_OSI_LIBRARY) +# define SENSOR_OSI_SHARED_EXPORT Q_DECL_EXPORT #else -# define SENSOR_OBJECT_DETECTOR_SHARED_EXPORT Q_DECL_IMPORT +# define SENSOR_OSI_SHARED_EXPORT Q_DECL_IMPORT #endif #include "include/modelInterface.h" diff --git a/sim/src/components/Sensor_OSI/src/objectDetectorBase.h b/sim/src/components/Sensor_OSI/src/objectDetectorBase.h index 6472c81f..a0bcbe58 100644 --- a/sim/src/components/Sensor_OSI/src/objectDetectorBase.h +++ b/sim/src/components/Sensor_OSI/src/objectDetectorBase.h @@ -9,13 +9,13 @@ * SPDX-License-Identifier: EPL-2.0 *******************************************************************************/ -/** \addtogroup SensorObjectDetector +/** \addtogroup SensorOSI * @{ -* \brief This file models the SensorObjectDetectors. +* \brief This file models the SensorOSI. * -* \details This file models the SensorObjectDetectors which can be part of an agent. -* The SensorObjectDetectors represent sensor which can detect other WorldObjects around the agent. -* Those sensor have different approaches in detecting other WorldObjects. +* \details This file models the Sensors which can be part of an agent. +* The SensorOSI represent sensors which can detect other WorldObjects around the agent. +* Those sensors have different approaches in detecting other WorldObjects. * The detected Objects can then be sent to the corresponding ADAS which uses them. * * \section MODULENAME_Outputs Outputs @@ -27,7 +27,7 @@ * Output channel IDs: * Output Id | signal class | contained variables * ----------|--------------|------------- -* 0 | SensorObjectDetectorToAdasSignal | out_detectedObjects +* 0 | SensorDataSignal | out_detectedObjects * * * \section MODULENAME_ExternalParameters External parameters @@ -50,11 +50,11 @@ #include "osi3/osi_sensordata.pb.h" //----------------------------------------------------------------------------- -/** \brief This class is the common base for all SensorObjectDetectors. -* \details This class is the common base for all SensorObjectDetectors. -* It provides the basic functionality for all object detectors. +/** \brief This class is the common base for all OSI sensors. +* \details This class is the common base for all OSI sensors. +* It provides the basic functionality for all sensors. * -* \ingroup SensorObjectDetector +* \ingroup SensorOSI */ //----------------------------------------------------------------------------- class ObjectDetectorBase : public SensorInterface diff --git a/sim/src/components/Sensor_OSI/src/sensorGeometric2D.h b/sim/src/components/Sensor_OSI/src/sensorGeometric2D.h index 3297e009..fdf90099 100644 --- a/sim/src/components/Sensor_OSI/src/sensorGeometric2D.h +++ b/sim/src/components/Sensor_OSI/src/sensorGeometric2D.h @@ -42,7 +42,7 @@ struct SensorDetectionResults /** \brief This file models a sensor which only detects agents in a 2D area (x/y) in front of the agent * \details This sensor does not consider height. * -* \ingroup SensorObjectDetector +* \ingroup SensorOSI */ //----------------------------------------------------------------------------- class SensorGeometric2D : public ObjectDetectorBase -- GitLab From 18f178868e9240adedac4f0f8b97300a22c6ca7b Mon Sep 17 00:00:00 2001 From: Weiss David Date: Mon, 20 Sep 2021 13:58:43 +0200 Subject: [PATCH 3/3] fix(Sensor_OSI): Apply failure probability Signed-off-by: Weiss David --- sim/src/components/Sensor_OSI/src/sensorGeometric2D.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/sim/src/components/Sensor_OSI/src/sensorGeometric2D.cpp b/sim/src/components/Sensor_OSI/src/sensorGeometric2D.cpp index 7b8c2ed1..30f805d6 100644 --- a/sim/src/components/Sensor_OSI/src/sensorGeometric2D.cpp +++ b/sim/src/components/Sensor_OSI/src/sensorGeometric2D.cpp @@ -289,10 +289,18 @@ SensorDetectionResults SensorGeometric2D::DetectObjects() for (const auto& object : results.detectedMovingObjects) { + if(HasDetectionError()) + { + continue; + } AddMovingObjectToSensorData(object, ownVelocity, ownAcceleration, ownPosition, yaw, yawRate); } for (const auto& object : results.detectedStationaryObjects) { + if(HasDetectionError()) + { + continue; + } AddStationaryObjectToSensorData(object, ownPosition, yaw); } -- GitLab