Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2020-2022 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "ad/rss/map/RssReachableSetsCalculation.hpp"
      10                 :            : #include <ad/map/geometry/PolygonOperation.hpp>
      11                 :            : #include <ad/map/route/LaneIntervalOperation.hpp>
      12                 :            : #include <ad/map/route/Planning.hpp>
      13                 :            : #include <ad/map/route/RouteOperation.hpp>
      14                 :            : #include <ad/rss/map/RssObjectConversion.hpp>
      15                 :            : #include <ad/rss/unstructured/TrajectoryPedestrian.hpp>
      16                 :            : #include <ad/rss/unstructured/TrajectoryVehicle.hpp>
      17                 :            : #include "ad/rss/map/RssWorldModelCreation.hpp"
      18                 :            : 
      19                 :            : namespace ad {
      20                 :            : namespace rss {
      21                 :            : namespace map {
      22                 :            : namespace RssReachableSetsCalculation {
      23                 :            : 
      24                 :          0 : bool calculateTrajectorySetsRestrictedToRoute(::ad::rss::world::ObjectId const &objectId,
      25                 :            :                                               ::ad::map::route::FullRoute const &route,
      26                 :            :                                               ::ad::rss::core::RelativeObjectState const &vehicleState,
      27                 :            :                                               ::ad::geometry::Polygon &brakePolygon,
      28                 :            :                                               ::ad::geometry::Polygon &continueForwardPolygon)
      29                 :            : {
      30         [ #  # ]:          0 :   ::ad::map::route::FullRoute shortenedRoute = route;
      31                 :          0 :   ::ad::physics::Distance conservativeMinStoppingDistance;
      32         [ #  # ]:          0 :   if (RssObjectConversion::calculateConservativeMinStoppingDistance(
      33                 :            :         objectId,
      34                 :          0 :         vehicleState.unstructured_object_state.speed_range.maximum,
      35         [ #  # ]:          0 :         vehicleState.dynamics,
      36                 :            :         conservativeMinStoppingDistance))
      37                 :            :   {
      38                 :            :     // ensure minimum length to be considered
      39                 :            :     // if cut too much, continue forward and brake polygons are cut in front at the same position leading to no reaction
      40                 :            :     // of ego at all
      41   [ #  #  #  # ]:          0 :     ::ad::map::route::shortenRouteToDistance(
      42                 :            :       shortenedRoute,
      43         [ #  # ]:          0 :       std::max(RssWorldModelCreation::cMinConnectedRouteLength, 2.0 * conservativeMinStoppingDistance),
      44                 :            :       ::ad::map::route::ShortenRouteToDistanceMode::AllowCutIntersection);
      45                 :            :   }
      46         [ #  # ]:          0 :   auto const shortenedRouteSectionBorders = ad::map::route::getENUBorderOfRoute(shortenedRoute);
      47         [ #  # ]:          0 :   auto routePolygon = ad::map::geometry::toPolygon(shortenedRouteSectionBorders);
      48                 :            : 
      49                 :          0 :   bool trajectory_result = false;
      50         [ #  # ]:          0 :   if ((vehicleState.object_type == ::ad::rss::world::ObjectType::Pedestrian)
      51         [ #  # ]:          0 :       || (vehicleState.object_type == ::ad::rss::world::ObjectType::ArtificialPedestrian))
      52                 :            :   {
      53                 :          0 :     ::ad::rss::unstructured::TrajectoryPedestrian trajectoryPedestrian;
      54                 :            :     trajectory_result
      55         [ #  # ]:          0 :       = trajectoryPedestrian.calculateTrajectorySets(vehicleState, brakePolygon, continueForwardPolygon);
      56                 :          0 :   }
      57                 :            :   else
      58                 :            :   {
      59                 :          0 :     ::ad::rss::unstructured::TrajectoryVehicle trajectoryVehicle;
      60         [ #  # ]:          0 :     trajectory_result = trajectoryVehicle.calculateTrajectorySets(vehicleState, brakePolygon, continueForwardPolygon);
      61                 :            :   }
      62                 :            : 
      63         [ #  # ]:          0 :   if (trajectory_result == true)
      64                 :            :   {
      65         [ #  # ]:          0 :     auto reference_point = ad::geometry::toPoint(vehicleState.unstructured_object_state.center_point);
      66                 :            :     brakePolygon
      67         [ #  # ]:          0 :       = ad::map::geometry::restrictPolygonToAreaClostestToReferencePoint(brakePolygon, routePolygon, reference_point);
      68                 :            :     // Do not cut continue forward, otherwhise at the sides, both polygons are cut at the same position leading to no
      69                 :            :     // reaction of ego from that direction at all! (This is comparable with someone else is approaching from behind;
      70                 :            :     // then the front vehicle is also not acting.) One could reenable this behavior with the argument: if someone enters
      71                 :            :     // the driveway of the ego from the side while ego is driving on the road and the other is entering already within
      72                 :            :     // the braking polygon area, then it's in the responsibility of the other traffic participant. But before reanabling
      73                 :            :     // this, one should provide a basic evasive maneuver behavior that at least brakes in this kind of situation to
      74                 :            :     // prevent from the accident (especially when considering pedestrians; other vehicles entering the road will be
      75                 :            :     // treated in a structured way after touching the road). So without detailed analysis of multiple critical
      76                 :            :     // scenarios, one should keep the continue forward polygon as is.
      77                 :            :     // continueForwardPolygon = ad::map::geometry::restrictPolygonToAreaClostestToReferencePoint(
      78                 :            :     //  continueForwardPolygon, routePolygon, reference_point);
      79                 :            :   }
      80                 :            : 
      81                 :          0 :   return trajectory_result;
      82                 :          0 : }
      83                 :            : 
      84                 :            : } // namespace RssReachableSetsCalculation
      85                 :            : } // namespace map
      86                 :            : } // namespace rss
      87                 :            : } // namespace ad