From a9c695c3c9622720597a24037be3cd0bae9061ec Mon Sep 17 00:00:00 2001
From: Andreas Rauschert <andreas.rb.rauschert@bmw.de>
Date: Mon, 25 Nov 2024 14:34:55 +0100
Subject: [PATCH 1/5] fix: convert scenario orientation
---
.../OscToMantle/ConvertScenarioPosition.cpp | 87 +++++++++----------
.../ConvertScenarioPositionTest.cpp | 53 ++++-------
2 files changed, 62 insertions(+), 78 deletions(-)
diff --git a/engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp b/engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp
index a283c94e..725a5ea0 100644
--- a/engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp
+++ b/engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp
@@ -20,23 +20,23 @@ namespace detail
{
void FillOrientation(const mantle_api::IEnvironment& environment,
const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IOrientation>& orientation_ptr,
+ const mantle_api::Orientation3<units::angle::radian_t>& orientation_at_pos,
mantle_api::Pose& pose)
{
- mantle_api::Orientation3<units::angle::radian_t> orientation{};
if (orientation_ptr != nullptr)
{
- orientation = mantle_api::Orientation3<units::angle::radian_t>{units::angle::radian_t(orientation_ptr->GetH()),
- units::angle::radian_t(orientation_ptr->GetP()),
- units::angle::radian_t(orientation_ptr->GetR())};
+ mantle_api::Orientation3<units::angle::radian_t> scenario_orientation{
+ units::angle::radian_t(orientation_ptr->GetH()),
+ units::angle::radian_t(orientation_ptr->GetP()),
+ units::angle::radian_t(orientation_ptr->GetR())};
if (orientation_ptr->GetType() == NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::RELATIVE)
{
- auto lane_orientation = environment.GetQueryService().GetLaneOrientation(pose.position);
- pose.orientation = lane_orientation + orientation;
+ pose.orientation = orientation_at_pos + scenario_orientation;
}
else
{
- pose.orientation = orientation;
+ pose.orientation = scenario_orientation;
}
}
}
@@ -50,9 +50,8 @@ mantle_api::Pose ConvertLanePosition(const mantle_api::IEnvironment& environment
units::length::meter_t(lane_position.GetS()),
units::length::meter_t(lane_position.GetOffset())};
- mantle_api::Pose pose{};
- pose.position = environment.GetConverter()->Convert(opendrive_lane_position);
- detail::FillOrientation(environment, lane_position.GetOrientation(), pose);
+ auto pose = environment.GetConverter()->Convert(opendrive_lane_position);
+ detail::FillOrientation(environment, lane_position.GetOrientation(), pose.orientation, pose);
return pose;
}
@@ -64,9 +63,8 @@ mantle_api::Pose ConvertRoadPosition(const mantle_api::IEnvironment& environment
units::length::meter_t(road_position.GetS()),
units::length::meter_t(road_position.GetT())};
- mantle_api::Pose pose{};
- pose.position = environment.GetConverter()->Convert(opendrive_road_position);
- detail::FillOrientation(environment, road_position.GetOrientation(), pose);
+ auto pose = environment.GetConverter()->Convert(opendrive_road_position);
+ detail::FillOrientation(environment, road_position.GetOrientation(), pose.orientation, pose);
return pose;
}
@@ -75,9 +73,10 @@ mantle_api::Pose ConvertGeoPosition(const mantle_api::IEnvironment& environment,
{
mantle_api::LatLonPosition lat_lon_position = ConvertToMantleLatLonPosition(geo_position);
- mantle_api::Pose pose{};
- pose.position = environment.GetConverter()->Convert(lat_lon_position);
- detail::FillOrientation(environment, geo_position.GetOrientation(), pose);
+ // TODO: For overlapping roads at lat/lon position get z coordinate & orientation based on OSC XML 1.3 attribute
+ // 'verticalRoadSelection'
+ auto pose = environment.GetConverter()->Convert(lat_lon_position);
+ detail::FillOrientation(environment, geo_position.GetOrientation(), pose.orientation, pose);
return pose;
}
@@ -129,7 +128,7 @@ mantle_api::Pose ConvertRelativeLanePosition(
if (calculated_pose.has_value())
{
pose.position = calculated_pose.value().position;
- detail::FillOrientation(environment, relative_lane_position.GetOrientation(), pose);
+ detail::FillOrientation(environment, relative_lane_position.GetOrientation(), calculated_pose.value().orientation, pose);
return pose;
}
@@ -181,36 +180,36 @@ std::optional<mantle_api::Pose> ConvertScenarioPosition(const std::shared_ptr<ma
mantle_api::LatLonPosition ConvertToMantleLatLonPosition(const NET_ASAM_OPENSCENARIO::v1_3::IGeoPosition& geo_position)
{
- mantle_api::LatLonPosition lat_lon_position{};
+ mantle_api::LatLonPosition lat_lon_position{};
- // lat/lon in degree has higher priority since lat/lon in rad was deprecated in OSC1.2
- if (geo_position.IsSetLatitudeDeg())
- {
- lat_lon_position.latitude = units::angle::degree_t(geo_position.GetLatitudeDeg());
- }
- else if (geo_position.IsSetLatitude())
- {
- lat_lon_position.latitude = units::angle::radian_t(geo_position.GetLatitude());
- }
- else
- {
- throw std::runtime_error("GeoPosition has neither latitudeDeg nor latitude defined. Please adjust the scenario.");
- }
+ // lat/lon in degree has higher priority since lat/lon in rad was deprecated in OSC1.2
+ if (geo_position.IsSetLatitudeDeg())
+ {
+ lat_lon_position.latitude = units::angle::degree_t(geo_position.GetLatitudeDeg());
+ }
+ else if (geo_position.IsSetLatitude())
+ {
+ lat_lon_position.latitude = units::angle::radian_t(geo_position.GetLatitude());
+ }
+ else
+ {
+ throw std::runtime_error("GeoPosition has neither latitudeDeg nor latitude defined. Please adjust the scenario.");
+ }
- if (geo_position.IsSetLongitudeDeg())
- {
- lat_lon_position.longitude = units::angle::degree_t(geo_position.GetLongitudeDeg());
- }
- else if (geo_position.IsSetLongitude())
- {
- lat_lon_position.longitude = units::angle::radian_t(geo_position.GetLongitude());
- }
- else
- {
- throw std::runtime_error("GeoPosition has neither longitudeDeg nor longitude defined. Please adjust the scenario.");
- }
+ if (geo_position.IsSetLongitudeDeg())
+ {
+ lat_lon_position.longitude = units::angle::degree_t(geo_position.GetLongitudeDeg());
+ }
+ else if (geo_position.IsSetLongitude())
+ {
+ lat_lon_position.longitude = units::angle::radian_t(geo_position.GetLongitude());
+ }
+ else
+ {
+ throw std::runtime_error("GeoPosition has neither longitudeDeg nor longitude defined. Please adjust the scenario.");
+ }
- return lat_lon_position;
+ return lat_lon_position;
}
} // namespace OpenScenarioEngine::v1_3
diff --git a/engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp b/engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp
index 75dce0a4..90270b13 100644
--- a/engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp
+++ b/engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp
@@ -48,7 +48,7 @@ protected:
EXPECT_CALL(dynamic_cast<const mantle_api::MockConverter&>(*(mockEnvironment->GetConverter())),
Convert(mantle_api::Position{lat_lon_position_deg}))
.Times(1)
- .WillRepeatedly(testing::Return(expected_position_));
+ .WillRepeatedly(testing::Return(expected_pose_));
}
if (set_rad)
{
@@ -59,7 +59,7 @@ protected:
EXPECT_CALL(dynamic_cast<const mantle_api::MockConverter&>(*(mockEnvironment->GetConverter())),
Convert(mantle_api::Position{lat_lon_position_}))
.Times(1)
- .WillRepeatedly(testing::Return(expected_position_));
+ .WillRepeatedly(testing::Return(expected_pose_));
}
}
@@ -83,7 +83,7 @@ protected:
EXPECT_CALL(dynamic_cast<const mantle_api::MockConverter&>(*(mockEnvironment->GetConverter())),
Convert(testing::_))
.Times(1)
- .WillRepeatedly(testing::Return(expected_position_));
+ .WillRepeatedly(testing::Return(expected_pose_));
auto lane_pos = std::make_shared<NET_ASAM_OPENSCENARIO::v1_3::LanePositionImpl>();
lane_pos->SetRoadId("road1");
@@ -136,7 +136,7 @@ protected:
mantle_api::LatLonPosition lat_lon_position_{1.5_rad, 0.5_rad};
mantle_api::LatLonPosition lat_lon_position_deg{42.123_deg, 11.65874_deg};
mantle_api::OpenDriveLanePosition open_drive_lane_position_{"road1", -2, 3.0_m, 4.0_m};
- mantle_api::Vec3<units::length::meter_t> expected_position_{1_m, 2_m, 3_m};
+ mantle_api::Pose expected_pose_{{1_m, 2_m, 3_m}, {4_rad, 5_rad, 6_rad}};
int relative_lane_{1};
double distance_{10.0};
double offset_{0.0};
@@ -164,17 +164,17 @@ TEST_F(ConvertScenarioPositionTest,
auto pos = GetWorldPosition();
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
- ASSERT_EQ(expected_position_, pose->position);
- ASSERT_EQ(mantle_api::Orientation3<units::angle::radian_t>(4_rad, 5_rad, 6_rad), pose->orientation);
+ ASSERT_EQ(expected_pose_.position, pose->position);
+ ASSERT_EQ(expected_pose_.orientation, pose->orientation);
}
TEST_F(ConvertScenarioPositionTest,
- GivenGeoPositionInRadWithNoOrientation_WhenConverting_ThenPositionConvertedAndOrientationZero)
+ GivenGeoPositionInRadWithZeroAbsoluteOrientation_WhenConverting_ThenPositionConvertedAndOrientationZero)
{
auto pos = GetGeoPosition();
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
- ASSERT_EQ(expected_position_, pose->position);
+ ASSERT_EQ(expected_pose_.position, pose->position);
ASSERT_EQ(mantle_api::Orientation3<units::angle::radian_t>(0_rad, 0_rad, 0_rad), pose->orientation);
}
@@ -184,7 +184,7 @@ TEST_F(ConvertScenarioPositionTest,
auto pos = GetGeoPosition(std::nullopt, NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::ABSOLUTE, true, false);
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
- ASSERT_EQ(expected_position_, pose->position);
+ ASSERT_EQ(expected_pose_.position, pose->position);
}
TEST_F(ConvertScenarioPositionTest,
@@ -193,7 +193,7 @@ TEST_F(ConvertScenarioPositionTest,
auto pos = GetGeoPosition(std::nullopt, NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::ABSOLUTE, true, true);
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
- ASSERT_EQ(expected_position_, pose->position);
+ ASSERT_EQ(expected_pose_.position, pose->position);
}
TEST_F(ConvertScenarioPositionTest,
@@ -217,21 +217,16 @@ TEST_F(ConvertScenarioPositionTest,
TEST_F(ConvertScenarioPositionTest,
GivenGeoPositionWithRelativeOrientation_WhenConverting_ThenOrientationIsRelativeToLaneOrientation)
{
- mantle_api::Orientation3<units::angle::radian_t> lane_orientation{10_rad, 11_rad, 12_rad};
mantle_api::Orientation3<units::angle::radian_t> expected_orientation{1.2_rad, 2.3_rad, 5.0_rad};
- EXPECT_CALL(dynamic_cast<const mantle_api::MockQueryService&>(mockEnvironment->GetQueryService()),
- GetLaneOrientation(expected_position_))
- .Times(1)
- .WillRepeatedly(testing::Return(lane_orientation));
auto pos = GetGeoPosition(expected_orientation, NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::RELATIVE);
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
- ASSERT_EQ(expected_orientation + lane_orientation, pose->orientation);
+ ASSERT_EQ(expected_pose_.orientation + expected_orientation, pose->orientation);
}
TEST_F(ConvertScenarioPositionTest,
- GivenRelativeLanePositionWithNoOrientation_WhenConverting_ThenPositionConvertedAndOrientationZero)
+ GivenRelativeLanePositionWithZeroAbsoluteOrientation_WhenConverting_ThenPositionConvertedAndOrientationZero)
{
auto pos = GetRelativeLanePosition();
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
@@ -274,18 +269,13 @@ TEST_F(ConvertScenarioPositionTest,
TEST_F(ConvertScenarioPositionTest,
GivenRelativeLanePositionWithRelativeOrientation_WhenConverting_ThenOrientationIsIsRelativeToLaneOrientation)
{
- mantle_api::Orientation3<units::angle::radian_t> lane_orientation{10_rad, 11_rad, 12_rad};
mantle_api::Orientation3<units::angle::radian_t> expected_orientation{1.2_rad, 2.3_rad, 5.0_rad};
- EXPECT_CALL(dynamic_cast<const mantle_api::MockQueryService&>(mockEnvironment->GetQueryService()),
- GetLaneOrientation(testing::_))
- .Times(1)
- .WillRepeatedly(testing::Return(lane_orientation));
auto pos =
GetRelativeLanePosition(0, expected_orientation, NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::RELATIVE);
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
- ASSERT_EQ(lane_orientation + expected_orientation, pose->orientation);
+ ASSERT_EQ(expected_pose_.orientation + expected_orientation, pose->orientation);
}
TEST_F(ConvertScenarioPositionTest,
@@ -306,17 +296,17 @@ TEST_F(ConvertScenarioPositionTest,
}
TEST_F(ConvertScenarioPositionTest,
- GivenGetLanePositionWithNoOrientation_WhenConverting_ThenPositionConvertedAndOrientationZero)
+ GivenLanePositionWithZeroAbsoluteOrientation_WhenConverting_ThenPositionConvertedAndOrientationZero)
{
auto pos = GetLanePosition();
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
- ASSERT_EQ(expected_position_, pose->position);
+ ASSERT_EQ(expected_pose_.position, pose->position);
ASSERT_EQ(mantle_api::Orientation3<units::angle::radian_t>(0_rad, 0_rad, 0_rad), pose->orientation);
}
TEST_F(ConvertScenarioPositionTest,
- GivenGetLanePositionWithAbsoluteOrientation_WhenConverting_ThenOrientationIsReturnedUnchanged)
+ GivenLanePositionWithAbsoluteOrientation_WhenConverting_ThenOrientationIsReturnedUnchanged)
{
mantle_api::Orientation3<units::angle::radian_t> expected_orientation{1.2_rad, 2.3_rad, 5.0_rad};
@@ -327,17 +317,12 @@ TEST_F(ConvertScenarioPositionTest,
}
TEST_F(ConvertScenarioPositionTest,
- GivenGetLanePositionWithRelativeOrientation_WhenConverting_ThenOrientationIsRelativeToLaneOrientation)
+ GivenLanePositionWithRelativeOrientation_WhenConverting_ThenOrientationIsRelativeToLaneOrientation)
{
- mantle_api::Orientation3<units::angle::radian_t> lane_orientation{10_rad, 11_rad, 12_rad};
mantle_api::Orientation3<units::angle::radian_t> expected_orientation{1.2_rad, 2.3_rad, 5.0_rad};
- EXPECT_CALL(dynamic_cast<const mantle_api::MockQueryService&>(mockEnvironment->GetQueryService()),
- GetLaneOrientation(expected_position_))
- .Times(1)
- .WillRepeatedly(testing::Return(lane_orientation));
auto pos = GetLanePosition(expected_orientation, NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::RELATIVE);
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
- ASSERT_EQ(expected_orientation + lane_orientation, pose->orientation);
-}
\ No newline at end of file
+ ASSERT_EQ(expected_pose_.orientation + expected_orientation, pose->orientation);
+}
--
GitLab
From 78cc4cb72989b43e5280a29a9c39c64221d25611 Mon Sep 17 00:00:00 2001
From: Andreas Rauschert <andreas.rb.rauschert@bmw.de>
Date: Mon, 25 Nov 2024 18:01:34 +0100
Subject: [PATCH 2/5] Fix compilation
- exchanged return value of Convert from position to pose
---
.../ConvertScenarioPositionTest.cpp | 8 +-
.../FollowTrajectoryActionTest.cpp | 648 +++++++++---------
2 files changed, 324 insertions(+), 332 deletions(-)
diff --git a/engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp b/engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp
index 90270b13..ef7c88ae 100644
--- a/engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp
+++ b/engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp
@@ -169,13 +169,12 @@ TEST_F(ConvertScenarioPositionTest,
}
TEST_F(ConvertScenarioPositionTest,
- GivenGeoPositionInRadWithZeroAbsoluteOrientation_WhenConverting_ThenPositionConvertedAndOrientationZero)
+ GivenGeoPositionInRad_WhenConverting_ThenPositionConverted)
{
auto pos = GetGeoPosition();
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
ASSERT_EQ(expected_pose_.position, pose->position);
- ASSERT_EQ(mantle_api::Orientation3<units::angle::radian_t>(0_rad, 0_rad, 0_rad), pose->orientation);
}
TEST_F(ConvertScenarioPositionTest,
@@ -275,7 +274,7 @@ TEST_F(ConvertScenarioPositionTest,
GetRelativeLanePosition(0, expected_orientation, NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::RELATIVE);
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
- ASSERT_EQ(expected_pose_.orientation + expected_orientation, pose->orientation);
+ ASSERT_EQ(expected_orientation, pose->orientation);
}
TEST_F(ConvertScenarioPositionTest,
@@ -296,13 +295,12 @@ TEST_F(ConvertScenarioPositionTest,
}
TEST_F(ConvertScenarioPositionTest,
- GivenLanePositionWithZeroAbsoluteOrientation_WhenConverting_ThenPositionConvertedAndOrientationZero)
+ GivenLanePosition_WhenConverting_ThenPositionConverted)
{
auto pos = GetLanePosition();
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
ASSERT_EQ(expected_pose_.position, pose->position);
- ASSERT_EQ(mantle_api::Orientation3<units::angle::radian_t>(0_rad, 0_rad, 0_rad), pose->orientation);
}
TEST_F(ConvertScenarioPositionTest,
diff --git a/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp b/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp
index 345b38ad..d45e4330 100644
--- a/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp
+++ b/engine/tests/Storyboard/MotionControlAction/FollowTrajectoryActionTest.cpp
@@ -15,9 +15,8 @@
#include "Conversion/OscToMantle/ConvertScenarioTimeReference.h"
#include "Conversion/OscToMantle/ConvertScenarioTrajectoryFollowingMode.h"
#include "Conversion/OscToMantle/ConvertScenarioTrajectoryRef.h"
-#include "Storyboard/MotionControlAction/FollowTrajectoryAction_impl.h"
#include "Storyboard/MotionControlAction/FollowTrajectoryAction.h"
-
+#include "Storyboard/MotionControlAction/FollowTrajectoryAction_impl.h"
#include "TestUtils.h"
#include "builders/ActionBuilder.h"
#include "builders/PositionBuilder.h"
@@ -33,360 +32,355 @@ using namespace testing::OpenScenarioEngine::v1_3;
class FollowTrajectoryActionTestFixture : public OpenScenarioEngineLibraryTestBase
{
-
- protected:
- void SetUp() override
- {
- OpenScenarioEngineLibraryTestBase::SetUp();
- auto engine_abort_flags = std::make_shared<EngineAbortFlags>(EngineAbortFlags::kNoAbort);
- auto entity_broker = std::make_shared<EntityBroker>(false);
- entity_broker->add("Ego");
- root_node_ = std::make_shared<FakeRootNode>(env_, engine_abort_flags, entity_broker);
- }
-
- std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPolylineWriter> GetPolylineWithRelativeLanePositions()
- {
- int relative_lane{1};
- double offset1{1.0};
- double offset2{1.0};
-
- testing::OpenScenarioEngine::v1_3::FakeRelativeLanePositionBuilder fake_relative_lane_position_builder1{relative_lane,
- offset1};
- testing::OpenScenarioEngine::v1_3::FakeRelativeLanePositionBuilder fake_relative_lane_position_builder2{relative_lane,
- offset2};
-
- fake_relative_lane_position_builder1.WithDs(1.0);
- fake_relative_lane_position_builder1.WithDsLane(2.0);
-
- fake_relative_lane_position_builder2.WithDs(1.0);
- fake_relative_lane_position_builder2.WithDsLane(2.0);
- return FakePolylineBuilder()
- .WithVertices({FakeVertexBuilder(0)
- .WithPosition(FakePositionBuilder{}
- .WithRelativeLanePosition(fake_relative_lane_position_builder1.Build())
- .Build())
- .Build(),
- FakeVertexBuilder(1)
- .WithPosition(FakePositionBuilder{}
- .WithRelativeLanePosition(fake_relative_lane_position_builder2.Build())
- .Build())
- .Build()})
- .Build();
- }
- std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPolylineWriter> GetPolylineWithGeoPositions()
- {
- mantle_api::LatLonPosition lat_lon_position1{42.123_deg, 11.65874_deg};
- mantle_api::LatLonPosition lat_lon_position2{52.123_deg, 11.65874_deg};
-
- return FakePolylineBuilder()
- .WithVertices({FakeVertexBuilder(0)
- .WithPosition(FakePositionBuilder{}
- .WithGeoPosition(FakeGeoPositionBuilder(lat_lon_position1.latitude(),
- lat_lon_position1.longitude())
- .Build())
- .Build())
- .Build(),
- FakeVertexBuilder(1)
- .WithPosition(FakePositionBuilder{}
- .WithGeoPosition(FakeGeoPositionBuilder(lat_lon_position2.latitude(),
- lat_lon_position2.longitude())
- .Build())
- .Build())
- .Build()})
- .Build();
- }
- std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPolylineWriter> GetPolylineWithLanePositions()
- {
- mantle_api::OpenDriveLanePosition open_drive_position1{"1", 2, 3.0_m, 0.0_m};
- mantle_api::OpenDriveLanePosition open_drive_position2{"1", 2, 4.0_m, 0.0_m};
- return FakePolylineBuilder()
- .WithVertices({FakeVertexBuilder(0)
- .WithPosition(FakePositionBuilder{}
- .WithLanePosition(
- FakeLanePositionBuilder(open_drive_position1.road,
- std::to_string(open_drive_position1.lane),
- open_drive_position1.t_offset(),
- open_drive_position1.s_offset())
- .Build())
- .Build())
- .Build(),
- FakeVertexBuilder(1)
- .WithPosition(FakePositionBuilder{}
- .WithLanePosition(
- FakeLanePositionBuilder(open_drive_position2.road,
- std::to_string(open_drive_position2.lane),
- open_drive_position2.t_offset(),
- open_drive_position2.s_offset())
- .Build())
- .Build())
- .Build()})
- .Build();
- }
- std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPolylineWriter> GetPolylineWithWorldPositions()
- {
- mantle_api::Vec3<units::length::meter_t> position1{1.0_m, 2.0_m, 0.0_m};
- mantle_api::Orientation3<units::angle::radian_t> orientation1{0.1_rad, 0.0_rad, 0.0_rad};
- mantle_api::Vec3<units::length::meter_t> position2{1.1_m, 2.1_m, 0.0_m};
-
- return FakePolylineBuilder()
- .WithVertices({FakeVertexBuilder(0)
- .WithPosition(FakePositionBuilder{}
- .WithWorldPosition(FakeWorldPositionBuilder(position1.x.value(),
- position1.y.value(),
- position1.z.value(),
- orientation1.yaw.value(),
- orientation1.pitch.value(),
- orientation1.roll.value())
- .Build())
- .Build())
- .Build(),
- FakeVertexBuilder(1)
- .WithPosition(FakePositionBuilder{}
- .WithWorldPosition(FakeWorldPositionBuilder(position2.x.value(),
- position2.y.value(),
- position2.z.value(),
- orientation1.yaw.value(),
- orientation1.pitch.value(),
- orientation1.roll.value())
- .Build())
- .Build())
- .Build()})
- .Build();
- }
-
- std::shared_ptr<FakeRootNode> root_node_{nullptr};
+protected:
+ void SetUp() override
+ {
+ OpenScenarioEngineLibraryTestBase::SetUp();
+ auto engine_abort_flags = std::make_shared<EngineAbortFlags>(EngineAbortFlags::kNoAbort);
+ auto entity_broker = std::make_shared<EntityBroker>(false);
+ entity_broker->add("Ego");
+ root_node_ = std::make_shared<FakeRootNode>(env_, engine_abort_flags, entity_broker);
+ }
+
+ std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPolylineWriter> GetPolylineWithRelativeLanePositions()
+ {
+ int relative_lane{1};
+ double offset1{1.0};
+ double offset2{1.0};
+
+ testing::OpenScenarioEngine::v1_3::FakeRelativeLanePositionBuilder fake_relative_lane_position_builder1{relative_lane,
+ offset1};
+ testing::OpenScenarioEngine::v1_3::FakeRelativeLanePositionBuilder fake_relative_lane_position_builder2{relative_lane,
+ offset2};
+
+ fake_relative_lane_position_builder1.WithDs(1.0);
+ fake_relative_lane_position_builder1.WithDsLane(2.0);
+
+ fake_relative_lane_position_builder2.WithDs(1.0);
+ fake_relative_lane_position_builder2.WithDsLane(2.0);
+ return FakePolylineBuilder()
+ .WithVertices({FakeVertexBuilder(0)
+ .WithPosition(FakePositionBuilder{}
+ .WithRelativeLanePosition(fake_relative_lane_position_builder1.Build())
+ .Build())
+ .Build(),
+ FakeVertexBuilder(1)
+ .WithPosition(FakePositionBuilder{}
+ .WithRelativeLanePosition(fake_relative_lane_position_builder2.Build())
+ .Build())
+ .Build()})
+ .Build();
+ }
+ std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPolylineWriter> GetPolylineWithGeoPositions()
+ {
+ mantle_api::LatLonPosition lat_lon_position1{42.123_deg, 11.65874_deg};
+ mantle_api::LatLonPosition lat_lon_position2{52.123_deg, 11.65874_deg};
+
+ return FakePolylineBuilder()
+ .WithVertices({FakeVertexBuilder(0)
+ .WithPosition(FakePositionBuilder{}
+ .WithGeoPosition(FakeGeoPositionBuilder(lat_lon_position1.latitude(),
+ lat_lon_position1.longitude())
+ .Build())
+ .Build())
+ .Build(),
+ FakeVertexBuilder(1)
+ .WithPosition(FakePositionBuilder{}
+ .WithGeoPosition(FakeGeoPositionBuilder(lat_lon_position2.latitude(),
+ lat_lon_position2.longitude())
+ .Build())
+ .Build())
+ .Build()})
+ .Build();
+ }
+ std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPolylineWriter> GetPolylineWithLanePositions()
+ {
+ mantle_api::OpenDriveLanePosition open_drive_position1{"1", 2, 3.0_m, 0.0_m};
+ mantle_api::OpenDriveLanePosition open_drive_position2{"1", 2, 4.0_m, 0.0_m};
+ return FakePolylineBuilder()
+ .WithVertices({FakeVertexBuilder(0)
+ .WithPosition(FakePositionBuilder{}
+ .WithLanePosition(
+ FakeLanePositionBuilder(open_drive_position1.road,
+ std::to_string(open_drive_position1.lane),
+ open_drive_position1.t_offset(),
+ open_drive_position1.s_offset())
+ .Build())
+ .Build())
+ .Build(),
+ FakeVertexBuilder(1)
+ .WithPosition(FakePositionBuilder{}
+ .WithLanePosition(
+ FakeLanePositionBuilder(open_drive_position2.road,
+ std::to_string(open_drive_position2.lane),
+ open_drive_position2.t_offset(),
+ open_drive_position2.s_offset())
+ .Build())
+ .Build())
+ .Build()})
+ .Build();
+ }
+ std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPolylineWriter> GetPolylineWithWorldPositions()
+ {
+ mantle_api::Vec3<units::length::meter_t> position1{1.0_m, 2.0_m, 0.0_m};
+ mantle_api::Orientation3<units::angle::radian_t> orientation1{0.1_rad, 0.0_rad, 0.0_rad};
+ mantle_api::Vec3<units::length::meter_t> position2{1.1_m, 2.1_m, 0.0_m};
+
+ return FakePolylineBuilder()
+ .WithVertices({FakeVertexBuilder(0)
+ .WithPosition(FakePositionBuilder{}
+ .WithWorldPosition(FakeWorldPositionBuilder(position1.x.value(),
+ position1.y.value(),
+ position1.z.value(),
+ orientation1.yaw.value(),
+ orientation1.pitch.value(),
+ orientation1.roll.value())
+ .Build())
+ .Build())
+ .Build(),
+ FakeVertexBuilder(1)
+ .WithPosition(FakePositionBuilder{}
+ .WithWorldPosition(FakeWorldPositionBuilder(position2.x.value(),
+ position2.y.value(),
+ position2.z.value(),
+ orientation1.yaw.value(),
+ orientation1.pitch.value(),
+ orientation1.roll.value())
+ .Build())
+ .Build())
+ .Build()})
+ .Build();
+ }
+
+ std::shared_ptr<FakeRootNode> root_node_{nullptr};
};
TEST_F(FollowTrajectoryActionTestFixture,
GivenFollowTrajectoryActionWithWorldPosition_WhenStartAction_ThenTransformedIntoCorrespondingControlStrategy)
{
+ auto fake_follow_trajectory_action =
+ FakeFollowTrajectoryActionBuilder{}
+ .WithTimeReference(FakeTimeReferenceBuilder().Build())
+ .WithTrajectory(FakeTrajectoryBuilder("TestTrajectory", false)
+ .WithShape(FakeShapeBuilder().WithPolyline(GetPolylineWithWorldPositions()).Build())
+ .Build())
+ .Build();
+
+ auto follow_trajectory_action = std::make_shared<Node::FollowTrajectoryAction>(fake_follow_trajectory_action);
+
+ std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
+
+ mantle_api::Vec3<units::length::meter_t> expected_position1{1_m, 2_m, 3_m};
+ mantle_api::Vec3<units::length::meter_t> expected_position2{2_m, 3_m, 4_m};
+ mantle_api::Orientation3<units::angle::radian_t> expected_orientation1{0.1_rad, 0.0_rad, 0.0_rad};
+
+ mantle_api::Pose expected_pose1{expected_position1, expected_orientation1};
+ mantle_api::Pose expected_pose2{expected_position2, expected_orientation1};
+
+ EXPECT_CALL(dynamic_cast<const mantle_api::MockGeometryHelper&>(*(env_->GetGeometryHelper())),
+ TranslateGlobalPositionLocally(testing::_, testing::_, testing::_))
+ .WillOnce(testing::Return(expected_position1))
+ .WillOnce(testing::Return(expected_position2));
+
+ EXPECT_CALL(*env_, UpdateControlStrategies(testing::_, testing::_))
+ .WillOnce(testing::SaveArg<1>(&control_strategies));
- auto fake_follow_trajectory_action =
- FakeFollowTrajectoryActionBuilder{}
- .WithTimeReference(FakeTimeReferenceBuilder().Build())
- .WithTrajectory(FakeTrajectoryBuilder("TestTrajectory", false)
- .WithShape(FakeShapeBuilder().WithPolyline(GetPolylineWithWorldPositions()).Build())
- .Build())
- .Build();
-
- auto follow_trajectory_action = std::make_shared<Node::FollowTrajectoryAction>(fake_follow_trajectory_action);
-
- std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
-
- mantle_api::Vec3<units::length::meter_t> expected_position1{1_m, 2_m, 3_m};
- mantle_api::Vec3<units::length::meter_t> expected_position2{2_m, 3_m, 4_m};
- mantle_api::Orientation3<units::angle::radian_t> expected_orientation1{0.1_rad, 0.0_rad, 0.0_rad};
-
- mantle_api::Pose expected_pose1{expected_position1, expected_orientation1};
- mantle_api::Pose expected_pose2{expected_position2, expected_orientation1};
-
- EXPECT_CALL(dynamic_cast<const mantle_api::MockGeometryHelper&>(*(env_->GetGeometryHelper())),
- TranslateGlobalPositionLocally(testing::_, testing::_, testing::_))
- .WillOnce(testing::Return(expected_position1))
- .WillOnce(testing::Return(expected_position2));
-
- EXPECT_CALL(*env_, UpdateControlStrategies(testing::_, testing::_))
- .WillOnce(testing::SaveArg<1>(&control_strategies));
-
- root_node_->setChild(follow_trajectory_action);
- root_node_->distributeData();
- root_node_->onInit();
- EXPECT_NO_THROW(root_node_->executeTick());
-
- ASSERT_THAT(control_strategies, testing::SizeIs(1));
- EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowTrajectory);
- auto& trajectory =
- std::dynamic_pointer_cast<mantle_api::FollowTrajectoryControlStrategy>(control_strategies.front())->trajectory;
- auto& polyline = std::get<mantle_api::PolyLine>(trajectory.type);
- EXPECT_THAT(polyline, testing::SizeIs(2));
- EXPECT_EQ(polyline.at(0).time, 0.0_s);
- EXPECT_EQ(polyline.at(0).pose, expected_pose1);
- EXPECT_EQ(polyline.at(1).time, 1.0_s);
- EXPECT_EQ(polyline.at(1).pose, expected_pose2);
+ root_node_->setChild(follow_trajectory_action);
+ root_node_->distributeData();
+ root_node_->onInit();
+ EXPECT_NO_THROW(root_node_->executeTick());
+
+ ASSERT_THAT(control_strategies, testing::SizeIs(1));
+ EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowTrajectory);
+ auto& trajectory =
+ std::dynamic_pointer_cast<mantle_api::FollowTrajectoryControlStrategy>(control_strategies.front())->trajectory;
+ auto& polyline = std::get<mantle_api::PolyLine>(trajectory.type);
+ EXPECT_THAT(polyline, testing::SizeIs(2));
+ EXPECT_EQ(polyline.at(0).time, 0.0_s);
+ EXPECT_EQ(polyline.at(0).pose, expected_pose1);
+ EXPECT_EQ(polyline.at(1).time, 1.0_s);
+ EXPECT_EQ(polyline.at(1).pose, expected_pose2);
}
TEST_F(FollowTrajectoryActionTestFixture,
GivenFollowTrajectoryActionWithLanePosition_WhenStartAction_ThenTransformedIntoCorrespondingControlStrategy)
{
+ using namespace OpenScenarioEngine::v1_3;
+ auto fake_follow_trajectory_action =
+ FakeFollowTrajectoryActionBuilder{}
+ .WithTimeReference(FakeTimeReferenceBuilder().Build())
+ .WithTrajectory(FakeTrajectoryBuilder("TestTrajectory", false)
+ .WithShape(FakeShapeBuilder().WithPolyline(GetPolylineWithLanePositions()).Build())
+ .Build())
+ .Build();
+
+ auto follow_trajectory_action = std::make_shared<Node::FollowTrajectoryAction>(fake_follow_trajectory_action);
+
+ std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
+
+ mantle_api::Vec3<units::length::meter_t> expected_position1{1_m, 2_m, 3_m};
+ mantle_api::Vec3<units::length::meter_t> expected_position2{2_m, 3_m, 4_m};
+ mantle_api::Orientation3<units::angle::radian_t> orientation{0.0_rad, 0.0_rad, 0.0_rad};
+
+ mantle_api::Pose expected_pose1{expected_position1, orientation};
+ mantle_api::Pose expected_pose2{expected_position2, orientation};
+
+ EXPECT_CALL(dynamic_cast<const mantle_api::MockGeometryHelper&>(*(env_->GetGeometryHelper())),
+ TranslateGlobalPositionLocally(testing::_, testing::_, testing::_))
+ .WillOnce(testing::Return(expected_position1))
+ .WillOnce(testing::Return(expected_position2));
+
+ EXPECT_CALL(*env_, UpdateControlStrategies(testing::_, testing::_))
+ .WillOnce(testing::SaveArg<1>(&control_strategies));
+
+ EXPECT_CALL(dynamic_cast<const mantle_api::MockConverter&>(*(env_->GetConverter())), Convert(testing::_))
+ .WillOnce(testing::Return(expected_pose1))
+ .WillOnce(testing::Return(expected_pose2));
- using namespace OpenScenarioEngine::v1_3;
- auto fake_follow_trajectory_action =
- FakeFollowTrajectoryActionBuilder{}
- .WithTimeReference(FakeTimeReferenceBuilder().Build())
- .WithTrajectory(FakeTrajectoryBuilder("TestTrajectory", false)
- .WithShape(FakeShapeBuilder().WithPolyline(GetPolylineWithLanePositions()).Build())
- .Build())
- .Build();
-
- auto follow_trajectory_action = std::make_shared<Node::FollowTrajectoryAction>(fake_follow_trajectory_action);
-
- std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
-
- mantle_api::Vec3<units::length::meter_t> expected_position1{1_m, 2_m, 3_m};
- mantle_api::Vec3<units::length::meter_t> expected_position2{2_m, 3_m, 4_m};
- mantle_api::Orientation3<units::angle::radian_t> orientation{0.0_rad, 0.0_rad, 0.0_rad};
-
- mantle_api::Pose expected_pose1{expected_position1, orientation};
- mantle_api::Pose expected_pose2{expected_position2, orientation};
-
- EXPECT_CALL(dynamic_cast<const mantle_api::MockGeometryHelper&>(*(env_->GetGeometryHelper())),
- TranslateGlobalPositionLocally(testing::_, testing::_, testing::_))
- .WillOnce(testing::Return(expected_position1))
- .WillOnce(testing::Return(expected_position2));
-
- EXPECT_CALL(*env_, UpdateControlStrategies(testing::_, testing::_))
- .WillOnce(testing::SaveArg<1>(&control_strategies));
-
- EXPECT_CALL(dynamic_cast<const mantle_api::MockConverter&>(*(env_->GetConverter())), Convert(testing::_))
- .WillOnce(testing::Return(expected_position1))
- .WillOnce(testing::Return(expected_position2));
-
- root_node_->setChild(follow_trajectory_action);
- root_node_->distributeData();
- root_node_->onInit();
- EXPECT_NO_THROW(root_node_->executeTick());
-
- ASSERT_THAT(control_strategies, testing::SizeIs(1));
- EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowTrajectory);
- auto& trajectory =
- std::dynamic_pointer_cast<mantle_api::FollowTrajectoryControlStrategy>(control_strategies.front())->trajectory;
- auto& polyline = std::get<mantle_api::PolyLine>(trajectory.type);
- EXPECT_THAT(polyline, testing::SizeIs(2));
- EXPECT_EQ(polyline.at(0).time, 0.0_s);
- EXPECT_EQ(polyline.at(0).pose, expected_pose1);
- EXPECT_EQ(polyline.at(1).time, 1.0_s);
- EXPECT_EQ(polyline.at(1).pose, expected_pose2);
+ root_node_->setChild(follow_trajectory_action);
+ root_node_->distributeData();
+ root_node_->onInit();
+ EXPECT_NO_THROW(root_node_->executeTick());
+
+ ASSERT_THAT(control_strategies, testing::SizeIs(1));
+ EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowTrajectory);
+ auto& trajectory =
+ std::dynamic_pointer_cast<mantle_api::FollowTrajectoryControlStrategy>(control_strategies.front())->trajectory;
+ auto& polyline = std::get<mantle_api::PolyLine>(trajectory.type);
+ EXPECT_THAT(polyline, testing::SizeIs(2));
+ EXPECT_EQ(polyline.at(0).time, 0.0_s);
+ EXPECT_EQ(polyline.at(0).pose, expected_pose1);
+ EXPECT_EQ(polyline.at(1).time, 1.0_s);
+ EXPECT_EQ(polyline.at(1).pose, expected_pose2);
}
TEST_F(FollowTrajectoryActionTestFixture,
GivenFollowTrajectoryActionWithGeoPosition_WhenStartAction_ThenTransformedIntoCorrespondingControlStrategy)
{
+ auto fake_follow_trajectory_action =
+ FakeFollowTrajectoryActionBuilder{}
+ .WithTimeReference(FakeTimeReferenceBuilder().Build())
+ .WithTrajectory(FakeTrajectoryBuilder("TestTrajectory", false)
+ .WithShape(FakeShapeBuilder().WithPolyline(GetPolylineWithGeoPositions()).Build())
+ .Build())
+ .Build();
+
+ auto follow_trajectory_action = std::make_shared<Node::FollowTrajectoryAction>(fake_follow_trajectory_action);
+
+ std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
+
+ mantle_api::Vec3<units::length::meter_t> expected_position1{1_m, 2_m, 3_m};
+ mantle_api::Vec3<units::length::meter_t> expected_position2{2_m, 3_m, 4_m};
+ mantle_api::Orientation3<units::angle::radian_t> orientation{0.0_rad, 0.0_rad, 0.0_rad};
+
+ mantle_api::Pose expected_pose1{expected_position1, orientation};
+ mantle_api::Pose expected_pose2{expected_position2, orientation};
+
+ EXPECT_CALL(*env_, UpdateControlStrategies(testing::_, testing::_))
+ .WillOnce(testing::SaveArg<1>(&control_strategies));
+
+ EXPECT_CALL(dynamic_cast<const mantle_api::MockGeometryHelper&>(*(env_->GetGeometryHelper())),
+ TranslateGlobalPositionLocally(testing::_, testing::_, testing::_))
+ .WillOnce(testing::Return(expected_position1))
+ .WillOnce(testing::Return(expected_position2));
+
+ EXPECT_CALL(dynamic_cast<const mantle_api::MockConverter&>(*(env_->GetConverter())), Convert(testing::_))
+ .WillOnce(testing::Return(expected_pose1))
+ .WillOnce(testing::Return(expected_pose2));
+
+ root_node_->setChild(follow_trajectory_action);
+ root_node_->distributeData();
+ root_node_->onInit();
+ EXPECT_NO_THROW(root_node_->executeTick());
- auto fake_follow_trajectory_action =
- FakeFollowTrajectoryActionBuilder{}
- .WithTimeReference(FakeTimeReferenceBuilder().Build())
- .WithTrajectory(FakeTrajectoryBuilder("TestTrajectory", false)
- .WithShape(FakeShapeBuilder().WithPolyline(GetPolylineWithGeoPositions()).Build())
- .Build())
- .Build();
-
- auto follow_trajectory_action = std::make_shared<Node::FollowTrajectoryAction>(fake_follow_trajectory_action);
-
- std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
-
- mantle_api::Vec3<units::length::meter_t> expected_position1{1_m, 2_m, 3_m};
- mantle_api::Vec3<units::length::meter_t> expected_position2{2_m, 3_m, 4_m};
- mantle_api::Orientation3<units::angle::radian_t> orientation{0.0_rad, 0.0_rad, 0.0_rad};
-
- mantle_api::Pose expected_pose1{expected_position1, orientation};
- mantle_api::Pose expected_pose2{expected_position2, orientation};
-
- EXPECT_CALL(*env_, UpdateControlStrategies(testing::_, testing::_))
- .WillOnce(testing::SaveArg<1>(&control_strategies));
-
- EXPECT_CALL(dynamic_cast<const mantle_api::MockGeometryHelper&>(*(env_->GetGeometryHelper())),
- TranslateGlobalPositionLocally(testing::_, testing::_, testing::_))
- .WillOnce(testing::Return(expected_position1))
- .WillOnce(testing::Return(expected_position2));
-
- EXPECT_CALL(dynamic_cast<const mantle_api::MockConverter&>(*(env_->GetConverter())), Convert(testing::_))
- .WillOnce(testing::Return(expected_position1))
- .WillOnce(testing::Return(expected_position2));
-
- root_node_->setChild(follow_trajectory_action);
- root_node_->distributeData();
- root_node_->onInit();
- EXPECT_NO_THROW(root_node_->executeTick());
-
- ASSERT_THAT(control_strategies, testing::SizeIs(1));
- EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowTrajectory);
- auto& trajectory =
- std::dynamic_pointer_cast<mantle_api::FollowTrajectoryControlStrategy>(control_strategies.front())->trajectory;
- auto& polyline = std::get<mantle_api::PolyLine>(trajectory.type);
- EXPECT_THAT(polyline, testing::SizeIs(2));
- EXPECT_EQ(polyline.at(0).time, 0.0_s);
- EXPECT_EQ(polyline.at(0).pose, expected_pose1);
- EXPECT_EQ(polyline.at(1).time, 1.0_s);
- EXPECT_EQ(polyline.at(1).pose, expected_pose2);
+ ASSERT_THAT(control_strategies, testing::SizeIs(1));
+ EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowTrajectory);
+ auto& trajectory =
+ std::dynamic_pointer_cast<mantle_api::FollowTrajectoryControlStrategy>(control_strategies.front())->trajectory;
+ auto& polyline = std::get<mantle_api::PolyLine>(trajectory.type);
+ EXPECT_THAT(polyline, testing::SizeIs(2));
+ EXPECT_EQ(polyline.at(0).time, 0.0_s);
+ EXPECT_EQ(polyline.at(0).pose, expected_pose1);
+ EXPECT_EQ(polyline.at(1).time, 1.0_s);
+ EXPECT_EQ(polyline.at(1).pose, expected_pose2);
}
TEST_F(FollowTrajectoryActionTestFixture,
GivenFollowTrajectoryActionWithRelativeLane_WhenStartAction_ThenTransformedIntoCorrespondingControlStrategy)
{
+ auto fake_follow_trajectory_action =
+ FakeFollowTrajectoryActionBuilder{}
+ .WithTimeReference(FakeTimeReferenceBuilder().Build())
+ .WithTrajectory(
+ FakeTrajectoryBuilder("TestTrajectory", false)
+ .WithShape(FakeShapeBuilder().WithPolyline(GetPolylineWithRelativeLanePositions()).Build())
+ .Build())
+ .Build();
+
+ auto follow_trajectory_action = std::make_shared<Node::FollowTrajectoryAction>(fake_follow_trajectory_action);
+
+ std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
+
+ mantle_api::Vec3<units::length::meter_t> expected_position1{1_m, 2_m, 3_m};
+ mantle_api::Vec3<units::length::meter_t> expected_position2{2_m, 3_m, 4_m};
+ mantle_api::Orientation3<units::angle::radian_t> orientation{0.0_rad, 0.0_rad, 0.0_rad};
+
+ mantle_api::Pose expected_pose1{expected_position1, orientation};
+ mantle_api::Pose expected_pose2{expected_position2, orientation};
- auto fake_follow_trajectory_action =
- FakeFollowTrajectoryActionBuilder{}
- .WithTimeReference(FakeTimeReferenceBuilder().Build())
- .WithTrajectory(
- FakeTrajectoryBuilder("TestTrajectory", false)
- .WithShape(FakeShapeBuilder().WithPolyline(GetPolylineWithRelativeLanePositions()).Build())
- .Build())
- .Build();
-
- auto follow_trajectory_action = std::make_shared<Node::FollowTrajectoryAction>(fake_follow_trajectory_action);
-
- std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
-
- mantle_api::Vec3<units::length::meter_t> expected_position1{1_m, 2_m, 3_m};
- mantle_api::Vec3<units::length::meter_t> expected_position2{2_m, 3_m, 4_m};
- mantle_api::Orientation3<units::angle::radian_t> orientation{0.0_rad, 0.0_rad, 0.0_rad};
-
- mantle_api::Pose expected_pose1{expected_position1, orientation};
- mantle_api::Pose expected_pose2{expected_position2, orientation};
-
- EXPECT_CALL(*env_, UpdateControlStrategies(testing::_, testing::_))
- .WillOnce(testing::SaveArg<1>(&control_strategies));
-
- EXPECT_CALL(dynamic_cast<const mantle_api::MockGeometryHelper&>(*(env_->GetGeometryHelper())),
- TranslateGlobalPositionLocally(testing::_, testing::_, testing::_))
- .WillOnce(testing::Return(expected_position1))
- .WillOnce(testing::Return(expected_position2));
-
- root_node_->setChild(follow_trajectory_action);
- root_node_->distributeData();
- root_node_->onInit();
- EXPECT_NO_THROW(root_node_->executeTick());
-
- ASSERT_THAT(control_strategies, testing::SizeIs(1));
- EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowTrajectory);
- auto& trajectory =
- std::dynamic_pointer_cast<mantle_api::FollowTrajectoryControlStrategy>(control_strategies.front())->trajectory;
- auto& polyline = std::get<mantle_api::PolyLine>(trajectory.type);
- EXPECT_THAT(polyline, testing::SizeIs(2));
- EXPECT_EQ(polyline.at(0).time, 0.0_s);
- EXPECT_EQ(polyline.at(0).pose, expected_pose1);
- EXPECT_EQ(polyline.at(1).time, 1.0_s);
- EXPECT_EQ(polyline.at(1).pose, expected_pose2);
+ EXPECT_CALL(*env_, UpdateControlStrategies(testing::_, testing::_))
+ .WillOnce(testing::SaveArg<1>(&control_strategies));
+
+ EXPECT_CALL(dynamic_cast<const mantle_api::MockGeometryHelper&>(*(env_->GetGeometryHelper())),
+ TranslateGlobalPositionLocally(testing::_, testing::_, testing::_))
+ .WillOnce(testing::Return(expected_position1))
+ .WillOnce(testing::Return(expected_position2));
+
+ root_node_->setChild(follow_trajectory_action);
+ root_node_->distributeData();
+ root_node_->onInit();
+ EXPECT_NO_THROW(root_node_->executeTick());
+
+ ASSERT_THAT(control_strategies, testing::SizeIs(1));
+ EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowTrajectory);
+ auto& trajectory =
+ std::dynamic_pointer_cast<mantle_api::FollowTrajectoryControlStrategy>(control_strategies.front())->trajectory;
+ auto& polyline = std::get<mantle_api::PolyLine>(trajectory.type);
+ EXPECT_THAT(polyline, testing::SizeIs(2));
+ EXPECT_EQ(polyline.at(0).time, 0.0_s);
+ EXPECT_EQ(polyline.at(0).pose, expected_pose1);
+ EXPECT_EQ(polyline.at(1).time, 1.0_s);
+ EXPECT_EQ(polyline.at(1).pose, expected_pose2);
}
TEST_F(FollowTrajectoryActionTestFixture,
GivenFollowTrajectoryActionWithRelativeLane_WhenStepAction_ThenOnlyCompleteAfterControlStrategyGoalReached)
{
- auto fake_follow_trajectory_action =
- FakeFollowTrajectoryActionBuilder{}
- .WithTimeReference(FakeTimeReferenceBuilder().Build())
- .WithTrajectory(
- FakeTrajectoryBuilder("TestTrajectory", false)
- .WithShape(FakeShapeBuilder().WithPolyline(GetPolylineWithRelativeLanePositions()).Build())
- .Build())
- .Build();
-
- auto follow_trajectory_action = std::make_shared<Node::FollowTrajectoryAction>(fake_follow_trajectory_action);
-
- EXPECT_CALL(*env_, HasControlStrategyGoalBeenReached(0, mantle_api::ControlStrategyType::kFollowTrajectory))
- .Times(2)
- .WillOnce(::testing::Return(false))
- .WillRepeatedly(::testing::Return(true));
- EXPECT_CALL(*env_, UpdateControlStrategies(0, testing::_)).Times(3);
-
- root_node_->setChild(follow_trajectory_action);
- root_node_->distributeData();
- root_node_->onInit();
- EXPECT_NO_THROW(root_node_->executeTick());
- EXPECT_EQ(follow_trajectory_action->status(), yase::NodeStatus::kRunning);
- root_node_->executeTick();
- EXPECT_EQ(follow_trajectory_action->status(), yase::NodeStatus::kSuccess);
- root_node_->executeTick();
+ auto fake_follow_trajectory_action =
+ FakeFollowTrajectoryActionBuilder{}
+ .WithTimeReference(FakeTimeReferenceBuilder().Build())
+ .WithTrajectory(
+ FakeTrajectoryBuilder("TestTrajectory", false)
+ .WithShape(FakeShapeBuilder().WithPolyline(GetPolylineWithRelativeLanePositions()).Build())
+ .Build())
+ .Build();
+
+ auto follow_trajectory_action = std::make_shared<Node::FollowTrajectoryAction>(fake_follow_trajectory_action);
+
+ EXPECT_CALL(*env_, HasControlStrategyGoalBeenReached(0, mantle_api::ControlStrategyType::kFollowTrajectory))
+ .Times(2)
+ .WillOnce(::testing::Return(false))
+ .WillRepeatedly(::testing::Return(true));
+ EXPECT_CALL(*env_, UpdateControlStrategies(0, testing::_)).Times(3);
+
+ root_node_->setChild(follow_trajectory_action);
+ root_node_->distributeData();
+ root_node_->onInit();
+ EXPECT_NO_THROW(root_node_->executeTick());
+ EXPECT_EQ(follow_trajectory_action->status(), yase::NodeStatus::kRunning);
+ root_node_->executeTick();
+ EXPECT_EQ(follow_trajectory_action->status(), yase::NodeStatus::kSuccess);
+ root_node_->executeTick();
}
TEST(FollowTrajectoryAction, GivenMatchingControlStrategyGoalIsReached_ReturnsTrue)
--
GitLab
From 60660b5e9cd3fc0d6fdc368f014e18bca1faad3a Mon Sep 17 00:00:00 2001
From: Andreas Rauschert <andreas.rb.rauschert@bmw.de>
Date: Thu, 5 Dec 2024 09:21:38 +0100
Subject: [PATCH 3/5] fix: Revert orientation calculation for geo position
- currently there is no lane selection possible with the GeoPosition, so
a change in the lane orientation calculation is not necessary
- fix according test, because mock environment returns different
orientation in GetLaneOrientation()
---
engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp | 3 ++-
.../Conversion/OscToMantle/ConvertScenarioPositionTest.cpp | 2 +-
2 files changed, 3 insertions(+), 2 deletions(-)
diff --git a/engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp b/engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp
index 725a5ea0..e5b2b6df 100644
--- a/engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp
+++ b/engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp
@@ -76,7 +76,8 @@ mantle_api::Pose ConvertGeoPosition(const mantle_api::IEnvironment& environment,
// TODO: For overlapping roads at lat/lon position get z coordinate & orientation based on OSC XML 1.3 attribute
// 'verticalRoadSelection'
auto pose = environment.GetConverter()->Convert(lat_lon_position);
- detail::FillOrientation(environment, geo_position.GetOrientation(), pose.orientation, pose);
+ auto orientation = environment.GetQueryService().GetLaneOrientation(pose.position);
+ detail::FillOrientation(environment, geo_position.GetOrientation(), orientation, pose);
return pose;
}
diff --git a/engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp b/engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp
index ef7c88ae..6e5432ee 100644
--- a/engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp
+++ b/engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp
@@ -221,7 +221,7 @@ TEST_F(ConvertScenarioPositionTest,
auto pos = GetGeoPosition(expected_orientation, NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::RELATIVE);
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
- ASSERT_EQ(expected_pose_.orientation + expected_orientation, pose->orientation);
+ ASSERT_EQ(mantle_api::Orientation3<units::angle::radian_t>(0_rad, 0_rad, 0_rad) + expected_orientation, pose->orientation);
}
TEST_F(ConvertScenarioPositionTest,
--
GitLab
From 762a99075dffdbf980d81f5cafa5abfac1d181b4 Mon Sep 17 00:00:00 2001
From: Andreas Rauschert <andreas.rb.rauschert@bmw.de>
Date: Thu, 5 Dec 2024 09:41:12 +0100
Subject: [PATCH 4/5] build: fix MantleAPI dependencies
---
README.md | 2 +-
engine/bazel/deps.bzl | 4 ++--
utils/ci/conan/conanfile.txt | 2 +-
utils/ci/conan/recipe/mantleapi/all/conandata.yml | 4 ++++
4 files changed, 8 insertions(+), 4 deletions(-)
diff --git a/README.md b/README.md
index e5c1ad61..831bd3a6 100644
--- a/README.md
+++ b/README.md
@@ -254,7 +254,7 @@ Converts `NET_ASAM_OPENSCENARIO::v1_3::ITransitionDynamics` object type to [mant
| Dependency | Commit | Version | License |
| ---------- | ------ | ------- | ------- |
-| [MantleAPI](https://gitlab.eclipse.org/eclipse/openpass/mantle-api) | a0baa757 | 5.2.0 | EPL 2.0 |
+| [MantleAPI](https://gitlab.eclipse.org/eclipse/openpass/mantle-api) | 17416e56 | 6.0.0 | EPL 2.0 |
| [OpenSCENARIO API](https://github.com/RA-Consulting-GmbH/openscenario.api.test/) | 5980e88 | 1.4.0 | Apache 2.0 |
| [YASE](https://gitlab.eclipse.org/eclipse/openpass/yase) | d0c0e58d | | EPL 2.0 |
| [Units](https://github.com/nholthaus/units) | b04d436 | 2.3.3 | MIT License |
diff --git a/engine/bazel/deps.bzl b/engine/bazel/deps.bzl
index cdb78e0e..8548964d 100644
--- a/engine/bazel/deps.bzl
+++ b/engine/bazel/deps.bzl
@@ -3,7 +3,7 @@ load("@bazel_tools//tools/build_defs/repo:utils.bzl", "maybe")
ECLIPSE_GITLAB = "https://gitlab.eclipse.org/eclipse"
-MANTLE_API_TAG = "v5.2.0"
+MANTLE_API_TAG = "17416e56c48d1215df6e5cd170c7cbe8d968edf0"
UNITS_TAG = "2.3.3"
YASE_TAG = "v0.0.1"
@@ -13,7 +13,7 @@ def osc_engine_deps():
http_archive,
name = "mantle_api",
url = ECLIPSE_GITLAB + "/openpass/mantle-api/-/archive/{tag}/mantle-api-{tag}.tar.gz".format(tag = MANTLE_API_TAG),
- sha256 = "a57cce809f1f8952e70499cbf4b63bc561765b4e7b4d96d8b3a7a3cabdf25fe6",
+ sha256 = "40580301b3a40badd5cacbdc721f152b17d424c30ab34694a9758af450db339b",
strip_prefix = "mantle-api-{tag}".format(tag = MANTLE_API_TAG),
)
diff --git a/utils/ci/conan/conanfile.txt b/utils/ci/conan/conanfile.txt
index ea08bcc3..2ffdd728 100644
--- a/utils/ci/conan/conanfile.txt
+++ b/utils/ci/conan/conanfile.txt
@@ -1,6 +1,6 @@
[requires]
units/2.3.3@openscenarioengine/testing
-mantleapi/v5.2.0@openscenarioengine/testing
+mantleapi/17416e56c48d1215df6e5cd170c7cbe8d968edf0@openscenarioengine/testing
yase/d0c0e58d17358044cc9018c74308b45f6097ecfb@openscenarioengine/testing
openscenario_api/v1.4.0@openscenarioengine/testing
diff --git a/utils/ci/conan/recipe/mantleapi/all/conandata.yml b/utils/ci/conan/recipe/mantleapi/all/conandata.yml
index e3511b45..1d101e16 100644
--- a/utils/ci/conan/recipe/mantleapi/all/conandata.yml
+++ b/utils/ci/conan/recipe/mantleapi/all/conandata.yml
@@ -37,5 +37,9 @@ sources:
url: https://gitlab.eclipse.org/eclipse/openpass/mantle-api.git
sha256: "a0baa757b0d4420686098831c9550c7bc18fc8d7"
+ "17416e56c48d1215df6e5cd170c7cbe8d968edf0":
+ url: https://gitlab.eclipse.org/eclipse/openpass/mantle-api.git
+ sha256: "17416e56c48d1215df6e5cd170c7cbe8d968edf0"
+
"default":
url: https://gitlab.eclipse.org/eclipse/openpass/mantle-api.git
--
GitLab
From 7af05ffaf4f9fe883ee0200797c3ba0fe22fc6f8 Mon Sep 17 00:00:00 2001
From: Andreas Rauschert <andreas.rb.rauschert@bmw.de>
Date: Thu, 5 Dec 2024 15:38:34 +0100
Subject: [PATCH 5/5] fix: calculate lane orientation only when necessary
---
.../OscToMantle/ConvertScenarioPosition.cpp | 11 +++++++----
1 file changed, 7 insertions(+), 4 deletions(-)
diff --git a/engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp b/engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp
index e5b2b6df..57e958f6 100644
--- a/engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp
+++ b/engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp
@@ -20,7 +20,7 @@ namespace detail
{
void FillOrientation(const mantle_api::IEnvironment& environment,
const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IOrientation>& orientation_ptr,
- const mantle_api::Orientation3<units::angle::radian_t>& orientation_at_pos,
+ std::optional<mantle_api::Orientation3<units::angle::radian_t>> orientation_at_pos,
mantle_api::Pose& pose)
{
if (orientation_ptr != nullptr)
@@ -32,7 +32,11 @@ void FillOrientation(const mantle_api::IEnvironment& environment,
if (orientation_ptr->GetType() == NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::RELATIVE)
{
- pose.orientation = orientation_at_pos + scenario_orientation;
+ if (!orientation_at_pos)
+ {
+ orientation_at_pos = environment.GetQueryService().GetLaneOrientation(pose.position);
+ }
+ pose.orientation = orientation_at_pos.value() + scenario_orientation;
}
else
{
@@ -76,8 +80,7 @@ mantle_api::Pose ConvertGeoPosition(const mantle_api::IEnvironment& environment,
// TODO: For overlapping roads at lat/lon position get z coordinate & orientation based on OSC XML 1.3 attribute
// 'verticalRoadSelection'
auto pose = environment.GetConverter()->Convert(lat_lon_position);
- auto orientation = environment.GetQueryService().GetLaneOrientation(pose.position);
- detail::FillOrientation(environment, geo_position.GetOrientation(), orientation, pose);
+ detail::FillOrientation(environment, geo_position.GetOrientation(), std::nullopt, pose);
return pose;
}
--
GitLab