From 214b279f19c7a9938d3e97b2f862a2487d28d281 Mon Sep 17 00:00:00 2001 From: Naida Goro Date: Wed, 18 May 2022 07:49:39 +0200 Subject: [PATCH 1/2] feat(Sensor_OSI): OSI Car2X components - Adapt Radio to work with OSI SensorData - Add radioUnitTests - Add V2X-Sensors, Senders and E2E-Test Signed-off-by: Naida Goro --- .../advanced_topics/simulator/world_osi.rst | 2 +- .../examples/Common/systemConfigBlueprint.xml | 11 + .../ADAS_Car2X/ProfilesCatalog.xml | 248 ++++++++++++++++++ .../ADAS_Car2X/simulationConfig.xml | 65 +++++ sim/include/radioInterface.h | 6 +- sim/include/worldInterface.h | 4 +- sim/src/common/DetectedObject.cpp | 21 -- sim/src/common/DetectedObject.h | 27 -- sim/src/common/xmlParser.h | 2 +- .../AlgorithmCar2XSender.cpp | 166 ++++++++++++ .../AlgorithmCar2XSender.h | 17 ++ .../AlgorithmCar2XSenderGlobal.h | 21 ++ .../AlgorithmCar2XSenderImplementation.cpp | 163 ++++++++++++ .../AlgorithmCar2XSenderImplementation.h | 115 ++++++++ .../AlgorithmCar2XSender/CMakeLists.txt | 30 +++ .../Algorithm_Longitudinal/src/longCalcs.h | 2 +- sim/src/components/CMakeLists.txt | 1 + .../ComponentController/src/condition.h | 4 +- .../src/Signals/sensor_driverDefinitions.h | 4 +- sim/src/components/Sensor_OSI/CMakeLists.txt | 4 +- sim/src/components/Sensor_OSI/sensorOSI.cpp | 18 ++ .../components/Sensor_OSI/src/sensorCar2X.cpp | 112 ++++++++ .../components/Sensor_OSI/src/sensorCar2X.h | 65 +++++ .../framework/agentBlueprintProvider.h | 2 +- .../modules/EventDetector/CollisionDetector.h | 8 +- .../Observation_Log/observationFileHandler.h | 2 +- .../observationFileHandler.h | 2 +- .../Spawners/Scenario/SpawnerScenario.h | 2 +- .../modules/Spawners/common/WorldAnalyzer.h | 4 +- .../modules/World_OSI/Localization.cpp | 2 +- .../modules/World_OSI/OWL/DataTypes.h | 6 +- .../modules/World_OSI/RadioImplementation.cpp | 6 +- .../modules/World_OSI/RadioImplementation.h | 6 +- .../modules/World_OSI/SceneryConverter.h | 2 +- .../modules/World_OSI/WorldDataQuery.h | 2 +- sim/tests/endToEndTests/test_end_to_end.json | 1 + sim/tests/fakes/gmock/fakeRadio.h | 6 +- .../Spawner_IntegrationTests/CMakeLists.txt | 1 + .../Spawner_IntegrationTests.cpp | 1 + sim/tests/unitTests/common/CMakeLists.txt | 2 +- .../unitTests/common/commonHelper_Tests.cpp | 1 + .../common/routeCalculation_Tests.cpp | 1 + .../unitTests/common/ttcCalculation_Tests.cpp | 1 + .../AlgorithmAEB/AlgorithmAebOSIUnitTests.h | 1 + .../Dynamics_Collision/CMakeLists.txt | 1 + .../dynamicsCollision_Tests.cpp | 3 +- .../components/Dynamics_TF/CMakeLists.txt | 1 + .../components/Dynamics_TF/trajectoryTester.h | 1 + .../LimiterAccVehComp/CMakeLists.txt | 1 + .../limiterAccVehComp_Tests.cpp | 1 + .../OpenScenarioActions/CMakeLists.txt | 1 + .../openScenarioActions_Tests.cpp | 1 + .../components/Sensor_Driver/CMakeLists.txt | 1 + .../Sensor_Driver/sensorDriver_Tests.cpp | 1 + .../components/Sensor_OSI/CMakeLists.txt | 5 + .../components/Sensor_OSI/radioUnitTests.cpp | 147 +++++++++++ .../components/Sensor_OSI/sensorOSI_Tests.cpp | 1 + .../core/opSimulation/CMakeLists.txt | 1 + .../opSimulation/Scheduler/CMakeLists.txt | 1 + .../Scheduler/agentParser_Tests.cpp | 1 + .../Scheduler/scheduler_Tests.cpp | 1 + .../Scheduler/taskBuilder_Tests.cpp | 1 + .../opSimulation/agentDataPublisher_Tests.cpp | 1 + .../modules/EventDetector/CMakeLists.txt | 1 + .../ConditionalEventDetector_Tests.cpp | 1 + .../EventDetector/DetectCollisionTests.cpp | 1 + .../modules/Observation_Log/CMakeLists.txt | 1 + .../Observation_Log/observationLog_Tests.cpp | 1 + .../SpawnerPreRunCommon/CMakeLists.txt | 1 + .../spawnerPreRunCommon_Tests.cpp | 1 + .../SpawnerRuntimeCommon/CMakeLists.txt | 1 + .../spawnerRuntimeCommon_Tests.cpp | 1 + .../modules/SpawnerScenario/CMakeLists.txt | 1 + .../SpawnerScenario/spawnerScenario_Tests.cpp | 1 + .../SpawnerWorldAnalyzer/CMakeLists.txt | 1 + .../spawnerWorldAnalyzer_Tests.cpp | 1 + .../modules/World_OSI/agentAdapter_Tests.cpp | 1 + .../modules/World_OSI/egoAgent_Tests.cpp | 1 + .../modules/World_OSI/lane_Tests.cpp | 1 + .../World_OSI/sceneryConverter_Tests.cpp | 1 + .../modules/World_OSI/worldQuery_Tests.cpp | 1 + 81 files changed, 1268 insertions(+), 88 deletions(-) create mode 100644 sim/contrib/examples/Configurations/ADAS_Car2X/ProfilesCatalog.xml create mode 100644 sim/contrib/examples/Configurations/ADAS_Car2X/simulationConfig.xml create mode 100644 sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSender.cpp create mode 100644 sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSender.h create mode 100644 sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSenderGlobal.h create mode 100644 sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSenderImplementation.cpp create mode 100644 sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSenderImplementation.h create mode 100644 sim/src/components/AlgorithmCar2XSender/CMakeLists.txt create mode 100644 sim/src/components/Sensor_OSI/src/sensorCar2X.cpp create mode 100644 sim/src/components/Sensor_OSI/src/sensorCar2X.h create mode 100644 sim/tests/unitTests/components/Sensor_OSI/radioUnitTests.cpp diff --git a/doc/source/advanced_topics/simulator/world_osi.rst b/doc/source/advanced_topics/simulator/world_osi.rst index 66db091a..7f08cd42 100644 --- a/doc/source/advanced_topics/simulator/world_osi.rst +++ b/doc/source/advanced_topics/simulator/world_osi.rst @@ -106,7 +106,7 @@ For each touched lane, the minimum and maximum s coordinate, and the minimum and In addition, if the reference point (i.e. the middle of the rear axle) or the mainLaneLocator (i.e. the middle of the agent front) are located within a LaneGeometryElement, s/t/yaw is calculated of each point, respectively. Further aggregation is done with respect to each road by calculating the minimum and maximum s for each road the agent intersects with. For the current route of an agent, the following information is stored: s/t/yaw of the reference point and mainLaneLocator on the route (roads along a route are not allowed to intersect), distance from the lane boundary to the left and right for the road(s) along the route, and OpenDRIVE Ids of the lanes on the route that the agent touches. -The results also holds information wether both the reference point and the mainLaneLocator lay on the route. +The results also holds information whether both the reference point and the mainLaneLocator lay on the route. In the currently implementation, these points must be located - otherwise the agent is despawened, as the agent cannot execute distance queries without a relation to its current route. diff --git a/sim/contrib/examples/Common/systemConfigBlueprint.xml b/sim/contrib/examples/Common/systemConfigBlueprint.xml index 4aa2fa46..4bb56b74 100755 --- a/sim/contrib/examples/Common/systemConfigBlueprint.xml +++ b/sim/contrib/examples/Common/systemConfigBlueprint.xml @@ -25,6 +25,17 @@ Sensor_Driver + + SenderCar2X + + 399 + 0 + 100 + 0 + + AlgorithmCar2XSender + + SensorObjectDetector diff --git a/sim/contrib/examples/Configurations/ADAS_Car2X/ProfilesCatalog.xml b/sim/contrib/examples/Configurations/ADAS_Car2X/ProfilesCatalog.xml new file mode 100644 index 00000000..30ec5cdf --- /dev/null +++ b/sim/contrib/examples/Configurations/ADAS_Car2X/ProfilesCatalog.xml @@ -0,0 +1,248 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sim/contrib/examples/Configurations/ADAS_Car2X/simulationConfig.xml b/sim/contrib/examples/Configurations/ADAS_Car2X/simulationConfig.xml new file mode 100644 index 00000000..9178734b --- /dev/null +++ b/sim/contrib/examples/Configurations/ADAS_Car2X/simulationConfig.xml @@ -0,0 +1,65 @@ + + ProfilesCatalog.xml + + 123 + 5 + 532725206 + + World_OSI + + + + Scenario.xosc + + + + + + + + + + + + + + + Germany + + + + Observation_Log + + + + + + + + + + + + + + + + + SpawnerScenario + PreRun + 1 + + + SpawnerPreRunCommon + PreRun + 0 + DefaultPreRunCommon + + + SpawnerRuntimeCommon + Runtime + 0 + DefaultRuntimeCommon + + + diff --git a/sim/include/radioInterface.h b/sim/include/radioInterface.h index e2ebec14..631f4f5b 100644 --- a/sim/include/radioInterface.h +++ b/sim/include/radioInterface.h @@ -18,7 +18,7 @@ #pragma once #include -#include "common/DetectedObject.h" +#include "common/sensorDataSignal.h" class RadioInterface { @@ -36,7 +36,7 @@ public: //! @param[in] positionY y-position of the sender //! @param[in] signalStrength signal strength of the antenna //! @param[in] objectInformation information to broadcast - virtual void Send(double positionX, double postionY, double signalStrength, DetectedObject objectInformation) = 0; + virtual void Send(double positionX, double postionY, double signalStrength, osi3::MovingObject objectInformation) = 0; //! Call the cloud to return all the information available at a position //! @@ -44,7 +44,7 @@ public: //! @param[in] positionY y-position of the receiver //! @param[in] sensitivity how strong the signal needs to be for the receiver to read the signal //! @return list of all the information which can be received at this position - virtual std::vector Receive(double positionX, double positionY, double sensitivity) = 0; + virtual std::vector Receive(double positionX, double positionY, double sensitivity) = 0; //! For each new timestep this function clears all signal of the previous timestep virtual void Reset() = 0; diff --git a/sim/include/worldInterface.h b/sim/include/worldInterface.h index 16026a4b..3bb22d34 100644 --- a/sim/include/worldInterface.h +++ b/sim/include/worldInterface.h @@ -29,11 +29,11 @@ #include "common/vector2d.h" #include "common/worldDefinitions.h" #include "include/streamInterface.h" -#include "include/radioInterface.h" class AgentInterface; class AgentBlueprintInterface; class ParameterInterface; +class RadioInterface; class SceneryInterface; class SceneryDynamicsInterface; class TrafficObjectInterface; @@ -623,7 +623,7 @@ public: virtual RouteQueryResult GetRelativeRoads(const RoadGraph& roadGraph, RoadGraphVertex startNode, double startDistance, double range) const = 0; //! Returns information about all lanes on the route in range. These info are the relative distances (start and end), - //! the laneId relative to the ego lane, the successors and predecessors if existing and the information wether the intended + //! the laneId relative to the ego lane, the successors and predecessors if existing and the information whether the intended //! driving direction of the lane is the same as the direction of the route. If the ego lane prematurely ends, then //! the further lane ids are relative to the middle of the road. //! diff --git a/sim/src/common/DetectedObject.cpp b/sim/src/common/DetectedObject.cpp index 0ccb2fa0..a4710954 100644 --- a/sim/src/common/DetectedObject.cpp +++ b/sim/src/common/DetectedObject.cpp @@ -16,16 +16,6 @@ const WorldObjectInterface* DetectedObject::GetWorldObject() const return worldObject; } -double DetectedObject::GetSensorInformation(SensorInformationType informationType) const -{ - return sensorInformation.at(informationType); -} - -void DetectedObject::SetSensorInformation(SensorInformationType informationType, double value) -{ - sensorInformation.insert({informationType, value}); -} - void DetectedObject::SetSensorMetadata(std::string typeOfSensor, int Id) { sensorType = typeOfSensor; @@ -52,16 +42,5 @@ bool operator==(const DetectedObject &dObj1, const DetectedObject &dObj2) dObj1.sensorId == dObj2.sensorId)) return false; - //compare map "sensorInformation" - if(dObj1.sensorInformation.size() != dObj2.sensorInformation.size()) - return false; - - typename std::map::const_iterator i, j; - for(i = dObj1.sensorInformation.begin(), j = dObj2.sensorInformation.begin(); i != dObj1.sensorInformation.end(); ++i, ++j) - { - if(i->first != j->first) return false; - if(i->second != j->second) return false; - } - return true; } diff --git a/sim/src/common/DetectedObject.h b/sim/src/common/DetectedObject.h index d28ffe71..a0400ea7 100644 --- a/sim/src/common/DetectedObject.h +++ b/sim/src/common/DetectedObject.h @@ -16,17 +16,6 @@ #include "include/worldObjectInterface.h" #include -//! Type of information detected by a sensor -enum class SensorInformationType -{ - Distance, - PositionX, - PositionY, - Yaw, - Velocity, - Acceleration -}; - class DetectedObject { public: @@ -39,21 +28,6 @@ public: //----------------------------------------------------------------------------- const WorldObjectInterface* GetWorldObject() const; - //----------------------------------------------------------------------------- - //! Get a specif value of your sensor (e.g. velocity, acceleration) - //! @param[in] informationType information to retrieve - //! - //! @return double value of required information - //----------------------------------------------------------------------------- - double GetSensorInformation(SensorInformationType informationType) const; - - //----------------------------------------------------------------------------- - //! Store desired sensor value. - //! @param[in] informationType information to add - //! @param[in] double sensor value - //----------------------------------------------------------------------------- - void SetSensorInformation(SensorInformationType informationType, double value); - //----------------------------------------------------------------------------- //! Store desired sensor value. //! @param[in] typeOfSensor sensor type @@ -76,7 +50,6 @@ public: private: const WorldObjectInterface* worldObject; - std::map sensorInformation; std::string sensorType = ""; int sensorId = -999; diff --git a/sim/src/common/xmlParser.h b/sim/src/common/xmlParser.h index f2ac7ab2..0f5aef1b 100644 --- a/sim/src/common/xmlParser.h +++ b/sim/src/common/xmlParser.h @@ -107,7 +107,7 @@ bool HasAttribute(QDomElement element, const std::string &attributeName); * @param[in] key Name how the value is specified in the xml file * @param[in] tag Name of the tags that should be imported * @param[out] probabilities Map where the probabilities get saved -* @param[in] mustAddUpToOne flag which specifies wether the sum of all probalities must be 1 +* @param[in] mustAddUpToOne flag which specifies whether the sum of all probalities must be 1 * @return true, if successful */ template diff --git a/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSender.cpp b/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSender.cpp new file mode 100644 index 00000000..cfafd0a4 --- /dev/null +++ b/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSender.cpp @@ -0,0 +1,166 @@ +/******************************************************************************** + * Copyright (c) 2018 in-tech GmbH + ********************************************************************************/ + +//----------------------------------------------------------------------------- +/** @file AlgorithmCar2XSender.cpp */ +//----------------------------------------------------------------------------- + +#include "AlgorithmCar2XSender.h" +#include "AlgorithmCar2XSenderImplementation.h" + +const std::string Version = "0.0.1"; +static const CallbackInterface *Callbacks = nullptr; + +extern "C" ALGORITHM_CAR_TO_X_SENDER_SHARED_EXPORT const std::string &OpenPASS_GetVersion() +{ + return Version; +} + +extern "C" ALGORITHM_CAR_TO_X_SENDER_SHARED_EXPORT ModelInterface *OpenPASS_CreateInstance( + std::string componentName, + bool isInit, + int priority, + int offsetTime, + int responseTime, + int cycleTime, + StochasticsInterface *stochastics, + WorldInterface *world, + const ParameterInterface *parameters, + PublisherInterface * const publisher, + AgentInterface *agent, + const CallbackInterface *callbacks) +{ + Callbacks = callbacks; + + try + { + return (ModelInterface*)(new (std::nothrow) AlgorithmCar2XSenderImplementation( + componentName, + isInit, + priority, + offsetTime, + responseTime, + cycleTime, + stochastics, + world, + parameters, + publisher, + callbacks, + agent)); + } + catch(const std::runtime_error &ex) + { + if(Callbacks != nullptr) + { + Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, ex.what()); + } + + return nullptr; + } + catch(...) + { + if(Callbacks != nullptr) + { + Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, "unexpected exception"); + } + + return nullptr; + } +} + +extern "C" ALGORITHM_CAR_TO_X_SENDER_SHARED_EXPORT void OpenPASS_DestroyInstance(ModelInterface *implementation) +{ + delete (AlgorithmCar2XSenderImplementation*)implementation; +} + +extern "C" ALGORITHM_CAR_TO_X_SENDER_SHARED_EXPORT bool OpenPASS_UpdateInput(ModelInterface *implementation, + int localLinkId, + const std::shared_ptr &data, + int time) +{ + try + { + implementation->UpdateInput(localLinkId, data, time); + } + catch(const std::runtime_error &ex) + { + if(Callbacks != nullptr) + { + Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, ex.what()); + } + + return false; + } + catch(...) + { + if(Callbacks != nullptr) + { + Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, "unexpected exception"); + } + + return false; + } + + return true; +} + +extern "C" ALGORITHM_CAR_TO_X_SENDER_SHARED_EXPORT bool OpenPASS_UpdateOutput(ModelInterface *implementation, + int localLinkId, + std::shared_ptr &data, + int time) +{ + try + { + implementation->UpdateOutput(localLinkId, data, time); + } + catch(const std::runtime_error &ex) + { + if(Callbacks != nullptr) + { + Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, ex.what()); + } + + return false; + } + catch(...) + { + if(Callbacks != nullptr) + { + Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, "unexpected exception"); + } + + return false; + } + + return true; +} + +extern "C" ALGORITHM_CAR_TO_X_SENDER_SHARED_EXPORT bool OpenPASS_Trigger(ModelInterface *implementation, + int time) +{ + try + { + implementation->Trigger(time); + } + catch(const std::runtime_error &ex) + { + if(Callbacks != nullptr) + { + Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, ex.what()); + } + + return false; + } + catch(...) + { + if(Callbacks != nullptr) + { + Callbacks->Log(CbkLogLevel::Error, __FILE__, __LINE__, "unexpected exception"); + } + + return false; + } + + return true; +} diff --git a/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSender.h b/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSender.h new file mode 100644 index 00000000..a7684fe4 --- /dev/null +++ b/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSender.h @@ -0,0 +1,17 @@ +/******************************************************************************** + * Copyright (c) 2018 in-tech GmbH + ********************************************************************************/ + +//----------------------------------------------------------------------------- +/** @file AlgorithmCar2XSender.h +* @brief This file provides the exported methods. +* +* This file provides the exported methods which are available outside of the library. */ +//----------------------------------------------------------------------------- + +#pragma once + +#include "AlgorithmCar2XSenderGlobal.h" +#include "include/modelInterface.h" + + diff --git a/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSenderGlobal.h b/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSenderGlobal.h new file mode 100644 index 00000000..358f11e1 --- /dev/null +++ b/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSenderGlobal.h @@ -0,0 +1,21 @@ +/******************************************************************************** + * Copyright (c) 2018 in-tech GmbH + ********************************************************************************/ + +//----------------------------------------------------------------------------- +/** @file AlgorithmCar2XSenderGlobal.h +* @brief This file contains DLL export declarations +**/ +//----------------------------------------------------------------------------- + +#pragma once + +#include + +#if defined(ALGORITHM_CAR_TO_X_SENDER_LIBRARY) +# define ALGORITHM_CAR_TO_X_SENDER_SHARED_EXPORT Q_DECL_EXPORT +#else +# define ALGORITHM_CAR_TO_X_SENDER_SHARED_EXPORT Q_DECL_IMPORT +#endif + + diff --git a/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSenderImplementation.cpp b/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSenderImplementation.cpp new file mode 100644 index 00000000..7bdc785e --- /dev/null +++ b/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSenderImplementation.cpp @@ -0,0 +1,163 @@ +/******************************************************************************** + * Copyright (c) 2018 in-tech GmbH + * 2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +//----------------------------------------------------------------------------- +/** @file AlgorithmCar2XSenderImplementation.cpp */ +//----------------------------------------------------------------------------- + +#include +#include +#include + +#include "include/observationInterface.h" +#include "include/worldInterface.h" +#include "include/parameterInterface.h" +#include "AlgorithmCar2XSenderImplementation.h" + +AlgorithmCar2XSenderImplementation::AlgorithmCar2XSenderImplementation(std::string componentName, + bool isInit, + int priority, + int offsetTime, + int responseTime, + int cycleTime, + StochasticsInterface *stochastics, + WorldInterface *world, + const ParameterInterface *parameters, + PublisherInterface * const publisher, + const CallbackInterface *callbacks, + AgentInterface *agent) : + UnrestrictedModelInterface( + componentName, + isInit, + priority, + offsetTime, + responseTime, + cycleTime, + stochastics, + world, + parameters, + publisher, + callbacks, + agent) + { + // read parameters + try + { + signalStrength = parameters->GetParametersDouble().at("SignalStrength"); + if(parameters->GetParametersBool().at("SendPositionXEnabled")) + { + informationToSend.push_back(SensorInformationType::PositionX); + } + if(parameters->GetParametersBool().at("SendPositionYEnabled")) + { + informationToSend.push_back(SensorInformationType::PositionY); + } + if(parameters->GetParametersBool().at("SendVelocityEnabled")) + { + informationToSend.push_back(SensorInformationType::Velocity); + } + if(parameters->GetParametersBool().at("SendAccelerationEnabled")) + { + informationToSend.push_back(SensorInformationType::Acceleration); + } + if(parameters->GetParametersBool().at("SendYawEnabled")) + { + informationToSend.push_back(SensorInformationType::Yaw); + } + } + catch(...) + { + const std::string msg = COMPONENTNAME + " could not init parameters"; + LOG(CbkLogLevel::Error, msg); + throw std::runtime_error(msg); + } + + try + { + if (GetPublisher() == nullptr) throw std::runtime_error(""); + } + catch(...) + { + const std::string msg = COMPONENTNAME + " invalid observation module setup"; + LOG(CbkLogLevel::Error, msg); + throw std::runtime_error(msg); + } +} + +void AlgorithmCar2XSenderImplementation::UpdateInput(int localLinkId, const std::shared_ptr &data, int time) +{ + Q_UNUSED(localLinkId); + Q_UNUSED(data); + Q_UNUSED(time); +} + +void AlgorithmCar2XSenderImplementation::UpdateOutput(int localLinkId, std::shared_ptr &data, int time) +{ + Q_UNUSED(localLinkId); + Q_UNUSED(data); + Q_UNUSED(time); +} + +void AlgorithmCar2XSenderImplementation::Trigger(int time) +{ + Q_UNUSED(time); + const auto movingObject = FillObjectInformation(); + + GetWorld()->GetRadio().Send(GetAgent()->GetPositionX(), GetAgent()->GetPositionY(), signalStrength, movingObject); +} + + +osi3::MovingObject AlgorithmCar2XSenderImplementation::FillObjectInformation() +{ + + osi3::MovingObject object; + Common::Vector2d ownVelocity{0.0, 0.0}; + Common::Vector2d ownAcceleration{0.0, 0.0}; + + for (auto information : informationToSend) + { + switch(information) + { + case SensorInformationType::PositionX: + object.mutable_base()->mutable_position()->set_x(GetAgent()->GetPositionX()); + break; + + case SensorInformationType::PositionY: + object.mutable_base()->mutable_position()->set_y(GetAgent()->GetPositionY()); + break; + + case SensorInformationType::Yaw: + object.mutable_base()->mutable_orientation()->set_yaw(GetAgent()->GetYaw()); + break; + + case SensorInformationType::Velocity: + ownVelocity = GetAgent()->GetVelocity(); + object.mutable_base()->mutable_velocity()->set_x(ownVelocity.x); + object.mutable_base()->mutable_velocity()->set_y(ownVelocity.y); + break; + + case SensorInformationType::Acceleration: + ownAcceleration = GetAgent()->GetAcceleration(); + object.mutable_base()->mutable_acceleration()->set_x(ownAcceleration.x); + object.mutable_base()->mutable_acceleration()->set_y(ownAcceleration.y); + break; + + default: + const std::string msg = COMPONENTNAME + " invalid sensorInformationType"; + LOG(CbkLogLevel::Debug, msg); + break; + } + + } + + return object; +} + diff --git a/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSenderImplementation.h b/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSenderImplementation.h new file mode 100644 index 00000000..7aa2c6c8 --- /dev/null +++ b/sim/src/components/AlgorithmCar2XSender/AlgorithmCar2XSenderImplementation.h @@ -0,0 +1,115 @@ +/******************************************************************************** + * Copyright (c) 2018-2021 in-tech GmbH + * 2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +#pragma once + +#include "common/accelerationSignal.h" +#include "common/primitiveSignals.h" +#include "common/sensorDataSignal.h" +#include "include/modelInterface.h" +#include "include/radioInterface.h" +#include "osi3/osi_sensordata.pb.h" + +//! Type of information detected by a sensor +enum class SensorInformationType +{ + Distance, + PositionX, + PositionY, + Yaw, + Velocity, + Acceleration +}; + +class AlgorithmCar2XSenderImplementation : public UnrestrictedModelInterface +{ +public: + const std::string COMPONENTNAME = "AlgorithmCar2XSender"; + + AlgorithmCar2XSenderImplementation( + std::string componentName, + bool isInit, + int priority, + int offsetTime, + int responseTime, + int cycleTime, + StochasticsInterface *stochastics, + WorldInterface *world, + const ParameterInterface *parameters, + PublisherInterface * const publisher, + const CallbackInterface *callbacks, + AgentInterface *agent); + AlgorithmCar2XSenderImplementation(const AlgorithmCar2XSenderImplementation&) = delete; + AlgorithmCar2XSenderImplementation(AlgorithmCar2XSenderImplementation&&) = delete; + AlgorithmCar2XSenderImplementation& operator=(const AlgorithmCar2XSenderImplementation&) = delete; + AlgorithmCar2XSenderImplementation& operator=(AlgorithmCar2XSenderImplementation&&) = delete; + virtual ~AlgorithmCar2XSenderImplementation() = default; + + /*! + * \brief Update Inputs + * + * Function is called by framework when another component delivers a signal over + * a channel to this component (scheduler calls update tasks of other component). + * + * Refer to module description for input channels and input ids. + * + * \param[in] localLinkId Corresponds to "id" of "ComponentInput" + * \param[in] data Referenced signal (copied by sending component) + * \param[in] time Current scheduling time + */ + virtual void UpdateInput(int localLinkId, const std::shared_ptr &data, int time); + + /*! + * \brief Update outputs. + * + * Function is called by framework when this Component.has to deliver a signal over + * a channel to another component (scheduler calls update task of this component). + * + * Refer to module description for output channels and output ids. + * + * \param[in] localLinkId Corresponds to "id" of "ComponentOutput" + * \param[out] data Referenced signal (copied by this component) + * \param[in] time Current scheduling time + */ + virtual void UpdateOutput(int localLinkId, std::shared_ptr &data, int time); + + /*! + * \brief Process data within component. + * + * Function is called by framework when the scheduler calls the trigger task + * of this component. + * + * Refer to module description for information about the module's task. + * + * \param[in] time Current scheduling time + */ + virtual void Trigger(int time); + +private: + + //----------------------------------------------------------------------------- + //! Sets information that the object can provide (e.g. Acceleration) + //! Provided information is set in profile + //! @param[in}] object instance to fill out + //----------------------------------------------------------------------------- + osi3::MovingObject FillObjectInformation(); + + std::vector informationToSend; + + double signalStrength = 0.0; + + int currentTimestep{0}; //! for observer + int triggerTime = std::numeric_limits::max(); + + + // event + std::string sequenceName {}; +}; diff --git a/sim/src/components/AlgorithmCar2XSender/CMakeLists.txt b/sim/src/components/AlgorithmCar2XSender/CMakeLists.txt new file mode 100644 index 00000000..aeaa9a2d --- /dev/null +++ b/sim/src/components/AlgorithmCar2XSender/CMakeLists.txt @@ -0,0 +1,30 @@ +################################################################################ +# Copyright (c) 2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) +# +# This program and the accompanying materials are made available under the +# terms of the Eclipse Public License 2.0 which is available at +# http://www.eclipse.org/legal/epl-2.0. +# +# SPDX-License-Identifier: EPL-2.0 +################################################################################ +set(COMPONENT_NAME AlgorithmCar2XSender) + +add_compile_definitions(ALGORITHM_CAR_TO_X_SENDER_LIBRARY) + +add_openpass_target( + NAME ${COMPONENT_NAME} TYPE library LINKAGE shared COMPONENT core + + HEADERS + AlgorithmCar2XSender.h + AlgorithmCar2XSenderGlobal.h + AlgorithmCar2XSenderImplementation.h + + SOURCES + AlgorithmCar2XSender.cpp + AlgorithmCar2XSenderImplementation.cpp + + LIBRARIES + Qt5::Core + + LINKOSI +) diff --git a/sim/src/components/Algorithm_Longitudinal/src/longCalcs.h b/sim/src/components/Algorithm_Longitudinal/src/longCalcs.h index 403863c8..35c7c447 100644 --- a/sim/src/components/Algorithm_Longitudinal/src/longCalcs.h +++ b/sim/src/components/Algorithm_Longitudinal/src/longCalcs.h @@ -89,7 +89,7 @@ public: double GetEngineSpeedByVelocity(double velocity, int gear); //! - //! \brief Checks wether the engineSpeed and acceleration/torque can be achieved by the engine + //! \brief Checks whether the engineSpeed and acceleration/torque can be achieved by the engine //! bool isWithinEngineLimits(int gear, double engineSpeed, double acceleration); inline bool isEngineSpeedWithinEngineLimits(double engineSpeed); diff --git a/sim/src/components/CMakeLists.txt b/sim/src/components/CMakeLists.txt index 03b36449..a6c45576 100644 --- a/sim/src/components/CMakeLists.txt +++ b/sim/src/components/CMakeLists.txt @@ -17,6 +17,7 @@ add_subdirectory(Action_LongitudinalDriver) add_subdirectory(Action_SecondaryDriverTasks) add_subdirectory(AgentUpdater) add_subdirectory(AlgorithmAFDM) +add_subdirectory(AlgorithmCar2XSender) add_subdirectory(Algorithm_AEB) #add_subdirectory(Algorithm_CruiseControlByDistance) add_subdirectory(Algorithm_ECU) diff --git a/sim/src/components/ComponentController/src/condition.h b/sim/src/components/ComponentController/src/condition.h index a177a124..37a29ddd 100644 --- a/sim/src/components/ComponentController/src/condition.h +++ b/sim/src/components/ComponentController/src/condition.h @@ -32,7 +32,7 @@ public: virtual ~Condition() = default; /*! - * \brief Returns wether this Condition is fullfilled. + * \brief Returns whether this Condition is fullfilled. * \param componentNameToComponentTypeAndStatesMap Information the ComponentController has gathered */ virtual bool IsFullfilled(const StateMapping &componentNameToComponentTypeAndStatesMap) const = 0; @@ -104,7 +104,7 @@ private: * \brief This Condition is fullfilled if two ComponentStateExpressions are * evaluated to the same value * - * The ComponentStateEquality is used either to check, wether the state of two components + * The ComponentStateEquality is used either to check, whether the state of two components * is the same, or to check of the state of some component is equal to some fixed value (e.g. Acting). */ class ComponentStateEquality : public Condition diff --git a/sim/src/components/Sensor_Driver/src/Signals/sensor_driverDefinitions.h b/sim/src/components/Sensor_Driver/src/Signals/sensor_driverDefinitions.h index 971cb97d..4d750f6c 100644 --- a/sim/src/components/Sensor_Driver/src/Signals/sensor_driverDefinitions.h +++ b/sim/src/components/Sensor_Driver/src/Signals/sensor_driverDefinitions.h @@ -57,7 +57,7 @@ struct LaneInformationTrafficRules //! This struct is used to transport data of a lane concerning its geometric features as seen by the driver struct LaneInformationGeometry { - //! Wether there is a lane on this position + //! Whether there is a lane on this position bool exists {false}; //! Curvature at current s position (default if not existing) double curvature {-999.0}; @@ -84,7 +84,7 @@ struct OwnVehicleInformation double distanceToLaneBoundaryLeft {-999.0}; //! Distance between the right front point and the right boundary of the lane it is in. double distanceToLaneBoundaryRight {-999.0}; - //! Wether this agent has collided with another object + //! Whether this agent has collided with another object bool collision {false}; }; diff --git a/sim/src/components/Sensor_OSI/CMakeLists.txt b/sim/src/components/Sensor_OSI/CMakeLists.txt index 107cda21..78c1d22f 100644 --- a/sim/src/components/Sensor_OSI/CMakeLists.txt +++ b/sim/src/components/Sensor_OSI/CMakeLists.txt @@ -1,5 +1,5 @@ ################################################################################ -# Copyright (c) 2020-2021 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) +# Copyright (c) 2020-2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) # # This program and the accompanying materials are made available under the # terms of the Eclipse Public License 2.0 which is available at @@ -18,11 +18,13 @@ add_openpass_target( sensorOSI.h src/objectDetectorBase.h src/sensorGeometric2D.h + src/sensorCar2X.h SOURCES sensorOSI.cpp src/objectDetectorBase.cpp src/sensorGeometric2D.cpp + src/sensorCar2X.cpp INCDIRS ../../core/opSimulation/modules/World_OSI diff --git a/sim/src/components/Sensor_OSI/sensorOSI.cpp b/sim/src/components/Sensor_OSI/sensorOSI.cpp index 651f3b65..c42535d4 100644 --- a/sim/src/components/Sensor_OSI/sensorOSI.cpp +++ b/sim/src/components/Sensor_OSI/sensorOSI.cpp @@ -1,5 +1,6 @@ /******************************************************************************** * Copyright (c) 2017-2019 in-tech GmbH + * 2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) * * This program and the accompanying materials are made available under the * terms of the Eclipse Public License 2.0 which is available at @@ -15,6 +16,7 @@ #include "include/parameterInterface.h" #include "sensorOSI.h" #include "src/sensorGeometric2D.h" +#include "src/sensorCar2X.h" const std::string Version = "0.0.1"; static const CallbackInterface *Callbacks = nullptr; @@ -60,6 +62,22 @@ extern "C" SENSOR_OSI_SHARED_EXPORT ModelInterface *OpenPASS_CreateInstance( callbacks, agent)); } + else if (sensorType == "ReceiverCar2X") + { + return (ModelInterface*)(new (std::nothrow) SensorCar2X( + componentName, + isInit, + priority, + offsetTime, + responseTime, + cycleTime, + stochastics, + world, + parameters, + publisher, + callbacks, + agent)); + } else { if (Callbacks != nullptr) diff --git a/sim/src/components/Sensor_OSI/src/sensorCar2X.cpp b/sim/src/components/Sensor_OSI/src/sensorCar2X.cpp new file mode 100644 index 00000000..1e6fbacc --- /dev/null +++ b/sim/src/components/Sensor_OSI/src/sensorCar2X.cpp @@ -0,0 +1,112 @@ +/******************************************************************************** + * Copyright (c) 2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +//----------------------------------------------------------------------------- +/** \brief sensorCar2X.cpp */ +//----------------------------------------------------------------------------- + +#include "sensorCar2X.h" + +#include "include/radioInterface.h" +#include "include/worldInterface.h" +#include "include/parameterInterface.h" + + +SensorCar2X::SensorCar2X( + std::string componentName, + bool isInit, + int priority, + int offsetTime, + int responseTime, + int cycleTime, + StochasticsInterface *stochastics, + WorldInterface *world, + const ParameterInterface *parameters, + PublisherInterface * const publisher, + const CallbackInterface *callbacks, + AgentInterface *agent) : + ObjectDetectorBase( + componentName, + isInit, + priority, + offsetTime, + responseTime, + cycleTime, + stochastics, + world, + parameters, + publisher, + callbacks, + agent) +{ + sensitivity = parameters->GetParametersDouble().at("Sensitivity"); +} + +void SensorCar2X::Trigger(int time) +{ + auto newSensorData = DetectObjects(); + sensorData = ApplyLatency(time, newSensorData); +} + +void SensorCar2X::UpdateInput(int, const std::shared_ptr &, int) +{ +} + +osi3::SensorData SensorCar2X::DetectObjects() +{ + + RadioInterface& radio = GetWorld()->GetRadio(); + + Position absolutePosition = GetAbsolutePosition(); + + sensorData = {}; + std::vector detectedObjects = radio.Receive(absolutePosition.xPos, absolutePosition.yPos, sensitivity); + + const auto sensorPosition = GetSensorPosition(); + const auto ownYaw = GetAgent()->GetYaw() + position.yaw; + const ObjectPointCustom mountingPosition{position.longitudinal, position.lateral}; + const auto ownVelocity = GetAgent()->GetVelocity(mountingPosition); + const auto ownAcceleration = GetAgent()->GetAcceleration(mountingPosition); + + for (auto &object : detectedObjects) + { + osi3::DetectedMovingObject* detectedObject = sensorData.add_moving_object(); + detectedObject->mutable_header()->add_ground_truth_id()->set_value(object.id().value()); + detectedObject->mutable_header()->add_sensor_id()->set_value(id); + + if(object.base().position().has_x() && object.base().position().has_y()) + { + point_t objectReferencePointGlobal{object.base().position().x(), object.base().position().y()}; + point_t objectReferencePointLocal = TransformPointToLocalCoordinates(objectReferencePointGlobal, sensorPosition, ownYaw); + detectedObject->mutable_base()->mutable_position()->set_x(objectReferencePointLocal.x()); + detectedObject->mutable_base()->mutable_position()->set_y(objectReferencePointLocal.y()); + } + if (object.base().orientation().has_yaw()) + { + detectedObject->mutable_base()->mutable_orientation()->set_yaw(object.base().orientation().yaw() - ownYaw); + } + if (object.base().velocity().has_x() && object.base().velocity().has_y()) + { + point_t objectVelocity{object.base().velocity().x(), object.base().velocity().y()}; + point_t relativeVelocity = CalculateRelativeVector(objectVelocity, {ownVelocity.x, ownVelocity.y}, ownYaw); + detectedObject->mutable_base()->mutable_velocity()->set_x(relativeVelocity.x()); + detectedObject->mutable_base()->mutable_velocity()->set_y(relativeVelocity.y()); + } + if (object.base().acceleration().has_x() && object.base().acceleration().has_y()) + { + point_t objectAcceleration{object.base().acceleration().x(), object.base().acceleration().y()}; + point_t relativeAcceleration = CalculateRelativeVector(objectAcceleration, {ownAcceleration.x, ownAcceleration.y}, ownYaw); + detectedObject->mutable_base()->mutable_acceleration()->set_x(relativeAcceleration.x()); + detectedObject->mutable_base()->mutable_acceleration()->set_y(relativeAcceleration.y()); + } + } + + return sensorData; +} \ No newline at end of file diff --git a/sim/src/components/Sensor_OSI/src/sensorCar2X.h b/sim/src/components/Sensor_OSI/src/sensorCar2X.h new file mode 100644 index 00000000..0057949c --- /dev/null +++ b/sim/src/components/Sensor_OSI/src/sensorCar2X.h @@ -0,0 +1,65 @@ +/******************************************************************************** + * Copyright (c) 2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) + * + * This program and the accompanying materials are made available under the + * terms of the Eclipse Public License 2.0 which is available at + * http://www.eclipse.org/legal/epl-2.0. + * + * SPDX-License-Identifier: EPL-2.0 + ********************************************************************************/ + +//----------------------------------------------------------------------------- +/** \file sensorCar2X.h +* \brief This file models a receiver which detects Car2X senders and acquires their detectedObjects. +*/ +//----------------------------------------------------------------------------- + +#pragma once + +#include "objectDetectorBase.h" +#include "common/sensorDataSignal.h" + +//----------------------------------------------------------------------------- +/** \brief This file models a sensor which detects vehicles via Car2X capability. +* \details This sensor uses a broker-class concept to "sense" Car2X senders via a radio cloud. +* +* \ingroup SensorObjectDetector +*/ +//----------------------------------------------------------------------------- +class SensorCar2X : public ObjectDetectorBase +{ +public: + SensorCar2X( + std::string componentName, + bool isInit, + int priority, + int offsetTime, + int responseTime, + int cycleTime, + StochasticsInterface *stochastics, + WorldInterface *world, + const ParameterInterface *parameters, + PublisherInterface * const publisher, + const CallbackInterface *callbacks, + AgentInterface *agent); + + void UpdateInput(int, const std::shared_ptr &, int); + void Trigger(int time); + + /** + * \brief We detect objects via calling the broker-class function RadioInterface::Receive(). + */ + osi3::SensorData DetectObjects(); + + +private: + double sensitivity; + osi3::SensorData sensorData; + +public: + SensorCar2X(const SensorCar2X&) = delete; + SensorCar2X(SensorCar2X&&) = delete; + SensorCar2X& operator=(const SensorCar2X&) = delete; + SensorCar2X& operator=(SensorCar2X&&) = delete; + ~SensorCar2X() = default; +}; \ No newline at end of file diff --git a/sim/src/core/opSimulation/framework/agentBlueprintProvider.h b/sim/src/core/opSimulation/framework/agentBlueprintProvider.h index 2458c54f..b94aec7a 100644 --- a/sim/src/core/opSimulation/framework/agentBlueprintProvider.h +++ b/sim/src/core/opSimulation/framework/agentBlueprintProvider.h @@ -29,7 +29,7 @@ public: /*! * \brief Samples an entire agent * - * \details First samples the agent profile and then depending on wether it is a static or dynamic profile samples the dynamic + * \details First samples the agent profile and then depending on whether it is a static or dynamic profile samples the dynamic * profiles and builds an AgentType from this or loads the AgentType from the specified SystemConfig * * \param agentProfileName name of agent profile to sample diff --git a/sim/src/core/opSimulation/modules/EventDetector/CollisionDetector.h b/sim/src/core/opSimulation/modules/EventDetector/CollisionDetector.h index c58267f8..64b34c25 100644 --- a/sim/src/core/opSimulation/modules/EventDetector/CollisionDetector.h +++ b/sim/src/core/opSimulation/modules/EventDetector/CollisionDetector.h @@ -11,9 +11,9 @@ //----------------------------------------------------------------------------- /** @file CollisionDetector.h -* @brief Detects wether agents collided. +* @brief Detects whether agents collided. * -* This manipulator detects wether agents collided with either other agents or traffic objects. +* This manipulator detects whether agents collided with either other agents or traffic objects. * Once a collision is detected a CollisionEvent is created and forwarded to the EventNetwork. */ //----------------------------------------------------------------------------- @@ -67,8 +67,8 @@ typedef enum static const double ROTATION_EPS = 0.0001; //----------------------------------------------------------------------------- -/** \brief This class detectes wether a collision happen in the simulation. -* \details This class detects wether an agent collided with either another agent +/** \brief This class detectes whether a collision happen in the simulation. +* \details This class detects whether an agent collided with either another agent * or a traffic object. In case a collision happend an event is created. * * \ingroup EventDetector */ diff --git a/sim/src/core/opSimulation/modules/Observation_Log/observationFileHandler.h b/sim/src/core/opSimulation/modules/Observation_Log/observationFileHandler.h index 94b60cdf..5da36633 100644 --- a/sim/src/core/opSimulation/modules/Observation_Log/observationFileHandler.h +++ b/sim/src/core/opSimulation/modules/Observation_Log/observationFileHandler.h @@ -126,7 +126,7 @@ protected: //add infos to the file stream /*! - * \brief Returns wether the agent has sensors or not. + * \brief Returns whether the agent has sensors or not. * * @return true if agent has sensors, otherwise false. */ diff --git a/sim/src/core/opSimulation/modules/Observation_LogAgent/observationFileHandler.h b/sim/src/core/opSimulation/modules/Observation_LogAgent/observationFileHandler.h index 28995817..896f774e 100644 --- a/sim/src/core/opSimulation/modules/Observation_LogAgent/observationFileHandler.h +++ b/sim/src/core/opSimulation/modules/Observation_LogAgent/observationFileHandler.h @@ -121,7 +121,7 @@ private: //add infos to the file stream /*! - * \brief Returns wether the agent has sensors or not. + * \brief Returns whether the agent has sensors or not. * * @return true if agent has sensors, otherwise false. */ diff --git a/sim/src/core/opSimulation/modules/Spawners/Scenario/SpawnerScenario.h b/sim/src/core/opSimulation/modules/Spawners/Scenario/SpawnerScenario.h index 2849f215..a5f9ae97 100644 --- a/sim/src/core/opSimulation/modules/Spawners/Scenario/SpawnerScenario.h +++ b/sim/src/core/opSimulation/modules/Spawners/Scenario/SpawnerScenario.h @@ -128,7 +128,7 @@ private: Route GetRandomRoute(const SpawnParameter& spawnParameter); /*! - * \brief Checks wether the agent is inside the world + * \brief Checks whether the agent is inside the world * * \param spawnParameter parameters containing spawn position */ diff --git a/sim/src/core/opSimulation/modules/Spawners/common/WorldAnalyzer.h b/sim/src/core/opSimulation/modules/Spawners/common/WorldAnalyzer.h index 27f8790d..8b7c4efd 100644 --- a/sim/src/core/opSimulation/modules/Spawners/common/WorldAnalyzer.h +++ b/sim/src/core/opSimulation/modules/Spawners/common/WorldAnalyzer.h @@ -122,7 +122,7 @@ public: const double agentRearLength, const double intendedVelocity) const; - //! Check wether the minimum distance the next object is satisfied + //! Check whether the minimum distance the next object is satisfied //! //! \param laneId Id of the lane //! \param sPosition s coordinate of own agent @@ -154,7 +154,7 @@ public: const Route &route, const VehicleModelParameters& vehicleModelParameters) const; - //! Check wether spawning an agent with given parameters will result in a crash + //! Check whether spawning an agent with given parameters will result in a crash //! //! \param laneStream LaneStream to spawn in //! \param sPosition s coordinate diff --git a/sim/src/core/opSimulation/modules/World_OSI/Localization.cpp b/sim/src/core/opSimulation/modules/World_OSI/Localization.cpp index 2667b0c1..dc58d750 100644 --- a/sim/src/core/opSimulation/modules/World_OSI/Localization.cpp +++ b/sim/src/core/opSimulation/modules/World_OSI/Localization.cpp @@ -91,7 +91,7 @@ std::function LocateOnGeometryElement(const OWL::Int const LocalizationElement& localizationElement = *value.second; boost::geometry::de9im::mask mask("*****F***"); // within - if (!boost::geometry::relate(point_t{point.x, point.y}, localizationElement.boost_polygon, mask)) //Check wether point is inside polygon + if (!boost::geometry::relate(point_t{point.x, point.y}, localizationElement.boost_polygon, mask)) //Check whether point is inside polygon { return; } diff --git a/sim/src/core/opSimulation/modules/World_OSI/OWL/DataTypes.h b/sim/src/core/opSimulation/modules/World_OSI/OWL/DataTypes.h index 8512dfd9..f7a2f798 100644 --- a/sim/src/core/opSimulation/modules/World_OSI/OWL/DataTypes.h +++ b/sim/src/core/opSimulation/modules/World_OSI/OWL/DataTypes.h @@ -742,7 +742,7 @@ struct LaneOverlap //! Returns the specification of the sign with the set relative distance virtual CommonTrafficSign::Entity GetSpecification(const double relativeDistance) const = 0; - //! Returns wether the sign is valid for the specified lane + //! Returns whether the sign is valid for the specified lane virtual bool IsValidForLane(OWL::Id laneId) const = 0; //!Returns the position of the reference point of the object in absolute coordinates (i. e. world coordinates) @@ -790,7 +790,7 @@ struct LaneOverlap //! Returns the s coordinate virtual double GetS() const = 0; - //! Returns wether the sign is valid for the specified lane + //! Returns whether the sign is valid for the specified lane virtual bool IsValidForLane(OWL::Id laneId) const = 0; //!Returns the position of the reference point of the object in absolute coordinates (i. e. world coordinates) @@ -843,7 +843,7 @@ struct LaneOverlap //! Returns the specification of the sign with the set relative distance virtual CommonTrafficSign::Entity GetSpecification(const double relativeDistance) const = 0; - //! Returns wether the sign is valid for the specified lane + //! Returns whether the sign is valid for the specified lane virtual bool IsValidForLane(OWL::Id laneId) const = 0; //!Returns the position of the reference point of the object in absolute coordinates (i. e. world coordinates) diff --git a/sim/src/core/opSimulation/modules/World_OSI/RadioImplementation.cpp b/sim/src/core/opSimulation/modules/World_OSI/RadioImplementation.cpp index c9ecd94f..23f2172d 100644 --- a/sim/src/core/opSimulation/modules/World_OSI/RadioImplementation.cpp +++ b/sim/src/core/opSimulation/modules/World_OSI/RadioImplementation.cpp @@ -11,15 +11,15 @@ #include "RadioImplementation.h" #include -void RadioImplementation::Send(double positionX, double postionY, double signalStrength, DetectedObject objectInformation) +void RadioImplementation::Send(double positionX, double postionY, double signalStrength, osi3::MovingObject objectInformation) { RadioSignal radioSignal{positionX, postionY, signalStrength, objectInformation}; signalVector.push_back(radioSignal); } -std::vector RadioImplementation::Receive(double positionX, double positionY, double sensitivity) +std::vector RadioImplementation::Receive(double positionX, double positionY, double sensitivity) { - std::vector detectedObjects{}; + std::vector detectedObjects{}; for (RadioSignal radioSignal : signalVector) { double deltaX= radioSignal.positionX - positionX; diff --git a/sim/src/core/opSimulation/modules/World_OSI/RadioImplementation.h b/sim/src/core/opSimulation/modules/World_OSI/RadioImplementation.h index 85988196..c1cd4da5 100644 --- a/sim/src/core/opSimulation/modules/World_OSI/RadioImplementation.h +++ b/sim/src/core/opSimulation/modules/World_OSI/RadioImplementation.h @@ -25,7 +25,7 @@ struct RadioSignal double positionX; double positionY; double signalStrength; - DetectedObject objectInformation; + osi3::MovingObject objectInformation; }; class RadioImplementation : public RadioInterface @@ -40,7 +40,7 @@ public: //! @param[in] signalStrength signal strength of sender (transmitting power [W]) //! @param[in] objectInformation sensor data //----------------------------------------------------------------------------- - void Send(double positionX, double postionY, double signalStrength, DetectedObject objectInformation) override; + void Send(double positionX, double postionY, double signalStrength, osi3::MovingObject objectInformation) override; //----------------------------------------------------------------------------- //! Retrieve available information at current postion from cloud. @@ -49,7 +49,7 @@ public: //! @param[in] sensitivity sensitivity of receiver (implemented as surface power density [W/m2]) //! @return data of senders "visible" at current position //----------------------------------------------------------------------------- - std::vector Receive(double positionX, double positionY, double sensitivity) override; + std::vector Receive(double positionX, double positionY, double sensitivity) override; //----------------------------------------------------------------------------- //! Resets the cloud for next simulation step diff --git a/sim/src/core/opSimulation/modules/World_OSI/SceneryConverter.h b/sim/src/core/opSimulation/modules/World_OSI/SceneryConverter.h index adb1c6e1..49d3ef95 100644 --- a/sim/src/core/opSimulation/modules/World_OSI/SceneryConverter.h +++ b/sim/src/core/opSimulation/modules/World_OSI/SceneryConverter.h @@ -315,7 +315,7 @@ private: //! \param signal OpenDrive specification of the traffic light //! \param position position of the traffic light in the world //! \param lanes lanes for which this traffic light is valid - //! \param openDriveType wether this traffic light has a yellow bulb + //! \param openDriveType whether this traffic light has a yellow bulb void CreateTrafficLight(RoadSignalInterface* signal, Position position, const OWL::Interfaces::Lanes& lanes, const std::string &openDriveType); void CreateObjects(); diff --git a/sim/src/core/opSimulation/modules/World_OSI/WorldDataQuery.h b/sim/src/core/opSimulation/modules/World_OSI/WorldDataQuery.h index 1774eefe..bc52e036 100644 --- a/sim/src/core/opSimulation/modules/World_OSI/WorldDataQuery.h +++ b/sim/src/core/opSimulation/modules/World_OSI/WorldDataQuery.h @@ -489,7 +489,7 @@ public: RouteQueryResult GetRelativeRoads(const RoadMultiStream& roadStream, double startPosition, double range) const; //! Returns information about all lanes on the roadStream in range. These info are the relative distances (start and end), - //! the laneId relative to the ego lane, the successors and predecessors if existing and the information wether the intended + //! the laneId relative to the ego lane, the successors and predecessors if existing and the information whether the intended //! driving direction of the lane is the same as the direction of the roadStream. If the ego lane prematurely ends, then //! the further lane ids are relative to the middle of the road. //! diff --git a/sim/tests/endToEndTests/test_end_to_end.json b/sim/tests/endToEndTests/test_end_to_end.json index 74b5fe05..35434a6b 100644 --- a/sim/tests/endToEndTests/test_end_to_end.json +++ b/sim/tests/endToEndTests/test_end_to_end.json @@ -4,6 +4,7 @@ "ADAS_AEB_CutIn", "ADAS_AEB_PreventingCollisionWithObstacle", "ADAS_AEB_PreventingCollisionWithObstacleInCurve", + "ADAS_Car2X", "AFDM_TJunction", "ByEntityCondition_RelativeLane", "ByEntityCondition_RelativeSpeed", diff --git a/sim/tests/fakes/gmock/fakeRadio.h b/sim/tests/fakes/gmock/fakeRadio.h index 95e9eaba..8f90b97a 100644 --- a/sim/tests/fakes/gmock/fakeRadio.h +++ b/sim/tests/fakes/gmock/fakeRadio.h @@ -14,14 +14,14 @@ #include "gmock/gmock.h" -#include "common/DetectedObject.h" +#include "common/sensorDataSignal.h" #include "include/radioInterface.h" class FakeRadio : public RadioInterface { public: - MOCK_METHOD4(Send, void(double, double, double, DetectedObject)); - MOCK_METHOD3(Receive, std::vector(double, double, double)); + MOCK_METHOD4(Send, void(double, double, double, osi3::MovingObject)); + MOCK_METHOD3(Receive, std::vector(double, double, double)); MOCK_METHOD0(Reset, void()); }; diff --git a/sim/tests/integrationTests/Spawner_IntegrationTests/CMakeLists.txt b/sim/tests/integrationTests/Spawner_IntegrationTests/CMakeLists.txt index f9e3ccb9..ed7c8e53 100644 --- a/sim/tests/integrationTests/Spawner_IntegrationTests/CMakeLists.txt +++ b/sim/tests/integrationTests/Spawner_IntegrationTests/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/Spawn add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT module DEFAULT_MAIN + LINKOSI SOURCES ${OPENPASS_SIMCORE_DIR}/core/common/log.cpp diff --git a/sim/tests/integrationTests/Spawner_IntegrationTests/Spawner_IntegrationTests.cpp b/sim/tests/integrationTests/Spawner_IntegrationTests/Spawner_IntegrationTests.cpp index 6586ff5f..ac3ac6d4 100644 --- a/sim/tests/integrationTests/Spawner_IntegrationTests/Spawner_IntegrationTests.cpp +++ b/sim/tests/integrationTests/Spawner_IntegrationTests/Spawner_IntegrationTests.cpp @@ -16,6 +16,7 @@ #include "fakeAgentFactory.h" #include "fakeCallback.h" #include "fakeParameter.h" +#include "fakeRadio.h" #include "fakeStochastics.h" #include "fakeStream.h" #include "fakeWorld.h" diff --git a/sim/tests/unitTests/common/CMakeLists.txt b/sim/tests/unitTests/common/CMakeLists.txt index 276af669..c324fb3c 100644 --- a/sim/tests/unitTests/common/CMakeLists.txt +++ b/sim/tests/unitTests/common/CMakeLists.txt @@ -13,7 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/common) add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT core DEFAULT_MAIN - LINK_OSI + LINKOSI SOURCES commonHelper_Tests.cpp diff --git a/sim/tests/unitTests/common/commonHelper_Tests.cpp b/sim/tests/unitTests/common/commonHelper_Tests.cpp index 791c4760..b209344d 100644 --- a/sim/tests/unitTests/common/commonHelper_Tests.cpp +++ b/sim/tests/unitTests/common/commonHelper_Tests.cpp @@ -10,6 +10,7 @@ ********************************************************************************/ #include "common/commonTools.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "gtest/gtest.h" diff --git a/sim/tests/unitTests/common/routeCalculation_Tests.cpp b/sim/tests/unitTests/common/routeCalculation_Tests.cpp index 4d1dc2d8..7f0f261b 100644 --- a/sim/tests/unitTests/common/routeCalculation_Tests.cpp +++ b/sim/tests/unitTests/common/routeCalculation_Tests.cpp @@ -10,6 +10,7 @@ ********************************************************************************/ #include "common/RoutePlanning/RouteCalculation.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "fakeAgent.h" #include "fakeEgoAgent.h" diff --git a/sim/tests/unitTests/common/ttcCalculation_Tests.cpp b/sim/tests/unitTests/common/ttcCalculation_Tests.cpp index 55067caf..c05329a9 100644 --- a/sim/tests/unitTests/common/ttcCalculation_Tests.cpp +++ b/sim/tests/unitTests/common/ttcCalculation_Tests.cpp @@ -15,6 +15,7 @@ #include "fakeAgent.h" #include "fakeParameter.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "fakeWorldObject.h" diff --git a/sim/tests/unitTests/components/AlgorithmAEB/AlgorithmAebOSIUnitTests.h b/sim/tests/unitTests/components/AlgorithmAEB/AlgorithmAebOSIUnitTests.h index 864cf605..e8cba6f2 100644 --- a/sim/tests/unitTests/components/AlgorithmAEB/AlgorithmAebOSIUnitTests.h +++ b/sim/tests/unitTests/components/AlgorithmAEB/AlgorithmAebOSIUnitTests.h @@ -12,6 +12,7 @@ #include "fakeAgent.h" #include "fakeParameter.h" #include "fakePublisher.h" +#include "fakeRadio.h" #include "fakeStochastics.h" #include "fakeWorld.h" #include "fakeWorldObject.h" diff --git a/sim/tests/unitTests/components/Dynamics_Collision/CMakeLists.txt b/sim/tests/unitTests/components/Dynamics_Collision/CMakeLists.txt index a82c7790..519bf748 100644 --- a/sim/tests/unitTests/components/Dynamics_Collision/CMakeLists.txt +++ b/sim/tests/unitTests/components/Dynamics_Collision/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/components/Dynamics_Collision/s add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT module DEFAULT_MAIN + LINKOSI SOURCES dynamicsCollision_Tests.cpp diff --git a/sim/tests/unitTests/components/Dynamics_Collision/dynamicsCollision_Tests.cpp b/sim/tests/unitTests/components/Dynamics_Collision/dynamicsCollision_Tests.cpp index 695f1c2c..775e7f17 100644 --- a/sim/tests/unitTests/components/Dynamics_Collision/dynamicsCollision_Tests.cpp +++ b/sim/tests/unitTests/components/Dynamics_Collision/dynamicsCollision_Tests.cpp @@ -12,8 +12,9 @@ #include "gmock/gmock.h" #include "fakeAgent.h" +#include "fakeRadio.h" #include "fakeWorld.h" - +#include "fakeRadio.h" #include "collisionImpl.h" using ::testing::Return; diff --git a/sim/tests/unitTests/components/Dynamics_TF/CMakeLists.txt b/sim/tests/unitTests/components/Dynamics_TF/CMakeLists.txt index 710b6a39..ef88830e 100644 --- a/sim/tests/unitTests/components/Dynamics_TF/CMakeLists.txt +++ b/sim/tests/unitTests/components/Dynamics_TF/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/components/Dynamics_TF/src) add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT module DEFAULT_MAIN + LINKOSI SOURCES dynamicsTF_Tests.cpp diff --git a/sim/tests/unitTests/components/Dynamics_TF/trajectoryTester.h b/sim/tests/unitTests/components/Dynamics_TF/trajectoryTester.h index d1e5d2e3..7073bb69 100644 --- a/sim/tests/unitTests/components/Dynamics_TF/trajectoryTester.h +++ b/sim/tests/unitTests/components/Dynamics_TF/trajectoryTester.h @@ -16,6 +16,7 @@ #include "fakePublisher.h" #include "fakeParameter.h" #include "fakeAgent.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "tfImplementation.h" diff --git a/sim/tests/unitTests/components/LimiterAccVehComp/CMakeLists.txt b/sim/tests/unitTests/components/LimiterAccVehComp/CMakeLists.txt index ec2dcdde..c3d49d76 100644 --- a/sim/tests/unitTests/components/LimiterAccVehComp/CMakeLists.txt +++ b/sim/tests/unitTests/components/LimiterAccVehComp/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/components/LimiterAccVehComp/sr add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT module DEFAULT_MAIN + LINKOSI SOURCES limiterAccVehComp_Tests.cpp diff --git a/sim/tests/unitTests/components/LimiterAccVehComp/limiterAccVehComp_Tests.cpp b/sim/tests/unitTests/components/LimiterAccVehComp/limiterAccVehComp_Tests.cpp index 84c5dfcf..0283b72d 100644 --- a/sim/tests/unitTests/components/LimiterAccVehComp/limiterAccVehComp_Tests.cpp +++ b/sim/tests/unitTests/components/LimiterAccVehComp/limiterAccVehComp_Tests.cpp @@ -13,6 +13,7 @@ #include "dontCare.h" #include "fakeAgent.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "limiterAccVehCompImpl.h" diff --git a/sim/tests/unitTests/components/OpenScenarioActions/CMakeLists.txt b/sim/tests/unitTests/components/OpenScenarioActions/CMakeLists.txt index 88419b1e..2a475f29 100644 --- a/sim/tests/unitTests/components/OpenScenarioActions/CMakeLists.txt +++ b/sim/tests/unitTests/components/OpenScenarioActions/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/components/OpenScenarioActions/ add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT module DEFAULT_MAIN + LINKOSI SOURCES openScenarioActions_Tests.cpp diff --git a/sim/tests/unitTests/components/OpenScenarioActions/openScenarioActions_Tests.cpp b/sim/tests/unitTests/components/OpenScenarioActions/openScenarioActions_Tests.cpp index dcbbefba..24870fe4 100644 --- a/sim/tests/unitTests/components/OpenScenarioActions/openScenarioActions_Tests.cpp +++ b/sim/tests/unitTests/components/OpenScenarioActions/openScenarioActions_Tests.cpp @@ -16,6 +16,7 @@ #include "fakeEgoAgent.h" #include "fakeEventNetwork.h" #include "fakeParameter.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "gmock/gmock.h" #include "gtest/gtest.h" diff --git a/sim/tests/unitTests/components/Sensor_Driver/CMakeLists.txt b/sim/tests/unitTests/components/Sensor_Driver/CMakeLists.txt index bbdbb09a..8d078513 100644 --- a/sim/tests/unitTests/components/Sensor_Driver/CMakeLists.txt +++ b/sim/tests/unitTests/components/Sensor_Driver/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/components/Sensor_Driver/src) add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT module DEFAULT_MAIN + LINKOSI SOURCES sensorDriver_Tests.cpp diff --git a/sim/tests/unitTests/components/Sensor_Driver/sensorDriver_Tests.cpp b/sim/tests/unitTests/components/Sensor_Driver/sensorDriver_Tests.cpp index 2fcd0515..94dd4620 100644 --- a/sim/tests/unitTests/components/Sensor_Driver/sensorDriver_Tests.cpp +++ b/sim/tests/unitTests/components/Sensor_Driver/sensorDriver_Tests.cpp @@ -15,6 +15,7 @@ #include "fakeAgent.h" #include "fakeEgoAgent.h" #include "fakePublisher.h" +#include "fakeRadio.h" #include "fakeWorldObject.h" #include "fakeWorld.h" #include "fakeStochastics.h" diff --git a/sim/tests/unitTests/components/Sensor_OSI/CMakeLists.txt b/sim/tests/unitTests/components/Sensor_OSI/CMakeLists.txt index eece33df..a7d5bdc1 100644 --- a/sim/tests/unitTests/components/Sensor_OSI/CMakeLists.txt +++ b/sim/tests/unitTests/components/Sensor_OSI/CMakeLists.txt @@ -17,8 +17,10 @@ add_openpass_target( SOURCES sensorOSI_Tests.cpp + radioUnitTests.cpp ${COMPONENT_SOURCE_DIR}/objectDetectorBase.cpp ${COMPONENT_SOURCE_DIR}/sensorGeometric2D.cpp + ${COMPONENT_SOURCE_DIR}/sensorCar2X.cpp ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/OWL/DataTypes.cpp ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/OWL/MovingObject.cpp ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/OWL/OpenDriveTypeMapper.cpp @@ -26,12 +28,14 @@ add_openpass_target( ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/WorldData.cpp ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/WorldDataException.cpp ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/WorldObjectAdapter.cpp + ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/RadioImplementation.cpp HEADERS sensorOSI_Tests.h sensorOSI_TestsCommon.h ${COMPONENT_SOURCE_DIR}/objectDetectorBase.h ${COMPONENT_SOURCE_DIR}/sensorGeometric2D.h + ${COMPONENT_SOURCE_DIR}/sensorCar2X.h ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/OWL/DataTypes.h ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/OWL/MovingObject.h ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/OWL/OpenDriveTypeMapper.h @@ -39,6 +43,7 @@ add_openpass_target( ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/WorldData.h ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/WorldDataException.h ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/WorldObjectAdapter.h + ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/World_OSI/RadioImplementation.h INCDIRS ${COMPONENT_SOURCE_DIR} diff --git a/sim/tests/unitTests/components/Sensor_OSI/radioUnitTests.cpp b/sim/tests/unitTests/components/Sensor_OSI/radioUnitTests.cpp new file mode 100644 index 00000000..6f5b450a --- /dev/null +++ b/sim/tests/unitTests/components/Sensor_OSI/radioUnitTests.cpp @@ -0,0 +1,147 @@ +/********************************************************************** +* Copyright (c) 2018-2019 in-tech GmbH +* 2022 Bayerische Motoren Werke Aktiengesellschaft (BMW AG) +* +* This program and the accompanying materials are made +* available under the terms of the Eclipse Public License 2.0 +* which is available at https://www.eclipse.org/legal/epl-2.0/ +* +* SPDX-License-Identifier: EPL-2.0 +**********************************************************************/ + + +#include "gtest/gtest.h" +#include "gmock/gmock.h" + +#include "fakeAgent.h" +#include "fakeStochastics.h" +#include "fakeParameter.h" +#include "fakePublisher.h" +#include "fakeRadio.h" +#include "fakeWorld.h" +#include "fakeWorldData.h" + +#include "sensorCar2X.h" +#include "common/sensorDataSignal.h" +#include "core/opSimulation/modules/World_OSI/RadioImplementation.h" + +using ::testing::_; +using ::testing::Return; +using ::testing::DoubleEq; +using ::testing::ReturnRef; +using ::testing::DoubleNear; +using ::testing::Expectation; +using ::testing::NiceMock; + +TEST(Radio_UnitTests, NoSendersRegistered_DetectsNothing) +{ + RadioImplementation radio{}; + std::vector detectedObjects = radio.Receive(0.0, 0.0, 1e-3); + + ASSERT_TRUE(detectedObjects.empty()); +} + +TEST(Radio_UnitTests, TwoSendersInProximityRegistered_DetectsTwoObjects) +{ + osi3::MovingObject remoteObject; + RadioImplementation radio{}; + + radio.Send(20.0, 0.0, 100.0, remoteObject); + radio.Send(100.0, 100.0, 100.0, remoteObject); + std::vector detectedObjects = radio.Receive(0.0, 0.0, 1e-7); + + ASSERT_EQ(detectedObjects.size(), 2); +} + +TEST(Radio_UnitTests, ThreeSendersRegisteredOneTooFarAway_DetectsTwoObjects) +{ + osi3::MovingObject remoteObject; + RadioImplementation radio{}; + + radio.Send(20.0, 20.0, 100.0, remoteObject); + radio.Send(100.0, 100.0, 100.0, remoteObject); + radio.Send(1000.0, 1000.0, 1.0, remoteObject); + std::vector detectedObjects = radio.Receive(0.0, 0.0, 1e-6); + + ASSERT_EQ(detectedObjects.size(), 2); +} + +TEST(Radio_UnitTests, TwoSendersRegisteredAtSensivityThreshold_DetectsTwoObjects) +{ + osi3::MovingObject remoteObject; + RadioImplementation radio{}; + + radio.Send(0.0, 0.0, 5.0, remoteObject); + radio.Send(1000.0, 0.0, 5.0, remoteObject); + std::vector detectedObjects = radio.Receive(500.0, 0.0, 1.5e-6); + + ASSERT_EQ(detectedObjects.size(), 2); +} + +TEST(Radio_UnitTests, OneSensorAtSensitivityThresholdOneOutsideThreshold_DetectsOneObject) +{ + osi3::MovingObject remoteObject; + RadioImplementation radio{}; + + radio.Send(0.0, 0.0, 5.0, remoteObject); + radio.Send(1050.0, 0.0, 5.0, remoteObject); + std::vector detectedObjects = radio.Receive(500.0, 0, 1.5e-6); + + ASSERT_EQ(detectedObjects.size(), 1); +} + +TEST(Radio_UnitTests, OneMountedSensor_DetectsTwoObjects) +{ + NiceMock fakeWorldInterface; + + NiceMock fakeStochastics; + ON_CALL(fakeStochastics, GetUniformDistributed(_, _)).WillByDefault(Return(1)); + ON_CALL(fakeStochastics, GetLogNormalDistributed(_, _)).WillByDefault(Return(1)); + + NiceMock fakeParameters; + NiceMock fakePublisher; + + std::map fakeDoubles = {{"FailureProbability", 0}, {"Latency", 0}, {"Sensitivity", 1e-5}, + {"Longitudinal", 1.0}, {"Lateral", 1.0}, {"Height", 0.0}, + {"Pitch", 0.0}, {"Yaw", 0.0}, {"Roll", 0.0} + }; + ON_CALL(fakeParameters, GetParametersDouble()).WillByDefault(ReturnRef(fakeDoubles)); + + std::map fakeInts = {{"Id", 0}}; + ON_CALL(fakeParameters, GetParametersInt()).WillByDefault(ReturnRef(fakeInts)); + + NiceMock fakeAgentInterface; + ON_CALL(fakeAgentInterface, GetPositionX()).WillByDefault(Return(100)); + ON_CALL(fakeAgentInterface, GetPositionY()).WillByDefault(Return(100)); + ON_CALL(fakeAgentInterface, GetYaw()).WillByDefault(Return(0)); + + std::vector fakeObjects; + fakeObjects.push_back(&fakeAgentInterface); + ON_CALL(fakeWorldInterface, GetWorldObjects()).WillByDefault(ReturnRef(fakeObjects)); + + osi3::MovingObject movingObject1; + osi3::MovingObject movingObject2; + + //Manipulate Radio + std::vector car2XObjects = {movingObject1, movingObject2}; + NiceMock fakeRadio; + EXPECT_CALL(fakeRadio, Receive(DoubleEq(101), DoubleEq(101), DoubleEq(1e-5))).WillOnce(Return(car2XObjects)); + ON_CALL(fakeWorldInterface, GetRadio()).WillByDefault(ReturnRef(fakeRadio)); + + SensorCar2X sensor( + "", + false, + 0, + 0, + 0, + 0, + &fakeStochastics, + &fakeWorldInterface, + &fakeParameters, + &fakePublisher, + nullptr, + &fakeAgentInterface); + + auto sensorData = sensor.DetectObjects(); + ASSERT_EQ(sensorData.moving_object_size(), 2); +} \ No newline at end of file diff --git a/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_Tests.cpp b/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_Tests.cpp index c507a984..9dd5f116 100644 --- a/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_Tests.cpp +++ b/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_Tests.cpp @@ -17,6 +17,7 @@ #include "fakeAgent.h" #include "fakePublisher.h" #include "fakeParameter.h" +#include "fakeRadio.h" #include "fakeStochastics.h" #include "fakeWorld.h" #include "fakeWorldData.h" diff --git a/sim/tests/unitTests/core/opSimulation/CMakeLists.txt b/sim/tests/unitTests/core/opSimulation/CMakeLists.txt index c09516d6..c543141e 100644 --- a/sim/tests/unitTests/core/opSimulation/CMakeLists.txt +++ b/sim/tests/unitTests/core/opSimulation/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/core/opSimulation) add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT core DEFAULT_MAIN + LINKOSI SOURCES ${OPENPASS_SIMCORE_DIR}/core/common/log.cpp diff --git a/sim/tests/unitTests/core/opSimulation/Scheduler/CMakeLists.txt b/sim/tests/unitTests/core/opSimulation/Scheduler/CMakeLists.txt index d75eda7c..4a6c1448 100644 --- a/sim/tests/unitTests/core/opSimulation/Scheduler/CMakeLists.txt +++ b/sim/tests/unitTests/core/opSimulation/Scheduler/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/core/opSimulation/framework/sch add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT core DEFAULT_MAIN + LINKOSI SOURCES ${COMPONENT_SOURCE_DIR}/agentParser.cpp diff --git a/sim/tests/unitTests/core/opSimulation/Scheduler/agentParser_Tests.cpp b/sim/tests/unitTests/core/opSimulation/Scheduler/agentParser_Tests.cpp index 04ad7d8a..f58cc62f 100644 --- a/sim/tests/unitTests/core/opSimulation/Scheduler/agentParser_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/Scheduler/agentParser_Tests.cpp @@ -17,6 +17,7 @@ #include "fakeAgent.h" #include "fakeAgentBlueprint.h" #include "fakeComponent.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "gmock/gmock.h" #include "gtest/gtest.h" diff --git a/sim/tests/unitTests/core/opSimulation/Scheduler/scheduler_Tests.cpp b/sim/tests/unitTests/core/opSimulation/Scheduler/scheduler_Tests.cpp index 8f844209..95995a59 100644 --- a/sim/tests/unitTests/core/opSimulation/Scheduler/scheduler_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/Scheduler/scheduler_Tests.cpp @@ -19,6 +19,7 @@ #include "fakeEventNetwork.h" #include "fakeManipulatorNetwork.h" #include "fakeObservationNetwork.h" +#include "fakeRadio.h" #include "fakeSpawnPointNetwork.h" #include "fakeWorld.h" #include "gmock/gmock.h" diff --git a/sim/tests/unitTests/core/opSimulation/Scheduler/taskBuilder_Tests.cpp b/sim/tests/unitTests/core/opSimulation/Scheduler/taskBuilder_Tests.cpp index 5456f2d1..d42153d4 100644 --- a/sim/tests/unitTests/core/opSimulation/Scheduler/taskBuilder_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/Scheduler/taskBuilder_Tests.cpp @@ -14,6 +14,7 @@ #include "fakeDataBuffer.h" #include "fakeEventDetectorNetwork.h" #include "fakeManipulatorNetwork.h" +#include "fakeRadio.h" #include "fakeSpawnPointNetwork.h" #include "fakeWorld.h" #include "gmock/gmock.h" diff --git a/sim/tests/unitTests/core/opSimulation/agentDataPublisher_Tests.cpp b/sim/tests/unitTests/core/opSimulation/agentDataPublisher_Tests.cpp index c1b46871..ad453dfe 100644 --- a/sim/tests/unitTests/core/opSimulation/agentDataPublisher_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/agentDataPublisher_Tests.cpp @@ -14,6 +14,7 @@ #include "fakeDataBuffer.h" #include "fakeEventNetwork.h" #include "fakeParameter.h" +#include "fakeRadio.h" #include "fakeStochastics.h" #include "fakeWorld.h" #include "gmock/gmock.h" diff --git a/sim/tests/unitTests/core/opSimulation/modules/EventDetector/CMakeLists.txt b/sim/tests/unitTests/core/opSimulation/modules/EventDetector/CMakeLists.txt index 051051b6..a2e0bc71 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/EventDetector/CMakeLists.txt +++ b/sim/tests/unitTests/core/opSimulation/modules/EventDetector/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/Event add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT core DEFAULT_MAIN + LINKOSI SOURCES ConditionalEventDetector_Tests.cpp diff --git a/sim/tests/unitTests/core/opSimulation/modules/EventDetector/ConditionalEventDetector_Tests.cpp b/sim/tests/unitTests/core/opSimulation/modules/EventDetector/ConditionalEventDetector_Tests.cpp index 432e4838..b196ec05 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/EventDetector/ConditionalEventDetector_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/EventDetector/ConditionalEventDetector_Tests.cpp @@ -18,6 +18,7 @@ #include "fakeEgoAgent.h" #include "fakeEventNetwork.h" #include "fakeParameter.h" +#include "fakeRadio.h" #include "fakeStochastics.h" #include "fakeWorld.h" diff --git a/sim/tests/unitTests/core/opSimulation/modules/EventDetector/DetectCollisionTests.cpp b/sim/tests/unitTests/core/opSimulation/modules/EventDetector/DetectCollisionTests.cpp index 78f3bb1c..b1d20fb8 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/EventDetector/DetectCollisionTests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/EventDetector/DetectCollisionTests.cpp @@ -13,6 +13,7 @@ #include "fakeAgent.h" #include "fakeTrafficObject.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "fakeEventNetwork.h" diff --git a/sim/tests/unitTests/core/opSimulation/modules/Observation_Log/CMakeLists.txt b/sim/tests/unitTests/core/opSimulation/modules/Observation_Log/CMakeLists.txt index d7c95572..514ec4dc 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/Observation_Log/CMakeLists.txt +++ b/sim/tests/unitTests/core/opSimulation/modules/Observation_Log/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/Obser add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT core DEFAULT_MAIN + LINKOSI SOURCES observationLog_Tests.cpp diff --git a/sim/tests/unitTests/core/opSimulation/modules/Observation_Log/observationLog_Tests.cpp b/sim/tests/unitTests/core/opSimulation/modules/Observation_Log/observationLog_Tests.cpp index 61d3da96..b856a027 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/Observation_Log/observationLog_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/Observation_Log/observationLog_Tests.cpp @@ -20,6 +20,7 @@ #include "fakeAgent.h" #include "fakeDataBuffer.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "fakeRunResult.h" diff --git a/sim/tests/unitTests/core/opSimulation/modules/SpawnerPreRunCommon/CMakeLists.txt b/sim/tests/unitTests/core/opSimulation/modules/SpawnerPreRunCommon/CMakeLists.txt index 5e7f364a..df6bab20 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/SpawnerPreRunCommon/CMakeLists.txt +++ b/sim/tests/unitTests/core/opSimulation/modules/SpawnerPreRunCommon/CMakeLists.txt @@ -14,6 +14,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/Spawn add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT core DEFAULT_MAIN + LINKOSI SOURCES spawnerPreRunCommon_Tests.cpp diff --git a/sim/tests/unitTests/core/opSimulation/modules/SpawnerPreRunCommon/spawnerPreRunCommon_Tests.cpp b/sim/tests/unitTests/core/opSimulation/modules/SpawnerPreRunCommon/spawnerPreRunCommon_Tests.cpp index c836c589..97d4e374 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/SpawnerPreRunCommon/spawnerPreRunCommon_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/SpawnerPreRunCommon/spawnerPreRunCommon_Tests.cpp @@ -13,6 +13,7 @@ #include "common/WorldAnalyzer.h" #include "fakeCallback.h" #include "fakeParameter.h" +#include "fakeRadio.h" #include "fakeStream.h" #include "fakeWorld.h" #include "gmock/gmock.h" diff --git a/sim/tests/unitTests/core/opSimulation/modules/SpawnerRuntimeCommon/CMakeLists.txt b/sim/tests/unitTests/core/opSimulation/modules/SpawnerRuntimeCommon/CMakeLists.txt index 929b10b9..72d6727f 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/SpawnerRuntimeCommon/CMakeLists.txt +++ b/sim/tests/unitTests/core/opSimulation/modules/SpawnerRuntimeCommon/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/Spawn add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT core DEFAULT_MAIN + LINKOSI SOURCES spawnerRuntimeCommon_Tests.cpp diff --git a/sim/tests/unitTests/core/opSimulation/modules/SpawnerRuntimeCommon/spawnerRuntimeCommon_Tests.cpp b/sim/tests/unitTests/core/opSimulation/modules/SpawnerRuntimeCommon/spawnerRuntimeCommon_Tests.cpp index 477f3c19..5e435dab 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/SpawnerRuntimeCommon/spawnerRuntimeCommon_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/SpawnerRuntimeCommon/spawnerRuntimeCommon_Tests.cpp @@ -18,6 +18,7 @@ #include "common/WorldAnalyzer.h" #include "fakeParameter.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "fakeCallback.h" diff --git a/sim/tests/unitTests/core/opSimulation/modules/SpawnerScenario/CMakeLists.txt b/sim/tests/unitTests/core/opSimulation/modules/SpawnerScenario/CMakeLists.txt index 30615404..a97e9f92 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/SpawnerScenario/CMakeLists.txt +++ b/sim/tests/unitTests/core/opSimulation/modules/SpawnerScenario/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/Spawn add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT core DEFAULT_MAIN + LINKOSI SOURCES spawnerScenario_Tests.cpp diff --git a/sim/tests/unitTests/core/opSimulation/modules/SpawnerScenario/spawnerScenario_Tests.cpp b/sim/tests/unitTests/core/opSimulation/modules/SpawnerScenario/spawnerScenario_Tests.cpp index e98a721c..334b86a9 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/SpawnerScenario/spawnerScenario_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/SpawnerScenario/spawnerScenario_Tests.cpp @@ -15,6 +15,7 @@ #include "fakeAgent.h" #include "fakeAgentBlueprintProvider.h" #include "fakeAgentFactory.h" +#include "fakeRadio.h" #include "fakeStochastics.h" #include "fakeScenario.h" #include "fakeWorld.h" diff --git a/sim/tests/unitTests/core/opSimulation/modules/SpawnerWorldAnalyzer/CMakeLists.txt b/sim/tests/unitTests/core/opSimulation/modules/SpawnerWorldAnalyzer/CMakeLists.txt index afa6de3d..c7ce3fe2 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/SpawnerWorldAnalyzer/CMakeLists.txt +++ b/sim/tests/unitTests/core/opSimulation/modules/SpawnerWorldAnalyzer/CMakeLists.txt @@ -13,6 +13,7 @@ set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/core/opSimulation/modules/Spawn add_openpass_target( NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT core DEFAULT_MAIN + LINKOSI SOURCES spawnerWorldAnalyzer_Tests.cpp diff --git a/sim/tests/unitTests/core/opSimulation/modules/SpawnerWorldAnalyzer/spawnerWorldAnalyzer_Tests.cpp b/sim/tests/unitTests/core/opSimulation/modules/SpawnerWorldAnalyzer/spawnerWorldAnalyzer_Tests.cpp index 1b53291b..684d4ed5 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/SpawnerWorldAnalyzer/spawnerWorldAnalyzer_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/SpawnerWorldAnalyzer/spawnerWorldAnalyzer_Tests.cpp @@ -14,6 +14,7 @@ #include "common/WorldAnalyzer.h" #include "fakeAgent.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "fakeStream.h" #include "dontCare.h" diff --git a/sim/tests/unitTests/core/opSimulation/modules/World_OSI/agentAdapter_Tests.cpp b/sim/tests/unitTests/core/opSimulation/modules/World_OSI/agentAdapter_Tests.cpp index 37183e0a..7ce8767b 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/World_OSI/agentAdapter_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/World_OSI/agentAdapter_Tests.cpp @@ -13,6 +13,7 @@ #include "AgentAdapter.h" #include "fakeMovingObject.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "fakeWorldData.h" #include "fakeCallback.h" diff --git a/sim/tests/unitTests/core/opSimulation/modules/World_OSI/egoAgent_Tests.cpp b/sim/tests/unitTests/core/opSimulation/modules/World_OSI/egoAgent_Tests.cpp index c856966d..21078100 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/World_OSI/egoAgent_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/World_OSI/egoAgent_Tests.cpp @@ -13,6 +13,7 @@ #include "egoAgent.h" #include "fakeAgent.h" +#include "fakeRadio.h" #include "fakeWorld.h" using ::testing::NiceMock; diff --git a/sim/tests/unitTests/core/opSimulation/modules/World_OSI/lane_Tests.cpp b/sim/tests/unitTests/core/opSimulation/modules/World_OSI/lane_Tests.cpp index 96269443..60648c3f 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/World_OSI/lane_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/World_OSI/lane_Tests.cpp @@ -10,6 +10,7 @@ #include "gtest/gtest.h" #include "gmock/gmock.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "fakeSection.h" #include "fakeLane.h" diff --git a/sim/tests/unitTests/core/opSimulation/modules/World_OSI/sceneryConverter_Tests.cpp b/sim/tests/unitTests/core/opSimulation/modules/World_OSI/sceneryConverter_Tests.cpp index f6b95339..779bda6d 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/World_OSI/sceneryConverter_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/World_OSI/sceneryConverter_Tests.cpp @@ -19,6 +19,7 @@ #include "fakeLaneManager.h" #include "fakeMovingObject.h" #include "fakeOdRoad.h" +#include "fakeRadio.h" #include "fakeRoadObject.h" #include "fakeRoadLaneSection.h" #include "fakeSection.h" diff --git a/sim/tests/unitTests/core/opSimulation/modules/World_OSI/worldQuery_Tests.cpp b/sim/tests/unitTests/core/opSimulation/modules/World_OSI/worldQuery_Tests.cpp index 40820e62..b9290bf4 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/World_OSI/worldQuery_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/World_OSI/worldQuery_Tests.cpp @@ -15,6 +15,7 @@ #include "WorldDataQuery.h" #include "common/globalDefinitions.h" +#include "fakeRadio.h" #include "fakeWorld.h" #include "fakeSection.h" #include "fakeLane.h" -- GitLab From 33e70ff18b4ad77f702363edd4a3e9ceec1018ee Mon Sep 17 00:00:00 2001 From: Reinhard Biegel Date: Wed, 7 Sep 2022 13:30:52 +0200 Subject: [PATCH 2/2] EventDetector_Tests: Add missing include statement for FakeRadio --- .../modules/EventDetector/collisionDetector_Tests.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/sim/tests/unitTests/core/opSimulation/modules/EventDetector/collisionDetector_Tests.cpp b/sim/tests/unitTests/core/opSimulation/modules/EventDetector/collisionDetector_Tests.cpp index 2100451e..9bbcf244 100644 --- a/sim/tests/unitTests/core/opSimulation/modules/EventDetector/collisionDetector_Tests.cpp +++ b/sim/tests/unitTests/core/opSimulation/modules/EventDetector/collisionDetector_Tests.cpp @@ -15,6 +15,7 @@ #include "fakeTrafficObject.h" #include "fakeCallback.h" #include "fakeEventNetwork.h" +#include "fakeRadio.h" #include "fakeStochastics.h" #include "fakeWorld.h" -- GitLab