diff --git a/sim/src/components/SensorAggregation_OSI/src/sensorAggregationImpl.cpp b/sim/src/components/SensorAggregation_OSI/src/sensorAggregationImpl.cpp index 37654892172f01a0514f4589487708825b1dc4f4..528b569f7ff3cbeb9fc3e81ff1beef89addfb09f 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 2d639d4b2f565d655b5990dfbd819ca4cbdbacf3..4ce0685c7fa9ca3e979b92d85a6bc1d2bdbc5e80 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/CMakeLists.txt b/sim/src/components/Sensor_OSI/CMakeLists.txt index b28a27eb3811272784fdd3233bb246e28172de7f..f62b8ac67be5a731464be0bd27a51bdea5e3d893 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 126cf0d2c22b7027a2c1b2f22ec217e384109e0c..61030329377f821997221f8e007442f18043854f 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 aafc861cd61a3a96ab7bae0021fd3eb7633ec4af..58762ea8b644e8fa74b63d851fcef73e81e3b6e5 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 88e9951738e2e3ad1abbbd59a3cd40d9115fddf7..e2d423515001706b3e77d14cb0ae912e12f17c86 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.cpp b/sim/src/components/Sensor_OSI/src/objectDetectorBase.cpp index ddc1e160bdf42ba3060c69c025ae53dcd0118718..43c3e6f9ae3f8375d0a7feb470f9ddfd3e1f73c1 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/src/components/Sensor_OSI/src/objectDetectorBase.h b/sim/src/components/Sensor_OSI/src/objectDetectorBase.h index 6472c81f725544098f04b91e456b6b40eb3bb633..a0bcbe58af20273e914b713c5688a52d76d618cc 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.cpp b/sim/src/components/Sensor_OSI/src/sensorGeometric2D.cpp index 7b8c2ed14dee67ac29818ccac113b74a4951843e..30f805d6260a831aeec43592a536b1acdf76d838 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); } diff --git a/sim/src/components/Sensor_OSI/src/sensorGeometric2D.h b/sim/src/components/Sensor_OSI/src/sensorGeometric2D.h index 3297e009b027c805f20f68e6e23eca8e40234ab2..fdf900999a4f21fcaebeea3a49c56ce8b883a41c 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 diff --git a/sim/tests/unitTests/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.cpp b/sim/tests/unitTests/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.cpp index cd430e0c9599aa521116e41f99874a290875b49c..35f972b16d4c8c6093f55c3a27ced83bea87dcde 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 1d06c830882b9d59d735b6334e283cbf1d7b05ee..9b8c7ba60841142e4ebf89532d2e64c68acaf548 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)); +}