diff --git a/sim/contrib/examples/AEB/configs/systemConfigBlueprint.xml b/sim/contrib/examples/AEB/configs/systemConfigBlueprint.xml
index fcbdac7cbb132b1198300bf2b0550817d006a47a..0c84330e673792ef7e99e057dc327551a3353778 100755
--- a/sim/contrib/examples/AEB/configs/systemConfigBlueprint.xml
+++ b/sim/contrib/examples/AEB/configs/systemConfigBlueprint.xml
@@ -37,14 +37,25 @@
<parameters/>
</component>
<component>
- <id>SensorFusion</id>
+ <id>SensorAggregation</id>
+ <schedule>
+ <priority>351</priority>
+ <offset>0</offset>
+ <cycle>100</cycle>
+ <response>0</response>
+ </schedule>
+ <library>SensorAggregation_OSI</library>
+ <parameters/>
+ </component>
+ <component>
+ <id>SensorFusionErrorless</id>
<schedule>
<priority>350</priority>
<offset>0</offset>
<cycle>100</cycle>
<response>0</response>
</schedule>
- <library>SensorFusion_OSI</library>
+ <library>SensorFusionErrorless_OSI</library>
<parameters/>
</component>
<component>
@@ -550,17 +561,6 @@
<input>2</input>
</target>
</connection>
- <connection>
- <id>9190</id>
- <source>
- <component>SensorFusion</component>
- <output>0</output>
- </source>
- <target>
- <component>AEB</component>
- <input>0</input>
- </target>
- </connection>
<connection>
<id>9292</id>
<source>
@@ -588,6 +588,28 @@
<input>100</input>
</target>
</connection>
+ <connection>
+ <id>9390</id>
+ <source>
+ <component>SensorAggregation</component>
+ <output>0</output>
+ </source>
+ <target>
+ <component>SensorFusionErrorless</component>
+ <input>0</input>
+ </target>
+ </connection>
+ <connection>
+ <id>9490</id>
+ <source>
+ <component>SensorFusionErrorless</component>
+ <output>0</output>
+ </source>
+ <target>
+ <component>AEB</component>
+ <input>0</input>
+ </target>
+ </connection>
<connection>
<id>9581</id>
<source>
@@ -618,7 +640,7 @@
<output>0</output>
</source>
<target>
- <component>SensorFusion</component>
+ <component>SensorAggregation</component>
<input>0</input>
</target>
</connection>
diff --git a/sim/contrib/examples/AEB/masterConfig.xml b/sim/contrib/examples/AEB/masterConfig.xml
index 9257e8b8598137c434b620a1d9fbb52543fcd69b..7cc7bd575eb4c6bb04762fd8a3e0e3fc9421ffec 100644
--- a/sim/contrib/examples/AEB/masterConfig.xml
+++ b/sim/contrib/examples/AEB/masterConfig.xml
@@ -1,7 +1,7 @@
<masterConfig>
<logLevel>0</logLevel>
<logFileMaster>OpenPassMaster.log</logFileMaster>
- <slave>OpenPassSlave</slave>
+ <slave>OpenPassSlave.exe</slave>
<libraries>lib</libraries>
<slaveConfigs>
<slaveConfig>
diff --git a/sim/contrib/examples/Static AgentProfiles/configs/systemConfigBlueprint.xml b/sim/contrib/examples/Static AgentProfiles/configs/systemConfigBlueprint.xml
index fcbdac7cbb132b1198300bf2b0550817d006a47a..0c84330e673792ef7e99e057dc327551a3353778 100755
--- a/sim/contrib/examples/Static AgentProfiles/configs/systemConfigBlueprint.xml
+++ b/sim/contrib/examples/Static AgentProfiles/configs/systemConfigBlueprint.xml
@@ -37,14 +37,25 @@
<parameters/>
</component>
<component>
- <id>SensorFusion</id>
+ <id>SensorAggregation</id>
+ <schedule>
+ <priority>351</priority>
+ <offset>0</offset>
+ <cycle>100</cycle>
+ <response>0</response>
+ </schedule>
+ <library>SensorAggregation_OSI</library>
+ <parameters/>
+ </component>
+ <component>
+ <id>SensorFusionErrorless</id>
<schedule>
<priority>350</priority>
<offset>0</offset>
<cycle>100</cycle>
<response>0</response>
</schedule>
- <library>SensorFusion_OSI</library>
+ <library>SensorFusionErrorless_OSI</library>
<parameters/>
</component>
<component>
@@ -550,17 +561,6 @@
<input>2</input>
</target>
</connection>
- <connection>
- <id>9190</id>
- <source>
- <component>SensorFusion</component>
- <output>0</output>
- </source>
- <target>
- <component>AEB</component>
- <input>0</input>
- </target>
- </connection>
<connection>
<id>9292</id>
<source>
@@ -588,6 +588,28 @@
<input>100</input>
</target>
</connection>
+ <connection>
+ <id>9390</id>
+ <source>
+ <component>SensorAggregation</component>
+ <output>0</output>
+ </source>
+ <target>
+ <component>SensorFusionErrorless</component>
+ <input>0</input>
+ </target>
+ </connection>
+ <connection>
+ <id>9490</id>
+ <source>
+ <component>SensorFusionErrorless</component>
+ <output>0</output>
+ </source>
+ <target>
+ <component>AEB</component>
+ <input>0</input>
+ </target>
+ </connection>
<connection>
<id>9581</id>
<source>
@@ -618,7 +640,7 @@
<output>0</output>
</source>
<target>
- <component>SensorFusion</component>
+ <component>SensorAggregation</component>
<input>0</input>
</target>
</connection>
diff --git a/sim/contrib/examples/Static AgentProfiles/masterConfig.xml b/sim/contrib/examples/Static AgentProfiles/masterConfig.xml
index 9257e8b8598137c434b620a1d9fbb52543fcd69b..7cc7bd575eb4c6bb04762fd8a3e0e3fc9421ffec 100644
--- a/sim/contrib/examples/Static AgentProfiles/masterConfig.xml
+++ b/sim/contrib/examples/Static AgentProfiles/masterConfig.xml
@@ -1,7 +1,7 @@
<masterConfig>
<logLevel>0</logLevel>
<logFileMaster>OpenPassMaster.log</logFileMaster>
- <slave>OpenPassSlave</slave>
+ <slave>OpenPassSlave.exe</slave>
<libraries>lib</libraries>
<slaveConfigs>
<slaveConfig>
diff --git a/sim/contrib/examples/StaticAgentCollision/configs/systemConfigBlueprint.xml b/sim/contrib/examples/StaticAgentCollision/configs/systemConfigBlueprint.xml
index 395ddccfeeaf475850b685224c0b0bb3b2046636..0c84330e673792ef7e99e057dc327551a3353778 100644
--- a/sim/contrib/examples/StaticAgentCollision/configs/systemConfigBlueprint.xml
+++ b/sim/contrib/examples/StaticAgentCollision/configs/systemConfigBlueprint.xml
@@ -37,14 +37,25 @@
<parameters/>
</component>
<component>
- <id>SensorFusion</id>
+ <id>SensorAggregation</id>
+ <schedule>
+ <priority>351</priority>
+ <offset>0</offset>
+ <cycle>100</cycle>
+ <response>0</response>
+ </schedule>
+ <library>SensorAggregation_OSI</library>
+ <parameters/>
+ </component>
+ <component>
+ <id>SensorFusionErrorless</id>
<schedule>
<priority>350</priority>
<offset>0</offset>
<cycle>100</cycle>
<response>0</response>
</schedule>
- <library>SensorFusion_OSI</library>
+ <library>SensorFusionErrorless_OSI</library>
<parameters/>
</component>
<component>
@@ -110,7 +121,7 @@
</parameters>
</component>
<component>
- <id>PrioritizerLateralVehicleComponents</id>
+ <id>PrioritizerSteeringVehicleComponents</id>
<schedule>
<priority>150</priority>
<offset>0</offset>
@@ -154,17 +165,6 @@
<library>Algorithm_Longitudinal</library>
<parameters/>
</component>
- <component>
- <id>Algorithm_LateralVehicleComponents</id>
- <schedule>
- <priority>100</priority>
- <offset>0</offset>
- <cycle>100</cycle>
- <response>0</response>
- </schedule>
- <library>Algorithm_Lateral</library>
- <parameters/>
- </component>
<component>
<id>Algorithm_LateralAfdm</id>
<schedule>
@@ -417,17 +417,6 @@
<input>1</input>
</target>
</connection>
- <connection>
- <id>1412</id>
- <source>
- <component>Algorithm_LateralVehicleComponents</component>
- <output>0</output>
- </source>
- <target>
- <component>PrioritizerSteering</component>
- <input>0</input>
- </target>
- </connection>
<connection>
<id>1512</id>
<source>
@@ -494,11 +483,11 @@
<connection>
<id>2414</id>
<source>
- <component>PrioritizerLateralVehicleComponents</component>
+ <component>PrioritizerSteeringVehicleComponents</component>
<output>0</output>
</source>
<target>
- <component>Algorithm_LateralVehicleComponents</component>
+ <component>PrioritizerSteering</component>
<input>0</input>
</target>
<target>
@@ -572,17 +561,6 @@
<input>2</input>
</target>
</connection>
- <connection>
- <id>9190</id>
- <source>
- <component>SensorFusion</component>
- <output>0</output>
- </source>
- <target>
- <component>AEB</component>
- <input>0</input>
- </target>
- </connection>
<connection>
<id>9292</id>
<source>
@@ -606,12 +584,30 @@
<input>100</input>
</target>
<target>
- <component>Algorithm_LateralVehicleComponents</component>
+ <component>Dynamics_RegularDriving</component>
<input>100</input>
</target>
+ </connection>
+ <connection>
+ <id>9390</id>
+ <source>
+ <component>SensorAggregation</component>
+ <output>0</output>
+ </source>
<target>
- <component>Dynamics_RegularDriving</component>
- <input>100</input>
+ <component>SensorFusionErrorless</component>
+ <input>0</input>
+ </target>
+ </connection>
+ <connection>
+ <id>9490</id>
+ <source>
+ <component>SensorFusionErrorless</component>
+ <output>0</output>
+ </source>
+ <target>
+ <component>AEB</component>
+ <input>0</input>
</target>
</connection>
<connection>
@@ -636,10 +632,6 @@
<component>Algorithm_LateralAfdm</component>
<input>101</input>
</target>
- <target>
- <component>Algorithm_LateralVehicleComponents</component>
- <input>101</input>
- </target>
</connection>
<connection>
<id>9900</id>
@@ -648,7 +640,7 @@
<output>0</output>
</source>
<target>
- <component>SensorFusion</component>
+ <component>SensorAggregation</component>
<input>0</input>
</target>
</connection>
diff --git a/sim/contrib/examples/StaticAgentCollision/masterConfig.xml b/sim/contrib/examples/StaticAgentCollision/masterConfig.xml
index 9257e8b8598137c434b620a1d9fbb52543fcd69b..7cc7bd575eb4c6bb04762fd8a3e0e3fc9421ffec 100644
--- a/sim/contrib/examples/StaticAgentCollision/masterConfig.xml
+++ b/sim/contrib/examples/StaticAgentCollision/masterConfig.xml
@@ -1,7 +1,7 @@
<masterConfig>
<logLevel>0</logLevel>
<logFileMaster>OpenPassMaster.log</logFileMaster>
- <slave>OpenPassSlave</slave>
+ <slave>OpenPassSlave.exe</slave>
<libraries>lib</libraries>
<slaveConfigs>
<slaveConfig>
diff --git a/sim/doc/DoxyGen/Function/ExampleFiles/systemConfigBlueprint.xml b/sim/doc/DoxyGen/Function/ExampleFiles/systemConfigBlueprint.xml
index d1e45a7c7fc43c91d13de2663a3029659eaeefb1..f80f9981d4b005598fb04314922900521d46118d 100644
--- a/sim/doc/DoxyGen/Function/ExampleFiles/systemConfigBlueprint.xml
+++ b/sim/doc/DoxyGen/Function/ExampleFiles/systemConfigBlueprint.xml
@@ -17,7 +17,7 @@
<component>
<id>Sensor_Driver</id>
<schedule>
- <priority>490</priority>
+ <priority>390</priority>
<offset>0</offset>
<cycle>100</cycle>
<response>0</response>
@@ -37,14 +37,25 @@
<parameters/>
</component>
<component>
- <id>SensorFusion</id>
+ <id>SensorAggregation</id>
+ <schedule>
+ <priority>351</priority>
+ <offset>0</offset>
+ <cycle>100</cycle>
+ <response>0</response>
+ </schedule>
+ <library>SensorAggregation_OSI</library>
+ <parameters/>
+ </component>
+ <component>
+ <id>SensorFusionErrorless</id>
<schedule>
<priority>350</priority>
<offset>0</offset>
<cycle>100</cycle>
<response>0</response>
</schedule>
- <library>SensorFusion_OSI</library>
+ <library>SensorFusionErrorless_OSI</library>
<parameters/>
</component>
<component>
@@ -61,7 +72,7 @@
<component>
<id>ComponentController</id>
<schedule>
- <priority>1</priority>
+ <priority>200</priority>
<offset>0</offset>
<cycle>100</cycle>
<response>0</response>
@@ -69,6 +80,17 @@
<library>ComponentController</library>
<parameters/>
</component>
+ <component>
+ <id>OpenScenarioActions</id>
+ <schedule>
+ <priority>400</priority>
+ <offset>0</offset>
+ <cycle>100</cycle>
+ <response>0</response>
+ </schedule>
+ <library>OpenScenarioActions</library>
+ <parameters/>
+ </component>
<component>
<id>AEB</id>
<schedule>
@@ -340,17 +362,6 @@
<input>2</input>
</target>
</connection>
- <connection>
- <id>283</id>
- <source>
- <component>Dynamics_TrajectoryFollower</component>
- <output>83</output>
- </source>
- <target>
- <component>ComponentController</component>
- <input>100</input>
- </target>
- </connection>
<connection>
<id>301</id>
<source>
@@ -540,14 +551,14 @@
</target>
</connection>
<connection>
- <id>9190</id>
+ <id>8471</id>
<source>
- <component>SensorFusion</component>
+ <component>OpenScenarioActions</component>
<output>0</output>
</source>
<target>
- <component>AEB</component>
- <input>0</input>
+ <component>Dynamics_TrajectoryFollower</component>
+ <input>2</input>
</target>
</connection>
<connection>
@@ -577,6 +588,28 @@
<input>100</input>
</target>
</connection>
+ <connection>
+ <id>9390</id>
+ <source>
+ <component>SensorAggregation</component>
+ <output>0</output>
+ </source>
+ <target>
+ <component>SensorFusionErrorless</component>
+ <input>0</input>
+ </target>
+ </connection>
+ <connection>
+ <id>9490</id>
+ <source>
+ <component>SensorFusionErrorless</component>
+ <output>0</output>
+ </source>
+ <target>
+ <component>AEB</component>
+ <input>0</input>
+ </target>
+ </connection>
<connection>
<id>9581</id>
<source>
@@ -607,7 +640,7 @@
<output>0</output>
</source>
<target>
- <component>SensorFusion</component>
+ <component>SensorAggregation</component>
<input>0</input>
</target>
</connection>
diff --git a/sim/doc/DoxyGen/Function/Markdown/Simulation/Input_Output/Input_Output.md b/sim/doc/DoxyGen/Function/Markdown/Simulation/Input_Output/Input_Output.md
index b89fc6cdd0a1f70d5fa396682f3d2429aa0f9551..48c173d82c39b4c65a00629df51b0c6c80aeb363 100644
--- a/sim/doc/DoxyGen/Function/Markdown/Simulation/Input_Output/Input_Output.md
+++ b/sim/doc/DoxyGen/Function/Markdown/Simulation/Input_Output/Input_Output.md
@@ -97,6 +97,9 @@ The table below can be used as orientation when a new module is introduced.
|---|-----------|-----------|-----------|-----------|
| ParametersAgentModules| ParametersAgent| 500 | Parameters | Sets all init-data and is updated cyclically |
| OpenScenarioActions | OpenScenarioActions | 400 | ADAS | Reads events from OpenScenario Actions and forwards them to other components |
+| SensorObjectDetector | Sensor_OSI | 398 | Sensor | Gets instantiated multiple time (one time per sensor) |
+| SensorAggregation | SensorAggregation_OSI | 351 | Sensor | - |
+| SensorFusionErrorless | SensorFusionErrorless_OSI | 350 | Sensor | - |
| AlgorithmAgentFollowingDriverModel | AlgorithmAgentFollowingDriverModel | 310 | DriverModels | - |
| AEB | AlgorithmAutonomousEmergencyBraking | 250 | ADAS | - |
| ComponentController | ComponentController | 200 | ADAS | Manages vehicle component states with regard to other vehicle component states and conditions and in response to events. |
@@ -179,6 +182,8 @@ With corresponding defined indices :
| ComponentController | Special | 83 |
| OpenScenarioActions | Special | 84 |
| Parameter_Vehicle | Sensor | 92 |
+| SensorAggregation | Sensor | 93 |
+| SensorFusion | Sensor | 94 |
| Sensor_Driver | Sensor | 95 |
**Ids for Signals (last two digits)**
@@ -206,6 +211,7 @@ With corresponding defined indices :
| SecondaryDriverTasks | Algorithm | 19 |
| Trajectory | OpenScenarioActions | 71 |
| SensorDriver | Sensor | 81 |
+| SensorData | Sensor | 90 |
| ParametersVehicle | Parameters | 92 |
\subsubsection io_input_systemconfigblueprint_paramters Parameters
diff --git a/sim/doc/DoxyGen/Function/Markdown/Simulation/Input_Output/Scenario.md b/sim/doc/DoxyGen/Function/Markdown/Simulation/Input_Output/Scenario.md
index d7b4cb4bbd7b35e63894027705f8b3b4981d946b..a02c33261ee10966f52edc3f9c5d16cf14495e12 100644
--- a/sim/doc/DoxyGen/Function/Markdown/Simulation/Input_Output/Scenario.md
+++ b/sim/doc/DoxyGen/Function/Markdown/Simulation/Input_Output/Scenario.md
@@ -188,7 +188,7 @@ Although OpenSCENARIO also states other ways for defining a position, we current
Unlike OpenSCENARIO we also allow some of these values to be stochastic.
This is marked by adding a subtag <Stochastics value="valuetype" stdDeviation="value" lowerBound="value" upperBound="value"/> to the <LanePosition> or <SpeedAction> tag.
The stochastics tag is intended to be used as NormalDistribution, but it is up to each module using it to define the actual usage.
-The valuetype can either be s, offset, velocity or rate.
+The valuetype can either be s, offset (if inside LanePosition), velocity or rate (if inside SpeedAction).
The value defined as attribute of the LanePosition/SpeedActionDynamics/AbsoluteTargetSpeed tag is then taken as mean value.
The VisibilityAction is optional.
If VisibilityAction is not defined the agent will be spawned.
@@ -215,6 +215,7 @@ Example
<SpeedActionTarget>
<AbsoluteTargetSpeed value="10.0" />
</SpeedActionTarget>
+ <Stochastics value="velocity" stdDeviation="2.0" lowerBound="5.0" upperBound="15.0"/>
</SpeedAction>
</LongitudinalAction>
</PrivateAction>
diff --git a/sim/doc/OSI World Setup Guide.pdf b/sim/doc/OSI World Setup Guide.pdf
index d5828165a7ad7e3fae64655ea91bb12bd9e25cfa..5d0fcf3b9143016e5b206566571c858c184da6fd 100644
Binary files a/sim/doc/OSI World Setup Guide.pdf and b/sim/doc/OSI World Setup Guide.pdf differ
diff --git a/sim/src/common/commonTools.h b/sim/src/common/commonTools.h
index c9ae21d843d5099dfb0df91b96a0b8a41ba948d4..0b3c00ab93f05519b35d3f06893b4e7aa273345c 100644
--- a/sim/src/common/commonTools.h
+++ b/sim/src/common/commonTools.h
@@ -117,6 +117,56 @@ static double CalculateMomentInertiaYaw(double mass, double length, double width
return std::make_optional<Common::Vector2d>(intersectionPointX, intersectionPointY);
}
+//! Calculates the net distance of the x and y coordinates of two bounding boxes
+//!
+//! \param ownBoundingBox first bounding box
+//! \param otherBoundingBox second bounding box
+//! \return net distance x, net distance y
+[[maybe_unused]] static std::pair<double, double> GetCartesianNetDistance(polygon_t ownBoundingBox, polygon_t otherBoundingBox)
+{
+ double ownMaxX{std::numeric_limits<double>::lowest()};
+ double ownMinX{std::numeric_limits<double>::max()};
+ double ownMaxY{std::numeric_limits<double>::lowest()};
+ double ownMinY{std::numeric_limits<double>::max()};
+ for (const auto& point : ownBoundingBox.outer())
+ {
+ ownMaxX = std::max(ownMaxX, bg::get<0>(point));
+ ownMinX = std::min(ownMinX, bg::get<0>(point));
+ ownMaxY = std::max(ownMaxY, bg::get<1>(point));
+ ownMinY = std::min(ownMinY, bg::get<1>(point));
+ }
+ double otherMaxX{std::numeric_limits<double>::lowest()};
+ double otherMinX{std::numeric_limits<double>::max()};
+ double otherMaxY{std::numeric_limits<double>::lowest()};
+ double otherMinY{std::numeric_limits<double>::max()};
+ for (const auto& point : otherBoundingBox.outer())
+ {
+ otherMaxX = std::max(otherMaxX, bg::get<0>(point));
+ otherMinX = std::min(otherMinX, bg::get<0>(point));
+ otherMaxY = std::max(otherMaxY, bg::get<1>(point));
+ otherMinY = std::min(otherMinY, bg::get<1>(point));
+ }
+ double netX{0.0};
+ if (ownMaxX < otherMinX)
+ {
+ netX = otherMinX - ownMaxX;
+ }
+ if (ownMinX > otherMaxX)
+ {
+ netX = otherMaxX - ownMinX;
+ }
+ double netY{0.0};
+ if (ownMaxY < otherMinY)
+ {
+ netY = otherMinY - ownMaxY;
+ }
+ if (ownMinY > otherMaxY)
+ {
+ netY = otherMaxY - ownMinY;
+ }
+ return {netX, netY};
+}
+
//-----------------------------------------------------------------------------
//! @brief Tokenizes string by delimiter.
//!
diff --git a/sim/src/components/SensorFusion_OSI/src/sensorFusionQuery.h b/sim/src/common/sensorFusionQuery.h
similarity index 61%
rename from sim/src/components/SensorFusion_OSI/src/sensorFusionQuery.h
rename to sim/src/common/sensorFusionQuery.h
index d836532fd2f65fd65e0a0d18ff061a34f6700476..4ac4f2457313fec6d6aab7fc11e7daf053e025d6 100644
--- a/sim/src/components/SensorFusion_OSI/src/sensorFusionQuery.h
+++ b/sim/src/common/sensorFusionQuery.h
@@ -1,5 +1,5 @@
/*******************************************************************************
-* Copyright (c) 2018, 2019 in-tech GmbH
+* Copyright (c) 2018, 2019, 2020 in-tech GmbH
* Copyright (c) 2020 HLRS, University of Stuttgart.
*
* This program and the accompanying materials are made
@@ -22,33 +22,39 @@
#include "osi3/osi_sensordata.pb.h"
-class SensorFusionHelperFunctions
+namespace SensorFusionHelperFunctions
{
-public:
- static std::vector<osi3::DetectedMovingObject> RetrieveMovingObjectsBySensorId(std::vector<int> sensorIds, const osi3::SensorData &sensorData)
+ std::vector<osi3::DetectedMovingObject> RetrieveMovingObjectsBySensorId(std::vector<int> sensorIds, const osi3::SensorData &sensorData)
{
std::vector<osi3::DetectedMovingObject> result;
auto detectedMovingObjects = sensorData.moving_object();
for (const auto& object : detectedMovingObjects)
{
- if(std::count(sensorIds.cbegin(), sensorIds.cend(), object.header().sensor_id(0).value()) > 0)
+ for (auto sensorId : object.header().sensor_id())
{
- result.push_back(object);
+ if(std::count(sensorIds.cbegin(), sensorIds.cend(), sensorId.value()) > 0)
+ {
+ result.push_back(object);
+ break;
+ }
}
}
return result;
}
-
- static std::vector<osi3::DetectedStationaryObject> RetrieveStationaryObjectsBySensorId(std::vector<int> sensorIds, const osi3::SensorData &sensorData)
+ std::vector<osi3::DetectedStationaryObject> RetrieveStationaryObjectsBySensorId(std::vector<int> sensorIds, const osi3::SensorData &sensorData)
{
std::vector<osi3::DetectedStationaryObject> result;
auto detectedStationaryObjects = sensorData.stationary_object();
for (const auto& object : detectedStationaryObjects)
{
- if(std::count(sensorIds.cbegin(), sensorIds.cend(), object.header().sensor_id(0).value()) > 0)
+ for (auto sensorId : object.header().sensor_id())
{
- result.push_back(object);
+ if(std::count(sensorIds.cbegin(), sensorIds.cend(), sensorId.value()) > 0)
+ {
+ result.push_back(object);
+ break;
+ }
}
}
return result;
diff --git a/sim/src/components/Algorithm_AEB/src/autonomousEmergencyBraking.cpp b/sim/src/components/Algorithm_AEB/src/autonomousEmergencyBraking.cpp
index 450d70397445e89494dba7587b91d7e84e44ebf6..1d4a42053fd1a37bbfe9d820239e3447564dc655 100644
--- a/sim/src/components/Algorithm_AEB/src/autonomousEmergencyBraking.cpp
+++ b/sim/src/components/Algorithm_AEB/src/autonomousEmergencyBraking.cpp
@@ -19,7 +19,7 @@
#include "common/commonTools.h"
#include "common/eventTypes.h"
-#include "components/SensorFusion_OSI/src/sensorFusionQuery.h"
+#include "common/sensorFusionQuery.h"
#include "boundingBoxCalculation.h"
AlgorithmAutonomousEmergencyBrakingImplementation::AlgorithmAutonomousEmergencyBrakingImplementation(
diff --git a/sim/src/components/CMakeLists.txt b/sim/src/components/CMakeLists.txt
index b59e9c4c283d361d63d8e67cbb68c65d0bf4de28..93c655ccd9efb90d74143a617cb1cb21c34cbf70 100644
--- a/sim/src/components/CMakeLists.txt
+++ b/sim/src/components/CMakeLists.txt
@@ -31,7 +31,8 @@ add_subdirectory(Dynamics_TF)
add_subdirectory(LimiterAccVehComp)
add_subdirectory(OpenScenarioActions)
add_subdirectory(Parameters_Vehicle)
-add_subdirectory(SensorFusion_OSI)
+add_subdirectory(SensorAggregation_OSI)
+add_subdirectory(SensorFusionErrorless_OSI)
#add_subdirectory(Sensor_Distance)
add_subdirectory(Sensor_Driver)
add_subdirectory(Sensor_OSI)
diff --git a/sim/src/components/SensorAggregation_OSI/CMakeLists.txt b/sim/src/components/SensorAggregation_OSI/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..17e86e631b0eb781462761bf948ed1a00923e854
--- /dev/null
+++ b/sim/src/components/SensorAggregation_OSI/CMakeLists.txt
@@ -0,0 +1,21 @@
+set(COMPONENT_NAME SensorAggregation_OSI)
+
+add_compile_definitions(SENSOR_AGGREGATION_LIBRARY)
+
+add_openpass_target(
+ NAME ${COMPONENT_NAME} TYPE library LINKAGE shared COMPONENT core
+
+ HEADERS
+ sensorAggregation.h
+ src/sensorAggregationImpl.h
+
+ SOURCES
+ sensorAggregation.cpp
+ src/sensorAggregationImpl.cpp
+
+ LIBRARIES
+ Qt5::Core
+ Common
+
+ LINKOSI
+)
diff --git a/sim/src/components/SensorAggregation_OSI/SensorAggregation_OSI.pro b/sim/src/components/SensorAggregation_OSI/SensorAggregation_OSI.pro
new file mode 100644
index 0000000000000000000000000000000000000000..bd80d9db1b3773decfb3f1402a166d95efe4c175
--- /dev/null
+++ b/sim/src/components/SensorAggregation_OSI/SensorAggregation_OSI.pro
@@ -0,0 +1,40 @@
+# /*********************************************************************
+# * Copyright (c) 2017, 2018, 2019 in-tech GmbH
+# *
+# * 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
+# **********************************************************************/
+
+#-----------------------------------------------------------------------------
+# \file SensorAggregation_OSI.pro
+# \brief This file contains the information for the QtCreator-project of the
+# module SensorAggregation_OSI
+#-----------------------------------------------------------------------------/
+
+DEFINES += SENSOR_AGGREGATION_LIBRARY
+CONFIG += OPENPASS_LIBRARY
+include(../../../global.pri)
+
+
+SUBDIRS += .\
+ src
+
+INCLUDEPATH += \
+ $$SUBDIRS \
+ ../../.. \
+ ../..
+
+SOURCES += \
+ sensorAggregation.cpp \
+ src/sensorAggregationImpl.cpp
+
+HEADERS += \
+ sensorAggregation.h \
+ src/sensorAggregationImpl.h
+
+LIBS += \
+ -lopen_simulation_interface \
+ -lprotobuf
diff --git a/sim/src/components/SensorAggregation_OSI/sensorAggregation.cpp b/sim/src/components/SensorAggregation_OSI/sensorAggregation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..55d1941aa07cc125ce122fad77352d8e49911e46
--- /dev/null
+++ b/sim/src/components/SensorAggregation_OSI/sensorAggregation.cpp
@@ -0,0 +1,172 @@
+/*******************************************************************************
+* Copyright (c) 2017, 2018, 2019, 2020 in-tech GmbH
+*
+* 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
+*******************************************************************************/
+
+//-----------------------------------------------------------------------------
+/** \file SensorAggregation.cpp */
+//-----------------------------------------------------------------------------
+
+#include "sensorAggregation.h"
+#include "src/sensorAggregationImpl.h"
+
+const std::string Version = "0.0.1";
+static const CallbackInterface *Callbacks = nullptr;
+
+extern "C" SENSOR_AGGREGATION_SHARED_EXPORT const std::string &OpenPASS_GetVersion()
+{
+ return Version;
+}
+
+extern "C" SENSOR_AGGREGATION_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) SensorAggregationImplementation(
+ 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" SENSOR_AGGREGATION_SHARED_EXPORT void OpenPASS_DestroyInstance(ModelInterface *implementation)
+{
+ delete (SensorAggregationImplementation*)implementation;
+}
+
+extern "C" SENSOR_AGGREGATION_SHARED_EXPORT bool OpenPASS_UpdateInput(ModelInterface *implementation,
+ int localLinkId,
+ const std::shared_ptr<SignalInterface const> &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" SENSOR_AGGREGATION_SHARED_EXPORT bool OpenPASS_UpdateOutput(ModelInterface *implementation,
+ int localLinkId,
+ std::shared_ptr<SignalInterface const> &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" SENSOR_AGGREGATION_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/SensorAggregation_OSI/sensorAggregation.h b/sim/src/components/SensorAggregation_OSI/sensorAggregation.h
new file mode 100644
index 0000000000000000000000000000000000000000..18f703a402c94b1b2bdade3b10f863a62358aea0
--- /dev/null
+++ b/sim/src/components/SensorAggregation_OSI/sensorAggregation.h
@@ -0,0 +1,30 @@
+/*******************************************************************************
+* Copyright (c) 2017, 2019, 2020 in-tech GmbH
+*
+* 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
+*******************************************************************************/
+
+//-----------------------------------------------------------------------------
+/** @file SensorAggregation.h
+* @brief This file provides the exported methods.
+*
+* This file provides the exported methods which are available outside of the library. */
+//-----------------------------------------------------------------------------
+
+#pragma once
+
+#include <QtCore/qglobal.h>
+
+#if defined(SENSOR_AGGREGATION_LIBRARY)
+# define SENSOR_AGGREGATION_SHARED_EXPORT Q_DECL_EXPORT
+#else
+# define SENSOR_AGGREGATION_SHARED_EXPORT Q_DECL_IMPORT
+#endif
+
+#include "include/modelInterface.h"
+
+
diff --git a/sim/src/components/SensorFusion_OSI/src/sensorFusionImpl.cpp b/sim/src/components/SensorAggregation_OSI/src/sensorAggregationImpl.cpp
similarity index 86%
rename from sim/src/components/SensorFusion_OSI/src/sensorFusionImpl.cpp
rename to sim/src/components/SensorAggregation_OSI/src/sensorAggregationImpl.cpp
index a8bc50c94267a91ae5e26e09d735edda4edbe31f..37654892172f01a0514f4589487708825b1dc4f4 100644
--- a/sim/src/components/SensorFusion_OSI/src/sensorFusionImpl.cpp
+++ b/sim/src/components/SensorAggregation_OSI/src/sensorAggregationImpl.cpp
@@ -9,13 +9,13 @@
*******************************************************************************/
//-----------------------------------------------------------------------------
-/** \brief SensorFusion.cpp */
+/** \brief sensorAggregationImpl.cpp */
//-----------------------------------------------------------------------------
-#include "sensorFusionImpl.h"
+#include "sensorAggregationImpl.h"
#include <qglobal.h>
-SensorFusionImplementation::SensorFusionImplementation(
+SensorAggregationImplementation::SensorAggregationImplementation(
std::string componentName,
bool isInit,
int priority,
@@ -44,7 +44,7 @@ SensorFusionImplementation::SensorFusionImplementation(
{
}
-void SensorFusionImplementation::UpdateInput(int localLinkId, const std::shared_ptr<SignalInterface const> &data, int time)
+void SensorAggregationImplementation::UpdateInput(int localLinkId, const std::shared_ptr<SignalInterface const> &data, int time)
{
if(time != previousTimeStamp) {
out_sensorData.Clear();
@@ -66,7 +66,7 @@ void SensorFusionImplementation::UpdateInput(int localLinkId, const std::shared_
out_sensorData.MergeFrom(signal->sensorData);
}
-void SensorFusionImplementation::UpdateOutput(int localLinkId, std::shared_ptr<SignalInterface const> &data, int time)
+void SensorAggregationImplementation::UpdateOutput(int localLinkId, std::shared_ptr<SignalInterface const> &data, int time)
{
Q_UNUSED(time);
@@ -98,7 +98,7 @@ void SensorFusionImplementation::UpdateOutput(int localLinkId, std::shared_ptr<S
}
}
-void SensorFusionImplementation::Trigger(int time)
+void SensorAggregationImplementation::Trigger(int time)
{
Q_UNUSED(time);
}
diff --git a/sim/src/components/SensorFusion_OSI/src/sensorFusionImpl.h b/sim/src/components/SensorAggregation_OSI/src/sensorAggregationImpl.h
similarity index 82%
rename from sim/src/components/SensorFusion_OSI/src/sensorFusionImpl.h
rename to sim/src/components/SensorAggregation_OSI/src/sensorAggregationImpl.h
index 3946842f1b004bfed2b1db2a8f250258113c9fcf..83f1ae9d4fe6a621367dd457783ead48c684b16e 100644
--- a/sim/src/components/SensorFusion_OSI/src/sensorFusionImpl.h
+++ b/sim/src/components/SensorAggregation_OSI/src/sensorAggregationImpl.h
@@ -1,5 +1,5 @@
/*******************************************************************************
-* Copyright (c) 2017, 2018, 2019 in-tech GmbH
+* Copyright (c) 2017, 2018, 2019, 2020 in-tech GmbH
* Copyright (c) 2020 HLRS, University of Stuttgart.
*
* This program and the accompanying materials are made
@@ -9,11 +9,11 @@
* SPDX-License-Identifier: EPL-2.0
*******************************************************************************/
-/** \addtogroup SensorFusion
+/** \addtogroup SensorAggregation
* @{
-* \brief This file models the SensorFusion.
+* \brief This file models the SensorAggregation.
*
-* \details This file models the SensorFusion which can be part of an agent.
+* \details This file models the SensorAggregation which can be part of an agent.
* This module gets OSI SensorData of all sensors of the vehicle and forwards a combined
* SensorData to the driver assistance systems.
*
@@ -48,18 +48,18 @@
#include "osi3/osi_sensordata.pb.h"
//-----------------------------------------------------------------------------
-/** \brief This class is the SensorFusion module.
+/** \brief This class is the SensorAggregation module.
* \details This class contains all logic regarding the sensor fusion.
*
-* \ingroup SensorFusion
+* \ingroup SensorAggregation
*/
//-----------------------------------------------------------------------------
-class SensorFusionImplementation : public UnrestrictedModelInterface
+class SensorAggregationImplementation : public UnrestrictedModelInterface
{
public:
const std::string COMPONENTNAME = "SensorFusion";
- SensorFusionImplementation(
+ SensorAggregationImplementation(
std::string componentName,
bool isInit,
int priority,
@@ -73,11 +73,11 @@ public:
const CallbackInterface *callbacks,
AgentInterface *agent);
- SensorFusionImplementation(const SensorFusionImplementation&) = delete;
- SensorFusionImplementation(SensorFusionImplementation&&) = delete;
- SensorFusionImplementation& operator=(const SensorFusionImplementation&) = delete;
- SensorFusionImplementation& operator=(SensorFusionImplementation&&) = delete;
- virtual ~SensorFusionImplementation() = default;
+ SensorAggregationImplementation(const SensorAggregationImplementation&) = delete;
+ SensorAggregationImplementation(SensorAggregationImplementation&&) = delete;
+ SensorAggregationImplementation& operator=(const SensorAggregationImplementation&) = delete;
+ SensorAggregationImplementation& operator=(SensorAggregationImplementation&&) = delete;
+ virtual ~SensorAggregationImplementation() = default;
/*!
* \brief Update Inputs
diff --git a/sim/src/components/SensorFusion_OSI/CMakeLists.txt b/sim/src/components/SensorFusionErrorless_OSI/CMakeLists.txt
similarity index 70%
rename from sim/src/components/SensorFusion_OSI/CMakeLists.txt
rename to sim/src/components/SensorFusionErrorless_OSI/CMakeLists.txt
index e07d54f00f8d3311ef6cf84def0e61353291795f..20b37f3d65bc6a4a86714af92e49e85b02cf0f86 100644
--- a/sim/src/components/SensorFusion_OSI/CMakeLists.txt
+++ b/sim/src/components/SensorFusionErrorless_OSI/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(COMPONENT_NAME SensorFusion_OSI)
+set(COMPONENT_NAME SensorFusionErrorless_OSI)
add_compile_definitions(SENSOR_FUSION_LIBRARY)
@@ -6,12 +6,11 @@ add_openpass_target(
NAME ${COMPONENT_NAME} TYPE library LINKAGE shared COMPONENT core
HEADERS
- sensorFusion.h
+ sensorFusionErrorless_OSI.h
src/sensorFusionImpl.h
- src/sensorFusionQuery.h
SOURCES
- sensorFusion.cpp
+ sensorFusionErrorless_OSI.cpp
src/sensorFusionImpl.cpp
LIBRARIES
diff --git a/sim/src/components/SensorFusion_OSI/SensorFusion_OSI.pro b/sim/src/components/SensorFusionErrorless_OSI/SensorFusionErrorless_OSI.pro
similarity index 87%
rename from sim/src/components/SensorFusion_OSI/SensorFusion_OSI.pro
rename to sim/src/components/SensorFusionErrorless_OSI/SensorFusionErrorless_OSI.pro
index a1c21b8fa3d4a1b0d620b01681e4f04eee23b01e..866637e7fe56588a75cae8e602c1e135359fffb7 100644
--- a/sim/src/components/SensorFusion_OSI/SensorFusion_OSI.pro
+++ b/sim/src/components/SensorFusionErrorless_OSI/SensorFusionErrorless_OSI.pro
@@ -9,9 +9,9 @@
# **********************************************************************/
#-----------------------------------------------------------------------------
-# \file SensorFusion_OSI.pro
+# \file SensorAggregation_OSI.pro
# \brief This file contains the information for the QtCreator-project of the
-# module SensorFusion_OSI
+# module SensorAggregation_OSI
#-----------------------------------------------------------------------------/
DEFINES += SENSOR_FUSION_LIBRARY
@@ -28,11 +28,11 @@ INCLUDEPATH += \
../..
SOURCES += \
- sensorFusion.cpp \
+ sensorFusionErrorless_OSI.cpp \
src/sensorFusionImpl.cpp
HEADERS += \
- sensorFusion.h \
+ sensorFusionErrorless_OSI.h \
src/sensorFusionImpl.h
LIBS += \
diff --git a/sim/src/components/SensorFusion_OSI/sensorFusion.cpp b/sim/src/components/SensorFusionErrorless_OSI/sensorFusionErrorless_OSI.cpp
similarity index 97%
rename from sim/src/components/SensorFusion_OSI/sensorFusion.cpp
rename to sim/src/components/SensorFusionErrorless_OSI/sensorFusionErrorless_OSI.cpp
index b3c88fdbcba86c0f21468c5d4aa76e818552185e..7178e3f40e44f62ba4b6fb593a2a7bd11463f522 100644
--- a/sim/src/components/SensorFusion_OSI/sensorFusion.cpp
+++ b/sim/src/components/SensorFusionErrorless_OSI/sensorFusionErrorless_OSI.cpp
@@ -12,7 +12,7 @@
/** \file SensorFusion.cpp */
//-----------------------------------------------------------------------------
-#include "sensorFusion.h"
+#include "sensorFusionErrorless_OSI.h"
#include "src/sensorFusionImpl.h"
const std::string Version = "0.0.1";
@@ -41,7 +41,7 @@ extern "C" SENSOR_FUSION_SHARED_EXPORT ModelInterface *OpenPASS_CreateInstance(
try
{
- return (ModelInterface*)(new (std::nothrow) SensorFusionImplementation(
+ return (ModelInterface*)(new (std::nothrow) SensorFusionErrorlessImplementation(
componentName,
isInit,
priority,
@@ -77,7 +77,7 @@ extern "C" SENSOR_FUSION_SHARED_EXPORT ModelInterface *OpenPASS_CreateInstance(
extern "C" SENSOR_FUSION_SHARED_EXPORT void OpenPASS_DestroyInstance(ModelInterface *implementation)
{
- delete (SensorFusionImplementation*)implementation;
+ delete (SensorFusionErrorlessImplementation*)implementation;
}
extern "C" SENSOR_FUSION_SHARED_EXPORT bool OpenPASS_UpdateInput(ModelInterface *implementation,
diff --git a/sim/src/components/SensorFusion_OSI/sensorFusion.h b/sim/src/components/SensorFusionErrorless_OSI/sensorFusionErrorless_OSI.h
similarity index 100%
rename from sim/src/components/SensorFusion_OSI/sensorFusion.h
rename to sim/src/components/SensorFusionErrorless_OSI/sensorFusionErrorless_OSI.h
diff --git a/sim/src/components/SensorFusionErrorless_OSI/src/sensorFusionImpl.cpp b/sim/src/components/SensorFusionErrorless_OSI/src/sensorFusionImpl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d639d4b2f565d655b5990dfbd819ca4cbdbacf3
--- /dev/null
+++ b/sim/src/components/SensorFusionErrorless_OSI/src/sensorFusionImpl.cpp
@@ -0,0 +1,127 @@
+/*******************************************************************************
+* Copyright (c) 2020 in-tech GmbH
+*
+* 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
+*******************************************************************************/
+
+//-----------------------------------------------------------------------------
+/** \brief sensorFusionImpl.cpp */
+//-----------------------------------------------------------------------------
+
+#include "sensorFusionImpl.h"
+#include <qglobal.h>
+
+SensorFusionErrorlessImplementation::SensorFusionErrorlessImplementation(
+ 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)
+{
+}
+
+void SensorFusionErrorlessImplementation::UpdateInput(int localLinkId, const std::shared_ptr<SignalInterface const> &data, [[maybe_unused]] int time)
+{
+ std::stringstream log;
+ log << COMPONENTNAME << " (component " << GetComponentName() << ", agent " << GetAgent()->GetId() << ", input data for local link " << localLinkId << ": ";
+ LOG(CbkLogLevel::Debug, log.str());
+
+ const std::shared_ptr<SensorDataSignal const> signal = std::dynamic_pointer_cast<SensorDataSignal const>(data);
+ if(!signal)
+ {
+ const std::string msg = COMPONENTNAME + " invalid signaltype";
+ LOG(CbkLogLevel::Debug, msg);
+ throw std::runtime_error(msg);
+ }
+
+ MergeSensorData(signal->sensorData);
+}
+
+void SensorFusionErrorlessImplementation::UpdateOutput(int localLinkId, std::shared_ptr<SignalInterface const> &data, [[maybe_unused]] int time)
+{
+ std::stringstream log;
+ log << COMPONENTNAME << " (component " << GetComponentName() << ", agent " << GetAgent()->GetId() << ", output data for local link " << localLinkId << ": ";
+ LOG(CbkLogLevel::Debug, log.str());
+
+
+ if(localLinkId == 0)
+ {
+ // to any ADAS
+ try
+ {
+ data = std::make_shared<SensorDataSignal const>(
+ out_sensorData);
+ }
+ catch(const std::bad_alloc&)
+ {
+ const std::string msg = COMPONENTNAME + " could not instantiate signal";
+ LOG(CbkLogLevel::Debug, msg);
+ throw std::runtime_error(msg);
+ }
+ }
+ else
+ {
+ const std::string msg = COMPONENTNAME + " invalid link";
+ LOG(CbkLogLevel::Debug, msg);
+ throw std::runtime_error(msg);
+ }
+}
+
+void SensorFusionErrorlessImplementation::Trigger(int)
+{
+}
+
+void SensorFusionErrorlessImplementation::MergeSensorData(const osi3::SensorData& in_SensorData)
+{
+ out_sensorData = {};
+ for (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();});
+ if (existingObject != out_sensorData.mutable_moving_object()->end())
+ {
+ existingObject->mutable_header()->mutable_sensor_id()->MergeFrom(movingObject.header().sensor_id());
+ }
+ else
+ {
+ out_sensorData.add_moving_object()->CopyFrom(movingObject);
+ }
+ }
+ for (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();});
+ if (existingObject != out_sensorData.mutable_stationary_object()->end())
+ {
+ existingObject->mutable_header()->mutable_sensor_id()->MergeFrom(stationaryObject.header().sensor_id());
+ }
+ else
+ {
+ out_sensorData.add_stationary_object()->CopyFrom(stationaryObject);
+ }
+ }
+}
diff --git a/sim/src/components/SensorFusionErrorless_OSI/src/sensorFusionImpl.h b/sim/src/components/SensorFusionErrorless_OSI/src/sensorFusionImpl.h
new file mode 100644
index 0000000000000000000000000000000000000000..f382e5b985c07bf9264866f2951814f7045c8344
--- /dev/null
+++ b/sim/src/components/SensorFusionErrorless_OSI/src/sensorFusionImpl.h
@@ -0,0 +1,94 @@
+/*******************************************************************************
+* Copyright (c) 2020 in-tech GmbH
+*
+* 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
+*******************************************************************************/
+
+/** \addtogroup SensorFusionErrorless
+* @{
+* \brief This file models the SensorFusionErrorless.
+*
+* \details This file models the SensorFusionErrorless which can be part of an agent.
+* This module gets OSI SensorData of the SensorAggregation and combines all
+* object with the same id into one.
+*
+* \section MODULENAME_Inputs Inputs
+* Input variables:
+* name | meaning
+* -----|------
+* sensorData | SensorData of a single sensor.
+*
+* Input channel IDs:
+* Input Id | signal class | contained variables
+* ----------|--------------|-------------
+* 0 | SensorDataSignal | sensorData
+*
+* \section MODULENAME_Outputs Outputs
+* Output variables:
+* name | meaning
+* -----|------
+* out_sensorData | Combined SensorData from all sensors.
+*
+* Output channel IDs:
+* Output Id | signal class | contained variables
+* ----------|--------------|-------------
+* 0 | SensorDataSignal | out_sensorData
+*
+* @} */
+
+#pragma once
+
+#include "include/modelInterface.h"
+#include "common/sensorDataSignal.h"
+#include "osi3/osi_sensordata.pb.h"
+
+//-----------------------------------------------------------------------------
+/** \brief This class is the SensorFusionErrorless module.
+* \details This class contains all logic regarding the sensor fusion.
+*
+* \ingroup SensorFusionErrorless
+*/
+//-----------------------------------------------------------------------------
+class SensorFusionErrorlessImplementation : public UnrestrictedModelInterface
+{
+public:
+ const std::string COMPONENTNAME = "SensorFusion";
+
+ SensorFusionErrorlessImplementation(
+ 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);
+
+ SensorFusionErrorlessImplementation(const SensorFusionErrorlessImplementation&) = delete;
+ SensorFusionErrorlessImplementation(SensorFusionErrorlessImplementation&&) = delete;
+ SensorFusionErrorlessImplementation& operator=(const SensorFusionErrorlessImplementation&) = delete;
+ SensorFusionErrorlessImplementation& operator=(SensorFusionErrorlessImplementation&&) = delete;
+ virtual ~SensorFusionErrorlessImplementation() = default;
+
+ virtual void UpdateInput(int localLinkId, const std::shared_ptr<SignalInterface const> &data, int time);
+
+ void UpdateOutput(int localLinkId, std::shared_ptr<SignalInterface const> &data, int time);
+
+ virtual void Trigger(int time);
+
+private:
+
+ void MergeSensorData(const osi3::SensorData& in_SensorData);
+
+ osi3::SensorData out_sensorData;
+};
+
+
diff --git a/sim/src/components/Sensor_Driver/src/sensor_driverImpl.cpp b/sim/src/components/Sensor_Driver/src/sensor_driverImpl.cpp
index 6a969ff71d54023842d283f055a7abfac37e12ae..2306d7b25f6e52a8de4842c41be891c2e60b0172 100644
--- a/sim/src/components/Sensor_Driver/src/sensor_driverImpl.cpp
+++ b/sim/src/components/Sensor_Driver/src/sensor_driverImpl.cpp
@@ -110,7 +110,7 @@ void SensorDriverImplementation::GetNewRoute()
}
auto [roadGraph, root] = GetWorld()->GetRoadGraph({roadIds.front(), GetAgent()->GetObjectPosition().mainLocatePoint.at(roadIds.front()).laneId < 0}, maxDepth);
std::map<RoadGraph::edge_descriptor, double> weights = GetWorld()->GetEdgeWeights(roadGraph);
- auto [route, target] = RouteCalculation::SampleRoute(roadGraph, root, weights, *GetStochastics());
+ auto target = RouteCalculation::SampleRoute(roadGraph, root, weights, *GetStochastics());
egoAgent.SetRoadGraph(std::move(roadGraph), root, target);
}
diff --git a/sim/src/components/Sensor_OSI/src/objectDetectorBase.cpp b/sim/src/components/Sensor_OSI/src/objectDetectorBase.cpp
index 25390d1e69a63bdf299ee9ee174896f3e60cc96c..ddc1e160bdf42ba3060c69c025ae53dcd0118718 100644
--- a/sim/src/components/Sensor_OSI/src/objectDetectorBase.cpp
+++ b/sim/src/components/Sensor_OSI/src/objectDetectorBase.cpp
@@ -104,6 +104,45 @@ void ObjectDetectorBase::UpdateOutput(int localLinkId, std::shared_ptr<SignalInt
}
}
+void ObjectDetectorBase::AddMovingObjectToSensorData(osi3::MovingObject object, point_t ownVelocity, point_t ownAcceleration, point_t ownPosition, double yaw, double yawRate)
+{
+ point_t objectReferencePointGlobal{object.base().position().x(), object.base().position().y()};
+ point_t objectReferencePointLocal = TransformPointToLocalCoordinates(objectReferencePointGlobal, ownPosition, yaw);
+
+ 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);
+ detectedObject->mutable_base()->mutable_dimension()->CopyFrom(object.base().dimension());
+
+ detectedObject->mutable_base()->mutable_position()->set_x(objectReferencePointLocal.x());
+ detectedObject->mutable_base()->mutable_position()->set_y(objectReferencePointLocal.y());
+ detectedObject->mutable_base()->mutable_orientation()->set_yaw(object.base().orientation().yaw() - yaw);
+ detectedObject->mutable_base()->mutable_orientation_rate()->set_yaw(object.base().orientation_rate().yaw() - yawRate);
+ point_t objectVelocity{object.base().velocity().x(), object.base().velocity().y()};
+ point_t relativeVelocity = CalculateRelativeVector(objectVelocity, ownVelocity, yaw);
+ detectedObject->mutable_base()->mutable_velocity()->set_x(relativeVelocity.x());
+ detectedObject->mutable_base()->mutable_velocity()->set_y(relativeVelocity.y());
+ point_t objectAcceleration{object.base().acceleration().x(), object.base().acceleration().y()};
+ point_t relativeAcceleration = CalculateRelativeVector(objectAcceleration, ownAcceleration, yaw);
+ detectedObject->mutable_base()->mutable_acceleration()->set_x(relativeAcceleration.x());
+ detectedObject->mutable_base()->mutable_acceleration()->set_y(relativeAcceleration.y());
+}
+
+void ObjectDetectorBase::AddStationaryObjectToSensorData(osi3::StationaryObject object, point_t ownPosition, double yaw)
+{
+ point_t objectReferencePointGlobal{object.base().position().x(), object.base().position().y()};
+ point_t objectReferencePointLocal = TransformPointToLocalCoordinates(objectReferencePointGlobal, ownPosition, yaw);
+
+ osi3::DetectedStationaryObject* detectedObject = sensorData.add_stationary_object();
+ detectedObject->mutable_header()->add_ground_truth_id()->set_value(object.id().value());
+ detectedObject->mutable_header()->add_sensor_id()->set_value(id);
+ detectedObject->mutable_base()->mutable_dimension()->CopyFrom(object.base().dimension());
+
+ detectedObject->mutable_base()->mutable_position()->set_x(objectReferencePointLocal.x());
+ detectedObject->mutable_base()->mutable_position()->set_y(objectReferencePointLocal.y());
+ detectedObject->mutable_base()->mutable_orientation()->set_yaw(object.base().orientation().yaw() - yaw);
+}
+
Position ObjectDetectorBase::GetAbsolutePosition()
{
Position absolutePosition;
@@ -269,3 +308,33 @@ void ObjectDetectorBase::ParseBasicParameter()
position.yaw = doubleParameters.at("Yaw");
position.roll = doubleParameters.at("Roll");
}
+
+const osi3::MovingObject* ObjectDetectorBase::FindHostVehicleInSensorView(const osi3::SensorView& sensorView)
+{
+ const auto hostVehicleIt = std::find_if(sensorView.global_ground_truth().moving_object().cbegin(),
+ sensorView.global_ground_truth().moving_object().cend(),
+ [&sensorView](const osi3::MovingObject& object)
+ {
+ return object.id().value() == sensorView.host_vehicle_id().value();
+ });
+
+ if (hostVehicleIt == sensorView.global_ground_truth().moving_object().cend())
+ {
+ throw std::runtime_error("Host vehicle not in SensorView");
+ }
+
+ return &(*hostVehicleIt);
+}
+
+point_t ObjectDetectorBase::GetHostVehiclePosition(const osi3::MovingObject* hostVehicle) const
+{
+ point_t bbCenterToRear{hostVehicle->vehicle_attributes().bbcenter_to_rear().x(), hostVehicle->vehicle_attributes().bbcenter_to_rear().y()};
+ bt::rotate_transformer<bg::radian, double, 2, 2> rotate(-hostVehicle->base().orientation().yaw());
+ bt::translate_transformer<double, 2, 2> bbCenter(hostVehicle->base().position().x(), hostVehicle->base().position().y());
+ point_t rotatedBbCenterToRear;
+ point_t ownPosition;
+ bg::transform(bbCenterToRear, rotatedBbCenterToRear, rotate);
+ bg::transform(rotatedBbCenterToRear, ownPosition, bbCenter);
+
+ return ownPosition;
+}
diff --git a/sim/src/components/Sensor_OSI/src/objectDetectorBase.h b/sim/src/components/Sensor_OSI/src/objectDetectorBase.h
index 45e8b1f2cfa9278109357bbd68ebab169db88a1c..6472c81f725544098f04b91e456b6b40eb3bb633 100644
--- a/sim/src/components/Sensor_OSI/src/objectDetectorBase.h
+++ b/sim/src/components/Sensor_OSI/src/objectDetectorBase.h
@@ -133,6 +133,27 @@ public:
protected:
+ /*!
+ * \brief Adds the information of a detected moving object as DetectedMovingObject to the sensor data
+ *
+ * \param object detected moving object
+ * \param ownVelocity velocity of own vehicle in global coordinates
+ * \param ownAcceleration acceleration of own vehicle in global coordinates
+ * \param ownPosition position of own vehicle in global coordinates
+ * \param yaw yaw of own vehicle in global coordinates
+ * \param yawRate yawRate of own vehicle in global coordinates
+ */
+ void AddMovingObjectToSensorData (osi3::MovingObject object, point_t ownVelocity, point_t ownAcceleration, point_t ownPosition, double yaw, double yawRate);
+
+ /*!
+ * \brief Adds the information of a detected stationary object as DetectedStationaryObject to the sensor data
+ *
+ * \param object stationary moving object
+ * \param ownPosition position of own vehicle in global coordinates
+ * \param yaw yaw of own vehicle in global coordinates
+ */
+ void AddStationaryObjectToSensorData (osi3::StationaryObject object, point_t ownPosition, double yaw);
+
/*!
* \brief Returns the absolute position of the sensor
*
@@ -226,6 +247,12 @@ protected:
*/
virtual point_t CalculateRelativeVector(const point_t absolute, const point_t own, double yaw);
+ //! Returns the MovingObject in the sensor view which was defined as host vehicle (by id)
+ static const osi3::MovingObject* FindHostVehicleInSensorView(const osi3::SensorView& sensorView);
+
+ //! Returns the world coordinate position of the host vehicle
+ point_t GetHostVehiclePosition(const osi3::MovingObject* hostVehicle) const;
+
std::list<std::pair<int, osi3::SensorData>> detectedObjectsBuffer;
osi3::SensorData sensorData;
diff --git a/sim/src/components/Sensor_OSI/src/sensorGeometric2D.cpp b/sim/src/components/Sensor_OSI/src/sensorGeometric2D.cpp
index 55bda33139cd728c35dfa3e22877e88847437ce1..aec47718be478c054714882fbc4276467ee53a0a 100644
--- a/sim/src/components/Sensor_OSI/src/sensorGeometric2D.cpp
+++ b/sim/src/components/Sensor_OSI/src/sensorGeometric2D.cpp
@@ -221,19 +221,6 @@ polygon_t SensorGeometric2D::CreateFivePointDetectionField() const
return detectionField;
}
-point_t SensorGeometric2D::GetHostVehiclePosition(const osi3::MovingObject* hostVehicle) const
-{
- point_t bbCenterToRear{hostVehicle->vehicle_attributes().bbcenter_to_rear().x(), hostVehicle->vehicle_attributes().bbcenter_to_rear().y()};
- bt::rotate_transformer<bg::radian, double, 2, 2> rotate(-hostVehicle->base().orientation().yaw());
- bt::translate_transformer<double, 2, 2> bbCenter(hostVehicle->base().position().x(), hostVehicle->base().position().y());
- point_t rotatedBbCenterToRear;
- point_t ownPosition;
- bg::transform(bbCenterToRear, rotatedBbCenterToRear, rotate);
- bg::transform(rotatedBbCenterToRear, ownPosition, bbCenter);
-
- return ownPosition;
-}
-
std::pair<point_t, polygon_t> SensorGeometric2D::CreateSensorDetectionField(const osi3::MovingObject* hostVehicle) const
{
polygon_t detectionField;
@@ -326,23 +313,6 @@ SensorDetectionResults SensorGeometric2D::DetectObjects()
return results;
}
-const osi3::MovingObject* SensorGeometric2D::FindHostVehicleInSensorView(const osi3::SensorView& sensorView)
-{
- const auto hostVehicleIt = std::find_if(sensorView.global_ground_truth().moving_object().cbegin(),
- sensorView.global_ground_truth().moving_object().cend(),
- [&sensorView](const osi3::MovingObject& object)
- {
- return object.id().value() == sensorView.host_vehicle_id().value();
- });
-
- if (hostVehicleIt == sensorView.global_ground_truth().moving_object().cend())
- {
- throw std::runtime_error("Host vehicle not in SensorView");
- }
-
- return &(*hostVehicleIt);
-}
-
std::pair<std::vector<const osi3::MovingObject*>, std::vector<const osi3::StationaryObject*>> SensorGeometric2D::GetObjectsInDetectionAreaFromSensorView(const osi3::SensorView& sensorView,
const point_t& sensorPositionGlobal,
const polygon_t& detectionField) const
@@ -439,45 +409,6 @@ std::pair<std::vector<T>, std::vector<T>> SensorGeometric2D::CalcVisualObstructi
return std::make_pair(visibleObjects, detectedObjects);
}
-void SensorGeometric2D::AddMovingObjectToSensorData(osi3::MovingObject object, point_t ownVelocity, point_t ownAcceleration, point_t ownPosition, double yaw, double yawRate)
-{
- point_t objectReferencePointGlobal{object.base().position().x(), object.base().position().y()};
- point_t objectReferencePointLocal = TransformPointToLocalCoordinates(objectReferencePointGlobal, ownPosition, yaw);
-
- 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);
- detectedObject->mutable_base()->mutable_dimension()->CopyFrom(object.base().dimension());
-
- detectedObject->mutable_base()->mutable_position()->set_x(objectReferencePointLocal.x());
- detectedObject->mutable_base()->mutable_position()->set_y(objectReferencePointLocal.y());
- detectedObject->mutable_base()->mutable_orientation()->set_yaw(object.base().orientation().yaw() - yaw);
- detectedObject->mutable_base()->mutable_orientation_rate()->set_yaw(object.base().orientation_rate().yaw() - yawRate);
- point_t objectVelocity{object.base().velocity().x(), object.base().velocity().y()};
- point_t relativeVelocity = CalculateRelativeVector(objectVelocity, ownVelocity, yaw);
- detectedObject->mutable_base()->mutable_velocity()->set_x(relativeVelocity.x());
- detectedObject->mutable_base()->mutable_velocity()->set_y(relativeVelocity.y());
- point_t objectAcceleration{object.base().acceleration().x(), object.base().acceleration().y()};
- point_t relativeAcceleration = CalculateRelativeVector(objectAcceleration, ownAcceleration, yaw);
- detectedObject->mutable_base()->mutable_acceleration()->set_x(relativeAcceleration.x());
- detectedObject->mutable_base()->mutable_acceleration()->set_y(relativeAcceleration.y());
-}
-
-void SensorGeometric2D::AddStationaryObjectToSensorData(osi3::StationaryObject object, point_t ownPosition, double yaw)
-{
- point_t objectReferencePointGlobal{object.base().position().x(), object.base().position().y()};
- point_t objectReferencePointLocal = TransformPointToLocalCoordinates(objectReferencePointGlobal, ownPosition, yaw);
-
- osi3::DetectedStationaryObject* detectedObject = sensorData.add_stationary_object();
- detectedObject->mutable_header()->add_ground_truth_id()->set_value(object.id().value());
- detectedObject->mutable_header()->add_sensor_id()->set_value(id);
- detectedObject->mutable_base()->mutable_dimension()->CopyFrom(object.base().dimension());
-
- detectedObject->mutable_base()->mutable_position()->set_x(objectReferencePointLocal.x());
- detectedObject->mutable_base()->mutable_position()->set_y(objectReferencePointLocal.y());
- detectedObject->mutable_base()->mutable_orientation()->set_yaw(object.base().orientation().yaw() - yaw);
-}
-
polygon_t SensorGeometric2D::CalcInitialBrightArea(point_t sensorPosition)
{
const double stepSize = 0.1;
diff --git a/sim/src/components/Sensor_OSI/src/sensorGeometric2D.h b/sim/src/components/Sensor_OSI/src/sensorGeometric2D.h
index 10ebd8e7e42edd6980304ce8ed6014ec2d15378a..50badd91481e30c9c92425133c25ae7c5afb472e 100644
--- a/sim/src/components/Sensor_OSI/src/sensorGeometric2D.h
+++ b/sim/src/components/Sensor_OSI/src/sensorGeometric2D.h
@@ -129,27 +129,6 @@ private:
*/
static double CalcObjectVisibilityPercentage(const polygon_t& boundingBox, const multi_polygon_t &brightArea);
- /*!
- * \brief Adds the information of a detected moving object as DetectedMovingObject to the sensor data
- *
- * \param object detected moving object
- * \param ownVelocity velocity of own vehicle in global coordinates
- * \param ownAcceleration acceleration of own vehicle in global coordinates
- * \param ownPosition position of own vehicle in global coordinates
- * \param yaw yaw of own vehicle in global coordinates
- * \param yawRate yawRate of own vehicle in global coordinates
- */
- void AddMovingObjectToSensorData (osi3::MovingObject object, point_t ownVelocity, point_t ownAcceleration, point_t ownPosition, double yaw, double yawRate);
-
- /*!
- * \brief Adds the information of a detected stationary object as DetectedStationaryObject to the sensor data
- *
- * \param object stationary moving object
- * \param ownPosition position of own vehicle in global coordinates
- * \param yaw yaw of own vehicle in global coordinates
- */
- void AddStationaryObjectToSensorData (osi3::StationaryObject object, point_t ownPosition, double yaw);
-
/*!
* \brief Returns true if opening angle is smaller than pi
*/
@@ -170,8 +149,6 @@ private:
*/
polygon_t CreateFivePointDetectionField() const;
- point_t GetHostVehiclePosition(const osi3::MovingObject* hostVehicle) const;
-
std::pair<point_t, polygon_t> CreateSensorDetectionField(const osi3::MovingObject* hostVehicle) const;
template<typename T>
static void ApplyVisualObstructionToDetectionArea(multi_polygon_t& brightArea,
@@ -186,7 +163,6 @@ private:
const point_t& sensorPositionGlobal,
const polygon_t& detectionField) const;
- static const osi3::MovingObject* FindHostVehicleInSensorView(const osi3::SensorView& sensorView);
std::string CreateAgentIdListString(const std::vector<OWL::Id>& owlIds) const;
bool enableVisualObstruction = false;
diff --git a/sim/src/core/slave/framework/dynamicAgentTypeGenerator.cpp b/sim/src/core/slave/framework/dynamicAgentTypeGenerator.cpp
index e8a7b3fa38cc8060cadec44b7696ea89a71c35a7..d6dfda27f47219d6136d8d0a670c452decd9e830 100644
--- a/sim/src/core/slave/framework/dynamicAgentTypeGenerator.cpp
+++ b/sim/src/core/slave/framework/dynamicAgentTypeGenerator.cpp
@@ -1,5 +1,5 @@
/*******************************************************************************
-* Copyright (c) 2019 in-tech GmbH
+* Copyright (c) 2019, 2020 in-tech GmbH
*
* This program and the accompanying materials are made
* available under the terms of the Eclipse Public License 2.0
@@ -111,11 +111,11 @@ DynamicAgentTypeGenerator& DynamicAgentTypeGenerator::GatherVehicleComponents()
DynamicAgentTypeGenerator& DynamicAgentTypeGenerator::GatherSensors()
{
- const std::string sensorFusionModulName = "SensorFusion";
+ const std::string sensorAggregationModulName = "SensorAggregation";
- GatherComponent(sensorFusionModulName, agentBuildInformation.agentType);
+ GatherComponent(sensorAggregationModulName, agentBuildInformation.agentType);
- int inputIdSensorFusion = systemConfigBlueprint->GetSystems().at(0)->GetComponents().at(sensorFusionModulName)->GetInputLinks().at(0);
+ int inputIdSensorAggregation = systemConfigBlueprint->GetSystems().at(0)->GetComponents().at(sensorAggregationModulName)->GetInputLinks().at(0);
int sensorNumber = 0;
auto vehicleProfile = profiles->GetVehicleProfiles().at(sampledProfiles.vehicleProfileName);
@@ -144,8 +144,8 @@ DynamicAgentTypeGenerator& DynamicAgentTypeGenerator::GatherSensors()
if (sensorNumber > 0)
{
- agentBuildInformation.agentType->GetComponents().at(sensorFusionModulName)->AddInputLink(sensorNumber,
- inputIdSensorFusion + sensorNumber);
+ agentBuildInformation.agentType->GetComponents().at(sensorAggregationModulName)->AddInputLink(sensorNumber,
+ inputIdSensorAggregation + sensorNumber);
}
// clone sensor and set specific parameters
diff --git a/sim/src/core/slave/framework/dynamicAgentTypeGenerator.h b/sim/src/core/slave/framework/dynamicAgentTypeGenerator.h
index ab230cf95f0326fb72b640885dbcad056b13869c..35ee9dc896eeaef01e0d2197a290878cbf59af6b 100644
--- a/sim/src/core/slave/framework/dynamicAgentTypeGenerator.h
+++ b/sim/src/core/slave/framework/dynamicAgentTypeGenerator.h
@@ -52,7 +52,8 @@ struct DefaultComponents
"Algorithm_LongitudinalVehicleComponents",
"PrioritizerAccelerationVehicleComponents",
"PrioritizerSteeringVehicleComponents",
- "LimiterAccelerationVehicleComponents"
+ "LimiterAccelerationVehicleComponents",
+ "SensorFusionErrorless"
};
};
diff --git a/sim/src/core/slave/importer/road.cpp b/sim/src/core/slave/importer/road.cpp
index c58dd03d803d8096c178f863058c4943af73152a..23352e56670270e4514f355e2ed33103c5e69c9b 100644
--- a/sim/src/core/slave/importer/road.cpp
+++ b/sim/src/core/slave/importer/road.cpp
@@ -19,7 +19,7 @@ extern "C"
namespace
{
-const double SQRT_PI_2 = std::sqrt(M_PI_2);
+const double SQRT_PI = std::sqrt(M_PI);
} // namespace
@@ -220,541 +220,116 @@ double RoadGeometryArc::GetDir(double sOffset) const
return GetDirArc(sOffset, curvature);
}
-Common::Vector2d RoadGeometrySpiral::HalfCoord(double sOffset, double tOffset) const
+RoadGeometrySpiral::RoadGeometrySpiral(double s, double x, double y, double hdg, double length, double curvStart, double curvEnd)
+ : RoadGeometry{s, x, y, hdg, length}, c_start{curvStart}, c_end{curvEnd}
{
- double _curvStart = curvStart;
- double _curvEnd = curvEnd;
-
- assert(_curvStart != _curvEnd);
- assert((0.0 <= _curvStart && 0.0 <= _curvEnd) || (0.0 >= _curvStart && 0.0 >= _curvEnd));
-
- if (length < sOffset)
+ if (length != 0.0)
{
- LOG_INTERN(LogLevel::Warning) << "exceeding length of geometry";
- sOffset = length;
+ c_dot = (c_end - c_start) / length;
}
-
- if (0.0 <= _curvStart && 0.0 <= _curvEnd)
+ else
{
- if (_curvStart < _curvEnd)
- {
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
-
- Common::Vector2d start;
- (void)fresnl(distanceStart / a / SQRT_PI_2, &start.y, &start.x);
- start.Scale(a * SQRT_PI_2);
-
- double distanceOffset = distanceStart + sOffset;
- Common::Vector2d offset;
- (void)fresnl(distanceOffset / a / SQRT_PI_2, &offset.y, &offset.x);
- offset.Scale(a * SQRT_PI_2);
- offset.Sub(start);
-
- double tangentAngle = distanceOffset * distanceOffset / a / a;
- if (0 > _curvEnd)
- {
- tangentAngle = -tangentAngle;
- }
-
- double normAngle = tangentAngle + M_PI_2;
- normAngle = std::fmod(normAngle, 2 * M_PI);
-
- // get perpendicular vector
- Common::Vector2d norm(1, 0);
- norm.Rotate(normAngle);
- norm.Scale(tOffset);
-
- offset.Add(norm);
- offset.Rotate(hdg);
-
- return Common::Vector2d(x + offset.x, y + offset.y);
- }
- else // _curvStart > _curvEnd ("curStart != _curvEnd" guaranteed by checks in caller)
- {
- std::swap(_curvStart, _curvEnd);
- sOffset = length - sOffset;
-
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
-
- Common::Vector2d start;
- (void)fresnl(distanceStart / a / SQRT_PI_2, &start.y, &start.x);
- start.Scale(a * SQRT_PI_2);
-
- double distanceOffset = distanceStart + sOffset;
- Common::Vector2d offset;
- (void)fresnl(distanceOffset / a / SQRT_PI_2, &offset.y, &offset.x);
- offset.Scale(a * SQRT_PI_2);
- offset.Sub(start);
-
- double tangentAngle = distanceOffset * distanceOffset / a / a;
- if (0 > _curvEnd)
- {
- tangentAngle = -tangentAngle;
- }
-
- double normAngle = tangentAngle + M_PI_2;
- normAngle = std::fmod(normAngle, 2 * M_PI);
-
- // get perpendicular vector
- Common::Vector2d norm(1, 0);
- norm.Rotate(normAngle);
- norm.Scale(tOffset);
-
- offset.Add(norm);
-
- // calculate end point
- Common::Vector2d endOffset;
- (void)fresnl(distanceEnd / a / SQRT_PI_2, &endOffset.y, &endOffset.x);
- endOffset.Scale(a * SQRT_PI_2);
- endOffset.Sub(start);
-
- // compensate for inverted curvatures
- double tangentAngleEnd = distanceEnd * distanceEnd / a / a;
- if (0 > _curvEnd)
- {
- tangentAngleEnd = -tangentAngleEnd;
- }
- tangentAngleEnd = -tangentAngleEnd + M_PI;
-
- offset.Sub(endOffset);
- offset.y = -offset.y;
- offset.Rotate(hdg - tangentAngleEnd);
-
- return Common::Vector2d(x + offset.x, y + offset.y);
- }
+ c_dot = 0.0;
}
- else // 0.0 >= _curvStart && 0.0 >= _curvEnd
- {
- _curvStart = -_curvStart;
- _curvEnd = -_curvEnd;
-
- if (_curvStart < _curvEnd)
- {
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
-
- Common::Vector2d start;
- (void)fresnl(distanceStart / a / SQRT_PI_2, &start.y, &start.x);
- start.Scale(a * SQRT_PI_2);
-
- double distanceOffset = distanceStart + sOffset;
- Common::Vector2d offset;
- (void)fresnl(distanceOffset / a / SQRT_PI_2, &offset.y, &offset.x);
- offset.Scale(a * SQRT_PI_2);
- offset.Sub(start);
-
- double tangentAngle = distanceOffset * distanceOffset / a / a;
- if (0 > _curvEnd)
- {
- tangentAngle = -tangentAngle;
- }
-
- double normAngle = tangentAngle + M_PI_2;
- normAngle = std::fmod(normAngle, 2 * M_PI);
-
- // get perpendicular vector
- Common::Vector2d norm(-1, 0);
- norm.Rotate(normAngle);
- norm.Scale(tOffset);
-
- offset.Add(norm);
- offset.y = -offset.y;
- offset.Rotate(hdg);
-
- return Common::Vector2d(x + offset.x, y + offset.y);
- }
- else // _curvStart > _curvEnd ("curStart != _curvEnd" guaranteed by checks in caller)
- {
- std::swap(_curvStart, _curvEnd);
- sOffset = length - sOffset;
-
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
-
- Common::Vector2d start;
- (void)fresnl(distanceStart / a / SQRT_PI_2, &start.y, &start.x);
- start.Scale(a * SQRT_PI_2);
-
- double distanceOffset = distanceStart + sOffset;
- Common::Vector2d offset;
- (void)fresnl(distanceOffset / a / SQRT_PI_2, &offset.y, &offset.x);
- offset.Scale(a * SQRT_PI_2);
- offset.Sub(start);
-
- double tangentAngle = distanceOffset * distanceOffset / a / a;
- if (0 > _curvEnd)
- {
- tangentAngle = -tangentAngle;
- }
-
- double normAngle = tangentAngle + M_PI_2;
- normAngle = std::fmod(normAngle, 2 * M_PI);
-
- // get perpendicular vector
- Common::Vector2d norm(-1, 0);
- norm.Rotate(normAngle);
- norm.Scale(tOffset);
-
- offset.Add(norm);
-
- // calculate end point
- Common::Vector2d endOffset;
- (void)fresnl(distanceEnd / a / SQRT_PI_2, &endOffset.y, &endOffset.x);
- endOffset.Scale(a * SQRT_PI_2);
- endOffset.Sub(start);
-
- // compensate for inverted curvatures
- double tangentAngleEnd = distanceEnd * distanceEnd / a / a;
- if (0 > _curvEnd)
- {
- tangentAngleEnd = -tangentAngleEnd;
- }
- tangentAngleEnd = tangentAngleEnd - M_PI;
-
- offset.Sub(endOffset);
- offset.Rotate(hdg - tangentAngleEnd);
-
- return Common::Vector2d(x + offset.x, y + offset.y);
- }
+ if (c_dot != 0.0)
+ {
+ l_start = c_start / c_dot;
}
-}
-
-Common::Vector2d RoadGeometrySpiral::FullCoord(double sOffset, double tOffset) const
-{
- if ((0.0 <= curvStart && 0.0 <= curvEnd) || (0.0 >= curvStart && 0.0 >= curvEnd))
+ else
{
- return HalfCoord(sOffset, tOffset);
+ l_start = 0.0;
}
- assert((0.0 > curvStart && 0.0 < curvEnd) || (0.0 < curvStart && 0.0 > curvEnd));
-
- // one degree of freedom: start position/end position can not be determined
- LOG_INTERN(LogLevel::Warning) << "could not calculate spiral coordinate";
+ double l_end = l_start + length;
+ double rl; // helper constant R * L
- return Common::Vector2d();
-}
-
-double RoadGeometrySpiral::HalfCurvature(double sOffset) const
-{
- double _curvStart = curvStart;
- double _curvEnd = curvEnd;
-
- assert(_curvStart != _curvEnd);
- assert((0.0 <= _curvStart && 0.0 <= _curvEnd) || (0.0 >= _curvStart && 0.0 >= _curvEnd));
-
- if (length < sOffset)
+ if (c_start != 0.0)
{
- LOG_INTERN(LogLevel::Warning) << "exceeding length of geometry";
- sOffset = length;
+ rl = l_start / c_start;
}
-
- if (0.0 <= _curvStart && 0.0 <= _curvEnd)
+ else if (c_end != 0.0)
{
- if (_curvStart < _curvEnd)
- {
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double distanceOffset = distanceStart + sOffset;
-
- // equation const = radiusEnd * distanceEnd = radiusOffset * distanceOffset
- // -> curvatureOffset = 1 / radiusOffset = distanceOffset / (radiusEnd * distanceEnd)
- return distanceOffset / radiusEnd / distanceEnd;
- }
- else // _curvStart > _curvEnd ("curStart != _curvEnd" guaranteed by checks in caller)
- {
- std::swap(_curvStart, _curvEnd);
- sOffset = length - sOffset;
-
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double distanceOffset = distanceStart + sOffset;
-
- // equation const = radiusEnd * distanceEnd = radiusOffset * distanceOffset
- // -> curvatureOffset = 1 / radiusOffset = distanceOffset / (radiusEnd * distanceEnd)
- return distanceOffset / radiusEnd / distanceEnd;
- }
+ rl = l_end / c_end;
}
- else // 0.0 >= _curvStart && 0.0 >= _curvEnd
- {
- _curvStart = -_curvStart;
- _curvEnd = -_curvEnd;
-
- if (_curvStart < _curvEnd)
- {
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double distanceOffset = distanceStart + sOffset;
-
- // equation const = radiusEnd * distanceEnd = radiusOffset * distanceOffset
- // -> curvatureOffset = 1 / radiusOffset = distanceOffset / (radiusEnd * distanceEnd)
- return -distanceOffset / radiusEnd / distanceEnd;
- }
- else // _curvStart > _curvEnd ("curStart != _curvEnd" guaranteed by checks in caller)
- {
- std::swap(_curvStart, _curvEnd);
- sOffset = length - sOffset;
-
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double distanceOffset = distanceStart + sOffset;
-
- // equation const = radiusEnd * distanceEnd = radiusOffset * distanceOffset
- // -> curvatureOffset = 1 / radiusOffset = distanceOffset / (radiusEnd * distanceEnd)
- return -distanceOffset / radiusEnd / distanceEnd;
- }
- }
-}
-
-double RoadGeometrySpiral::FullCurvature(double sOffset) const
-{
- if ((0.0 <= curvStart && 0.0 <= curvEnd) || (0.0 >= curvStart && 0.0 >= curvEnd))
+ else
{
- return HalfCurvature(sOffset);
+ t_start = 0.0;
+ a = 0.0;
+ sign = 0.0;
+ return;
}
- assert((0.0 > curvStart && 0.0 < curvEnd) || (0.0 < curvStart && 0.0 > curvEnd));
-
- // one degree of freedom: start position/end position can not be determined
- LOG_INTERN(LogLevel::Warning) << "could not calculate spiral curvature";
-
- return 0.0;
+ t_start = 0.5 * l_start * curvStart;
+ a = std::sqrt(std::abs(rl));
+ sign = std::signbit(rl) ? -1.0 : 1.0;
}
-double RoadGeometrySpiral::HalfDir(double sOffset) const
+Common::Vector2d RoadGeometrySpiral::FullCoord(double sOffset, double tOffset) const
{
- double _curvStart = curvStart;
- double _curvEnd = curvEnd;
-
- assert(_curvStart != _curvEnd);
- assert((0.0 <= _curvStart && 0.0 <= _curvEnd) || (0.0 >= _curvStart && 0.0 >= _curvEnd));
-
- if (length < sOffset)
- {
- LOG_INTERN(LogLevel::Warning) << "exceeding length of geometry";
- sOffset = length;
- }
-
- if (0.0 <= _curvStart && 0.0 <= _curvEnd)
- {
- if (_curvStart < _curvEnd)
- {
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
+ // curvature of the spiral at sOffset
+ double curvAtsOffet = c_start + c_dot * sOffset;
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
+ // start and end coordinates of spiral
+ Common::Vector2d start, end;
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
+ // calculate x and y of spiral start, assuming sOffset = 0 means start of spiral with curvature curvStart
+ (void)fresnl(l_start / a / SQRT_PI, &start.y, &start.x);
+ start.Scale(a * SQRT_PI);
+ start.y *= sign;
- double distanceOffset = distanceStart + sOffset;
- double tangentAngle = distanceOffset * distanceOffset / a / a;
+ // calculate x and y of spiral end, assuming l_start + sOffset means end of spiral with curvature curvEnd
+ (void)fresnl((l_start + sOffset) / a / SQRT_PI, &end.y, &end.x);
+ end.Scale(a * SQRT_PI);
+ end.y *= sign;
- return hdg + tangentAngle;
- }
- else // _curvStart > _curvEnd ("curStart != _curvEnd" guaranteed by checks in caller)
- {
- std::swap(_curvStart, _curvEnd);
- sOffset = length - sOffset;
-
- assert(0.0 != _curvEnd);
+ // delta x, y from start of spiral to end of spiral
+ auto diff = end - start;
- double radiusEnd = 1.0 / _curvEnd;
+ // tangent angle at end of spiral
+ double t_end = (l_start + sOffset) * curvAtsOffet / 2.0;
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
+ // heading change of spiral
+ double dHdg = t_end - t_start;
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
+ // rotate delta x, y to match starting direction and origin heading
+ diff.Rotate(-t_start+hdg);
- double distanceOffset = distanceStart + sOffset;
+ // heading at end of spiral
+ auto endHdg = hdg + dHdg;
- double tangentAngle = distanceOffset * distanceOffset / a / a;
+ // calculate t-offset to spiral
+ Common::Vector2d unit{1.0,0};
+ unit.Rotate(endHdg + M_PI_2);
+ unit.Scale(tOffset);
- // compensate for inverted curvatures
- double tangentAngleEnd = distanceEnd * distanceEnd / a / a;
-
- return hdg + tangentAngleEnd - tangentAngle;
- }
- }
- else // 0.0 >= _curvStart && 0.0 >= _curvEnd
- {
- _curvStart = -_curvStart;
- _curvEnd = -_curvEnd;
-
- if (_curvStart < _curvEnd)
- {
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
-
- double distanceOffset = distanceStart + sOffset;
-
- double tangentAngle = distanceOffset * distanceOffset / a / a;
-
- return hdg - tangentAngle;
- }
- else // _curvStart > _curvEnd ("curStart != _curvEnd" guaranteed by checks in caller)
- {
- std::swap(_curvStart, _curvEnd);
- sOffset = length - sOffset;
-
- assert(0.0 != _curvEnd);
-
- double radiusEnd = 1.0 / _curvEnd;
-
- // 1. equation (definition of clothoid): const = radiusStart * distanceStart = radiusEnd * distanceEnd
- // 2. equation: length = distanceEnd - distanceStart
- // -> distanceEnd = radiusStart * length / (radiusStart - radiusEnd)
- // -> formed such that equation copes with _curvStart==0 (infinite radiusStart)
- double distanceEnd = length / (1 - radiusEnd * _curvStart);
- assert(length <= distanceEnd);
-
- double distanceStart = distanceEnd - length;
- double a = std::sqrt(2 * radiusEnd * distanceEnd);
-
- double distanceOffset = distanceStart + sOffset;
-
- double tangentAngle = distanceOffset * distanceOffset / a / a;
-
- // compensate for inverted curvatures
- double tangentAngleEnd = distanceEnd * distanceEnd / a / a;
+ return diff + unit + Common::Vector2d(x, y);
+}
- return hdg - (tangentAngleEnd - tangentAngle);
- }
- }
+double RoadGeometrySpiral::FullCurvature(double sOffset) const
+{
+ return c_start + c_dot * sOffset;
}
double RoadGeometrySpiral::FullDir(double sOffset) const
{
- if ((0.0 <= curvStart && 0.0 <= curvEnd) || (0.0 >= curvStart && 0.0 >= curvEnd))
- {
- return HalfDir(sOffset);
- }
-
- assert((0.0 > curvStart && 0.0 < curvEnd) || (0.0 < curvStart && 0.0 > curvEnd));
-
- // one degree of freedom: start position/end position can not be determined
- LOG_INTERN(LogLevel::Warning) << "could not calculate spiral curvature";
+ // tangent_angle = L / (2 * R) = 0.5 * L * curv
+ // direction change in spiral = tangent_end - tangent_start
- return 0.0;
+ double curvEnd = FullCurvature(sOffset);
+ return hdg + 0.5 * (curvEnd * (l_start + sOffset) - c_start * l_start);
}
Common::Vector2d RoadGeometrySpiral::GetCoord(double sOffset, double tOffset) const
{
- if (0.0 == curvStart && 0.0 == curvEnd)
+ if (0.0 == c_start && 0.0 == c_end)
{
return GetCoordLine(sOffset, tOffset);
}
- if (std::abs(curvStart - curvEnd) < 1e-6 /* assumed to be equal */)
+ if (std::abs(c_start - c_end) < 1e-6 /* assumed to be equal */)
{
- return GetCoordArc(sOffset, tOffset, curvStart);
+ return GetCoordArc(sOffset, tOffset, c_start);
}
return FullCoord(sOffset, tOffset);
@@ -762,14 +337,14 @@ Common::Vector2d RoadGeometrySpiral::GetCoord(double sOffset, double tOffset) co
double RoadGeometrySpiral::GetDir(double sOffset) const
{
- if (0.0 == curvStart && 0.0 == curvEnd)
+ if (0.0 == c_start && 0.0 == c_end)
{
return GetDirLine(sOffset);
}
- if (std::abs(curvStart - curvEnd) < 1e-6 /* assumed to be equal */)
+ if (std::abs(c_start - c_end) < 1e-6 /* assumed to be equal */)
{
- return GetDirArc(sOffset, curvStart);
+ return GetDirArc(sOffset, c_start);
}
return FullDir(sOffset);
diff --git a/sim/src/core/slave/importer/road.h b/sim/src/core/slave/importer/road.h
index 4e6af4508ffdda4b72317d7642ad9638d9e367dc..483a3bb7160d51aaee79977bdee896a6d5092c29 100644
--- a/sim/src/core/slave/importer/road.h
+++ b/sim/src/core/slave/importer/road.h
@@ -586,97 +586,46 @@ class RoadGeometryArc : public RoadGeometry
//-----------------------------------------------------------------------------
class RoadGeometrySpiral : public RoadGeometry
{
- public:
- RoadGeometrySpiral(double s, double x, double y, double hdg, double length, double curvStart, double curvEnd)
- : RoadGeometry{s, x, y, hdg, length}, curvStart{curvStart}, curvEnd{curvEnd}
- {
- }
+public:
+ RoadGeometrySpiral(double s, double x, double y, double hdg, double length, double curvStart, double curvEnd);
virtual ~RoadGeometrySpiral() override = default;
virtual Common::Vector2d GetCoord(double sOffset, double tOffset) const override;
virtual double GetDir(double sOffset) const override;
+private:
//-----------------------------------------------------------------------------
- //! Returns the curvature at the start of the spiral.
- //!
- //! @return curvature at the start of the spiral
- //-----------------------------------------------------------------------------
- double GetCurvStart() const
- {
- return curvStart;
- }
-
- //-----------------------------------------------------------------------------
- //! Returns the curvature at the end of the spiral.
- //!
- //! @return curvature at the end of the spiral
- //-----------------------------------------------------------------------------
- double GetCurvEnd() const
- {
- return curvEnd;
- }
-
- private:
- //-----------------------------------------------------------------------------
- //! Calculates the x/y coordinates as vector. Only valid if the start and end
- //! curvature of the spiral are either both positive or negative.
+ //! Calculates the x/y coordinates as vector.
//!
//! @param[in] sOffset s offset within geometry section
//! @param[in] tOffset offset to the left
//! @return vector with the x/y coordinates
//-----------------------------------------------------------------------------
- Common::Vector2d HalfCoord(double sOffset, double tOffset) const;
-
- //-----------------------------------------------------------------------------
- //! Calculates the x/y coordinates as vector. Returns an empty vector, if start
- //! and end curvature of the spiral have different signs.
- //!
- //! @param[in] sOffset s offset within geometry section
- //! @param[in] tOffset offset to the left
- //! @return vector with the x/y coordinates, empty vector,
- //! if start and end curvature of the spiral have
- //! different signs
- //-----------------------------------------------------------------------------
Common::Vector2d FullCoord(double sOffset, double tOffset) const;
//-----------------------------------------------------------------------------
- //! Calculates the curvature. Only valid if the start and end
- //! curvature of the spiral are either both positive or negative.
+ //! Calculates the curvature.
//!
//! @param[in] sOffset s offset within geometry section
//! @return curvature
//-----------------------------------------------------------------------------
- double HalfCurvature(double sOffset) const;
-
- //-----------------------------------------------------------------------------
- //! Calculates the curvature. Returns 0.0, if start
- //! and end curvature of the spiral have different signs.
- //!
- //! @param[in] sOffset s offset within geometry section
- //! @return curvature, 0.0, if start and end curvature
- //! of the spiral have different signs
- //-----------------------------------------------------------------------------
double FullCurvature(double sOffset) const;
//-----------------------------------------------------------------------------
- //! Calculates the direction. Only valid if the start and end
- //! curvature of the spiral are either both positive or negative.
- //!
- //! @param[in] sOffset s offset within geometry section
- //! @return direction
- //-----------------------------------------------------------------------------
- double HalfDir(double sOffset) const;
-
- //-----------------------------------------------------------------------------
- //! Calculates the direction. Returns 0.0, if start
- //! and end curvature of the spiral have different signs.
+ //! Calculates the direction.
//!
//! @param[in] sOffset s offset within geometry section
//! @return direction
//-----------------------------------------------------------------------------
double FullDir(double sOffset) const;
- double curvStart;
- double curvEnd;
+
+ double c_start; //!< spiral starting curvature
+ double c_end; //!< spiral end curvature
+ double a; //!< clothoid parameter of spiral
+ double sign; //!< direction of curvature change (needes to correct Fresnel integral results)
+ double c_dot; //!< change of curvature per unit
+ double l_start; //!< offset of starting point along spiral
+ double t_start; //!< tangent angle at start point
};
//-----------------------------------------------------------------------------
diff --git a/sim/src/core/slave/modules/Observation_Log/observationLogConstants.h b/sim/src/core/slave/modules/Observation_Log/observationLogConstants.h
index f21e1f3529e113c05e907ff32229b5b67442a8ec..c0e17f75f5b2e81617bb8339b45296de3e40f77a 100644
--- a/sim/src/core/slave/modules/Observation_Log/observationLogConstants.h
+++ b/sim/src/core/slave/modules/Observation_Log/observationLogConstants.h
@@ -65,7 +65,7 @@ struct OutputTags
const QString SAMPLE = "Sample";
const QString SCENERYFILE = "SceneryFile";
const QString VEHICLEATTRIBUTES = "VehicleAttributes";
- const QString TRIGGERINGENTITIES = "openpass::type::TriggeringEntities";
+ const QString TRIGGERINGENTITIES = "TriggeringEntities";
const QString AFFECTEDENTITIES = "AffectedEntities";
};
diff --git a/sim/src/core/slave/modules/World_OSI/RoutePlanning/RouteCalculation.h b/sim/src/core/slave/modules/World_OSI/RoutePlanning/RouteCalculation.h
index 6ecd994608ce7cca671b647da17d0ec09b862aa6..8513189d1d4f4e97d104b03719e2f6377ca1e5b1 100644
--- a/sim/src/core/slave/modules/World_OSI/RoutePlanning/RouteCalculation.h
+++ b/sim/src/core/slave/modules/World_OSI/RoutePlanning/RouteCalculation.h
@@ -15,7 +15,7 @@
namespace RouteCalculation
{
- RoadGraphVertex FilterRoadGraphByStartPositionRecursive (const RoadGraph& roadGraph, RoadGraphVertex current, int maxDepth, RoadGraph& filteredGraph)
+ RoadGraphVertex FilterRoadGraphByStartPositionRecursive(const RoadGraph& roadGraph, RoadGraphVertex current, int maxDepth, RoadGraph& filteredGraph)
{
const auto& routeElement = get(RouteElement(), roadGraph, current);
auto newVertex = add_vertex(routeElement, filteredGraph);
@@ -30,20 +30,32 @@ namespace RouteCalculation
return newVertex;
}
- std::pair<RoadGraph, RoadGraphVertex> FilterRoadGraphByStartPosition (const RoadGraph& roadGraph, RoadGraphVertex start, int maxDepth)
+ //! Returns the road graph as a tree with defined maximum depth relative to a given start position.
+ //! The same route element can appear multiple times in the result, if there are multiple paths to it from the start position
+ //!
+ //! \param roadGraph entire road network
+ //! \param start start position in the network
+ //! \param maxDepth maximum depth of resulting tree
+ //! \return road network as tree with given root
+ std::pair<RoadGraph, RoadGraphVertex> FilterRoadGraphByStartPosition(const RoadGraph& roadGraph, RoadGraphVertex start, int maxDepth)
{
RoadGraph filteredGraph;
auto root = FilterRoadGraphByStartPositionRecursive(roadGraph, start, maxDepth, filteredGraph);
return {filteredGraph, root};
}
- std::pair<RoadGraph, RoadGraphVertex> SampleRoute (const RoadGraph& roadGraph,
- RoadGraphVertex root,
- const std::map<RoadGraphEdge, double>& weights,
- StochasticsInterface& stochastics)
+ //! Random draws a target leaf in the given road graph tree based on the propability of each edge
+ //!
+ //! \param roadGraph tree of road network starting at the agents current position
+ //! \param root root vertex of the roadGraph
+ //! \param weights weight map of all edges of the graph
+ //! \param stochastics stochastics module
+ //! \return sampled target vertex
+ RoadGraphVertex SampleRoute(const RoadGraph& roadGraph,
+ RoadGraphVertex root,
+ const std::map<RoadGraphEdge, double>& weights,
+ StochasticsInterface& stochastics)
{
- RoadGraph route;
- auto firstVertex = add_vertex(get(RouteElement(), roadGraph, root), route);
auto current = root;
bool reachedEnd = false;
while (!reachedEnd)
@@ -65,14 +77,11 @@ namespace RouteCalculation
probalitySum += weights.at(*successor);
if (roll <= probalitySum)
{
- auto secondVertex = add_vertex(get(RouteElement(), roadGraph, target(*successor, roadGraph)), route);
- add_edge(firstVertex, secondVertex, route);
current = target(*successor, roadGraph);
- firstVertex = secondVertex;
break;
}
}
}
- return {route, firstVertex};
+ return current;
}
}
diff --git a/sim/src/sim.pro b/sim/src/sim.pro
index cd5b7f3f51535e1a5585ddd159b262753ca28467..0d8026b0bec122126642102cb7f2b4f99218af1c 100644
--- a/sim/src/sim.pro
+++ b/sim/src/sim.pro
@@ -44,7 +44,8 @@ SUBDIRS = \
components/Sensor_Driver \
components/Sensor_OSI \
components/Sensor_RecordState \
- components/SensorFusion_OSI \
+ components/SensorAggregation_OSI \
+ components/SensorFusionErrorless_OSI \
components/SignalPrioritizer
slave.file = core/slave/OpenPassSlave.pro
diff --git a/sim/tests/unitTests/CMakeLists.txt b/sim/tests/unitTests/CMakeLists.txt
index 7c9c91f6eb31589a45d711d40cc7bb45c50ccff7..8450ff6fdfddcf2c88f68a818ee12308ef4e9fad 100644
--- a/sim/tests/unitTests/CMakeLists.txt
+++ b/sim/tests/unitTests/CMakeLists.txt
@@ -10,7 +10,8 @@ add_subdirectory(components/Dynamics_Collision)
add_subdirectory(components/Dynamics_TF)
add_subdirectory(components/LimiterAccVehComp)
add_subdirectory(components/OpenScenarioActions)
-add_subdirectory(components/SensorFusion_OSI)
+add_subdirectory(components/SensorAggregation_OSI)
+add_subdirectory(components/SensorFusionErrorless_OSI)
add_subdirectory(components/Sensor_Driver)
add_subdirectory(components/Sensor_OSI)
add_subdirectory(components/SignalPrioritizer)
diff --git a/sim/tests/unitTests/common/CMakeLists.txt b/sim/tests/unitTests/common/CMakeLists.txt
index ac93f696e095d1da82f7cf0e645d1e5f7515b91e..c3d7fdc9aad957b4056a7a1bbaee466eacf2fb97 100644
--- a/sim/tests/unitTests/common/CMakeLists.txt
+++ b/sim/tests/unitTests/common/CMakeLists.txt
@@ -7,7 +7,10 @@ add_openpass_target(
LINK_OSI
SOURCES
+ commonHelper_Tests.cpp
ttcCalculation_Tests.cpp
+ tokenizeString_Tests.cpp
+ vectorToString_Tests.cpp
HEADERS
${COMPONENT_SOURCE_DIR}/commonTools.h
diff --git a/sim/tests/unitTests/common/Common_Tests.pro b/sim/tests/unitTests/common/Common_Tests.pro
index 25d0ab58b786fa7509f3ad24778b61bee1dfa09f..a48f5c361ad0f5ba209c88b24c59b682b5eccd48 100644
--- a/sim/tests/unitTests/common/Common_Tests.pro
+++ b/sim/tests/unitTests/common/Common_Tests.pro
@@ -29,6 +29,7 @@ HEADERS += \
$$UNIT_UNDER_TEST/commonTools.h
SOURCES += \
+ commonHelper_Tests.cpp \
tokenizeString_Tests.cpp \
ttcCalculation_Tests.cpp \
vectorToString_Tests.cpp
diff --git a/sim/tests/unitTests/common/commonHelper_Tests.cpp b/sim/tests/unitTests/common/commonHelper_Tests.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fde532a4241efc3345f8b75b9b3b63b3d8d5f19e
--- /dev/null
+++ b/sim/tests/unitTests/common/commonHelper_Tests.cpp
@@ -0,0 +1,48 @@
+# /*********************************************************************
+# * Copyright (c) 2020 in-tech GmbH
+# *
+# * 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 "common/commonTools.h"
+
+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+
+using ::testing::TestWithParam;
+using ::testing::Values;
+using ::testing::Return;
+using ::testing::DoubleNear;
+
+struct CartesianNetDistance_Data
+{
+ polygon_t ownBoundingBox;
+ polygon_t otherBoundingBox;
+ double expectedNetX;
+ double expectedNetY;
+};
+
+class CartesianNetDistanceTest: public ::TestWithParam<CartesianNetDistance_Data>
+{
+};
+
+TEST_P(CartesianNetDistanceTest, GetCartesianNetDistance_ReturnsCorrectDistances)
+{
+ auto data = GetParam();
+ auto [netX, netY] = CommonHelper::GetCartesianNetDistance(data.ownBoundingBox, data.otherBoundingBox);
+
+ ASSERT_THAT(netX, DoubleNear(data.expectedNetX, 1e-3));
+ ASSERT_THAT(netY, DoubleNear(data.expectedNetY, 1e-3));
+}
+
+INSTANTIATE_TEST_CASE_P(CartesianNetDistanceTestCase, CartesianNetDistanceTest, ::testing::Values(
+// ownBoundingBox otherBoundingBox x y
+ CartesianNetDistance_Data{polygon_t{{{0,0},{1,0},{1,1},{0,1}}}, polygon_t{{{1,0},{2,0},{2,1},{1,1}}}, 0,0},
+ CartesianNetDistance_Data{polygon_t{{{-1,0},{0,-1},{1,0},{0,1}}},polygon_t{{{1,1},{2,1},{2,2},{1,2}}}, 0,0},
+ CartesianNetDistance_Data{polygon_t{{{-1,0},{0,-1},{1,0},{0,1}}},polygon_t{{{3,4},{4,4},{4,6},{3,6}}}, 2,3},
+ CartesianNetDistance_Data{polygon_t{{{-1,0},{0,-1},{1,0},{0,1}}},polygon_t{{{-10,-10},{-8,-10},{-8,-9},{-10,-9}}},-7,-8}
+));
diff --git a/sim/tests/unitTests/components/SensorAggregation_OSI/CMakeLists.txt b/sim/tests/unitTests/components/SensorAggregation_OSI/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dac9fd9eb98f9841ed5161c3d98a950c55a78026
--- /dev/null
+++ b/sim/tests/unitTests/components/SensorAggregation_OSI/CMakeLists.txt
@@ -0,0 +1,22 @@
+set(COMPONENT_TEST_NAME SensorAggregationOSI_Tests)
+set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/components/SensorAggregation_OSI/src)
+
+add_openpass_target(
+ NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT core
+ DEFAULT_MAIN
+ LINKOSI
+
+ SOURCES
+ sensorAggregationOSI_Tests.cpp
+ ${COMPONENT_SOURCE_DIR}/sensorAggregationImpl.cpp
+
+ HEADERS
+ ${COMPONENT_SOURCE_DIR}/sensorAggregationImpl.h
+
+ INCDIRS
+ ${COMPONENT_SOURCE_DIR}
+
+ LIBRARIES
+ Qt5::Core
+)
+
diff --git a/sim/tests/unitTests/components/SensorFusion_OSI/sensorFusionOSI_Tests.cpp b/sim/tests/unitTests/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.cpp
similarity index 93%
rename from sim/tests/unitTests/components/SensorFusion_OSI/sensorFusionOSI_Tests.cpp
rename to sim/tests/unitTests/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.cpp
index 561ef51b1aca3320225b2b7be9cf31c756826899..cd430e0c9599aa521116e41f99874a290875b49c 100644
--- a/sim/tests/unitTests/components/SensorFusion_OSI/sensorFusionOSI_Tests.cpp
+++ b/sim/tests/unitTests/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.cpp
@@ -1,5 +1,5 @@
/*******************************************************************************
-* Copyright (c) 2019 in-tech GmbH
+* Copyright (c) 2019, 2020 in-tech GmbH
*
* This program and the accompanying materials are made
* available under the terms of the Eclipse Public License 2.0
@@ -13,7 +13,7 @@
#include "fakeAgent.h"
-#include "sensorFusionImpl.h"
+#include "sensorAggregationImpl.h"
#include "common/sensorDataSignal.h"
#include "osi3/osi_sensordata.pb.h"
@@ -21,7 +21,7 @@ using ::testing::Eq;
using ::testing::NiceMock;
using ::testing::Return;
-TEST(SensorFusionOSI_Unittest, TestAppendingDetectedObjectsWithinTheSameTimestamp)
+TEST(SensorAggregationOSI_Unittest, TestAppendingDetectedObjectsWithinTheSameTimestamp)
{
osi3::SensorData sensorData1;
auto movingObject1 = sensorData1.add_moving_object();
@@ -42,7 +42,7 @@ TEST(SensorFusionOSI_Unittest, TestAppendingDetectedObjectsWithinTheSameTimestam
NiceMock<FakeAgent> fakeAgent;
ON_CALL(fakeAgent, GetId()).WillByDefault(Return(0));
- SensorFusionImplementation sensorFusion("",
+ SensorAggregationImplementation sensorFusion("",
false,
0,
0,
@@ -74,7 +74,7 @@ TEST(SensorFusionOSI_Unittest, TestAppendingDetectedObjectsWithinTheSameTimestam
}
-TEST(SensorFusionOSI_Unittest, TestResettingDetectedObjectsInNewTimestamp)
+TEST(SensorAggregationOSI_Unittest, TestResettingDetectedObjectsInNewTimestamp)
{
osi3::SensorData sensorData1;
auto movingObject1 = sensorData1.add_moving_object();
@@ -95,7 +95,7 @@ TEST(SensorFusionOSI_Unittest, TestResettingDetectedObjectsInNewTimestamp)
NiceMock<FakeAgent> fakeAgent;
ON_CALL(fakeAgent, GetId()).WillByDefault(Return(0));
- SensorFusionImplementation sensorFusion("",
+ SensorAggregationImplementation sensorFusion("",
false,
0,
0,
diff --git a/sim/tests/unitTests/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.pro b/sim/tests/unitTests/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.pro
new file mode 100644
index 0000000000000000000000000000000000000000..b1802b849b9891d3972ae6775579c0d1e6b7d1a1
--- /dev/null
+++ b/sim/tests/unitTests/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.pro
@@ -0,0 +1,32 @@
+# /*********************************************************************
+# * Copyright (c) 2019 in-tech GmbH
+# *
+# * 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
+# **********************************************************************/
+
+CONFIG += OPENPASS_GTEST \
+ OPENPASS_GTEST_DEFAULT_MAIN
+
+include(../../../testing.pri)
+
+UNIT_UNDER_TEST = $$OPEN_SRC/components/SensorAggregation_OSI/src
+
+INCLUDEPATH += \
+ . \
+ $$UNIT_UNDER_TEST \
+ ../../../..
+
+HEADERS += \
+ $$UNIT_UNDER_TEST/sensorAggregationImpl.h
+
+SOURCES += \
+ $$UNIT_UNDER_TEST/sensorAggregationImpl.cpp \
+ sensorAggregationOSI_Tests.cpp
+
+LIBS += \
+ -lopen_simulation_interface \
+ -lprotobuf
diff --git a/sim/tests/unitTests/components/SensorFusion_OSI/CMakeLists.txt b/sim/tests/unitTests/components/SensorFusionErrorless_OSI/CMakeLists.txt
similarity index 71%
rename from sim/tests/unitTests/components/SensorFusion_OSI/CMakeLists.txt
rename to sim/tests/unitTests/components/SensorFusionErrorless_OSI/CMakeLists.txt
index 7cdddcd481c7d45f8267cda10b13802199ec3018..44dc34edc9fcc1bb6a92c20ff532e78118f9c43a 100644
--- a/sim/tests/unitTests/components/SensorFusion_OSI/CMakeLists.txt
+++ b/sim/tests/unitTests/components/SensorFusionErrorless_OSI/CMakeLists.txt
@@ -1,23 +1,22 @@
-set(COMPONENT_TEST_NAME SensorFusionOSI_Tests)
-set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/components/SensorFusion_OSI/src)
+set(COMPONENT_TEST_NAME SensorFusionErrorless_Tests)
+set(COMPONENT_SOURCE_DIR ${OPENPASS_SIMCORE_DIR}/components/SensorFusionErrorless_OSI/src)
add_openpass_target(
NAME ${COMPONENT_TEST_NAME} TYPE test COMPONENT core
DEFAULT_MAIN
- LINKOSI
SOURCES
- sensorFusionOSI_Tests.cpp
+ sensorFusionErrorless_Tests.cpp
${COMPONENT_SOURCE_DIR}/sensorFusionImpl.cpp
HEADERS
${COMPONENT_SOURCE_DIR}/sensorFusionImpl.h
- ${COMPONENT_SOURCE_DIR}/sensorFusionQuery.h
INCDIRS
${COMPONENT_SOURCE_DIR}
LIBRARIES
Qt5::Core
-)
+ LINKOSI
+)
diff --git a/sim/tests/unitTests/components/SensorFusionErrorless_OSI/sensorFusionErrorless_Tests.cpp b/sim/tests/unitTests/components/SensorFusionErrorless_OSI/sensorFusionErrorless_Tests.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1d06c830882b9d59d735b6334e283cbf1d7b05ee
--- /dev/null
+++ b/sim/tests/unitTests/components/SensorFusionErrorless_OSI/sensorFusionErrorless_Tests.cpp
@@ -0,0 +1,168 @@
+/*******************************************************************************
+* Copyright (c) 2020 in-tech GmbH
+*
+* 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 "sensorFusionImpl.h"
+
+using ::testing::Eq;
+
+TEST(SensorFusionErrorless_Tests, SensorDataWithMovingObjects_IsMergedAppropriately)
+{
+ FakeAgent fakeAgent;
+
+ auto sensorFusion = SensorFusionErrorlessImplementation("",
+ false,
+ 0,
+ 0,
+ 0,
+ 100,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ &fakeAgent);
+ unsigned int idA = 7;
+ unsigned int idB = 8;
+ unsigned int idC = 9;
+
+ unsigned int idSensor1 = 101;
+ unsigned int idSensor2 = 102;
+
+ osi3::SensorData sensorData;
+ auto movingObject1a = sensorData.add_moving_object();
+ movingObject1a->mutable_header()->add_ground_truth_id()->set_value(idA);
+ movingObject1a->mutable_header()->add_sensor_id()->set_value(idSensor1);
+ movingObject1a->mutable_base()->mutable_position()->set_x(10);
+ movingObject1a->mutable_base()->mutable_position()->set_y(11);
+ auto movingObject1b = sensorData.add_moving_object();
+ movingObject1b->mutable_header()->add_ground_truth_id()->set_value(idB);
+ movingObject1b->mutable_header()->add_sensor_id()->set_value(idSensor1);
+ movingObject1b->mutable_base()->mutable_position()->set_x(20);
+ movingObject1b->mutable_base()->mutable_position()->set_y(21);
+
+ auto movingObject2b = sensorData.add_moving_object();
+ movingObject2b->mutable_header()->add_ground_truth_id()->set_value(idB);
+ movingObject2b->mutable_header()->add_sensor_id()->set_value(idSensor2);
+ movingObject2b->mutable_base()->mutable_position()->set_x(20);
+ movingObject2b->mutable_base()->mutable_position()->set_y(21);
+ auto movingObject2c = sensorData.add_moving_object();
+ movingObject2c->mutable_header()->add_ground_truth_id()->set_value(idC);
+ movingObject2c->mutable_header()->add_sensor_id()->set_value(idSensor2);
+ movingObject2c->mutable_base()->mutable_position()->set_x(30);
+ movingObject2c->mutable_base()->mutable_position()->set_y(31);
+
+ auto signal = std::make_shared<SensorDataSignal>(sensorData);
+ sensorFusion.UpdateInput(0, signal, 0);
+
+ sensorFusion.Trigger(0);
+
+ std::shared_ptr<const SignalInterface> output;
+ sensorFusion.UpdateOutput(0, output, 0);
+ auto outSensorDataSignal = std::dynamic_pointer_cast<const SensorDataSignal>(output);
+ auto outSensorData = outSensorDataSignal->sensorData;
+
+ ASSERT_THAT(outSensorData.moving_object_size(), Eq(3));
+ ASSERT_THAT(outSensorData.moving_object(0).header().ground_truth_id(0).value(), Eq(idA));
+ ASSERT_THAT(outSensorData.moving_object(0).header().sensor_id_size(), Eq(1));
+ ASSERT_THAT(outSensorData.moving_object(0).header().sensor_id(0).value(), Eq(idSensor1));
+ ASSERT_THAT(outSensorData.moving_object(0).base().position().x(), Eq(10));
+ ASSERT_THAT(outSensorData.moving_object(0).base().position().y(), Eq(11));
+ ASSERT_THAT(outSensorData.moving_object(1).header().ground_truth_id(0).value(), Eq(idB));
+ ASSERT_THAT(outSensorData.moving_object(1).header().sensor_id_size(), Eq(2));
+ ASSERT_THAT(outSensorData.moving_object(1).header().sensor_id(0).value(), Eq(idSensor1));
+ ASSERT_THAT(outSensorData.moving_object(1).header().sensor_id(1).value(), Eq(idSensor2));
+ ASSERT_THAT(outSensorData.moving_object(1).base().position().x(), Eq(20));
+ ASSERT_THAT(outSensorData.moving_object(1).base().position().y(), Eq(21));
+ ASSERT_THAT(outSensorData.moving_object(2).header().ground_truth_id(0).value(), Eq(idC));
+ ASSERT_THAT(outSensorData.moving_object(2).header().sensor_id_size(), Eq(1));
+ ASSERT_THAT(outSensorData.moving_object(2).header().sensor_id(0).value(), Eq(idSensor2));
+ ASSERT_THAT(outSensorData.moving_object(2).base().position().x(), Eq(30));
+ ASSERT_THAT(outSensorData.moving_object(2).base().position().y(), Eq(31));
+}
+
+TEST(SensorFusionErrorless_Tests, SensorDataWithStationaryObjects_IsMergedAppropriately)
+{
+ FakeAgent fakeAgent;
+
+ auto sensorFusion = SensorFusionErrorlessImplementation("",
+ false,
+ 0,
+ 0,
+ 0,
+ 100,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ &fakeAgent);
+ unsigned int idA = 7;
+ unsigned int idB = 8;
+ unsigned int idC = 9;
+
+ unsigned int idSensor1 = 101;
+ unsigned int idSensor2 = 102;
+
+ osi3::SensorData sensorData;
+ auto stationaryObject1a = sensorData.add_stationary_object();
+ stationaryObject1a->mutable_header()->add_ground_truth_id()->set_value(idA);
+ stationaryObject1a->mutable_header()->add_sensor_id()->set_value(idSensor1);
+ stationaryObject1a->mutable_base()->mutable_position()->set_x(10);
+ stationaryObject1a->mutable_base()->mutable_position()->set_y(11);
+ auto stationaryObject1b = sensorData.add_stationary_object();
+ stationaryObject1b->mutable_header()->add_ground_truth_id()->set_value(idB);
+ stationaryObject1b->mutable_header()->add_sensor_id()->set_value(idSensor1);
+ stationaryObject1b->mutable_base()->mutable_position()->set_x(20);
+ stationaryObject1b->mutable_base()->mutable_position()->set_y(21);
+
+ auto stationaryObject2b = sensorData.add_stationary_object();
+ stationaryObject2b->mutable_header()->add_ground_truth_id()->set_value(idB);
+ stationaryObject2b->mutable_header()->add_sensor_id()->set_value(idSensor2);
+ stationaryObject2b->mutable_base()->mutable_position()->set_x(20);
+ stationaryObject2b->mutable_base()->mutable_position()->set_y(21);
+ auto stationaryObject2c = sensorData.add_stationary_object();
+ stationaryObject2c->mutable_header()->add_ground_truth_id()->set_value(idC);
+ stationaryObject2c->mutable_header()->add_sensor_id()->set_value(idSensor2);
+ stationaryObject2c->mutable_base()->mutable_position()->set_x(30);
+ stationaryObject2c->mutable_base()->mutable_position()->set_y(31);
+
+ auto signal = std::make_shared<SensorDataSignal>(sensorData);
+ sensorFusion.UpdateInput(0, signal, 0);
+
+ sensorFusion.Trigger(0);
+
+ std::shared_ptr<const SignalInterface> output;
+ sensorFusion.UpdateOutput(0, output, 0);
+ auto outSensorDataSignal = std::dynamic_pointer_cast<const SensorDataSignal>(output);
+ auto outSensorData = outSensorDataSignal->sensorData;
+
+ ASSERT_THAT(outSensorData.stationary_object_size(), Eq(3));
+ ASSERT_THAT(outSensorData.stationary_object(0).header().ground_truth_id(0).value(), Eq(idA));
+ ASSERT_THAT(outSensorData.stationary_object(0).header().sensor_id_size(), Eq(1));
+ ASSERT_THAT(outSensorData.stationary_object(0).header().sensor_id(0).value(), Eq(idSensor1));
+ ASSERT_THAT(outSensorData.stationary_object(0).base().position().x(), Eq(10));
+ ASSERT_THAT(outSensorData.stationary_object(0).base().position().y(), Eq(11));
+ ASSERT_THAT(outSensorData.stationary_object(1).header().ground_truth_id(0).value(), Eq(idB));
+ ASSERT_THAT(outSensorData.stationary_object(1).header().sensor_id_size(), Eq(2));
+ ASSERT_THAT(outSensorData.stationary_object(1).header().sensor_id(0).value(), Eq(idSensor1));
+ ASSERT_THAT(outSensorData.stationary_object(1).header().sensor_id(1).value(), Eq(idSensor2));
+ ASSERT_THAT(outSensorData.stationary_object(1).base().position().x(), Eq(20));
+ ASSERT_THAT(outSensorData.stationary_object(1).base().position().y(), Eq(21));
+ ASSERT_THAT(outSensorData.stationary_object(2).header().ground_truth_id(0).value(), Eq(idC));
+ ASSERT_THAT(outSensorData.stationary_object(2).header().sensor_id_size(), Eq(1));
+ ASSERT_THAT(outSensorData.stationary_object(2).header().sensor_id(0).value(), Eq(idSensor2));
+ ASSERT_THAT(outSensorData.stationary_object(2).base().position().x(), Eq(30));
+ ASSERT_THAT(outSensorData.stationary_object(2).base().position().y(), Eq(31));
+}
diff --git a/sim/tests/unitTests/components/SensorFusion_OSI/sensorFusionOSI_Tests.pro b/sim/tests/unitTests/components/SensorFusionErrorless_OSI/sensorFusionErrorless_Tests.pro
similarity index 87%
rename from sim/tests/unitTests/components/SensorFusion_OSI/sensorFusionOSI_Tests.pro
rename to sim/tests/unitTests/components/SensorFusionErrorless_OSI/sensorFusionErrorless_Tests.pro
index e5f854c80600acead5153fc887e5a2e97b6d8e34..2385b171e0d95076dbd34f7e1c34f31099adf329 100644
--- a/sim/tests/unitTests/components/SensorFusion_OSI/sensorFusionOSI_Tests.pro
+++ b/sim/tests/unitTests/components/SensorFusionErrorless_OSI/sensorFusionErrorless_Tests.pro
@@ -13,7 +13,7 @@ CONFIG += OPENPASS_GTEST \
include(../../../testing.pri)
-UNIT_UNDER_TEST = $$OPEN_SRC/components/SensorFusion_OSI/src
+UNIT_UNDER_TEST = $$OPEN_SRC/components/SensorFusionErrorless_OSI/src
INCLUDEPATH += \
. \
@@ -25,7 +25,7 @@ HEADERS += \
SOURCES += \
$$UNIT_UNDER_TEST/sensorFusionImpl.cpp \
- sensorFusionOSI_Tests.cpp
+ sensorFusionErrorless_Tests.cpp
LIBS += \
-lopen_simulation_interface \
diff --git a/sim/tests/unitTests/components/Sensor_OSI/CMakeLists.txt b/sim/tests/unitTests/components/Sensor_OSI/CMakeLists.txt
index 64039a1d58ee5239d49dbba82b88203b177e6c16..c696684a1a07fd0fcf073c565c267121d228ebca 100644
--- a/sim/tests/unitTests/components/Sensor_OSI/CMakeLists.txt
+++ b/sim/tests/unitTests/components/Sensor_OSI/CMakeLists.txt
@@ -18,6 +18,7 @@ add_openpass_target(
HEADERS
sensorOSI_Tests.h
+ sensorOSI_TestsCommon.h
${COMPONENT_SOURCE_DIR}/objectDetectorBase.h
${COMPONENT_SOURCE_DIR}/sensorGeometric2D.h
${OPENPASS_SIMCORE_DIR}/core/slave/modules/World_OSI/OWL/DataTypes.h
diff --git a/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_Tests.cpp b/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_Tests.cpp
index 6857000dd58e7219850633982f09452fd8c472ef..a638ab79544e7737f60e022d1c64ac7329de2211 100644
--- a/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_Tests.cpp
+++ b/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_Tests.cpp
@@ -21,7 +21,7 @@
#include "fakeWorldData.h"
#include "sensorGeometric2D.h"
-
+#include "sensorOSI_TestsCommon.h"
#include "common/boostGeometryCommon.h"
using ::testing::_;
@@ -33,131 +33,6 @@ using ::testing::ReturnRef;
using ::testing::StrEq;
using ::testing::Ne;
-double constexpr EPSILON = 1e-9;
-
-bool IsEqual(double lhs, double rhs)
-{
- return std::abs(lhs - rhs) < EPSILON;
-}
-
-class MovingObjectParameter
-{
-public:
- MovingObjectParameter(unsigned int id,
- Common::Vector2d position,
- Common::Vector2d velocity,
- Common::Vector2d acceleration,
- double yaw) :
- id(id),
- position(position),
- velocity(velocity),
- acceleration(acceleration),
- yaw(yaw)
- {}
-
- MovingObjectParameter (const osi3::DetectedMovingObject& movingObject)
- {
- id = movingObject.header().ground_truth_id(0).value();
- position = Common::Vector2d(movingObject.base().position().x(), movingObject.base().position().y());
- velocity = Common::Vector2d(movingObject.base().velocity().x(), movingObject.base().velocity().y());
- acceleration = Common::Vector2d(movingObject.base().acceleration().x(), movingObject.base().acceleration().y());
- yaw = movingObject.base().orientation().yaw();
- }
-
- bool operator==(const MovingObjectParameter& rhs) const
- {
- if (id != rhs.id)
- {
- return false;
- }
-
- if (position != rhs.position ||
- velocity != rhs.velocity ||
- acceleration != rhs.acceleration)
- {
- return false;
- }
-
- if (!IsEqual(yaw, rhs.yaw))
- {
- return false;
- }
-
- return true;
- }
-
- /// \brief This stream will be shown in case the test fails
- friend std::ostream& operator<<(std::ostream& os, const MovingObjectParameter& obj)
- {
- os << "id: " << obj.id << ", "
- << "position: (" << obj.position.x << ", " << obj.position.y << "), "
- << "velocity: (" << obj.velocity.x << ", " << obj.velocity.y << "), "
- << "acceleration: (" << obj.acceleration.x << ", " << obj.acceleration.y << "), "
- << "yaw: " << obj.yaw;
-
- return os;
- }
-
- unsigned int id;
- Common::Vector2d position;
- Common::Vector2d velocity;
- Common::Vector2d acceleration;
- double yaw;
-};
-
-class StationaryObjectParameter
-{
-public:
- StationaryObjectParameter(unsigned int id,
- Common::Vector2d position,
- double yaw) :
- id(id),
- position(position),
- yaw(yaw)
- {}
-
- StationaryObjectParameter (const osi3::DetectedStationaryObject& stationaryObject)
- {
- id = stationaryObject.header().ground_truth_id(0).value();
- position = Common::Vector2d(stationaryObject.base().position().x(), stationaryObject.base().position().y());
- yaw = stationaryObject.base().orientation().yaw();
- }
-
- bool operator==(const StationaryObjectParameter& rhs)
- {
- if (id != rhs.id)
- {
- return false;
- }
-
- if (position != rhs.position)
- {
- return false;
- }
-
- if (!IsEqual(yaw, rhs.yaw))
- {
- return false;
- }
-
- return true;
- }
-
- /// \brief This stream will be shown in case the test fails
- friend std::ostream& operator<<(std::ostream& os, const StationaryObjectParameter& obj)
- {
- os << "id: " << obj.id << ", "
- << "position: (" << obj.position.x << ", " << obj.position.y << "), "
- << "yaw: " << obj.yaw;
-
- return os;
- }
-
- unsigned int id;
- Common::Vector2d position;
- double yaw;
-};
-
class DetectObjects_Data
{
public:
diff --git a/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_Tests.pro b/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_Tests.pro
index 3cf3ae8026f6df1ab1754e5d92b927012a28ab68..4d749dce1a6311af9a0a91cb3fe4f9a54eca201b 100644
--- a/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_Tests.pro
+++ b/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_Tests.pro
@@ -32,7 +32,8 @@ HEADERS += \
$$WORLD_OSI/OWL/OpenDriveTypeMapper.h \
$$WORLD_OSI/WorldObjectAdapter.h \
$$WORLD_OSI/WorldData.h \
- $$WORLD_OSI/WorldDataException.h
+ $$WORLD_OSI/WorldDataException.h \
+ sensorOSI_TestsCommon.h
SOURCES += \
$$UNIT_UNDER_TEST/sensorGeometric2D.cpp \
diff --git a/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_TestsCommon.h b/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_TestsCommon.h
new file mode 100644
index 0000000000000000000000000000000000000000..7357bd5f90465568e0d6d1c88d64ddfb72b91cf6
--- /dev/null
+++ b/sim/tests/unitTests/components/Sensor_OSI/sensorOSI_TestsCommon.h
@@ -0,0 +1,122 @@
+#include "common/vector2d.h"
+#include "osi3/osi_detectedobject.pb.h"
+
+double constexpr EPSILON = 1e-9;
+
+class MovingObjectParameter
+{
+public:
+ MovingObjectParameter(unsigned int id,
+ Common::Vector2d position,
+ Common::Vector2d velocity,
+ Common::Vector2d acceleration,
+ double yaw) :
+ id(id),
+ position(position),
+ velocity(velocity),
+ acceleration(acceleration),
+ yaw(yaw)
+ {}
+
+ MovingObjectParameter (const osi3::DetectedMovingObject& movingObject)
+ {
+ id = movingObject.header().ground_truth_id(0).value();
+ position = Common::Vector2d(movingObject.base().position().x(), movingObject.base().position().y());
+ velocity = Common::Vector2d(movingObject.base().velocity().x(), movingObject.base().velocity().y());
+ acceleration = Common::Vector2d(movingObject.base().acceleration().x(), movingObject.base().acceleration().y());
+ yaw = movingObject.base().orientation().yaw();
+ }
+
+ bool operator==(const MovingObjectParameter& rhs) const
+ {
+ if (id != rhs.id)
+ {
+ return false;
+ }
+
+ if (position != rhs.position ||
+ velocity != rhs.velocity ||
+ acceleration != rhs.acceleration)
+ {
+ return false;
+ }
+
+ if ((yaw - rhs.yaw) > EPSILON)
+ {
+ return false;
+ }
+
+ return true;
+ }
+
+ /// \brief This stream will be shown in case the test fails
+ friend std::ostream& operator<<(std::ostream& os, const MovingObjectParameter& obj)
+ {
+ os << "id: " << obj.id << ", "
+ << "position: (" << obj.position.x << ", " << obj.position.y << "), "
+ << "velocity: (" << obj.velocity.x << ", " << obj.velocity.y << "), "
+ << "acceleration: (" << obj.acceleration.x << ", " << obj.acceleration.y << "), "
+ << "yaw: " << obj.yaw;
+
+ return os;
+ }
+
+ unsigned int id;
+ Common::Vector2d position;
+ Common::Vector2d velocity;
+ Common::Vector2d acceleration;
+ double yaw;
+};
+
+class StationaryObjectParameter
+{
+public:
+ StationaryObjectParameter(unsigned int id,
+ Common::Vector2d position,
+ double yaw) :
+ id(id),
+ position(position),
+ yaw(yaw)
+ {}
+
+ StationaryObjectParameter (const osi3::DetectedStationaryObject& stationaryObject)
+ {
+ id = stationaryObject.header().ground_truth_id(0).value();
+ position = Common::Vector2d(stationaryObject.base().position().x(), stationaryObject.base().position().y());
+ yaw = stationaryObject.base().orientation().yaw();
+ }
+
+ bool operator==(const StationaryObjectParameter& rhs)
+ {
+ if (id != rhs.id)
+ {
+ return false;
+ }
+
+ if (position != rhs.position)
+ {
+ return false;
+ }
+
+ if ((yaw - rhs.yaw) > EPSILON)
+ {
+ return false;
+ }
+
+ return true;
+ }
+
+ /// \brief This stream will be shown in case the test fails
+ friend std::ostream& operator<<(std::ostream& os, const StationaryObjectParameter& obj)
+ {
+ os << "id: " << obj.id << ", "
+ << "position: (" << obj.position.x << ", " << obj.position.y << "), "
+ << "yaw: " << obj.yaw;
+
+ return os;
+ }
+
+ unsigned int id;
+ Common::Vector2d position;
+ double yaw;
+};
diff --git a/sim/tests/unitTests/core/slave/CMakeLists.txt b/sim/tests/unitTests/core/slave/CMakeLists.txt
index 8bb1a5a3be94377bb47030782ba2eef40e8ee9dd..dd133fb549d68c1a6d5950dd1c327535c7621f2e 100644
--- a/sim/tests/unitTests/core/slave/CMakeLists.txt
+++ b/sim/tests/unitTests/core/slave/CMakeLists.txt
@@ -57,6 +57,7 @@ add_openpass_target(
${COMPONENT_SOURCE_DIR}/importer/scenarioImporterHelper.cpp
# SceneryImporter
+ roadGeometry_Tests.cpp
sceneryImporter_Tests.cpp
${COMPONENT_SOURCE_DIR}/importer/connection.cpp
${COMPONENT_SOURCE_DIR}/importer/junction.cpp
diff --git a/sim/tests/unitTests/core/slave/agentSampler_Tests.cpp b/sim/tests/unitTests/core/slave/agentSampler_Tests.cpp
index 02e9910b33b778423a8b12ebfd07dc40a8430ba7..caa4ca982094eae1ed1f60de1c3597c5648eff47 100644
--- a/sim/tests/unitTests/core/slave/agentSampler_Tests.cpp
+++ b/sim/tests/unitTests/core/slave/agentSampler_Tests.cpp
@@ -1,5 +1,5 @@
/*******************************************************************************
-* Copyright (c) 2018, 2019 in-tech GmbH
+* Copyright (c) 2018, 2019, 2020 in-tech GmbH
*
* This program and the accompanying materials are made
* available under the terms of the Eclipse Public License 2.0
@@ -300,9 +300,9 @@ TEST(DynamicAgentTypeGenerator, GatherSensors)
std::map<int, std::shared_ptr<SimulationSlave::AgentTypeInterface>> systems = {{0, fakeAgentType}};
std::map<std::string, std::shared_ptr<SimulationSlave::ComponentType>> components{};
- auto sensorFusion = std::make_shared<SimulationSlave::ComponentType>("SensorFusion", false, 0, 0, 0, 0, "SensorFusion");
- sensorFusion->AddInputLink(0, 100);
- components.insert(std::make_pair("SensorFusion", sensorFusion));
+ auto sensorAggregation = std::make_shared<SimulationSlave::ComponentType>("SensorAggregation", false, 0, 0, 0, 0, "SensorAggregation");
+ sensorAggregation->AddInputLink(0, 100);
+ components.insert(std::make_pair("SensorAggregation", sensorAggregation));
auto sensorObjectDetector = std::make_shared<SimulationSlave::ComponentType>("SensorObjectDetector", false, 0, 0, 0, 0, "SensorObjectDetector");
sensorObjectDetector->AddOutputLink(3, 100);
@@ -317,14 +317,14 @@ TEST(DynamicAgentTypeGenerator, GatherSensors)
const auto& gatheredComponents = agentBuildInformation.agentType->GetComponents();
ASSERT_THAT(gatheredComponents.size(), Eq(3));
- ASSERT_THAT(gatheredComponents.count("SensorFusion"), Eq(1));
+ ASSERT_THAT(gatheredComponents.count("SensorAggregation"), Eq(1));
ASSERT_THAT(gatheredComponents.count("Sensor_5"), Eq(1));
ASSERT_THAT(gatheredComponents.count("Sensor_7"), Eq(1));
- ASSERT_THAT(gatheredComponents.at("SensorFusion")->GetInputLinks().size(), Eq(2));
- ASSERT_THAT(gatheredComponents.at("SensorFusion")->GetModelLibrary(), Eq("SensorFusion"));
- ASSERT_THAT(gatheredComponents.at("SensorFusion")->GetInputLinks().at(0), Eq(100));
- ASSERT_THAT(gatheredComponents.at("SensorFusion")->GetInputLinks().at(1), Eq(101));
+ ASSERT_THAT(gatheredComponents.at("SensorAggregation")->GetInputLinks().size(), Eq(2));
+ ASSERT_THAT(gatheredComponents.at("SensorAggregation")->GetModelLibrary(), Eq("SensorAggregation"));
+ ASSERT_THAT(gatheredComponents.at("SensorAggregation")->GetInputLinks().at(0), Eq(100));
+ ASSERT_THAT(gatheredComponents.at("SensorAggregation")->GetInputLinks().at(1), Eq(101));
ASSERT_THAT(gatheredComponents.at("Sensor_5")->GetModelLibrary(), Eq("SensorObjectDetector"));
ASSERT_THAT(gatheredComponents.at("Sensor_5")->GetOutputLinks().at(3), Eq(100));
diff --git a/sim/tests/unitTests/core/slave/openPassSlave_Tests.pro b/sim/tests/unitTests/core/slave/openPassSlave_Tests.pro
index 91dc50f971cbb3aa7fe125ae99873c1393ebdb8e..88494689fded45358f0029771fb038fdcdc1b18e 100644
--- a/sim/tests/unitTests/core/slave/openPassSlave_Tests.pro
+++ b/sim/tests/unitTests/core/slave/openPassSlave_Tests.pro
@@ -101,6 +101,7 @@ SCENERYIMPORTER_TESTS = \
$$UNIT_UNDER_TEST/importer/road/roadSignal.cpp \
$$UNIT_UNDER_TEST/importer/road/roadObject.cpp \
\
+ roadGeometry_Tests.cpp \
sceneryImporter_Tests.cpp
SCENARIOIMPORTER_TESTS = \
diff --git a/sim/tests/unitTests/core/slave/profilesImporter_Tests.cpp b/sim/tests/unitTests/core/slave/profilesImporter_Tests.cpp
index 86736db319f473a03eb7ffefc73c58e1390f89e8..681ce1fbcf4cad765e0a3d71116f23486150e017 100644
--- a/sim/tests/unitTests/core/slave/profilesImporter_Tests.cpp
+++ b/sim/tests/unitTests/core/slave/profilesImporter_Tests.cpp
@@ -33,7 +33,7 @@ TEST(ProfilesImporter_ImportAllVehicleComponentsOfVehicleProfile, GivenValidXml_
"</Profiles>"
"<SensorLinks>"
"<SensorLink SensorId=\"0\" InputId=\"Camera\"/>"
- "<SensorLink SensorId=\"1\" InputId=\"DrivingCorridor\"/>"
+ "<SensorLink SensorId=\"1\" InputId=\"OtherSensor\"/>"
"<SensorLink SensorId=\"2\" InputId=\"Camera\"/>"
"</SensorLinks>"
"</Component>"
@@ -64,7 +64,7 @@ TEST(ProfilesImporter_ImportAllVehicleComponentsOfVehicleProfile, WithMissingCom
"</Profiles>"
"<SensorLinks>"
"<SensorLink SensorId=\"0\" InputId=\"Camera\"/>"
- "<SensorLink SensorId=\"1\" InputId=\"DrivingCorridor\"/>"
+ "<SensorLink SensorId=\"1\" InputId=\"OtherSensor\"/>"
"<SensorLink SensorId=\"2\" InputId=\"Camera\"/>"
"</SensorLinks>"
"</Component>"
@@ -87,7 +87,7 @@ TEST(ProfilesImporter_ImportAllVehicleComponentsOfVehicleProfile, SumOfProbabili
"</Profiles>"
"<SensorLinks>"
"<SensorLink SensorId=\"0\" InputId=\"Camera\"/>"
- "<SensorLink SensorId=\"1\" InputId=\"DrivingCorridor\"/>"
+ "<SensorLink SensorId=\"1\" InputId=\"OtherSensor\"/>"
"<SensorLink SensorId=\"2\" InputId=\"Camera\"/>"
"</SensorLinks>"
"</Component>"
@@ -110,7 +110,7 @@ TEST(ProfilesImporter_ImportAllSensorsOfVehicleProfile, GivenValidXml_ImportsVal
"</Sensor>"
"<Sensor Id=\"1\">"
"<Position Name=\"Somewhere\" Longitudinal=\"0.0\" Lateral=\"0.0\" Height=\"0.5\" Pitch=\"0.0\" Yaw=\"0.0\" Roll=\"0.0\"/>"
- "<Profile Type=\"DrivingCorridor\" Name=\"Camera\"/>"
+ "<Profile Type=\"OtherSensor\" Name=\"Camera\"/>"
"</Sensor>"
"</Sensors>"
"</root>"
@@ -149,7 +149,7 @@ TEST(ProfilesImporter_ImportAllSensorsOfVehicleProfile, SensorsTagMissing_Throws
"</Sensor>"
"<Sensor Id=\"1\">"
"<Position Name=\"Somewhere\" Longitudinal=\"0.0\" Lateral=\"0.0\" Height=\"0.5\" Pitch=\"0.0\" Yaw=\"0.0\" Roll=\"0.0\"/>"
- "<Profile Type=\"DrivingCorridor\" Name=\"Camera\"/>"
+ "<Profile Type=\"OtherSensor\" Name=\"Camera\"/>"
"</Sensor>"
"</root>"
);
diff --git a/sim/tests/unitTests/core/slave/roadGeometry_Tests.cpp b/sim/tests/unitTests/core/slave/roadGeometry_Tests.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ff91a322a3bead1c87c586b69a1e9671b5bb7a34
--- /dev/null
+++ b/sim/tests/unitTests/core/slave/roadGeometry_Tests.cpp
@@ -0,0 +1,267 @@
+/*******************************************************************************
+* Copyright (c) 2020 in-tech GmbH
+*
+* 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 "common/opMath.h"
+
+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+
+#include "road.h"
+
+using ::testing::DoubleNear;
+
+constexpr double MAX_GEOMETRY_ERROR = 0.001;
+
+struct GeometrySpiral_Data
+{
+ double x; //!< geometry origin x
+ double y; //!< geometry origin y
+ double hdg; //!< geometry origin heading
+ double length; //!< lenth of spiral between curvature start and end
+ double curvStart; //!< curvature at s = 0
+ double curvEnd; //!< curvature at s = length
+
+ double s; //!< query s coordinate
+ double t; //!< query t coordinate
+
+ double expected_x; //!< expected x coordinate for query
+ double expected_y; //!< expected y coordinate for query
+ double expected_hdg; //!< expected heading for query
+
+ /// \brief This stream will be shown in case the test fails
+ friend std::ostream& operator<<(std::ostream& os, const GeometrySpiral_Data& obj)
+ {
+ return os
+ << "x: " << obj.x
+ << ", y: " << obj.y
+ << ", hdg: " << obj.hdg
+ << ", length: " << obj.length
+ << ", curvStart: " << obj.curvStart
+ << ", curvend: " << obj.curvEnd
+ << ", s: " << obj.s
+ << ", t: " << obj.t
+ << ", expected_x: " << obj.expected_x
+ << ", expected_y: " << obj.expected_y
+ << ", expected_hdg: " << obj.expected_hdg;
+ }
+};
+
+class RoadGeometries_Spiral : public ::testing::TestWithParam<GeometrySpiral_Data>
+{
+public:
+ virtual ~RoadGeometries_Spiral()
+ {
+ }
+};
+
+TEST_P(RoadGeometries_Spiral, GetCoordAndGetDir_ReturnCorrectValues)
+{
+ GeometrySpiral_Data data = GetParam();
+
+ const RoadGeometrySpiral rgs{0, data.x, data.y, data.hdg, data.length, data.curvStart, data.curvEnd};
+ const auto res = rgs.GetCoord(data.s, data.t);
+ const auto hdg = rgs.GetDir(data.s);
+
+ EXPECT_THAT(res.x, DoubleNear(data.expected_x, MAX_GEOMETRY_ERROR));
+ EXPECT_THAT(res.y, DoubleNear(data.expected_y, MAX_GEOMETRY_ERROR));
+ EXPECT_THAT(hdg, DoubleNear(data.expected_hdg, MAX_GEOMETRY_ERROR));
+}
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTAndZeroOrigin, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.00, 0.00, 100.0, 0.0, 100.000, 0.000, 0.0000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.00, 0.01, 100.0, 0.0, 97.529, 16.371, 0.5000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.00, -0.01, 100.0, 0.0, 97.529, -16.371, -0.5000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.01, 0.00, 100.0, 0.0, 93.438, 32.391, 0.5000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.01, 0.00, 100.0, 0.0, 93.438, -32.391, -0.5000 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTAndNonzeroOrigin, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 1.0, 1.0, 0.0, 100.0, 0.00, 0.00, 100.0, 0.0, 101.000, 1.000, 0.0000 },
+ GeometrySpiral_Data{ -1.0, -1.0, 0.0, 100.0, 0.00, 0.00, 100.0, 0.0, 99.000, -1.000, 0.0000 },
+ GeometrySpiral_Data{ -1.0, 1.0, 0.0, 100.0, 0.00, 0.00, 100.0, 0.0, 99.000, 1.000, 0.0000 },
+ GeometrySpiral_Data{ 1.0, -1.0, 0.0, 100.0, 0.00, 0.00, 100.0, 0.0, 101.000, -1.000, 0.0000},
+
+ GeometrySpiral_Data{ 1.0, 1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 0.0, 98.529, 17.371, 0.5000 },
+ GeometrySpiral_Data{ -1.0, -1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 0.0, 96.529, 15.371, 0.5000 },
+ GeometrySpiral_Data{ -1.0, 1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 0.0, 96.529, 17.371, 0.5000 },
+ GeometrySpiral_Data{ 1.0, -1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 0.0, 98.529, 15.371, 0.5000 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndNonzeroTAndNonzeroOrigin, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 1.0, 1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 1.0, 98.049, 18.249, 0.5000 },
+ GeometrySpiral_Data{ -1.0, -1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 1.0, 96.049, 16.249, 0.5000 },
+ GeometrySpiral_Data{ -1.0, 1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 1.0, 96.049, 18.249, 0.5000 },
+ GeometrySpiral_Data{ 1.0, -1.0, 0.0, 100.0, 0.00, 0.01, 100.0, 1.0, 98.049, 16.249, 0.5000 },
+ GeometrySpiral_Data{ 1.0, 1.0, 0.0, 100.0, 0.00, 0.01, 100.0, -1.0, 99.008, 16.494, 0.5000 },
+ GeometrySpiral_Data{ -1.0, -1.0, 0.0, 100.0, 0.00, 0.01, 100.0, -1.0, 97.008, 14.494, 0.5000 },
+ GeometrySpiral_Data{ -1.0, 1.0, 0.0, 100.0, 0.00, 0.01, 100.0, -1.0, 97.008, 16.494, 0.5000 },
+ GeometrySpiral_Data{ 1.0, -1.0, 0.0, 100.0, 0.00, 0.01, 100.0, -1.0, 99.008, 14.494, 0.5000 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTAndNonzeroOriginHeading, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 0.0, 0.0, M_PI_4, 100.0, 0.00, 0.00, 100.0, 0.0, 70.711, 70.711, 0.7854 },
+ GeometrySpiral_Data{ 0.0, 0.0, 3 * M_PI_4, 100.0, 0.00, 0.00, 100.0, 0.0, -70.711, 70.711, 2.3570 },
+ GeometrySpiral_Data{ 0.0, 0.0, -3 * M_PI_4, 100.0, 0.00, 0.00, 100.0, 0.0, -70.711, -70.711, -2.3570 },
+ GeometrySpiral_Data{ 0.0, 0.0, 5 * M_PI_4, 100.0, 0.00, 0.00, 100.0, 0.0, -70.711, -70.711, 3.9270 },
+ GeometrySpiral_Data{ 0.0, 0.0, -M_PI_4, 100.0, 0.00, 0.00, 100.0, 0.0, 70.711, -70.711, -0.7854 },
+ GeometrySpiral_Data{ 0.0, 0.0, 7 * M_PI_4, 100.0, 0.00, 0.00, 100.0, 0.0, 70.711, -70.711, 5.4978 },
+
+ GeometrySpiral_Data{ 0.0, 0.0, M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, 57.387, 80.540, 1.2854 },
+ GeometrySpiral_Data{ 0.0, 0.0, 3 * M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, -80.540, 57.387, 2.8562 },
+ GeometrySpiral_Data{ 0.0, 0.0, -3 * M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, -57.387, -80.540, -1.8562 },
+ GeometrySpiral_Data{ 0.0, 0.0, 5 * M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, -57.387, -80.540, 4.4270 },
+ GeometrySpiral_Data{ 0.0, 0.0, -M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, 80.540, -57.387, -0.2854 },
+ GeometrySpiral_Data{ 0.0, 0.0, 7 * M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, 80.540, -57.387, 5.9978 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTAndNonzeroOriginAndHeading, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 1.0, 1.0, M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, 58.387, 81.540, 1.2854 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, 0.00, 0.01, 100.0, 0.0, 81.540, -56.387, -0.2854 },
+ GeometrySpiral_Data{ -1.0, -1.0, 3 * M_PI_4, 100.0, 0.00, -0.01, 100.0, 0.0, -58.387, 79.540, 1.8562 },
+ GeometrySpiral_Data{ -1.0, 1.0, -3 * M_PI_4, 100.0, 0.00, -0.01, 100.0, 0.0, -81.540, -56.387, -2.8562 },
+ GeometrySpiral_Data{ 1.0, -1.0, 5 * M_PI_4, 100.0, 0.00, -0.01, 100.0, 0.0, -79.540, -58.387, 3.4270 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTWithNonzeroStartAndEndCurvature, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.01, 0.02, 100.0, 0.0, 71.564, 55.928, 1.5 },
+ GeometrySpiral_Data{ 0.0, 0.0, M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, 11.057, 90.151, 2.2854 },
+ GeometrySpiral_Data{ 0.0, 0.0, -M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, 90.151, -11.057, 0.7146 },
+ GeometrySpiral_Data{ 0.0, 0.0, 3 * M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, -90.151, 11.057, 3.8562 },
+ GeometrySpiral_Data{ 0.0, 0.0, -3 * M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, -11.057, -90.151, -0.8562 },
+
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.01, -0.02, 100.0, 0.0, 71.564, -55.928, -1.5 },
+ GeometrySpiral_Data{ 0.0, 0.0, M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, 90.151, 11.057, -0.7146 },
+ GeometrySpiral_Data{ 0.0, 0.0, -M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, 11.057, -90.151, -2.2854 },
+ GeometrySpiral_Data{ 0.0, 0.0, 3 * M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, -11.057, 90.151, 0.8562 },
+ GeometrySpiral_Data{ 0.0, 0.0, -3 * M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, -90.151, -11.057, -3.8562 },
+
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.02, 0.01, 100.0, 0.0, 60.850, 67.429, 1.5 },
+ GeometrySpiral_Data{ 0.0, 0.0, M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, -4.652, 90.707, 2.2854 },
+ GeometrySpiral_Data{ 0.0, 0.0, -M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, 90.707, 4.652, 0.7146 },
+ GeometrySpiral_Data{ 0.0, 0.0, 3 * M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, -90.707, -4.652, 3.8562 },
+ GeometrySpiral_Data{ 0.0, 0.0, -3 * M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, 4.652, -90.707, -0.8562 },
+
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.02, -0.01, 100.0, 0.0, 60.850, -67.429, -1.5 },
+ GeometrySpiral_Data{ 0.0, 0.0, M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, 90.707, -4.652, -0.7146 },
+ GeometrySpiral_Data{ 0.0, 0.0, -M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, -4.652, -90.707, -2.2854 },
+ GeometrySpiral_Data{ 0.0, 0.0, 3 * M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, 4.652, 90.707, 0.8562 },
+ GeometrySpiral_Data{ 0.0, 0.0, -3 * M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, -90.707, 4.652, -3.8562 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTWithNonzeroStartAndEndCurvatureAndNonzeroOrigin, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, 0.01, 0.02, 100.0, 0.0, 71.564, 56.928, 1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, 12.057, 90.151, 2.2854 },
+ GeometrySpiral_Data{ 1.0, 0.0, -M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, 91.151, -11.057, 0.7146 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, -89.151, 11.057, 3.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, 0.01, 0.02, 100.0, 0.0, -11.057, -89.151, -0.8562 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, -0.01, -0.02, 100.0, 0.0, 71.564, -54.928, -1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, 91.151, 11.057, -0.7146 },
+ GeometrySpiral_Data{ 1.0, 0.0, -M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, 12.057, -90.151, -2.2854 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, -10.057, 90.151, 0.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, -0.01, -0.02, 100.0, 0.0, -90.151, -10.057, -3.8562 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, 0.02, 0.01, 100.0, 0.0, 60.850, 68.429, 1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, -3.652, 90.707, 2.2854 },
+ GeometrySpiral_Data{ 1.0, 0.0, -M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, 91.707, 4.652, 0.7146 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, -89.707, -4.652, 3.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, 0.02, 0.01, 100.0, 0.0, 4.652, -89.707, -0.8562 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, -0.02, -0.01, 100.0, 0.0, 60.850, -66.429, -1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, 91.707, -4.652, -0.7146 },
+ GeometrySpiral_Data{ 1.0, 0.0, -M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, -3.652, -90.707, -2.2854 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, 5.652, 90.707, 0.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, -0.02, -0.01, 100.0, 0.0, -90.707, 5.652, -3.8562 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndNonzeroTWithNonzeroStartAndEndCurvatureAndNonzeroOrigin, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, 0.01, 0.02, 100.0, 2.0, 69.569, 57.069, 1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, 0.01, 0.02, 100.0, -2.0, 13.568, 91.461, 2.2854 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, 0.01, 0.02, 100.0, 2.0, 89.840, -8.546, 0.7146 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, 0.01, 0.02, 100.0, -2.0, -90.461, 12.568, 3.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, 0.01, 0.02, 100.0, 2.0, -9.546, -87.840, -0.8562 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, -0.01, -0.02, 100.0, -2.0, 69.569, -55.069, -1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, -0.01, -0.02, 100.0, 2.0, 92.461, 12.568, -0.7146 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, -0.01, -0.02, 100.0, -2.0, 10.546, -87.840, -2.2854 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, -0.01, -0.02, 100.0, 2.0, -11.568, 91.461, 0.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, -0.01, -0.02, 100.0, -2.0, -88.840, -8.546, -3.8562 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, 0.02, 0.01, 100.0, 2.0, 58.855, 68.570, 1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, 0.02, 0.01, 100.0, 2.0, -5.163, 89.396, 2.2854 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, 0.02, 0.01, 100.0, -2.0, 93.017, 4.141, 0.7146 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, 0.02, 0.01, 100.0, -2.0, -91.017, -3.141, 3.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, 0.02, 0.01, 100.0, -2.0, 3.141, -91.017, -0.8562 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, -0.02, -0.01, 100.0, -2.0, 58.855, -66.570, -1.5 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, -0.02, -0.01, 100.0, -2.0, 90.396, -6.163, -0.7146 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, -0.02, -0.01, 100.0, -2.0, -5.163, -88.396, -2.2854 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, -0.02, -0.01, 100.0, 2.0, 4.141, 92.017, 0.8562 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, -0.02, -0.01, 100.0, 2.0, -92.017, 4.141, -3.8562 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtCenterOfGeometryAndNonzeroTWithNonzeroStartAndEndCurvatureAndNonzeroOrigin, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, 0.01, 0.02, 50.0, 2.0, 45.942, 16.759, 0.6250 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, 0.01, 0.02, 50.0, -2.0, 26.291, 42.991, 1.4104 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, 0.01, 0.02, 50.0, 2.0, 44.630, -20.343, -0.1604 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, 0.01, 0.02, 50.0, -2.0, -41.991, 25.291, 2.9812 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, 0.01, 0.02, 50.0, 2.0, -21.343, -42.630, -1.7312 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, -0.01, -0.02, 50.0, 2.0, 48.283, -11.516, -0.6250 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, -0.01, -0.02, 50.0, -2.0, 44.630, 21.343, 0.1604 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, -0.01, -0.02, 50.0, 2.0, 26.291, -41.991, -1.4104 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, -0.01, -0.02, 50.0, -2.0, -20.343, 43.630, 1.7312 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, -0.01, -0.02, 50.0, 2.0, -42.991, -24.291, -2.9812 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, 0.02, 0.01, 50.0, 2.0, 41.880, 23.716, 0.8750 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, 0.02, 0.01, 50.0, 2.0, 14.551, 45.677, 1.6604 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, 0.02, 0.01, 50.0, -2.0, 47.035, -16.535, 0.0896 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, 0.02, 0.01, 50.0, -2.0, -45.035, 17.535, 3.2312 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, 0.02, 0.01, 50.0, -2.0, -17.535, -45.035, -1.4812 },
+
+ GeometrySpiral_Data{ 0.0, 1.0, 0.0, 100.0, -0.02, -0.01, 50.0, -2.0, 41.880, -21.716, -0.8750 },
+ GeometrySpiral_Data{ 1.0, 0.0, M_PI_4, 100.0, -0.02, -0.01, 50.0, -2.0, 46.677, 13.551, -0.0896 },
+ GeometrySpiral_Data{ 1.0, 1.0, -M_PI_4, 100.0, -0.02, -0.01, 50.0, -2.0, 14.551, -44.677, -1.6604 },
+ GeometrySpiral_Data{ 1.0, 0.0, 3 * M_PI_4, 100.0, -0.02, -0.01, 50.0, 2.0, -16.535, 46.035, 1.4812 },
+ GeometrySpiral_Data{ 0.0, 1.0, -3 * M_PI_4, 100.0, -0.02, -0.01, 50.0, 2.0, -46.035, -16.535, -3.2312 }
+));
+
+INSTANTIATE_TEST_SUITE_P(AtEndOfGeometryAndZeroTWithAlternatingCurvatureSign, RoadGeometries_Spiral, ::testing::Values(
+ // | origin | spirial definition | query pos | expected result |
+ // | x y hdg | len c_start c_end | s t | x y hdg |
+ GeometrySpiral_Data{ -49.688, -4.148, 0.25, 100.0, -0.01, 0.01, 50.0, 0.0, 0.000, 0.000, 0.0000 },
+ GeometrySpiral_Data{ -49.688, 4.148, -0.25, 100.0, 0.01, -0.01, 50.0, 0.0, 0.000, 0.000, 0.0000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.01, 0.01, 50.0, 0.0, 49.170, -8.274, -0.2500 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.01, 0.01, 100.0, 0.0, 98.340, -16.548, 0.0000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.01, -0.01, 50.0, 0.0, 49.170, 8.274, 0.2500 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.01, -0.01, 100.0, 0.0, 98.340, 16.548, 0.0000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.02, 0.01, 100.0, 0.0, 86.252, -47.254, -0.5000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, -0.02, 0.01, 50.0, 0.0, 45.747, -18.029, -0.6250 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.02, -0.01, 100.0, 0.0, 86.252, 47.254, 0.5000 },
+ GeometrySpiral_Data{ 0.0, 0.0, 0.0, 100.0, 0.02, -0.01, 50.0, 0.0, 45.747, 18.029, 0.6250 }
+));
diff --git a/sim/tests/unitTests/unitTests.pro b/sim/tests/unitTests/unitTests.pro
index ade3e5da1eeb7980f63b56d166a8e2f96071398d..fd38aee5703b95dbdfd1b428f3fa769c5f4173b4 100644
--- a/sim/tests/unitTests/unitTests.pro
+++ b/sim/tests/unitTests/unitTests.pro
@@ -30,7 +30,8 @@ SUBDIRS = \
openScenarioActions_Tests \
scheduler_Tests \
sensorDriver_Tests \
- sensorFusionOSI_Tests \
+ sensorAggregation_Tests \
+ sensorFusionErrorless_Tests \
sensorOSI_Tests \
signalPrioritizer_Tests \
spawnPointScenario_Tests \
@@ -87,8 +88,11 @@ scheduler_Tests.file = \
sensorDriver_Tests.file = \
$$PWD/components/Sensor_Driver/sensorDriver_Tests.pro
-sensorFusionOSI_Tests.file = \
- $$PWD/components/SensorFusion_OSI/sensorFusionOSI_Tests.pro
+sensorAggregation_Tests.file = \
+ $$PWD/components/SensorAggregation_OSI/sensorAggregationOSI_Tests.pro
+
+sensorFusionErrorless_Tests.file = \
+ $$PWD/components/SensorFusionErrorless_OSI/sensorFusionErrorless_Tests.pro
sensorOSI_Tests.file = \
$$PWD/components/Sensor_OSI/sensorOSI_Tests.pro