diff --git a/README.md b/README.md
index d511aa968bd560ccc1be53376aea9d6884a7b052..ea6369e7260fd7f3a316939427e19135e412fb17 100644
--- a/README.md
+++ b/README.md
@@ -113,7 +113,7 @@ The following Actions and Conditions of [ASAM OpenSCENARIO XML](https://publicat
| ByEntityCondition | [RelativeDistanceCondition](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/RelativeDistanceCondition.html) | Supports only `RelativeDistanceType::kLongitudinal && CoordinateSystem::kEntity` |
| ByEntityCondition | [RelativeSpeedCondition](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/RelativeSpeedCondition.html) | âœ”ï¸ Complete |
| ByEntityCondition | [SpeedCondition](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/SpeedCondition.html) | âœ”ï¸ Without direction |
-| ByEntityCondition | [TimeHeadwayCondition](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/TimeHeadwayCondition.html) | Supports only `DistanceType::kEuclidean` and `CoordinateSystem::kEntity` or `CoordinateSystem::kLane` or `CoordinateSystem::kRoad(freespace=false)` |
+| ByEntityCondition | [TimeHeadwayCondition](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/TimeHeadwayCondition.html) | Supports only `DistanceType::kEuclidean` and `CoordinateSystem::kEntity` or `CoordinateSystem::kLane` or `CoordinateSystem::kRoad` |
| ByEntityCondition | [TimeToCollisionCondition](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/TimeToCollisionCondition.html) | In clarification: [Issue #7](https://gitlab.eclipse.org/eclipse/openopass/openscenario1_engine/-/issues/7) |
| ByEntityCondition | [TraveledDistanceCondition](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/TraveledDistanceCondition.html) | âœ”ï¸ Complete |
| ByValueCondition | [SimulationTimeCondition](https://publications.pages.asam.net/standards/ASAM_OpenSCENARIO/ASAM_OpenSCENARIO_XML/latest/generated/content/SimulationTimeCondition.html) | âœ”ï¸ Complete |
@@ -269,7 +269,7 @@ For more information on how to define these properties, refer to the documentati
| [CPM](https://github.com/cpm-cmake/CPM.cmake) | 03705fc | 0.36.0 | MIT License |
| [googletest](https://github.com/google/googletest) | f8d7d77 | 1.14.0 | BSD-3-Clause License |
| [JSON for Modern C++](https://github.com/nlohmann/json) | db78ac1 | 3.9.1 | MIT License |
-| [MantleAPI](https://gitlab.eclipse.org/eclipse/openpass/mantle-api) | 7d2ed281 | 18.0.0 | EPL 2.0 |
+| [MantleAPI](https://gitlab.eclipse.org/eclipse/openpass/mantle-api) | b1b08843 | 21.0.0 | EPL 2.0 |
| [openpass/stochastics-library](https://gitlab.eclipse.org/eclipse/openpass/stochastics-library) | 6c9dde71 | 0.11.0 | EPL 2.0 |
| [OpenSCENARIO API](https://github.com/RA-Consulting-GmbH/openscenario.api.test/) | 5980e88 | 1.4.0 | Apache 2.0 |
| [Units](https://github.com/nholthaus/units) | e27eed9 | 2.3.4 | MIT License |
diff --git a/engine/src/Storyboard/ByEntityCondition/TimeHeadwayCondition_impl.cpp b/engine/src/Storyboard/ByEntityCondition/TimeHeadwayCondition_impl.cpp
index d2f70a55c1a434acb8d6843b000224f787a1b63c..e5d4d5917e3885fd69ddc749a1ae40175363cbd3 100644
--- a/engine/src/Storyboard/ByEntityCondition/TimeHeadwayCondition_impl.cpp
+++ b/engine/src/Storyboard/ByEntityCondition/TimeHeadwayCondition_impl.cpp
@@ -17,6 +17,41 @@
namespace OpenScenarioEngine::v1_3
{
+namespace detail
+{
+// Normalize angle to [-π, π] range
+constexpr units::angle::radian_t NormalizeAngle(units::angle::radian_t angle) noexcept
+{
+ constexpr auto pi = units::angle::radian_t{M_PI};
+ constexpr auto two_pi = 2.0 * pi;
+ while (angle > pi)
+ {
+ angle -= two_pi;
+ }
+ while (angle < -pi)
+ {
+ angle += two_pi;
+ }
+ return angle;
+};
+
+// Get road orientation in entity direction
+mantle_api::Orientation3<units::angle::radian_t> GetRoadOrientationInEntityDirection(
+ mantle_api::Orientation3<units::angle::radian_t> road_orientation,
+ const mantle_api::IEntity& entity) noexcept
+{
+ const auto entity_yaw = entity.GetOrientation().yaw;
+ const auto angle_diff = NormalizeAngle(road_orientation.yaw - entity_yaw);
+ const auto is_opposite_direction = units::math::abs(angle_diff) > units::angle::radian_t{M_PI_2};
+
+ if (is_opposite_direction)
+ {
+ road_orientation.yaw = -road_orientation.yaw;
+ }
+
+ return road_orientation;
+}
+} // namespace detail
bool TimeHeadwayCondition::IsSatisfied() const
{
if (values.relativeDistanceType != RelativeDistanceType::kLongitudinal)
@@ -51,7 +86,7 @@ bool TimeHeadwayCondition::IsSatisfied() const
if (!longitudinal_lane_distance.has_value())
{
throw std::runtime_error(
- "TimeHeadwayCondition: CoordinateSystem is set to \"LANE\", but can not get the longitudinal distance "
+ "TimeHeadwayCondition: \"coordinateSystem\" is set to \"lane\", but can not get the longitudinal distance "
"of the reference and the triggering entities along the lane center line. Please adjust scenario.");
}
distance = longitudinal_lane_distance.value();
@@ -72,27 +107,51 @@ bool TimeHeadwayCondition::IsSatisfied() const
EntityUtils::GetCornerPositionsInLocalSortedByLongitudinalDistance(
mantle.environment, *ref_entity, ref_entity->GetPosition(), ref_entity_lane_pose.value().orientation);
- distance = distance - (units::math::abs(trigger_entity_corners_along_lane.front().x) +
- units::math::abs(ref_entity_corners_along_lane.back().x));
+ distance -= (units::math::abs(trigger_entity_corners_along_lane.front().x) +
+ units::math::abs(ref_entity_corners_along_lane.back().x));
+ distance = units::math::max(distance, units::length::meter_t(0.0));
}
}
else if (values.coordinateSystem == CoordinateSystem::kRoad)
{
- if (values.freespace)
- {
- Logger::Error(R"(TimeHeadwayCondition: The "road" coordinate system does not support freespace for now. Returning false.)");
- return false;
- }
const auto longitudinal_lane_distance =
mantle.environment->GetQueryService().GetLongitudinalRoadDistanceBetweenPositions(trigger_entity->GetPosition(),
ref_entity->GetPosition());
if (!longitudinal_lane_distance.has_value())
{
throw std::runtime_error(
- "TimeHeadwayCondition: CoordinateSystem is set to \"ROAD\", but can not get the longitudinal distance "
+ "TimeHeadwayCondition: \"coordinateSystem\" is set to \"road\", but can not get the longitudinal distance "
"of the reference and the triggering entities along the Road Refenrence line. Please adjust scenario.");
}
distance = longitudinal_lane_distance.value();
+
+ if (values.freespace)
+ {
+ const auto trigger_entity_road_orientation =
+ mantle.environment->GetQueryService().GetRoadOrientation(trigger_entity->GetPosition());
+ const auto ref_entity_road_orientation =
+ mantle.environment->GetQueryService().GetRoadOrientation(ref_entity->GetPosition());
+ if (!trigger_entity_road_orientation || !ref_entity_road_orientation)
+ {
+ throw std::runtime_error(
+ "TimeHeadwayCondition: \"coordinateSystem\" is set to \"road\" and \"freespace\" is set to \"true\", "
+ "but can not get the road orientation for the triggering entity or the reference entity. Please adjust scenario.");
+ }
+
+ const auto road_orientation_in_trigger_entity_direction = detail::GetRoadOrientationInEntityDirection(*trigger_entity_road_orientation, *trigger_entity);
+ const auto road_orientation_in_ref_entity_direction = detail::GetRoadOrientationInEntityDirection(*ref_entity_road_orientation, *ref_entity);
+
+ const auto trigger_entity_corners_along_road =
+ EntityUtils::GetCornerPositionsInLocalSortedByLongitudinalDistance(
+ mantle.environment, *trigger_entity, trigger_entity->GetPosition(), road_orientation_in_trigger_entity_direction);
+ const auto ref_entity_corners_along_road =
+ EntityUtils::GetCornerPositionsInLocalSortedByLongitudinalDistance(
+ mantle.environment, *ref_entity, ref_entity->GetPosition(), road_orientation_in_ref_entity_direction);
+
+ distance -= (units::math::abs(trigger_entity_corners_along_road.front().x) +
+ units::math::abs(ref_entity_corners_along_road.back().x));
+ distance = units::math::max(distance, units::length::meter_t(0.0));
+ }
}
else
{
diff --git a/engine/tests/Storyboard/ByEntityCondition/TimeHeadwayConditionTest.cpp b/engine/tests/Storyboard/ByEntityCondition/TimeHeadwayConditionTest.cpp
index 1ff79ff2e4ca5d27951cd03a34ac0f3af4edfed7..40eb65b8eed3bfd6e59fd000fb4c91b1e12ae045 100644
--- a/engine/tests/Storyboard/ByEntityCondition/TimeHeadwayConditionTest.cpp
+++ b/engine/tests/Storyboard/ByEntityCondition/TimeHeadwayConditionTest.cpp
@@ -86,29 +86,34 @@ TEST_F(TimeHeadwayConditionTestFixture,
}
TEST_F(TimeHeadwayConditionTestFixture,
- GivenConditionWithCoordinateSystemRoadAndFreespaceTrue_WhenEvaluate_ThenReturnsFalse)
+ GivenConditionWithCoordinateSystemRoadButCanNotCalculateDistance_WhenEvaluate_ThenThrowRuntimeError)
{
condition_values_.coordinateSystem = OpenScenarioEngine::v1_3::CoordinateSystem::kRoad;
- condition_values_.freespace = true;
+
+ auto& mocked_query_service = dynamic_cast<const mantle_api::MockLaneLocationQueryService&>(fake_environment_->GetQueryService());
+
+ ON_CALL(mocked_query_service, GetLongitudinalRoadDistanceBetweenPositions(_, _))
+ .WillByDefault(Return(std::nullopt));
auto time_headway_condition = OpenScenarioEngine::v1_3::TimeHeadwayCondition(condition_values_,
{fake_environment_});
- EXPECT_FALSE(time_headway_condition.IsSatisfied());
- EXPECT_THAT(LOGGER->LastLogLevel(), mantle_api::LogLevel::kError);
- EXPECT_THAT(LOGGER->LastLogMessage(), HasSubstr("does not support freespace"));
+ EXPECT_THROW(time_headway_condition.IsSatisfied(), std::runtime_error);
}
TEST_F(TimeHeadwayConditionTestFixture,
- GivenConditionWithCoordinateSystemRoadButCanNotCalculateDistance_WhenEvaluate_ThenThrowRuntimeError)
+ GivenConditionWithCoordinateSystemRoadButCanNotGetRoadOrientation_WhenEvaluate_ThenThrowRuntimeError)
{
condition_values_.coordinateSystem = OpenScenarioEngine::v1_3::CoordinateSystem::kRoad;
+ condition_values_.freespace = true;
auto& mocked_query_service = dynamic_cast<const mantle_api::MockLaneLocationQueryService&>(fake_environment_->GetQueryService());
- EXPECT_CALL(mocked_query_service, GetLongitudinalRoadDistanceBetweenPositions(_, _))
- .Times(1)
- .WillRepeatedly(Return(std::nullopt));
+ ON_CALL(mocked_query_service, GetLongitudinalRoadDistanceBetweenPositions(_, _))
+ .WillByDefault(Return(units::length::meter_t{}));
+
+ ON_CALL(mocked_query_service, GetRoadOrientation(_))
+ .WillByDefault(Return(std::nullopt));
auto time_headway_condition = OpenScenarioEngine::v1_3::TimeHeadwayCondition(condition_values_,
{fake_environment_});
@@ -315,7 +320,7 @@ TEST_P(TimeHeadwayConditionTestForCoordinateSystemWithFreespaceFalse,
auto& mocked_query_service = dynamic_cast<const mantle_api::MockLaneLocationQueryService&>(fake_environment_->GetQueryService());
auto& mocked_entity = dynamic_cast<mantle_api::MockVehicle&>(*fake_environment_->GetEntityRepository().Get(""));
- EXPECT_CALL(mocked_entity, GetVelocity()).WillOnce(Return(triggering_entity_velocity_));
+ ON_CALL(mocked_entity, GetVelocity()).WillByDefault(Return(triggering_entity_velocity_));
EXPECT_CALL(mocked_query_service, GetLongitudinalRoadDistanceBetweenPositions(_, _))
.WillOnce(Return(relative_distance_));
@@ -325,3 +330,47 @@ TEST_P(TimeHeadwayConditionTestForCoordinateSystemWithFreespaceFalse,
EXPECT_EQ(time_headway_condition.IsSatisfied(), expected_satisfied_status_);
}
+
+TEST_P(TimeHeadwayConditionTestForCoordinateSystemWithFreespaceTrue,
+ GivenCoordinateSystemIsRoadWithTestInputsAndFreespaceTrue_WhenEvaluate_ThenExpectCorrectSatisfiedStatus)
+{
+ auto rule = OpenScenarioEngine::v1_3::Rule(NET_ASAM_OPENSCENARIO::v1_3::Rule::RuleEnum::EQUAL_TO, condition_value_);
+ condition_values_.rule = rule;
+ condition_values_.coordinateSystem = OpenScenarioEngine::v1_3::CoordinateSystem::kRoad;
+
+ auto& mocked_query_service = dynamic_cast<const mantle_api::MockLaneLocationQueryService&>(fake_environment_->GetQueryService());
+ auto& mocked_entity = dynamic_cast<mantle_api::MockVehicle&>(*fake_environment_->GetEntityRepository().Get(""));
+ auto& mocked_geo_helper = dynamic_cast<const mantle_api::MockGeometryHelper&>(*(fake_environment_->GetGeometryHelper()));
+
+ ON_CALL(mocked_entity, GetVelocity()).WillByDefault(Return(triggering_entity_velocity_));
+
+ const auto headmost_corner = mantle_api::Vec3<units::length::meter_t>{.x = 3.0_m, .y = 0_m, .z = 0_m};
+ const auto rearmost_corner = mantle_api::Vec3<units::length::meter_t>{.x = -2.0_m, .y = 0_m, .z = 0_m};
+ const auto other_corner = mantle_api::Vec3<units::length::meter_t>{.x = 0.0_m, .y = 0_m, .z = 0_m};
+ const auto entity_length = units::math::abs(headmost_corner.x) + units::math::abs(rearmost_corner.x);
+
+ EXPECT_CALL(mocked_query_service, GetLongitudinalRoadDistanceBetweenPositions(_, _))
+ .WillOnce(Return(relative_distance_ + entity_length));
+
+ EXPECT_CALL(mocked_query_service, GetRoadOrientation(_))
+ .Times(2)
+ .WillRepeatedly(Return(Orientation3<units::angle::radian_t>{}));
+
+ testing::InSequence s;
+ // called two iterations for the trigger entity and reference entity respectively
+ EXPECT_CALL(mocked_geo_helper, TranslateGlobalPositionLocally(_, _, _))
+ .Times(8)
+ .WillOnce(Return(headmost_corner))
+ .WillOnce(Return(rearmost_corner))
+ .WillRepeatedly(Return(other_corner));
+ EXPECT_CALL(mocked_geo_helper, TranslateGlobalPositionLocally(_, _, _))
+ .Times(8)
+ .WillOnce(Return(headmost_corner))
+ .WillOnce(Return(rearmost_corner))
+ .WillRepeatedly(Return(other_corner));
+
+ auto time_headway_condition = OpenScenarioEngine::v1_3::TimeHeadwayCondition(condition_values_,
+ {fake_environment_});
+
+ EXPECT_EQ(time_headway_condition.IsSatisfied(), expected_satisfied_status_);
+}
diff --git a/engine/third_party/mantle_api/mantle_api.bzl b/engine/third_party/mantle_api/mantle_api.bzl
index 270c21bcea3c10d28b6ced1aa7caa5e88854fb22..408636372308f92d45e905881c8eaf665bda5c79 100644
--- a/engine/third_party/mantle_api/mantle_api.bzl
+++ b/engine/third_party/mantle_api/mantle_api.bzl
@@ -1,14 +1,14 @@
load("@bazel_tools//tools/build_defs/repo:http.bzl", "http_archive")
load("@bazel_tools//tools/build_defs/repo:utils.bzl", "maybe")
-_TAG = "v18.0.0"
+_TAG = "v21.0.0"
def mantle_api():
maybe(
http_archive,
name = "mantle_api",
url = "https://gitlab.eclipse.org/eclipse/openpass/mantle-api/-/archive/{tag}/mantle-api-{tag}.tar.gz".format(tag = _TAG),
- sha256 = "4c413e3bd035531f232c5b54aeaa329b82c9e80926f44cd6364d6892034e87d5",
+ sha256 = "68465d0e9d7f9c082781e33b06da24c4dbae218160899de34c5a8f1764de0eeb",
strip_prefix = "mantle-api-{tag}".format(tag = _TAG),
type = "tar.gz",
)
diff --git a/utils/ci/conan/conanfile.txt b/utils/ci/conan/conanfile.txt
index 4f986fd063a6514d025192216362d5b6728db469..f17aa95fcb8897e15007bcf4838ce1eeb060aa4f 100644
--- a/utils/ci/conan/conanfile.txt
+++ b/utils/ci/conan/conanfile.txt
@@ -1,5 +1,5 @@
[requires]
-mantleapi/v18.0.0@openscenarioengine/testing
+mantleapi/v21.0.0@openscenarioengine/testing
nlohmann_json/v3.9.1@openscenarioengine/testing
openscenario_api/v1.4.0@openscenarioengine/testing
stochastics/0.11.0@openscenarioengine/testing
diff --git a/utils/ci/conan/recipe/mantleapi/all/conandata.yml b/utils/ci/conan/recipe/mantleapi/all/conandata.yml
index 81018803ef8b87d8af9aa9086d3395b067ac0c16..0291ec43b1a3e297d6ba36b0ef3b0db0e7bf4147 100644
--- a/utils/ci/conan/recipe/mantleapi/all/conandata.yml
+++ b/utils/ci/conan/recipe/mantleapi/all/conandata.yml
@@ -69,5 +69,9 @@ sources:
url: https://gitlab.eclipse.org/eclipse/openpass/mantle-api.git
sha256: "7d2ed281116d59d7270d4b8e01811e64a7dc665c"
+ "21.0.0":
+ url: https://gitlab.eclipse.org/eclipse/openpass/mantle-api.git
+ sha256: "b1b08843020108dbe05aed4564704a214da714fb"
+
"default":
url: https://gitlab.eclipse.org/eclipse/openpass/mantle-api.git