Skip to content
Snippets Groups Projects
Commit fc0e0e51 authored by Ziqi Zhou's avatar Ziqi Zhou
Browse files

Merge branch 'feat/support-freespace-in-TimeHeadwayCondition' into 'main'

Feat: support freespace in time headway condition

See merge request !256
parents 5efd201b 9b781712
No related branches found
No related tags found
1 merge request!256Feat: support freespace in time headway condition
......@@ -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 |
......
......@@ -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
{
......
......@@ -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_);
}
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",
)
[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
......
......@@ -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
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