Skip to content
Snippets Groups Projects
Commit 15c9c985 authored by David Weiß's avatar David Weiß Committed by Reinhard Biegel
Browse files

feat(Global): Support for roll angle and improved OpenSCENARIO vehicle models


Multiple components have been adapted for roll angle support.
Vehicle models parameters are now structured like in OpenSCENARIO XML.

Signed-off-by: default avatarWeiss David <david.weiss@in-tech.com>
parent 0c090cf6
No related branches found
No related tags found
2 merge requests!64Merge 0.8,!18New features and bugfixes and build system improvements
Showing
with 222 additions and 496 deletions
...@@ -66,6 +66,7 @@ TEST_P(TtcCalcualtionTest, CalculateObjectTTC_ReturnsCorrectTtc) ...@@ -66,6 +66,7 @@ TEST_P(TtcCalcualtionTest, CalculateObjectTTC_ReturnsCorrectTtc)
ON_CALL(ego, GetPositionX()).WillByDefault(Return(data.egoX)); ON_CALL(ego, GetPositionX()).WillByDefault(Return(data.egoX));
ON_CALL(ego, GetPositionY()).WillByDefault(Return(data.egoY)); ON_CALL(ego, GetPositionY()).WillByDefault(Return(data.egoY));
ON_CALL(ego, GetYaw()).WillByDefault(Return(data.egoYaw)); ON_CALL(ego, GetYaw()).WillByDefault(Return(data.egoYaw));
ON_CALL(ego, GetRoll()).WillByDefault(Return(0.0));
ON_CALL(ego, GetYawRate()).WillByDefault(Return(data.egoYawRate)); ON_CALL(ego, GetYawRate()).WillByDefault(Return(data.egoYawRate));
ON_CALL(ego, GetYawAcceleration()).WillByDefault(Return(data.egoYawAcceleration)); ON_CALL(ego, GetYawAcceleration()).WillByDefault(Return(data.egoYawAcceleration));
ON_CALL(ego, GetVelocity(VelocityScope::DirectionX)).WillByDefault(Return(data.egoVelocityX)); ON_CALL(ego, GetVelocity(VelocityScope::DirectionX)).WillByDefault(Return(data.egoVelocityX));
...@@ -79,6 +80,7 @@ TEST_P(TtcCalcualtionTest, CalculateObjectTTC_ReturnsCorrectTtc) ...@@ -79,6 +80,7 @@ TEST_P(TtcCalcualtionTest, CalculateObjectTTC_ReturnsCorrectTtc)
ON_CALL(opponent, GetPositionX()).WillByDefault(Return(data.opponentX)); ON_CALL(opponent, GetPositionX()).WillByDefault(Return(data.opponentX));
ON_CALL(opponent, GetPositionY()).WillByDefault(Return(data.opponentY)); ON_CALL(opponent, GetPositionY()).WillByDefault(Return(data.opponentY));
ON_CALL(opponent, GetYaw()).WillByDefault(Return(data.opponentYaw)); ON_CALL(opponent, GetYaw()).WillByDefault(Return(data.opponentYaw));
ON_CALL(opponent, GetRoll()).WillByDefault(Return(0.0));
ON_CALL(opponent, GetYawRate()).WillByDefault(Return(data.opponentYawRate)); ON_CALL(opponent, GetYawRate()).WillByDefault(Return(data.opponentYawRate));
ON_CALL(opponent, GetYawAcceleration()).WillByDefault(Return(data.opponentYawAcceleration)); ON_CALL(opponent, GetYawAcceleration()).WillByDefault(Return(data.opponentYawAcceleration));
ON_CALL(opponent, GetVelocity(VelocityScope::DirectionX)).WillByDefault(Return(data.opponentVelocityX)); ON_CALL(opponent, GetVelocity(VelocityScope::DirectionX)).WillByDefault(Return(data.opponentVelocityX));
......
/*******************************************************************************
* 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
*******************************************************************************/
#include "osi3/osi_detectedobject.pb.h"
#include "gtest/gtest.h"
#include "gmock/gmock.h"
#include "fakeAgent.h"
#include "boundingBoxCalculation.h"
using ::testing::Return;
using ::testing::Lt;
using ::testing::NiceMock;
class BoundingBoxCalculation_Tests : public ::testing::Test
{
public:
BoundingBoxCalculation_Tests()
{
ON_CALL(fakeEgoAgent, GetLength()).WillByDefault(Return(2.0));
ON_CALL(fakeEgoAgent, GetWidth()).WillByDefault(Return(1.0));
ON_CALL(fakeEgoAgent, GetDistanceReferencePointToLeadingEdge()).WillByDefault(Return(1.0));
}
void SetEgoValues (double velocity, double acceleration, double yawRate)
{
ON_CALL(fakeEgoAgent, GetVelocity()).WillByDefault(Return(velocity));
ON_CALL(fakeEgoAgent, GetAcceleration()).WillByDefault(Return(acceleration));
ON_CALL(fakeEgoAgent, GetYawRate()).WillByDefault(Return(yawRate));
}
NiceMock<FakeAgent> fakeEgoAgent;
};
TEST_F(BoundingBoxCalculation_Tests, MovingObjectAtTimestampZero)
{
SetEgoValues(0.0, 0.0, 0.0);
osi3::BaseMoving baseMoving;
baseMoving.mutable_dimension()->set_length(3.0);
baseMoving.mutable_dimension()->set_width(3.5);
baseMoving.mutable_position()->set_x(-20.0);
baseMoving.mutable_position()->set_y(35.0);
baseMoving.mutable_orientation()->set_yaw(M_PI * 0.5);
BoundingBoxCalculation bbCalculation(&fakeEgoAgent, 1.0, 1.5);
double boxPoints[][2]
{
{ -22.5, 33 },
{ -22.5, 37 },
{ -17.5, 37 },
{ -17.5, 33 },
{ -22.5, 33 }
};
polygon_t expectedBox;
bg::append(expectedBox, boxPoints);
bg::correct(expectedBox);
polygon_t result = bbCalculation.CalculateBoundingBox(0.0, baseMoving);
bg::correct(result);
multi_polygon_t differenceMissing;
bg::difference(expectedBox, result, differenceMissing);
multi_polygon_t differenceOverhanging;
bg::difference(result, expectedBox, differenceOverhanging);
EXPECT_THAT(bg::area(differenceMissing), Lt(0.1));
EXPECT_THAT(bg::area(differenceOverhanging), Lt(0.1));
}
TEST_F(BoundingBoxCalculation_Tests, MovingObjectMovingInStraightLine)
{
SetEgoValues(0.0, 0.0, 0.0);
osi3::BaseMoving baseMoving;
baseMoving.mutable_dimension()->set_length(3.0);
baseMoving.mutable_dimension()->set_width(3.5);
baseMoving.mutable_position()->set_x(-20.0);
baseMoving.mutable_position()->set_y(35.0);
baseMoving.mutable_orientation()->set_yaw(M_PI * 0.5);
baseMoving.mutable_velocity()->set_y(10.0);
baseMoving.mutable_acceleration()->set_y(-2.0);
BoundingBoxCalculation bbCalculation(&fakeEgoAgent, 1.0, 1.5);
double boxPoints[][2]
{
{ -22.5, 49 },
{ -22.5, 53 },
{ -17.5, 53 },
{ -17.5, 49 },
{ -22.5, 49 }
};
polygon_t expectedBox;
bg::append(expectedBox, boxPoints);
bg::correct(expectedBox);
polygon_t result = bbCalculation.CalculateBoundingBox(2.0, baseMoving);
bg::correct(result);
multi_polygon_t differenceMissing;
bg::difference(expectedBox, result, differenceMissing);
multi_polygon_t differenceOverhanging;
bg::difference(result, expectedBox, differenceOverhanging);
EXPECT_THAT(bg::area(differenceMissing), Lt(0.1));
EXPECT_THAT(bg::area(differenceOverhanging), Lt(0.1));
}
TEST_F(BoundingBoxCalculation_Tests, MovingObjectMovingInCircleLeft)
{
SetEgoValues(0.0, 0.0, 0.0);
osi3::BaseMoving baseMoving;
baseMoving.mutable_dimension()->set_length(3.0);
baseMoving.mutable_dimension()->set_width(3.5);
baseMoving.mutable_position()->set_x(-20.0);
baseMoving.mutable_position()->set_y(35.0);
baseMoving.mutable_orientation()->set_yaw(M_PI * 0.5);
baseMoving.mutable_orientation_rate()->set_yaw(M_PI * 0.25);
baseMoving.mutable_velocity()->set_y(M_PI * 2.5);
baseMoving.mutable_acceleration()->set_y(0.0);
BoundingBoxCalculation bbCalculation(&fakeEgoAgent, 1.0, 1.5);
double boxPoints[][2]
{
{ -32, 42.5 },
{ -32, 47.5 },
{ -28, 47.5 },
{ -28, 42.5 },
{ -32, 42.5 }
};
polygon_t expectedBox;
bg::append(expectedBox, boxPoints);
bg::correct(expectedBox);
polygon_t result = bbCalculation.CalculateBoundingBox(2.0, baseMoving);
bg::correct(result);
multi_polygon_t differenceMissing;
bg::difference(expectedBox, result, differenceMissing);
multi_polygon_t differenceOverhanging;
bg::difference(result, expectedBox, differenceOverhanging);
EXPECT_THAT(bg::area(differenceMissing), Lt(0.1));
EXPECT_THAT(bg::area(differenceOverhanging), Lt(0.1));
}
TEST_F(BoundingBoxCalculation_Tests, MovingObjectMovingInCircleRight)
{
SetEgoValues(0.0, 0.0, 0.0);
osi3::BaseMoving baseMoving;
baseMoving.mutable_dimension()->set_length(3.0);
baseMoving.mutable_dimension()->set_width(3.5);
baseMoving.mutable_position()->set_x(-20.0);
baseMoving.mutable_position()->set_y(35.0);
baseMoving.mutable_orientation()->set_yaw(M_PI * 0.5);
baseMoving.mutable_orientation_rate()->set_yaw(-M_PI * 0.25);
baseMoving.mutable_velocity()->set_y(M_PI * 2.5);
baseMoving.mutable_acceleration()->set_y(0.0);
BoundingBoxCalculation bbCalculation(&fakeEgoAgent, 1.0, 1.5);
double boxPoints[][2]
{
{ -12, 42.5 },
{ -12, 47.5 },
{ -8, 47.5 },
{ -8, 42.5 },
{ -12, 42.5 }
};
polygon_t expectedBox;
bg::append(expectedBox, boxPoints);
bg::correct(expectedBox);
polygon_t result = bbCalculation.CalculateBoundingBox(2.0, baseMoving);
bg::correct(result);
multi_polygon_t differenceMissing;
bg::difference(expectedBox, result, differenceMissing);
multi_polygon_t differenceOverhanging;
bg::difference(result, expectedBox, differenceOverhanging);
EXPECT_THAT(bg::area(differenceMissing), Lt(0.1));
EXPECT_THAT(bg::area(differenceOverhanging), Lt(0.1));
}
TEST_F(BoundingBoxCalculation_Tests, MovingObjectOwnAgentMovesInStraightLine)
{
SetEgoValues(10.0, 2.0, 0.0);
osi3::BaseMoving baseMoving;
baseMoving.mutable_dimension()->set_length(3.0);
baseMoving.mutable_dimension()->set_width(3.5);
baseMoving.mutable_position()->set_x(-20.0);
baseMoving.mutable_position()->set_y(35.0);
baseMoving.mutable_orientation()->set_yaw(-M_PI * 0.5);
baseMoving.mutable_velocity()->set_x(-10.0);
baseMoving.mutable_velocity()->set_y(-10.0);
baseMoving.mutable_acceleration()->set_y(1.0);
baseMoving.mutable_acceleration()->set_x(-2.0);
BoundingBoxCalculation bbCalculation(&fakeEgoAgent, 1.0, 1.5);
double boxPoints[][2]
{
{ -22.5, 15 },
{ -22.5, 19 },
{ -17.5, 19 },
{ -17.5, 15 },
{ -22.5, 15 }
};
polygon_t expectedBox;
bg::append(expectedBox, boxPoints);
bg::correct(expectedBox);
polygon_t result = bbCalculation.CalculateBoundingBox(2.0, baseMoving);
bg::correct(result);
multi_polygon_t differenceMissing;
bg::difference(expectedBox, result, differenceMissing);
multi_polygon_t differenceOverhanging;
bg::difference(result, expectedBox, differenceOverhanging);
EXPECT_THAT(bg::area(differenceMissing), Lt(0.1));
EXPECT_THAT(bg::area(differenceOverhanging), Lt(0.1));
}
TEST_F(BoundingBoxCalculation_Tests, MovingObjectBothAgentsMovingInCircle)
{
SetEgoValues(M_PI * 2.5, 0.0, M_PI * 0.25);
osi3::BaseMoving baseMoving;
baseMoving.mutable_dimension()->set_length(3.0);
baseMoving.mutable_dimension()->set_width(3.5);
baseMoving.mutable_position()->set_x(-20.0);
baseMoving.mutable_position()->set_y(35.0);
baseMoving.mutable_orientation()->set_yaw(0.0);
baseMoving.mutable_orientation_rate()->set_yaw(-M_PI * 0.5);
BoundingBoxCalculation bbCalculation(&fakeEgoAgent, 1.0, 1.5);
double boxPoints[][2]
{
{ -12.5, 23 },
{ -12.5, 27 },
{ -7.5, 27 },
{ -7.5, 23 },
{ -12.5, 23 }
};
polygon_t expectedBox;
bg::append(expectedBox, boxPoints);
bg::correct(expectedBox);
polygon_t result = bbCalculation.CalculateBoundingBox(2.0, baseMoving);
bg::correct(result);
multi_polygon_t differenceMissing;
bg::difference(expectedBox, result, differenceMissing);
multi_polygon_t differenceOverhanging;
bg::difference(result, expectedBox, differenceOverhanging);
EXPECT_THAT(bg::area(differenceMissing), Lt(0.1));
EXPECT_THAT(bg::area(differenceOverhanging), Lt(0.1));
}
TEST_F(BoundingBoxCalculation_Tests, OwnAgentMovingInStraightLine)
{
SetEgoValues(10.0, 2.0, 0.0);
BoundingBoxCalculation bbCalculation(&fakeEgoAgent, 1.0, 1.0);
double boxPoints[][2]
{
{ 22.5, -1 },
{ 22.5, 1 },
{ 25.5, 1 },
{ 25.5, -1 },
{ 22.5, -1 }
};
polygon_t expectedBox;
bg::append(expectedBox, boxPoints);
bg::correct(expectedBox);
polygon_t result = bbCalculation.CalculateOwnBoundingBox(2.0);
bg::correct(result);
multi_polygon_t differenceMissing;
bg::difference(expectedBox, result, differenceMissing);
multi_polygon_t differenceOverhanging;
bg::difference(result, expectedBox, differenceOverhanging);
EXPECT_THAT(bg::area(differenceMissing), Lt(0.1));
EXPECT_THAT(bg::area(differenceOverhanging), Lt(0.1));
}
TEST_F(BoundingBoxCalculation_Tests, OwnAgentMovingInCircle)
{
SetEgoValues(M_PI * 2.5, 0.0, M_PI * 0.25);
BoundingBoxCalculation bbCalculation(&fakeEgoAgent, 1.0, 1.0);
double boxPoints[][2]
{
{ 9, 8.5 },
{ 9, 11.5 },
{ 11, 11.5 },
{ 11, 8.5 },
{ 9, 8.5 }
};
polygon_t expectedBox;
bg::append(expectedBox, boxPoints);
bg::correct(expectedBox);
polygon_t result = bbCalculation.CalculateOwnBoundingBox(2.0);
bg::correct(result);
multi_polygon_t differenceMissing;
bg::difference(expectedBox, result, differenceMissing);
multi_polygon_t differenceOverhanging;
bg::difference(result, expectedBox, differenceOverhanging);
EXPECT_THAT(bg::area(differenceMissing), Lt(0.1));
EXPECT_THAT(bg::area(differenceOverhanging), Lt(0.1));
}
TEST_F(BoundingBoxCalculation_Tests, StationaryObject)
{
osi3::BaseStationary baseStationary;
baseStationary.mutable_dimension()->set_length(3.0);
baseStationary.mutable_dimension()->set_width(4.0);
baseStationary.mutable_position()->set_x(20.0);
baseStationary.mutable_position()->set_y(-10.0);
baseStationary.mutable_orientation()->set_yaw(-M_PI * 0.5);
BoundingBoxCalculation bbCalculation(&fakeEgoAgent, 1.0, 2.0);
double boxPoints[][2]
{
{ 17, -12 },
{ 17, -8 },
{ 23, -8 },
{ 23, -12 },
{ 17, -12 }
};
polygon_t expectedBox;
bg::append(expectedBox, boxPoints);
bg::correct(expectedBox);
polygon_t result = bbCalculation.CalculateBoundingBox(baseStationary);
bg::correct(result);
multi_polygon_t differenceMissing;
bg::difference(expectedBox, result, differenceMissing);
multi_polygon_t differenceOverhanging;
bg::difference(result, expectedBox, differenceOverhanging);
EXPECT_THAT(bg::area(differenceMissing), Lt(0.1));
EXPECT_THAT(bg::area(differenceOverhanging), Lt(0.1));
}
...@@ -8,14 +8,11 @@ add_openpass_target( ...@@ -8,14 +8,11 @@ add_openpass_target(
SOURCES SOURCES
AlgorithmAeb_Tests.cpp AlgorithmAeb_Tests.cpp
BoundingBoxCalculation_Tests.cpp
${COMPONENT_SOURCE_DIR}/autonomousEmergencyBraking.cpp ${COMPONENT_SOURCE_DIR}/autonomousEmergencyBraking.cpp
${COMPONENT_SOURCE_DIR}/boundingBoxCalculation.cpp
HEADERS HEADERS
AlgorithmAebOSIUnitTests.h AlgorithmAebOSIUnitTests.h
${COMPONENT_SOURCE_DIR}/autonomousEmergencyBraking.h ${COMPONENT_SOURCE_DIR}/autonomousEmergencyBraking.h
${COMPONENT_SOURCE_DIR}/boundingBoxCalculation.h
INCDIRS INCDIRS
${COMPONENT_SOURCE_DIR} ${COMPONENT_SOURCE_DIR}
......
...@@ -24,14 +24,11 @@ HEADERS += \ ...@@ -24,14 +24,11 @@ HEADERS += \
AlgorithmAebOSIUnitTests.h \ AlgorithmAebOSIUnitTests.h \
$$relative_path($$OPEN_SRC)/common/boostGeometryCommon.h \ $$relative_path($$OPEN_SRC)/common/boostGeometryCommon.h \
$$relative_path($$OPEN_SRC)/common/vector2d.h \ $$relative_path($$OPEN_SRC)/common/vector2d.h \
$$relative_path($$OPEN_SRC)/components/Algorithm_AEB/src/autonomousEmergencyBraking.h \ $$relative_path($$OPEN_SRC)/components/Algorithm_AEB/src/autonomousEmergencyBraking.h
$$relative_path($$OPEN_SRC)/components/Algorithm_AEB/src/boundingBoxCalculation.h
SOURCES += \ SOURCES += \
AlgorithmAeb_Tests.cpp \ AlgorithmAeb_Tests.cpp \
BoundingBoxCalculation_Tests.cpp \ $$relative_path($$OPEN_SRC)/components/Algorithm_AEB/src/autonomousEmergencyBraking.cpp
$$relative_path($$OPEN_SRC)/components/Algorithm_AEB/src/autonomousEmergencyBraking.cpp \
$$relative_path($$OPEN_SRC)/components/Algorithm_AEB/src/boundingBoxCalculation.cpp
LIBS += \ LIBS += \
-lopen_simulation_interface \ -lopen_simulation_interface \
......
...@@ -52,11 +52,12 @@ TEST_P(AlgorithmLongitudinalCalculationsGetEngineTorqueMax, AlgorithmLongitudina ...@@ -52,11 +52,12 @@ TEST_P(AlgorithmLongitudinalCalculationsGetEngineTorqueMax, AlgorithmLongitudina
// Set up test // Set up test
VehicleModelParameters vehicleParameters; VehicleModelParameters vehicleParameters;
vehicleParameters.maximumEngineTorque = 270.; vehicleParameters.properties = {{"MaximumEngineTorque", 270.},
vehicleParameters.minimumEngineSpeed = 800.; {"MinimumEngineSpeed", 800.},
vehicleParameters.maximumEngineSpeed = 4000.; {"MaximumEngineSpeed", 4000.}};
AlgorithmLongitudinalCalculations calculation{0.0, 0.0, vehicleParameters}; std::function<void (CbkLogLevel, const char*, int, const std::string&)> Log;
AlgorithmLongitudinalCalculations calculation{0.0, 0.0, vehicleParameters, Log};
// Call test // Call test
double result = calculation.GetEngineTorqueMax(data.input_EngineSpeed); double result = calculation.GetEngineTorqueMax(data.input_EngineSpeed);
...@@ -114,12 +115,13 @@ TEST_P(AlgorithmLongitudinalCalculationsGetEngineTorqueMin, AlgorithmLongitudina ...@@ -114,12 +115,13 @@ TEST_P(AlgorithmLongitudinalCalculationsGetEngineTorqueMin, AlgorithmLongitudina
// Set up test // Set up test
VehicleModelParameters vehicleParameters; VehicleModelParameters vehicleParameters;
vehicleParameters.maximumEngineTorque = 270.; vehicleParameters.properties = {{"MaximumEngineTorque", 270.},
vehicleParameters.minimumEngineTorque = 30.; {"MinimumEngineTorque", 30.},
vehicleParameters.minimumEngineSpeed = 800.; {"MinimumEngineSpeed", 800.},
vehicleParameters.maximumEngineSpeed = 4000.; {"MaximumEngineSpeed", 4000.}};
AlgorithmLongitudinalCalculations calculation{0.0, 0.0, vehicleParameters}; std::function<void (CbkLogLevel, const char*, int, const std::string&)> Log;
AlgorithmLongitudinalCalculations calculation{0.0, 0.0, vehicleParameters, Log};
// Call test // Call test
double result = calculation.GetEngineTorqueMin(data.input_EngineSpeed); double result = calculation.GetEngineTorqueMin(data.input_EngineSpeed);
...@@ -177,13 +179,19 @@ TEST_P(AlgorithmLongitudinalCalculationsGetAccFromEngineTorque, AlgorithmLongitu ...@@ -177,13 +179,19 @@ TEST_P(AlgorithmLongitudinalCalculationsGetAccFromEngineTorque, AlgorithmLongitu
// Set up test // Set up test
VehicleModelParameters vehicleParameters; VehicleModelParameters vehicleParameters;
vehicleParameters.axleRatio = 2.8; vehicleParameters.properties = {{"AxleRatio", 2.8},
std::vector<double> gRatios = {0., 4.1, 2.5, 1.4, 1., .9, .7}; {"GearRatio0", 0},
vehicleParameters.gearRatios = gRatios; {"GearRatio1", 4.1},
vehicleParameters.staticWheelRadius = .35; {"GearRatio2", 2.5},
vehicleParameters.weight = 800.; {"GearRatio3", 1.4},
{"GearRatio4", 1.},
AlgorithmLongitudinalCalculations calculation{0.0, 0.0, vehicleParameters}; {"GearRatio5", .9},
{"GearRatio6", .7},
{"Mass", 800}};
vehicleParameters.rearAxle.wheelDiameter = 0.7;
std::function<void (CbkLogLevel, const char*, int, const std::string&)> Log;
AlgorithmLongitudinalCalculations calculation{0.0, 0.0, vehicleParameters, Log};
// Call test // Call test
double result = calculation.GetAccFromEngineTorque(data.input_EngineTorque, data.input_ChosenGear); double result = calculation.GetAccFromEngineTorque(data.input_EngineTorque, data.input_ChosenGear);
...@@ -240,12 +248,18 @@ TEST_P(AlgorithmLongitudinalCalculationsGetEngineSpeedByVelocity, AlgorithmLongi ...@@ -240,12 +248,18 @@ TEST_P(AlgorithmLongitudinalCalculationsGetEngineSpeedByVelocity, AlgorithmLongi
// Set up tests // Set up tests
VehicleModelParameters vehicleParameters; VehicleModelParameters vehicleParameters;
vehicleParameters.axleRatio = 1.; vehicleParameters.properties = {{"AxleRatio", 1},
std::vector<double> gRatios = {0., 4.1, 2.5, 1.4, 1., .9, .7}; {"GearRatio1", 4.1},
vehicleParameters.gearRatios = gRatios; {"GearRatio2", 2.5},
vehicleParameters.staticWheelRadius = .25; {"GearRatio3", 1.4},
{"GearRatio4", 1.},
AlgorithmLongitudinalCalculations calculation{0.0, 0.0, vehicleParameters}; {"GearRatio5", .9},
{"GearRatio6", .7},
{"Mass", 800}};
vehicleParameters.rearAxle.wheelDiameter = 0.5;
std::function<void (CbkLogLevel, const char*, int, const std::string&)> Log;
AlgorithmLongitudinalCalculations calculation{0.0, 0.0, vehicleParameters, Log};
// Call test // Call test
double result = calculation.GetEngineSpeedByVelocity(data.input_Velocity, 4) * 2 * M_PI; double result = calculation.GetEngineSpeedByVelocity(data.input_Velocity, 4) * 2 * M_PI;
...@@ -302,14 +316,19 @@ TEST_P(AlgorithmLongitudinalCalculationsGetEngineTorqueAtGear, AlgorithmLongitud ...@@ -302,14 +316,19 @@ TEST_P(AlgorithmLongitudinalCalculationsGetEngineTorqueAtGear, AlgorithmLongitud
// Set up tests // Set up tests
VehicleModelParameters vehicleParameters; VehicleModelParameters vehicleParameters;
vehicleParameters.weight = 500.; vehicleParameters.properties = {{"AxleRatio", 1.},
vehicleParameters.staticWheelRadius = .25; {"GearRatio1", 4.1},
vehicleParameters.numberOfGears = 6; {"GearRatio2", 2.5},
vehicleParameters.axleRatio = 1.; {"GearRatio3", 1.4},
std::vector<double> gRatios = {0., 4.1, 2.5, 1.4, 1., .9, .7}; {"GearRatio4", 1.},
vehicleParameters.gearRatios = gRatios; {"GearRatio5", .9},
{"GearRatio6", .7},
AlgorithmLongitudinalCalculations calculation{0.0, 0.0, vehicleParameters}; {"NumberOfGears", 6},
{"Mass", 500}};
vehicleParameters.rearAxle.wheelDiameter = 0.5;
std::function<void (CbkLogLevel, const char*, int, const std::string&)> Log;
AlgorithmLongitudinalCalculations calculation{0.0, 0.0, vehicleParameters, Log};
// Call test // Call test
double result = calculation.GetEngineTorqueAtGear(data.input_Gear, data.input_Acceleration); double result = calculation.GetEngineTorqueAtGear(data.input_Gear, data.input_Acceleration);
...@@ -370,16 +389,17 @@ TEST_P(AlgorithmLongitudinalCalculationsPedalPosition, AlgorithmLongitudinalCalc ...@@ -370,16 +389,17 @@ TEST_P(AlgorithmLongitudinalCalculationsPedalPosition, AlgorithmLongitudinalCalc
// Set up test // Set up test
VehicleModelParameters vehicleParameters; VehicleModelParameters vehicleParameters;
vehicleParameters.maximumEngineTorque = data.engineTorqueMax; vehicleParameters.properties = {{"AxleRatio", 1},
vehicleParameters.numberOfGears = 1; {"GearRatio1", 1},
vehicleParameters.gearRatios = {1.0, 1.0}; {"NumberOfGears", 1},
vehicleParameters.axleRatio = 1.0; {"MaximumEngineTorque", data.engineTorqueMax},
vehicleParameters.staticWheelRadius = 1.0; {"MinimumEngineSpeed", -10000},
vehicleParameters.weight = 1000.0; {"MaximumEngineSpeed", 10000},
vehicleParameters.minimumEngineSpeed = -10000; {"Mass", 1000}};
vehicleParameters.maximumEngineSpeed = 10000; vehicleParameters.rearAxle.wheelDiameter = 2.;
AlgorithmLongitudinalCalculations calculation{0.0, data.input_Acceleration, vehicleParameters}; std::function<void (CbkLogLevel, const char*, int, const std::string&)> Log;
AlgorithmLongitudinalCalculations calculation{0.0, data.input_Acceleration, vehicleParameters, Log};
// Call test // Call test
calculation.CalculatePedalPositions(); calculation.CalculatePedalPositions();
......
...@@ -19,14 +19,15 @@ ...@@ -19,14 +19,15 @@
using ::testing::Return; using ::testing::Return;
using ::testing::ReturnPointee; using ::testing::ReturnPointee;
using ::testing::NiceMock; using ::testing::NiceMock;
using ::testing::DoubleNear;
class DynamicsCollision_Test : public ::testing::Test class DynamicsCollision_Test : public ::testing::Test
{ {
public: public:
DynamicsCollision_Test() DynamicsCollision_Test()
{ {
heavyVehicle.weight = 2000.0; heavyVehicle.properties = {{"Mass", 2000.0}};
lightVehicle.weight = 1000.0; lightVehicle.properties = {{"Mass", 1000.0}};
} }
protected: protected:
...@@ -73,8 +74,8 @@ TEST_F(DynamicsCollision_Test, CollisionOfTwoAgentsOnlyInXDirection) ...@@ -73,8 +74,8 @@ TEST_F(DynamicsCollision_Test, CollisionOfTwoAgentsOnlyInXDirection)
dynamicsCollision.Trigger(0); dynamicsCollision.Trigger(0);
ASSERT_DOUBLE_EQ(dynamicsCollision.GetVelocity(), 30.0); ASSERT_THAT(dynamicsCollision.GetVelocity(), DoubleNear(30.0, 1e-3));
ASSERT_DOUBLE_EQ(dynamicsCollision.GetMovingDirection(), 0.0); ASSERT_THAT(dynamicsCollision.GetMovingDirection(), DoubleNear(0.0, 1e-3));
} }
TEST_F(DynamicsCollision_Test, CollisionOfTwoAgentsOrthogonal) TEST_F(DynamicsCollision_Test, CollisionOfTwoAgentsOrthogonal)
...@@ -117,8 +118,8 @@ TEST_F(DynamicsCollision_Test, CollisionOfTwoAgentsOrthogonal) ...@@ -117,8 +118,8 @@ TEST_F(DynamicsCollision_Test, CollisionOfTwoAgentsOrthogonal)
dynamicsCollision.Trigger(0); dynamicsCollision.Trigger(0);
double expectedVelocity = 20.0 * std::sqrt(2); double expectedVelocity = 20.0 * std::sqrt(2);
ASSERT_DOUBLE_EQ(dynamicsCollision.GetVelocity(), expectedVelocity); ASSERT_THAT(dynamicsCollision.GetVelocity(), DoubleNear(expectedVelocity, 1e-3));
ASSERT_DOUBLE_EQ(dynamicsCollision.GetMovingDirection(), 0.25 * M_PI); ASSERT_THAT(dynamicsCollision.GetMovingDirection(), DoubleNear(0.25 * M_PI, 1e-3));
} }
TEST_F(DynamicsCollision_Test, CollisionOfTwoAgentsOpposingDirections) TEST_F(DynamicsCollision_Test, CollisionOfTwoAgentsOpposingDirections)
...@@ -211,8 +212,8 @@ TEST_F(DynamicsCollision_Test, CollisionOfThreeAgentsOnlyInXDirection) ...@@ -211,8 +212,8 @@ TEST_F(DynamicsCollision_Test, CollisionOfThreeAgentsOnlyInXDirection)
dynamicsCollision.Trigger(0); dynamicsCollision.Trigger(0);
ASSERT_DOUBLE_EQ(dynamicsCollision.GetVelocity(), 25.0); ASSERT_THAT(dynamicsCollision.GetVelocity(), DoubleNear(25.0, 1e-3));
ASSERT_DOUBLE_EQ(dynamicsCollision.GetMovingDirection(), 0.0); ASSERT_THAT(dynamicsCollision.GetMovingDirection(), DoubleNear(0.0, 1e-3));
} }
TEST_F(DynamicsCollision_Test, CollisionOfThreeAgentsInDifferentDirections) TEST_F(DynamicsCollision_Test, CollisionOfThreeAgentsInDifferentDirections)
...@@ -264,8 +265,8 @@ TEST_F(DynamicsCollision_Test, CollisionOfThreeAgentsInDifferentDirections) ...@@ -264,8 +265,8 @@ TEST_F(DynamicsCollision_Test, CollisionOfThreeAgentsInDifferentDirections)
dynamicsCollision.Trigger(0); dynamicsCollision.Trigger(0);
double expectedVelocity = 20.0 * std::sqrt(2); double expectedVelocity = 20.0 * std::sqrt(2);
ASSERT_DOUBLE_EQ(dynamicsCollision.GetVelocity(), expectedVelocity); ASSERT_THAT(dynamicsCollision.GetVelocity(), DoubleNear(expectedVelocity, 1e-3));
ASSERT_DOUBLE_EQ(dynamicsCollision.GetMovingDirection(), -0.25 * M_PI); ASSERT_THAT(dynamicsCollision.GetMovingDirection(), DoubleNear(-0.25 * M_PI, 1e-3));
} }
TEST_F(DynamicsCollision_Test, CollisionOfAgentWithFixedObject) TEST_F(DynamicsCollision_Test, CollisionOfAgentWithFixedObject)
...@@ -298,7 +299,7 @@ TEST_F(DynamicsCollision_Test, CollisionOfAgentWithFixedObject) ...@@ -298,7 +299,7 @@ TEST_F(DynamicsCollision_Test, CollisionOfAgentWithFixedObject)
dynamicsCollision.Trigger(0); dynamicsCollision.Trigger(0);
ASSERT_DOUBLE_EQ(dynamicsCollision.GetVelocity(), 0.0); ASSERT_THAT(dynamicsCollision.GetVelocity(), DoubleNear(0.0, 1e-3));
} }
...@@ -95,16 +95,20 @@ TEST_P(MaximumLimit, ...@@ -95,16 +95,20 @@ TEST_P(MaximumLimit,
const auto accelerationInputSignal = std::make_shared<AccelerationSignal const>(ComponentState::Acting, INFINITY); const auto accelerationInputSignal = std::make_shared<AccelerationSignal const>(ComponentState::Acting, INFINITY);
VehicleModelParameters fakeVehicleParameters; VehicleModelParameters fakeVehicleParameters;
fakeVehicleParameters.weight = testInput.weight; fakeVehicleParameters.properties = {{"Mass", testInput.weight},
fakeVehicleParameters.frontSurface = testInput.frontSurface; {"FrontSurface", testInput.frontSurface},
fakeVehicleParameters.airDragCoefficient = testInput.airDragCoefficient; {"AirDragCoefficient", testInput.airDragCoefficient},
fakeVehicleParameters.maximumEngineTorque = testInput.maximumEngineTorque; {"MaximumEngineTorque", testInput.maximumEngineTorque},
fakeVehicleParameters.maximumEngineSpeed = testInput.maximumEngineSpeed; {"MinimumEngineSpeed", testInput.minimumEngineSpeed},
fakeVehicleParameters.minimumEngineSpeed = testInput.minimumEngineSpeed; {"MaximumEngineSpeed", testInput.maximumEngineSpeed},
fakeVehicleParameters.gearRatios = testInput.gearRatios; {"AxleRatio", testInput.axleRatio},
fakeVehicleParameters.axleRatio = testInput.axleRatio; {"FrictionCoefficient", 1.0},
fakeVehicleParameters.staticWheelRadius = testInput.staticWheelRadius; {"NumberOfGears", testInput.gearRatios.size()}};
fakeVehicleParameters.frictionCoeff = 1.0; for (size_t i = 0; i < testInput.gearRatios.size(); i++)
{
fakeVehicleParameters.properties.insert({"GearRatio"+std::to_string(i+1), testInput.gearRatios[i]});
}
fakeVehicleParameters.rearAxle.wheelDiameter = 2 * testInput.staticWheelRadius;
const auto vehicleParameterInputSignal = std::make_shared<ParametersVehicleSignal const>(fakeVehicleParameters); const auto vehicleParameterInputSignal = std::make_shared<ParametersVehicleSignal const>(fakeVehicleParameters);
......
...@@ -350,8 +350,8 @@ TEST(DynamicAgentTypeGenerator, SetVehicleModelParameters) ...@@ -350,8 +350,8 @@ TEST(DynamicAgentTypeGenerator, SetVehicleModelParameters)
ON_CALL(profiles, GetVehicleProfiles()).WillByDefault(ReturnRef(vehicleProfiles)); ON_CALL(profiles, GetVehicleProfiles()).WillByDefault(ReturnRef(vehicleProfiles));
VehicleModelParameters vehicleModelParameters; VehicleModelParameters vehicleModelParameters;
vehicleModelParameters.length = 5.0; vehicleModelParameters.boundingBoxDimensions.length = 5.0;
vehicleModelParameters.width = 2.0; vehicleModelParameters.boundingBoxDimensions.width = 2.0;
ON_CALL(vehicleModels, GetVehicleModel("SomeVehicleModel", _)).WillByDefault(Return(vehicleModelParameters)); ON_CALL(vehicleModels, GetVehicleModel("SomeVehicleModel", _)).WillByDefault(Return(vehicleModelParameters));
openScenario::Parameters assignedParameters; openScenario::Parameters assignedParameters;
...@@ -360,8 +360,8 @@ TEST(DynamicAgentTypeGenerator, SetVehicleModelParameters) ...@@ -360,8 +360,8 @@ TEST(DynamicAgentTypeGenerator, SetVehicleModelParameters)
.SetVehicleModelParameters(assignedParameters); .SetVehicleModelParameters(assignedParameters);
ASSERT_THAT(agentBuildInformation.vehicleModelName, Eq("SomeVehicleModel")); ASSERT_THAT(agentBuildInformation.vehicleModelName, Eq("SomeVehicleModel"));
ASSERT_THAT(agentBuildInformation.vehicleModelParameters.length, Eq(5.0)); ASSERT_THAT(agentBuildInformation.vehicleModelParameters.boundingBoxDimensions.length, Eq(5.0));
ASSERT_THAT(agentBuildInformation.vehicleModelParameters.width, Eq(2.0)); ASSERT_THAT(agentBuildInformation.vehicleModelParameters.boundingBoxDimensions.width, Eq(2.0));
} }
TEST(DynamicParametersSampler, SampleSensorLatencies) TEST(DynamicParametersSampler, SampleSensorLatencies)
......
...@@ -41,7 +41,9 @@ MATCHER_P(MatchesAgentBlueprint, referenceAgentBlueprint, "matches blueprint") ...@@ -41,7 +41,9 @@ MATCHER_P(MatchesAgentBlueprint, referenceAgentBlueprint, "matches blueprint")
} }
const auto actualVehicleModelParameters = arg->GetVehicleModelParameters(); const auto actualVehicleModelParameters = arg->GetVehicleModelParameters();
const auto expectedVehicleModelParameters = referenceAgentBlueprint.GetVehicleModelParameters(); const auto expectedVehicleModelParameters = referenceAgentBlueprint.GetVehicleModelParameters();
if (!(actualVehicleModelParameters.length == expectedVehicleModelParameters.length && actualVehicleModelParameters.width == expectedVehicleModelParameters.width && actualVehicleModelParameters.distanceReferencePointToLeadingEdge == expectedVehicleModelParameters.distanceReferencePointToLeadingEdge)) if (!(actualVehicleModelParameters.boundingBoxDimensions.length == expectedVehicleModelParameters.boundingBoxDimensions.length
&& actualVehicleModelParameters.boundingBoxDimensions.width == expectedVehicleModelParameters.boundingBoxDimensions.width
&& actualVehicleModelParameters.boundingBoxCenter.x == expectedVehicleModelParameters.boundingBoxCenter.x))
{ {
return false; return false;
} }
...@@ -80,9 +82,9 @@ TEST(SpawnPointScenario, Trigger_SpawnsEgoAgentAccordingToScenarioWorldPosition) ...@@ -80,9 +82,9 @@ TEST(SpawnPointScenario, Trigger_SpawnsEgoAgentAccordingToScenarioWorldPosition)
std::optional<AgentBlueprint> actualAgentBlueprintOptional; std::optional<AgentBlueprint> actualAgentBlueprintOptional;
AgentBlueprint actualAgentBlueprint; AgentBlueprint actualAgentBlueprint;
VehicleModelParameters vehicleModelParameters; VehicleModelParameters vehicleModelParameters;
vehicleModelParameters.length = 1; vehicleModelParameters.boundingBoxDimensions.length = 1;
vehicleModelParameters.width = 0.5; vehicleModelParameters.boundingBoxDimensions.width = 0.5;
vehicleModelParameters.distanceReferencePointToLeadingEdge = 0.5; vehicleModelParameters.boundingBoxCenter.x = 0.;
actualAgentBlueprint.SetVehicleModelParameters(vehicleModelParameters); actualAgentBlueprint.SetVehicleModelParameters(vehicleModelParameters);
actualAgentBlueprintOptional = actualAgentBlueprint; actualAgentBlueprintOptional = actualAgentBlueprint;
...@@ -159,9 +161,9 @@ TEST(SpawnPointScenario, Trigger_SpawnsEgoAgentAccordingToScenarioLanePosition) ...@@ -159,9 +161,9 @@ TEST(SpawnPointScenario, Trigger_SpawnsEgoAgentAccordingToScenarioLanePosition)
std::optional<AgentBlueprint> actualAgentBlueprintOptional; std::optional<AgentBlueprint> actualAgentBlueprintOptional;
AgentBlueprint actualAgentBlueprint; AgentBlueprint actualAgentBlueprint;
VehicleModelParameters vehicleModelParameters; VehicleModelParameters vehicleModelParameters;
vehicleModelParameters.length = 1; vehicleModelParameters.boundingBoxDimensions.length = 1;
vehicleModelParameters.width = 0.5; vehicleModelParameters.boundingBoxDimensions.width = 0.5;
vehicleModelParameters.distanceReferencePointToLeadingEdge = 0.5; vehicleModelParameters.boundingBoxCenter.x = 0.;
actualAgentBlueprint.SetVehicleModelParameters(vehicleModelParameters); actualAgentBlueprint.SetVehicleModelParameters(vehicleModelParameters);
actualAgentBlueprintOptional = actualAgentBlueprint; actualAgentBlueprintOptional = actualAgentBlueprint;
...@@ -245,9 +247,9 @@ TEST(SpawnPointScenario, Trigger_SpawnsScenarioAgentAccordingToScenarioWorldPosi ...@@ -245,9 +247,9 @@ TEST(SpawnPointScenario, Trigger_SpawnsScenarioAgentAccordingToScenarioWorldPosi
std::optional<AgentBlueprint> actualAgentBlueprintOptional; std::optional<AgentBlueprint> actualAgentBlueprintOptional;
AgentBlueprint actualAgentBlueprint; AgentBlueprint actualAgentBlueprint;
VehicleModelParameters vehicleModelParameters; VehicleModelParameters vehicleModelParameters;
vehicleModelParameters.length = 1; vehicleModelParameters.boundingBoxDimensions.length = 1;
vehicleModelParameters.width = 0.5; vehicleModelParameters.boundingBoxDimensions.width = 0.5;
vehicleModelParameters.distanceReferencePointToLeadingEdge = 0.5; vehicleModelParameters.boundingBoxCenter.x = 0.;
actualAgentBlueprint.SetVehicleModelParameters(vehicleModelParameters); actualAgentBlueprint.SetVehicleModelParameters(vehicleModelParameters);
actualAgentBlueprintOptional = actualAgentBlueprint; actualAgentBlueprintOptional = actualAgentBlueprint;
...@@ -324,9 +326,9 @@ TEST(SpawnPointScenario, Trigger_SpawnsScenarioAgentAccordingToScenarioLanePosit ...@@ -324,9 +326,9 @@ TEST(SpawnPointScenario, Trigger_SpawnsScenarioAgentAccordingToScenarioLanePosit
std::optional<AgentBlueprint> actualAgentBlueprintOptional; std::optional<AgentBlueprint> actualAgentBlueprintOptional;
AgentBlueprint actualAgentBlueprint; AgentBlueprint actualAgentBlueprint;
VehicleModelParameters vehicleModelParameters; VehicleModelParameters vehicleModelParameters;
vehicleModelParameters.length = 1; vehicleModelParameters.boundingBoxDimensions.length = 1;
vehicleModelParameters.width = 0.5; vehicleModelParameters.boundingBoxDimensions.width = 0.5;
vehicleModelParameters.distanceReferencePointToLeadingEdge = 0.5; vehicleModelParameters.boundingBoxCenter.x = 0.;
actualAgentBlueprint.SetVehicleModelParameters(vehicleModelParameters); actualAgentBlueprint.SetVehicleModelParameters(vehicleModelParameters);
actualAgentBlueprintOptional = actualAgentBlueprint; actualAgentBlueprintOptional = actualAgentBlueprint;
......
...@@ -12,6 +12,7 @@ ...@@ -12,6 +12,7 @@
#include "gmock/gmock.h" #include "gmock/gmock.h"
#include "AgentAdapter.h" #include "AgentAdapter.h"
#include "fakeMovingObject.h"
using ::testing::Return; using ::testing::Return;
using ::testing::ReturnRef; using ::testing::ReturnRef;
...@@ -23,6 +24,7 @@ using ::testing::Le; ...@@ -23,6 +24,7 @@ using ::testing::Le;
using ::testing::Lt; using ::testing::Lt;
using ::testing::AllOf; using ::testing::AllOf;
using ::testing::DoubleEq; using ::testing::DoubleEq;
using ::testing::ElementsAreArray;
TEST(MovingObject_Tests, SetAndGetReferencePointPosition_ReturnsCorrectPosition) TEST(MovingObject_Tests, SetAndGetReferencePointPosition_ReturnsCorrectPosition)
{ {
...@@ -33,7 +35,7 @@ TEST(MovingObject_Tests, SetAndGetReferencePointPosition_ReturnsCorrectPosition) ...@@ -33,7 +35,7 @@ TEST(MovingObject_Tests, SetAndGetReferencePointPosition_ReturnsCorrectPosition)
osi3::MovingObject osiObject; osi3::MovingObject osiObject;
OWL::Implementation::MovingObject movingObject(&osiObject, nullptr); OWL::Implementation::MovingObject movingObject(&osiObject, nullptr);
movingObject.SetLength(8.0); movingObject.SetLength(8.0);
movingObject.SetDistanceReferencPointToLeadingEdge(7.0); movingObject.SetBoundingBoxCenterToRear(7.0);
movingObject.SetYaw(0.5); movingObject.SetYaw(0.5);
movingObject.SetReferencePointPosition(position); movingObject.SetReferencePointPosition(position);
OWL::Primitive::AbsPosition resultPosition = movingObject.GetReferencePointPosition(); OWL::Primitive::AbsPosition resultPosition = movingObject.GetReferencePointPosition();
...@@ -51,7 +53,7 @@ TEST(MovingObject_Tests, SetAndGetReferencePointPositionWithYawChangeInBetween_R ...@@ -51,7 +53,7 @@ TEST(MovingObject_Tests, SetAndGetReferencePointPositionWithYawChangeInBetween_R
osi3::MovingObject osiObject; osi3::MovingObject osiObject;
OWL::Implementation::MovingObject movingObject(&osiObject, nullptr); OWL::Implementation::MovingObject movingObject(&osiObject, nullptr);
movingObject.SetLength(8.0); movingObject.SetLength(8.0);
movingObject.SetDistanceReferencPointToLeadingEdge(7.0); movingObject.SetBoundingBoxCenterToRear(7.0);
movingObject.SetYaw(0.5); movingObject.SetYaw(0.5);
movingObject.SetReferencePointPosition(position); movingObject.SetReferencePointPosition(position);
movingObject.SetYaw(0.7); movingObject.SetYaw(0.7);
...@@ -70,7 +72,7 @@ TEST(MovingObject_Tests, SetReferencePointPosition_SetsCorrectPositionOnOSIObjec ...@@ -70,7 +72,7 @@ TEST(MovingObject_Tests, SetReferencePointPosition_SetsCorrectPositionOnOSIObjec
osi3::MovingObject osiObject; osi3::MovingObject osiObject;
OWL::Implementation::MovingObject movingObject(&osiObject, nullptr); OWL::Implementation::MovingObject movingObject(&osiObject, nullptr);
movingObject.SetLength(8.0); movingObject.SetLength(8.0);
movingObject.SetDistanceReferencPointToLeadingEdge(6.0); movingObject.SetBoundingBoxCenterToRear(-2.0);
movingObject.SetYaw(M_PI * 0.25); movingObject.SetYaw(M_PI * 0.25);
movingObject.SetReferencePointPosition(position); movingObject.SetReferencePointPosition(position);
auto resultPosition = osiObject.base().position(); auto resultPosition = osiObject.base().position();
...@@ -100,4 +102,66 @@ TEST(MovingObject_Tests, SetAgentType_MapsCorrectOSIType) ...@@ -100,4 +102,66 @@ TEST(MovingObject_Tests, SetAgentType_MapsCorrectOSIType)
movingObject.SetType(AgentVehicleType::Pedestrian); movingObject.SetType(AgentVehicleType::Pedestrian);
ASSERT_THAT(osiObject.type(), osi3::MovingObject_Type::MovingObject_Type_TYPE_PEDESTRIAN); ASSERT_THAT(osiObject.type(), osi3::MovingObject_Type::MovingObject_Type_TYPE_PEDESTRIAN);
} }
\ No newline at end of file
struct CalculateBoundingBoxData
{
double yaw;
double roll;
std::vector<std::pair<double,double>> expectedResult;
};
class CalculateBoundingBox_Tests : public testing::Test,
public ::testing::WithParamInterface<CalculateBoundingBoxData>
{
};
class TestWorldObject : public WorldObjectAdapter
{
public:
TestWorldObject (OWL::Interfaces::WorldObject& baseTrafficObject) :
WorldObjectAdapter(baseTrafficObject)
{}
virtual double GetLaneRemainder(const std::string& roadId, Side) const{};
virtual ObjectTypeOSI GetType() const {}
virtual const ObjectPosition& GetObjectPosition() const {}
virtual double GetDistanceToStartOfRoad(MeasurementPoint mp, std::string roadId) const {return 0;}
virtual double GetVelocity(VelocityScope velocityScope) const {return 0;}
virtual bool Locate() {return false;}
virtual void Unlocate() {};
};
TEST_P(CalculateBoundingBox_Tests, CalculateBoundingBox_ReturnCorrectPoints)
{
const auto& data = GetParam();
OWL::Fakes::MovingObject movingObject;
OWL::Primitive::AbsPosition position{10, 20, 0};
ON_CALL(movingObject, GetReferencePointPosition()).WillByDefault(Return(position));
OWL::Primitive::Dimension dimension{6.0, 2.0, 1.6};
ON_CALL(movingObject, GetDimension()).WillByDefault(Return(dimension));
OWL::Primitive::AbsOrientation orientation{data.yaw, 0, data.roll};
ON_CALL(movingObject, GetAbsOrientation()).WillByDefault(Return(orientation));
TestWorldObject object(movingObject);
auto result = object.GetBoundingBox2D();
std::vector<std::pair<double,double>> resultPoints;
for (const point_t point : result.outer())
{
resultPoints.emplace_back(bg::get<0>(point), bg::get<1>(point));
}
resultPoints.pop_back(); //in boost the last point is equal to the first
ASSERT_THAT(resultPoints, ElementsAreArray(data.expectedResult));
}
INSTANTIATE_TEST_SUITE_P(BoundingBoxTest, CalculateBoundingBox_Tests,
testing::Values(
//! yaw roll expectedResult
CalculateBoundingBoxData{0.0, 0.0, {{7.0, 19.0},{7.0, 21.0}, {13.0, 21.0}, {13.0, 19.0}}},
CalculateBoundingBoxData{M_PI_2, 0.0, {{11.0, 17.0},{9.0, 17.0}, {9.0, 23.0}, {11.0, 23.0}}},
CalculateBoundingBoxData{0.0, M_PI_4, {{7.0, 20 - 2.6*M_SQRT1_2},{7.0, 20.0 + M_SQRT1_2}, {13.0, 20.0 + M_SQRT1_2}, {13.0, 20.0 - 2.6*M_SQRT1_2}}},
CalculateBoundingBoxData{0.0, -M_PI_4, {{7.0, 20 - M_SQRT1_2},{7.0, 20.0 + 2.6*M_SQRT1_2}, {13.0, 20.0 + 2.6*M_SQRT1_2}, {13.0, 20.0 - M_SQRT1_2}}}
));
...@@ -49,7 +49,7 @@ const std::string validVehicleString{ ...@@ -49,7 +49,7 @@ const std::string validVehicleString{
"<Center x=\"1.6\" y=\"1.7\" z=\"1.8\"/>" "<Center x=\"1.6\" y=\"1.7\" z=\"1.8\"/>"
"<Dimensions width=\"1.9\" length=\"2.0\" height=\"2.1\"/>" "<Dimensions width=\"1.9\" length=\"2.0\" height=\"2.1\"/>"
"</BoundingBox>" "</BoundingBox>"
"<Performance maxSpeed=\"22.0\" maxDeceleration=\"23.0\"/>" "<Performance maxSpeed=\"22.0\" maxAcceleration=\"10.0\" maxDeceleration=\"23.0\"/>"
"<Axles>" "<Axles>"
"<FrontAxle maxSteering=\"1.1\" wheelDiameter=\"26.0\" trackWidth=\"27.0\" positionX=\"28.0\" positionZ=\"29.0\"/>" "<FrontAxle maxSteering=\"1.1\" wheelDiameter=\"26.0\" trackWidth=\"27.0\" positionX=\"28.0\" positionZ=\"29.0\"/>"
"<RearAxle maxSteering=\"0.0\" wheelDiameter=\"31.0\" trackWidth=\"32.0\" positionX=\"0.0\" positionZ=\"34.0\"/>" "<RearAxle maxSteering=\"0.0\" wheelDiameter=\"31.0\" trackWidth=\"32.0\" positionX=\"0.0\" positionZ=\"34.0\"/>"
...@@ -93,7 +93,7 @@ const std::string parametrizedVehicleString{ ...@@ -93,7 +93,7 @@ const std::string parametrizedVehicleString{
"<Center x=\"$CustomX\" y=\"1.7\" z=\"1.8\"/>" "<Center x=\"$CustomX\" y=\"1.7\" z=\"1.8\"/>"
"<Dimensions width=\"1.9\" length=\"$CustomLength\" height=\"2.1\"/>" "<Dimensions width=\"1.9\" length=\"$CustomLength\" height=\"2.1\"/>"
"</BoundingBox>" "</BoundingBox>"
"<Performance maxSpeed=\"$CustomSpeed\" maxDeceleration=\"23.0\"/>" "<Performance maxSpeed=\"$CustomSpeed\" maxAcceleration=\"10.0\" maxDeceleration=\"23.0\"/>"
"<Axles>" "<Axles>"
"<FrontAxle maxSteering=\"1.1\" wheelDiameter=\"26.0\" trackWidth=\"$CustomFrontTrackWidth\" positionX=\"28.0\" positionZ=\"29.0\"/>" "<FrontAxle maxSteering=\"1.1\" wheelDiameter=\"26.0\" trackWidth=\"$CustomFrontTrackWidth\" positionX=\"28.0\" positionZ=\"29.0\"/>"
"<RearAxle maxSteering=\"0.0\" wheelDiameter=\"31.0\" trackWidth=\"32.0\" positionX=\"0.0\" positionZ=\"34.0\"/>" "<RearAxle maxSteering=\"0.0\" wheelDiameter=\"31.0\" trackWidth=\"32.0\" positionX=\"0.0\" positionZ=\"34.0\"/>"
...@@ -134,15 +134,24 @@ TEST(VehicleModelsImporter, GivenValidVehicle_ImportsCorrectGeometry) ...@@ -134,15 +134,24 @@ TEST(VehicleModelsImporter, GivenValidVehicle_ImportsCorrectGeometry)
ASSERT_NO_THROW(VehicleModelsImporter::ImportVehicleModel(fakeVehicleRoot, vehicleModelMap)); ASSERT_NO_THROW(VehicleModelsImporter::ImportVehicleModel(fakeVehicleRoot, vehicleModelMap));
ASSERT_NO_THROW(vehicleModelParameters = vehicleModelMap.at("validCar").Get(EMPTY_PARAMETERS)); ASSERT_NO_THROW(vehicleModelParameters = vehicleModelMap.at("validCar").Get(EMPTY_PARAMETERS));
EXPECT_THAT(vehicleModelParameters.length, DoubleEq(2.0)); EXPECT_THAT(vehicleModelParameters.boundingBoxDimensions.length, DoubleEq(2.0));
EXPECT_THAT(vehicleModelParameters.width, DoubleEq(1.9)); EXPECT_THAT(vehicleModelParameters.boundingBoxDimensions.width, DoubleEq(1.9));
EXPECT_THAT(vehicleModelParameters.height, DoubleEq(2.1)); EXPECT_THAT(vehicleModelParameters.boundingBoxDimensions.height, DoubleEq(2.1));
EXPECT_THAT(vehicleModelParameters.trackwidth, DoubleEq(32.0)); EXPECT_THAT(vehicleModelParameters.boundingBoxCenter.x, DoubleEq(1.6));
EXPECT_THAT(vehicleModelParameters.wheelbase, DoubleEq(28.0)); EXPECT_THAT(vehicleModelParameters.boundingBoxCenter.y, DoubleEq(1.7));
EXPECT_THAT(vehicleModelParameters.heightCOG, DoubleEq(0.0)); EXPECT_THAT(vehicleModelParameters.boundingBoxCenter.z, DoubleEq(1.8));
EXPECT_THAT(vehicleModelParameters.distanceReferencePointToFrontAxle, DoubleEq(28.0)); EXPECT_THAT(vehicleModelParameters.performance.maxSpeed, DoubleEq(22.0));
EXPECT_THAT(vehicleModelParameters.distanceReferencePointToLeadingEdge, DoubleEq(2.6)); // BB center x + length / 2 EXPECT_THAT(vehicleModelParameters.performance.maxDeceleration, DoubleEq(23.0));
EXPECT_THAT(vehicleModelParameters.staticWheelRadius, DoubleEq(15.5)); EXPECT_THAT(vehicleModelParameters.frontAxle.maxSteering, DoubleEq(1.1));
EXPECT_THAT(vehicleModelParameters.frontAxle.wheelDiameter, DoubleEq(26.0));
EXPECT_THAT(vehicleModelParameters.frontAxle.trackWidth, DoubleEq(27.));
EXPECT_THAT(vehicleModelParameters.frontAxle.positionX, DoubleEq(28.));
EXPECT_THAT(vehicleModelParameters.frontAxle.positionZ, DoubleEq(29.));
EXPECT_THAT(vehicleModelParameters.rearAxle.maxSteering, DoubleEq(0.0));
EXPECT_THAT(vehicleModelParameters.rearAxle.wheelDiameter, DoubleEq(31.0));
EXPECT_THAT(vehicleModelParameters.rearAxle.trackWidth, DoubleEq(32.));
EXPECT_THAT(vehicleModelParameters.rearAxle.positionX, DoubleEq(0.));
EXPECT_THAT(vehicleModelParameters.rearAxle.positionZ, DoubleEq(34.));
} }
TEST(VehicleModelsImporter, GivenValidVehicle_ImportsCorrectEngineParameters) TEST(VehicleModelsImporter, GivenValidVehicle_ImportsCorrectEngineParameters)
...@@ -155,11 +164,11 @@ TEST(VehicleModelsImporter, GivenValidVehicle_ImportsCorrectEngineParameters) ...@@ -155,11 +164,11 @@ TEST(VehicleModelsImporter, GivenValidVehicle_ImportsCorrectEngineParameters)
ASSERT_NO_THROW(VehicleModelsImporter::ImportVehicleModel(fakeVehicleRoot, vehicleModelMap)); ASSERT_NO_THROW(VehicleModelsImporter::ImportVehicleModel(fakeVehicleRoot, vehicleModelMap));
ASSERT_NO_THROW(vehicleModelParameters = vehicleModelMap.at("validCar").Get(EMPTY_PARAMETERS)); ASSERT_NO_THROW(vehicleModelParameters = vehicleModelMap.at("validCar").Get(EMPTY_PARAMETERS));
EXPECT_THAT(vehicleModelParameters.decelerationFromPowertrainDrag, DoubleEq(3.0)); EXPECT_THAT(vehicleModelParameters.properties.at("DecelerationFromPowertrainDrag"), DoubleEq(3.0));
EXPECT_THAT(vehicleModelParameters.minimumEngineTorque, DoubleEq(7.0)); EXPECT_THAT(vehicleModelParameters.properties.at("MinimumEngineTorque"), DoubleEq(7.0));
EXPECT_THAT(vehicleModelParameters.maximumEngineTorque, DoubleEq(8.0)); EXPECT_THAT(vehicleModelParameters.properties.at("MaximumEngineTorque"), DoubleEq(8.0));
EXPECT_THAT(vehicleModelParameters.minimumEngineSpeed, DoubleEq(9.0)); EXPECT_THAT(vehicleModelParameters.properties.at("MinimumEngineSpeed"), DoubleEq(9.0));
EXPECT_THAT(vehicleModelParameters.maximumEngineSpeed, DoubleEq(10.0)); EXPECT_THAT(vehicleModelParameters.properties.at("MaximumEngineSpeed"), DoubleEq(10.0));
} }
TEST(VehicleModelsImporter, GivenValidGeometry_ImportsCorrectDynamics) TEST(VehicleModelsImporter, GivenValidGeometry_ImportsCorrectDynamics)
...@@ -172,18 +181,15 @@ TEST(VehicleModelsImporter, GivenValidGeometry_ImportsCorrectDynamics) ...@@ -172,18 +181,15 @@ TEST(VehicleModelsImporter, GivenValidGeometry_ImportsCorrectDynamics)
ASSERT_NO_THROW(VehicleModelsImporter::ImportVehicleModel(fakeVehicleRoot, vehicleModelMap)); ASSERT_NO_THROW(VehicleModelsImporter::ImportVehicleModel(fakeVehicleRoot, vehicleModelMap));
ASSERT_NO_THROW(vehicleModelParameters = vehicleModelMap.at("validCar").Get(EMPTY_PARAMETERS)); ASSERT_NO_THROW(vehicleModelParameters = vehicleModelMap.at("validCar").Get(EMPTY_PARAMETERS));
EXPECT_THAT(vehicleModelParameters.steeringRatio, DoubleEq(15.0)); EXPECT_THAT(vehicleModelParameters.properties.at("SteeringRatio"), DoubleEq(15.0));
EXPECT_THAT(vehicleModelParameters.axleRatio, DoubleEq(2.0)); EXPECT_THAT(vehicleModelParameters.properties.at("AxleRatio"), DoubleEq(2.0));
EXPECT_THAT(vehicleModelParameters.airDragCoefficient, DoubleEq(1.0)); EXPECT_THAT(vehicleModelParameters.properties.at("AirDragCoefficient"), DoubleEq(1.0));
EXPECT_THAT(vehicleModelParameters.frictionCoeff, DoubleEq(4.0)); EXPECT_THAT(vehicleModelParameters.properties.at("FrictionCoefficient"), DoubleEq(4.0));
EXPECT_THAT(vehicleModelParameters.frontSurface, DoubleEq(5.0)); EXPECT_THAT(vehicleModelParameters.properties.at("FrontSurface"), DoubleEq(5.0));
EXPECT_THAT(vehicleModelParameters.weight, DoubleEq(24.0)); EXPECT_THAT(vehicleModelParameters.properties.at("Mass"), DoubleEq(24.0));
EXPECT_THAT(vehicleModelParameters.maxVelocity, DoubleEq(22.0)); EXPECT_THAT(vehicleModelParameters.properties.at("MomentInertiaRoll"), DoubleEq(11.0));
EXPECT_THAT(vehicleModelParameters.maximumSteeringWheelAngleAmplitude, DoubleEq(1.1 * 15.0)); // maxSteering front wheel * SteeringRatio EXPECT_THAT(vehicleModelParameters.properties.at("MomentInertiaPitch"), DoubleEq(12.0));
EXPECT_THAT(vehicleModelParameters.maxCurvature, DoubleEq(std::sin(1.1) / 28.0)); // sin(maxSteering fron wheel) / wheelbase EXPECT_THAT(vehicleModelParameters.properties.at("MomentInertiaYaw"), DoubleEq(13.0));
EXPECT_THAT(vehicleModelParameters.momentInertiaRoll, DoubleEq(11.0));
EXPECT_THAT(vehicleModelParameters.momentInertiaPitch, DoubleEq(12.0));
EXPECT_THAT(vehicleModelParameters.momentInertiaYaw, DoubleEq(13.0));
} }
TEST(VehicleModelsImporter, GivenValidVehicle_ImportsCorrectGearing) TEST(VehicleModelsImporter, GivenValidVehicle_ImportsCorrectGearing)
...@@ -196,8 +202,10 @@ TEST(VehicleModelsImporter, GivenValidVehicle_ImportsCorrectGearing) ...@@ -196,8 +202,10 @@ TEST(VehicleModelsImporter, GivenValidVehicle_ImportsCorrectGearing)
ASSERT_NO_THROW(VehicleModelsImporter::ImportVehicleModel(fakeVehicleRoot, vehicleModelMap)); ASSERT_NO_THROW(VehicleModelsImporter::ImportVehicleModel(fakeVehicleRoot, vehicleModelMap));
ASSERT_NO_THROW(vehicleModelParameters = vehicleModelMap.at("validCar").Get(EMPTY_PARAMETERS)); ASSERT_NO_THROW(vehicleModelParameters = vehicleModelMap.at("validCar").Get(EMPTY_PARAMETERS));
EXPECT_THAT(vehicleModelParameters.numberOfGears, Eq(3)); EXPECT_THAT(vehicleModelParameters.properties.at("NumberOfGears"), DoubleEq(3));
EXPECT_THAT(vehicleModelParameters.gearRatios, ElementsAre(0.0, 6.0, 6.1, 6.2)); EXPECT_THAT(vehicleModelParameters.properties.at("GearRatio1"), DoubleEq(6));
EXPECT_THAT(vehicleModelParameters.properties.at("GearRatio2"), DoubleEq(6.1));
EXPECT_THAT(vehicleModelParameters.properties.at("GearRatio3"), DoubleEq(6.2));
} }
TEST(VehicleModelsImporter, GivenParametrizedVehicle_ImportsCorrectParameters) TEST(VehicleModelsImporter, GivenParametrizedVehicle_ImportsCorrectParameters)
...@@ -210,10 +218,10 @@ TEST(VehicleModelsImporter, GivenParametrizedVehicle_ImportsCorrectParameters) ...@@ -210,10 +218,10 @@ TEST(VehicleModelsImporter, GivenParametrizedVehicle_ImportsCorrectParameters)
ASSERT_NO_THROW(VehicleModelsImporter::ImportVehicleModel(fakeVehicleRoot, vehicleModelMap)); ASSERT_NO_THROW(VehicleModelsImporter::ImportVehicleModel(fakeVehicleRoot, vehicleModelMap));
ASSERT_NO_THROW(vehicleModelParameters = vehicleModelMap.at("validCar")); ASSERT_NO_THROW(vehicleModelParameters = vehicleModelMap.at("validCar"));
EXPECT_THAT(vehicleModelParameters.length.name, Eq("CustomLength")); EXPECT_THAT(vehicleModelParameters.boundingBoxDimensions.length.name, Eq("CustomLength"));
EXPECT_THAT(vehicleModelParameters.length.defaultValue, DoubleEq(3.0)); EXPECT_THAT(vehicleModelParameters.boundingBoxDimensions.length.defaultValue, DoubleEq(3.0));
EXPECT_THAT(vehicleModelParameters.maxVelocity.name, Eq("CustomSpeed")); EXPECT_THAT(vehicleModelParameters.performance.maxSpeed.name, Eq("CustomSpeed"));
EXPECT_THAT(vehicleModelParameters.maxVelocity.defaultValue, DoubleEq(30.0)); EXPECT_THAT(vehicleModelParameters.performance.maxSpeed.defaultValue, DoubleEq(30.0));
EXPECT_THAT(vehicleModelParameters.frontAxle.trackWidth.name, Eq("CustomFrontTrackWidth")); EXPECT_THAT(vehicleModelParameters.frontAxle.trackWidth.name, Eq("CustomFrontTrackWidth"));
EXPECT_THAT(vehicleModelParameters.frontAxle.trackWidth.defaultValue, DoubleEq(1.9)); EXPECT_THAT(vehicleModelParameters.frontAxle.trackWidth.defaultValue, DoubleEq(1.9));
} }
...@@ -253,8 +261,7 @@ TEST(VehicleModelsImporter, GivenValidPedestrian_ImportsCorrectValues) ...@@ -253,8 +261,7 @@ TEST(VehicleModelsImporter, GivenValidPedestrian_ImportsCorrectValues)
// workaround for ground truth not being able to handle pedestrians // workaround for ground truth not being able to handle pedestrians
//EXPECT_THAT(vehicleModelParameters.vehicleType, Eq(AgentVehicleType::Pedestrian)); //EXPECT_THAT(vehicleModelParameters.vehicleType, Eq(AgentVehicleType::Pedestrian));
EXPECT_THAT(vehicleModelParameters.vehicleType, Eq(AgentVehicleType::Car)); EXPECT_THAT(vehicleModelParameters.vehicleType, Eq(AgentVehicleType::Car));
EXPECT_THAT(vehicleModelParameters.width, DoubleEq(4.0)); EXPECT_THAT(vehicleModelParameters.boundingBoxDimensions.width, DoubleEq(4.0));
EXPECT_THAT(vehicleModelParameters.length, DoubleEq(5.0)); EXPECT_THAT(vehicleModelParameters.boundingBoxDimensions.length, DoubleEq(5.0));
EXPECT_THAT(vehicleModelParameters.height, DoubleEq(6.0)); EXPECT_THAT(vehicleModelParameters.boundingBoxDimensions.height, DoubleEq(6.0));
EXPECT_THAT(vehicleModelParameters.weight, DoubleEq(7.0));
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment