Skip to content
Snippets Groups Projects
Commit a42c8c52 authored by Andreas Rauschert's avatar Andreas Rauschert
Browse files

fix!: Convert scenario orientation for lane and road position

parent ac037ced
No related branches found
No related tags found
1 merge request!220fix!: Convert scenario orientation for lane and road position
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 456 additions and 384 deletions
README.md
+ 1
− 1
View file @ a42c8c52
......@@ -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) | a5530013 | 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 |
......
engine/bazel/deps.bzl
+ 2
− 2
View file @ a42c8c52
......@@ -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 = "v6.0.0"
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 = "d6365096eceefd6848f7429382279b337e9cb8292a984ef54137dbdebd4ce599",
strip_prefix = "mantle-api-{tag}".format(tag = MANTLE_API_TAG),
)
......
engine/src/Conversion/OscToMantle/ConvertScenarioPosition.cpp
+ 65
− 44
View file @ a42c8c52
......@@ -18,30 +18,51 @@ namespace OpenScenarioEngine::v1_3
{
namespace detail
{
void FillOrientation(const mantle_api::IEnvironment& environment,
template <class... Ts>
struct overloaded : Ts...
{
using Ts::operator()...;
};
template <class... Ts>
overloaded(Ts...) -> overloaded<Ts...>;
void FillOrientation(mantle_api::IEnvironment& environment,
const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IOrientation>& orientation_ptr,
const mantle_api::Position& position,
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())};
auto scenario_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())};
if (orientation_ptr->GetType() == NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::RELATIVE)
{
auto lane_orientation = environment.GetQueryService().GetLaneOrientation(pose.position);
pose.orientation = lane_orientation + orientation;
mantle_api::Orientation3<units::angle::radian_t> track_orientation =
std::visit(
overloaded{
[&environment](const mantle_api::OpenDriveRoadPosition& road_pos)
{ return environment.GetConverter()->GetRoadOrientation(road_pos); },
[&environment](const mantle_api::OpenDriveLanePosition& lane_pos)
{ return environment.GetConverter()->GetLaneOrientation(lane_pos); },
[&environment, &pose](const mantle_api::LatLonPosition&)
{ return environment.GetQueryService().GetLaneOrientation(pose.position); },
[&environment, &pose](const mantle_api::Vec3<units::length::meter_t>&)
{ return environment.GetQueryService().GetLaneOrientation(pose.position); },
},
position);
pose.orientation = track_orientation + scenario_orientation;
}
else
{
pose.orientation = orientation;
pose.orientation = scenario_orientation;
}
}
}
mantle_api::Pose ConvertLanePosition(const mantle_api::IEnvironment& environment,
mantle_api::Pose ConvertLanePosition(mantle_api::IEnvironment& environment,
const NET_ASAM_OPENSCENARIO::v1_3::ILanePosition& lane_position)
{
const mantle_api::OpenDriveLanePosition opendrive_lane_position{
......@@ -52,11 +73,11 @@ mantle_api::Pose ConvertLanePosition(const mantle_api::IEnvironment& environment
mantle_api::Pose pose{};
pose.position = environment.GetConverter()->Convert(opendrive_lane_position);
detail::FillOrientation(environment, lane_position.GetOrientation(), pose);
detail::FillOrientation(environment, lane_position.GetOrientation(), opendrive_lane_position, pose);
return pose;
}
mantle_api::Pose ConvertRoadPosition(const mantle_api::IEnvironment& environment,
mantle_api::Pose ConvertRoadPosition(mantle_api::IEnvironment& environment,
const NET_ASAM_OPENSCENARIO::v1_3::IRoadPosition& road_position)
{
const mantle_api::OpenDriveRoadPosition opendrive_road_position{
......@@ -66,18 +87,18 @@ mantle_api::Pose ConvertRoadPosition(const mantle_api::IEnvironment& environment
mantle_api::Pose pose{};
pose.position = environment.GetConverter()->Convert(opendrive_road_position);
detail::FillOrientation(environment, road_position.GetOrientation(), pose);
detail::FillOrientation(environment, road_position.GetOrientation(), opendrive_road_position, pose);
return pose;
}
mantle_api::Pose ConvertGeoPosition(const mantle_api::IEnvironment& environment,
mantle_api::Pose ConvertGeoPosition(mantle_api::IEnvironment& environment,
const NET_ASAM_OPENSCENARIO::v1_3::IGeoPosition& geo_position)
{
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);
detail::FillOrientation(environment, geo_position.GetOrientation(), lat_lon_position, pose);
return pose;
}
......@@ -95,7 +116,7 @@ mantle_api::Pose ConvertWorldPosition(const NET_ASAM_OPENSCENARIO::v1_3::IWorldP
}
mantle_api::Pose ConvertRelativeLanePosition(
const mantle_api::IEnvironment& environment,
mantle_api::IEnvironment& environment,
const NET_ASAM_OPENSCENARIO::v1_3::IRelativeLanePosition& relative_lane_position)
{
mantle_api::Pose pose{};
......@@ -129,7 +150,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(), pose.position, pose);
return pose;
}
......@@ -147,7 +168,7 @@ mantle_api::Pose ConvertRelativeLanePosition(
std::optional<mantle_api::Pose> ConvertScenarioPosition(const std::shared_ptr<mantle_api::IEnvironment>& environment_ptr,
const std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPosition>& pos)
{
const auto& environment = *environment_ptr;
auto& environment = *environment_ptr;
if (const auto& world_position = pos->GetWorldPosition())
{
......@@ -181,36 +202,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
engine/tests/Conversion/OscToMantle/ConvertScenarioPositionTest.cpp
+ 62
− 9
View file @ a42c8c52
......@@ -45,7 +45,7 @@ protected:
{
geo_pos->SetLatitudeDeg(42.123);
geo_pos->SetLongitudeDeg(11.65874);
EXPECT_CALL(dynamic_cast<const mantle_api::MockConverter&>(*(mockEnvironment->GetConverter())),
EXPECT_CALL(dynamic_cast<mantle_api::MockConverter&>(*(mockEnvironment->GetConverter())),
Convert(mantle_api::Position{lat_lon_position_deg}))
.Times(1)
.WillRepeatedly(testing::Return(expected_position_));
......@@ -56,7 +56,7 @@ protected:
geo_pos->SetLongitude(0.5);
if (!set_degree)
{
EXPECT_CALL(dynamic_cast<const mantle_api::MockConverter&>(*(mockEnvironment->GetConverter())),
EXPECT_CALL(dynamic_cast<mantle_api::MockConverter&>(*(mockEnvironment->GetConverter())),
Convert(mantle_api::Position{lat_lon_position_}))
.Times(1)
.WillRepeatedly(testing::Return(expected_position_));
......@@ -80,7 +80,7 @@ protected:
NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext type =
NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::ABSOLUTE) const
{
EXPECT_CALL(dynamic_cast<const mantle_api::MockConverter&>(*(mockEnvironment->GetConverter())),
EXPECT_CALL(dynamic_cast<mantle_api::MockConverter&>(*(mockEnvironment->GetConverter())),
Convert(testing::_))
.Times(1)
.WillRepeatedly(testing::Return(expected_position_));
......@@ -102,6 +102,32 @@ protected:
return pos;
}
std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPosition> GetRoadPosition(
std::optional<mantle_api::Orientation3<units::angle::radian_t>> opt_orientation = std::nullopt,
NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext type =
NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::ABSOLUTE) const
{
EXPECT_CALL(dynamic_cast<mantle_api::MockConverter&>(*(mockEnvironment->GetConverter())),
Convert(testing::_))
.Times(1)
.WillRepeatedly(testing::Return(expected_position_));
auto road_pos = std::make_shared<NET_ASAM_OPENSCENARIO::v1_3::RoadPositionImpl>();
road_pos->SetRoadId("road1");
road_pos->SetT(4.0);
road_pos->SetS(3.0);
if (opt_orientation.has_value())
{
auto orientation_writer = GetOrientationWriter(opt_orientation.value(), type);
road_pos->SetOrientation(orientation_writer);
}
auto pos = std::make_shared<NET_ASAM_OPENSCENARIO::v1_3::PositionImpl>();
pos->SetRoadPosition(road_pos);
return pos;
}
std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_3::IPosition> GetRelativeLanePosition(
double ds = 0.0,
std::optional<mantle_api::Orientation3<units::angle::radian_t>> opt_orientation = std::nullopt,
......@@ -306,7 +332,7 @@ TEST_F(ConvertScenarioPositionTest,
}
TEST_F(ConvertScenarioPositionTest,
GivenGetLanePositionWithNoOrientation_WhenConverting_ThenPositionConvertedAndOrientationZero)
GivenLanePositionWithNoOrientation_WhenConverting_ThenPositionConvertedAndOrientationZero)
{
auto pos = GetLanePosition();
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
......@@ -316,7 +342,7 @@ TEST_F(ConvertScenarioPositionTest,
}
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,12 +353,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_))
EXPECT_CALL(dynamic_cast<mantle_api::MockConverter&>(*(mockEnvironment->GetConverter())),
GetLaneOrientation(testing::_))
.Times(1)
.WillRepeatedly(testing::Return(lane_orientation));
......@@ -340,4 +366,31 @@ TEST_F(ConvertScenarioPositionTest,
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
ASSERT_EQ(expected_orientation + lane_orientation, pose->orientation);
}
\ No newline at end of file
}
TEST_F(ConvertScenarioPositionTest,
GivenRoadPositionWithAbsoluteOrientation_WhenConverting_ThenOrientationIsReturnedUnchanged)
{
mantle_api::Orientation3<units::angle::radian_t> expected_orientation{1.2_rad, 2.3_rad, 5.0_rad};
auto pos = GetRoadPosition(expected_orientation, NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::ABSOLUTE);
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
ASSERT_EQ(expected_orientation, pose->orientation);
}
TEST_F(ConvertScenarioPositionTest,
GivenRoadPositionWithRelativeOrientation_WhenConverting_ThenOrientationIsRelativeToRoadOrientation)
{
mantle_api::Orientation3<units::angle::radian_t> road_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<mantle_api::MockConverter&>(*(mockEnvironment->GetConverter())),
GetRoadOrientation(testing::_))
.Times(1)
.WillRepeatedly(testing::Return(road_orientation));
auto pos = GetRoadPosition(expected_orientation, NET_ASAM_OPENSCENARIO::v1_3::ReferenceContext::RELATIVE);
const auto pose = OpenScenarioEngine::v1_3::ConvertScenarioPosition(mockEnvironment, pos);
ASSERT_EQ(expected_orientation + road_orientation, pose->orientation);
}
utils/ci/conan/conanfile.txt
+ 1
− 1
View file @ a42c8c52
[requires]
units/2.3.3@openscenarioengine/testing
mantleapi/v5.2.0@openscenarioengine/testing
mantleapi/v6.0.0@openscenarioengine/testing
yase/d0c0e58d17358044cc9018c74308b45f6097ecfb@openscenarioengine/testing
openscenario_api/v1.4.0@openscenarioengine/testing
......
utils/ci/conan/recipe/mantleapi/all/conandata.yml
+ 4
− 0
View file @ a42c8c52
......@@ -37,5 +37,9 @@ sources:
url: https://gitlab.eclipse.org/eclipse/openpass/mantle-api.git
sha256: "a0baa757b0d4420686098831c9550c7bc18fc8d7"
"6.0.0":
url: https://gitlab.eclipse.org/eclipse/openpass/mantle-api.git
sha256: "a5530013edcd2028ce48d1dcd7e90a512c158f27"
"default":
url: https://gitlab.eclipse.org/eclipse/openpass/mantle-api.git
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment

Copyright © Eclipse Foundation, Inc. All Rights Reserved.     Privacy Policy | Terms of Use | Copyright Agent