diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 8bf5f664b3b..5252176abfc 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -620,151 +620,6 @@ class SimulatorCore return std::numeric_limits::quiet_NaN(); } } - - static auto evaluateTimeToCollisionCondition(const Entity & from, const Entity & to) - { - /* - This function uses the Separating Axis Theorem (SAT) to find the - Time-To-Collision (TTC) of two entities in the world coordinate - system. - - First, the expected time of collision is obtained independently for - each separating axis. Then, we check whether the polygons of the - entities actually collide after that time. If no collision occurs, the - expected collision time is rejected. The lowest value of the expected - collision times obtained for each separate axis is then returned as - the TTC. - */ - - if (const auto entity_a = core->getEntity(from.name())) { - if (const auto entity_b = core->getEntity(to.name())) { - struct OrientedBoundingBox - { - Eigen::Vector3d position; - - Eigen::Matrix3d rotation; - - Eigen::Vector3d size; // [width, height, depth] - - explicit OrientedBoundingBox( - const geometry_msgs::msg::Pose & pose, - const traffic_simulator_msgs::msg::BoundingBox & box) - : position( - pose.position.x + box.center.x, pose.position.y + box.center.y, - pose.position.z + box.center.z), - rotation(math::geometry::getRotationMatrix(pose.orientation)), - size(box.dimensions.y / 2, box.dimensions.z / 2, box.dimensions.x / 2) - { - } - - auto axis(std::size_t index) const -> Eigen::Vector3d { return rotation.col(index); } - }; - - const Eigen::Vector3d v = evaluateRelativeSpeed(from, to); - - const auto a = OrientedBoundingBox(entity_a->getMapPose(), entity_a->getBoundingBox()); - const auto b = OrientedBoundingBox(entity_b->getMapPose(), entity_b->getBoundingBox()); - - const auto axes = std::array{ - a.axis(0), - a.axis(1), - a.axis(2), - - b.axis(0), - b.axis(1), - b.axis(2), - - a.axis(0).cross(b.axis(0)), - a.axis(0).cross(b.axis(1)), - a.axis(0).cross(b.axis(2)), - - a.axis(1).cross(b.axis(0)), - a.axis(1).cross(b.axis(1)), - a.axis(1).cross(b.axis(2)), - - a.axis(2).cross(b.axis(0)), - a.axis(2).cross(b.axis(1)), - a.axis(2).cross(b.axis(2)), - }; - - Eigen::Vector3d d = b.position - a.position; - - auto t_min = std::numeric_limits::infinity(); - - auto projection = [&](const auto & box, const auto & axis) { - return std::abs(axis.dot(box.axis(0)) * box.size(0)) + - std::abs(axis.dot(box.axis(1)) * box.size(1)) + - std::abs(axis.dot(box.axis(2)) * box.size(2)); - }; - - for (const auto & axis : axes) { - /* - The variable `separation` means how far apart two Oriented - Bounding Boxes (OBBs) are on the separation axis. When it is - positive, it means they are separated by `separation`, i.e. the - OBBs do not overlap on the separation axis. When it is less than - zero, it means the OBBs overlap on the separation axis. - - d.dot(axis) is the projection of the center distance between the - two OBBs onto the separation axis, and projection(a, axis) and - projection(b, axis) are half the projected range of the boxes - when the two OBBs are projected onto the separation axis. - */ - if (const auto separation = - std::abs(d.dot(axis)) - projection(a, axis) - projection(b, axis); - 0 < separation) { - /* - v.dot(axis) is the projection of the relative distance onto - the separation axis. In other words, we want to find the TTC - for each separation axis. The relative velocity is the - velocity vector of the observed object measured in the - observer's rest frame, so a negative value means that the two - OBBs are approaching each other. - */ - if (const auto relative_speed = v.dot(axis); relative_speed < 0) { - /* - Since this t is only a Time-To-Collision on this separation - axis, it is necessary to check that a and b are not - separated on all separation axes after t seconds. - */ - const auto t = separation / -relative_speed; - - const auto a_t = a; - - const auto b_t = [&]() { - /* - Since t is the exact time when contact occurs, it may not - be possible to say that a and b are in collision after t - seconds. Therefore, 0.01 seconds is added to t so that - collision can be expected to occur with certainty. This - value "0.01" has no technical basis and is an arbitrary - value. - */ - constexpr auto tolerance = 0.01; - auto copy = b; - copy.position += v * (t + tolerance); - return copy; - }(); - - Eigen::Vector3d d_t = b_t.position - a_t.position; - - if (std::all_of(axes.begin(), axes.end(), [&](const auto & axis) { - const auto separation_t = - std::abs(d_t.dot(axis)) - projection(a_t, axis) - projection(b_t, axis); - return separation_t <= 0; - })) { - t_min = std::min(t_min, t); - } - } - } - } - - return t_min; - } - } - - return std::numeric_limits::quiet_NaN(); - } }; class NonStandardOperation : private CoordinateSystemConversion diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp index 17a34204c03..ff290830df5 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_to_collision_condition.hpp @@ -141,19 +141,18 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio } }; - auto directional_dimension = [&]() { - switch (relative_distance_type) { - case RelativeDistanceType::longitudinal: - return std::optional(DirectionalDimension::longitudinal); - case RelativeDistanceType::lateral: - return std::optional(DirectionalDimension::lateral); - default: - case RelativeDistanceType::euclidianDistance: - return std::optional(std::nullopt); - }; - }; - auto speed = [&]() { + auto directional_dimension = [&]() { + switch (relative_distance_type) { + case RelativeDistanceType::longitudinal: + return std::optional(DirectionalDimension::longitudinal); + case RelativeDistanceType::lateral: + return std::optional(DirectionalDimension::lateral); + default: + case RelativeDistanceType::euclidianDistance: + return std::optional(std::nullopt); + }; + }; if (time_to_collision_condition_target.is()) { return RelativeSpeedCondition::evaluate( entities, triggering_entity, time_to_collision_condition_target.as(), @@ -163,15 +162,7 @@ struct TimeToCollisionCondition : private Scope, private SimulatorCore::Conditio } }; - if ( - time_to_collision_condition_target.is() and - coordinate_system == CoordinateSystem::entity and - relative_distance_type == RelativeDistanceType::euclidianDistance and freespace) { - return evaluateTimeToCollisionCondition( - triggering_entity, time_to_collision_condition_target.as()); - } else { - return distance() / std::max(speed(), 0.0); - } + return distance() / std::max(speed(), 0.0); } auto evaluate() diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml index 555aad18bad..213ac429acd 100644 --- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.TimeToCollisionCondition.yaml @@ -25,13 +25,6 @@ OpenSCENARIO: name: '' Properties: Property: [] - - name: vehicle_02 - CatalogReference: { catalogName: sample_vehicle, entryName: sample_vehicle } - ObjectController: - Controller: - name: '' - Properties: - Property: [] Storyboard: Init: Actions: @@ -72,28 +65,6 @@ OpenSCENARIO: Property: - name: maxSpeed value: '0.5' - - entityRef: vehicle_02 - PrivateAction: - - TeleportAction: - Position: - LanePosition: - roadId: '' - laneId: '34576' - s: 0 - offset: 0 - Orientation: - type: relative - h: 0 - p: 0 - r: 0 - - ControllerAction: - AssignControllerAction: - Controller: - name: '' - Properties: - Property: - - name: maxSpeed - value: '2' # If 2.5, collision will occur. Story: - name: '' Act: @@ -245,25 +216,6 @@ OpenSCENARIO: TimeToCollisionConditionTarget: EntityRef: entityRef: vehicle_01 - - Condition: - - name: '' - delay: 0 - conditionEdge: none - ByEntityCondition: - TriggeringEntities: - triggeringEntitiesRule: any - EntityRef: [ entityRef: ego ] - EntityCondition: - TimeToCollisionCondition: - freespace: true - rule: lessThan - value: 0 - relativeDistanceType: cartesianDistance - coordinateSystem: entity - routingAlgorithm: undefined - TimeToCollisionConditionTarget: - EntityRef: - entityRef: vehicle_02 Action: - name: '' UserDefinedAction: