Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2019-2022 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "ad/rss/map/RssObjectDataOperation.hpp"
      10                 :            : #include <ad/map/lane/Operation.hpp>
      11                 :            : #include <ad/map/match/AdMapMatching.hpp>
      12                 :            : #include <ad/map/match/MapMatchedOperation.hpp>
      13                 :            : #include <ad/map/route/Operation.hpp>
      14                 :            : #include "ad/rss/map/Logging.hpp"
      15                 :            : 
      16                 :            : namespace ad {
      17                 :            : namespace rss {
      18                 :            : namespace map {
      19                 :            : 
      20                 :         28 : RssObjectDataOnRoute calculateRssObjectDataOnRoute(ad::map::route::FullRoute const &route,
      21                 :            :                                                    RssObjectData const &object_data)
      22                 :            : {
      23         [ +  - ]:         28 :   RssObjectDataOnRoute object_data_on_route;
      24                 :         28 :   object_data_on_route.object_within_route = false;
      25                 :         28 :   object_data_on_route.object_center_within_route = false;
      26                 :         28 :   object_data_on_route.is_valid = false;
      27                 :            : 
      28         [ -  + ]:         28 :   if (route.road_segments.empty())
      29                 :            :   {
      30   [ #  #  #  # ]:          0 :     getLogger()->info("calculateRssObjectDataOnRoute() route empty {}.", object_data);
      31                 :          0 :     return object_data_on_route;
      32                 :            :   }
      33         [ +  - ]:         28 :   ad::map::route::FindWaypointResult centerPointOnRoute(route);
      34                 :         28 :   if (object_data.match_object.map_matched_bounding_box.reference_point_positions.size()
      35         [ +  - ]:         28 :       > static_cast<std::size_t>(ad::map::match::ObjectReferencePoints::Center))
      36                 :            :   {
      37                 :         28 :     ad::map::point::ParaPointList inLaneMatches;
      38                 :         28 :     ad::map::point::ParaPointList outLaneMatches;
      39                 :         28 :     for (auto &matchPosition : object_data.match_object.map_matched_bounding_box
      40         [ +  + ]:        140 :                                  .reference_point_positions[int32_t(ad::map::match::ObjectReferencePoints::Center)])
      41                 :            :     {
      42                 :            :       // consider only longitudinal in lane matches here; we are not interrested in points in upcoming lanes in front
      43   [ +  -  +  + ]:         84 :       if (ad::map::match::isLongitudinalInLaneMatch(matchPosition))
      44                 :            :       {
      45         [ +  + ]:         69 :         if (matchPosition.type == ad::map::match::MapMatchedPositionType::LANE_IN)
      46                 :            :         {
      47         [ +  - ]:         42 :           inLaneMatches.push_back(matchPosition.lane_point.para_point);
      48                 :            :         }
      49                 :            :         else
      50                 :            :         {
      51         [ +  - ]:         27 :           outLaneMatches.push_back(matchPosition.lane_point.para_point);
      52                 :            :         }
      53                 :            :       }
      54                 :            :     }
      55                 :            : 
      56                 :            :     // check if object is on the route
      57                 :            :     auto const objectOnRouteResult
      58         [ +  - ]:         28 :       = ad::map::route::objectOnRoute(object_data.match_object.map_matched_bounding_box, route);
      59         [ +  - ]:         28 :     if (objectOnRouteResult.isValid())
      60                 :            :     {
      61                 :         28 :       object_data_on_route.object_within_route = true;
      62                 :            : 
      63   [ +  -  +  - ]:         28 :       centerPointOnRoute = ad::map::route::findNearestWaypoint(inLaneMatches, route);
      64         [ +  - ]:         28 :       if (centerPointOnRoute.isValid())
      65                 :            :       {
      66                 :         28 :         object_data_on_route.object_center_within_route = true;
      67                 :            :         object_data_on_route.object_center_position_projected_on_route
      68                 :         28 :           = object_data.match_object.enu_position.center_point;
      69                 :         28 :         object_data_on_route.object_center_distance_to_route = physics::Distance(0.);
      70                 :         28 :         object_data_on_route.is_valid = true;
      71   [ +  -  +  - ]:         56 :         getLogger()->trace("calculateRssObjectDataOnRoute() object center in lane match ({}) {}:{} route: {}",
      72                 :            :                            inLaneMatches,
      73         [ +  - ]:         28 :                            centerPointOnRoute.laneSegmentIterator->lane_interval.lane_id,
      74                 :            :                            centerPointOnRoute.queryPosition.parametric_offset,
      75                 :            :                            route);
      76                 :            :       }
      77                 :            :     }
      78                 :            : 
      79         [ -  + ]:         28 :     if (!object_data_on_route.is_valid)
      80                 :            :     {
      81   [ #  #  #  # ]:          0 :       centerPointOnRoute = ad::map::route::findNearestWaypoint(outLaneMatches, route);
      82         [ #  # ]:          0 :       if (centerPointOnRoute.isValid())
      83                 :            :       {
      84                 :            :         object_data_on_route.object_center_position_projected_on_route
      85         [ #  # ]:          0 :           = ad::map::lane::getENULanePoint(centerPointOnRoute.queryPosition);
      86                 :            :         object_data_on_route.object_center_distance_to_route
      87                 :          0 :           = ad::map::point::distance(object_data_on_route.object_center_position_projected_on_route,
      88         [ #  # ]:          0 :                                      object_data.match_object.enu_position.center_point);
      89                 :          0 :         object_data_on_route.is_valid = true;
      90   [ #  #  #  # ]:          0 :         getLogger()->trace("calculateRssObjectDataOnRoute() object center out lane match ({}) {}:{} route: {}",
      91                 :            :                            outLaneMatches,
      92         [ #  # ]:          0 :                            centerPointOnRoute.laneSegmentIterator->lane_interval.lane_id,
      93                 :            :                            centerPointOnRoute.queryPosition.parametric_offset,
      94                 :            :                            route);
      95                 :            :       }
      96                 :            :     }
      97                 :         28 :   }
      98                 :            :   else
      99                 :            :   {
     100   [ #  #  #  # ]:          0 :     getLogger()->warn(
     101                 :            :       "calculateRssObjectDataOnRoute() object match_object {} not valid. Try to apply fall-back calculation.",
     102                 :            :       object_data);
     103                 :            :   }
     104                 :            : 
     105         [ -  + ]:         28 :   if (!object_data_on_route.is_valid)
     106                 :            :   {
     107   [ #  #  #  # ]:          0 :     getLogger()->debug(
     108                 :            :       "calculateRssObjectDataOnRoute() Fall-back calculation: matched object center {} too far away from route {}.",
     109                 :            :       object_data,
     110                 :            :       route);
     111                 :            :     auto mapMatchedPositionConfidenceList = ad::map::match::AdMapMatching::findRouteLanes(
     112   [ #  #  #  # ]:          0 :       ad::map::point::toECEF(object_data.match_object.enu_position.center_point), route);
     113         [ #  # ]:          0 :     if (mapMatchedPositionConfidenceList.empty())
     114                 :            :     {
     115   [ #  #  #  # ]:          0 :       getLogger()->warn(
     116                 :            :         "calculateRssObjectDataOnRoute() object center point not matched against any lane of the route {} {}.",
     117                 :            :         object_data,
     118                 :            :         route);
     119                 :          0 :       return object_data_on_route;
     120                 :            :     }
     121                 :            :     centerPointOnRoute
     122   [ #  #  #  # ]:          0 :       = ad::map::route::findWaypoint(mapMatchedPositionConfidenceList.front().lane_point.para_point, route);
     123         [ #  # ]:          0 :     if (!centerPointOnRoute.isValid())
     124                 :            :     {
     125   [ #  #  #  # ]:          0 :       getLogger()->error("calculateRssObjectDataOnRoute() nearest matched point {} not found in route unexpectedly {}.",
     126         [ #  # ]:          0 :                          mapMatchedPositionConfidenceList.front().lane_point.para_point,
     127                 :            :                          route);
     128                 :          0 :       return object_data_on_route;
     129                 :            :     }
     130   [ #  #  #  # ]:          0 :     getLogger()->trace("calculateRssObjectDataOnRoute() nearest individual map match result {} from all {}.",
     131                 :          0 :                        mapMatchedPositionConfidenceList.front(),
     132                 :            :                        mapMatchedPositionConfidenceList);
     133                 :            : 
     134                 :            :     object_data_on_route.object_center_position_projected_on_route
     135         [ #  # ]:          0 :       = ad::map::point::toENU(mapMatchedPositionConfidenceList.front().matched_point);
     136                 :            :     object_data_on_route.object_center_distance_to_route
     137                 :          0 :       = mapMatchedPositionConfidenceList.front().matched_point_distance;
     138                 :          0 :     object_data_on_route.is_valid = true;
     139         [ #  # ]:          0 :   }
     140                 :            : 
     141         [ -  + ]:         28 :   if (!centerPointOnRoute.isValid())
     142                 :            :   {
     143   [ #  #  #  # ]:          0 :     getLogger()->error("calculateRssObjectDataOnRoute() FindWaypointResult unexpectedly invalid.");
     144                 :          0 :     object_data_on_route.is_valid = false;
     145                 :          0 :     return object_data_on_route;
     146                 :            :   }
     147                 :            : 
     148         [ +  - ]:         28 :   object_data_on_route.object_center_distance_along_route = ad::map::route::calcLength(centerPointOnRoute);
     149                 :            : 
     150                 :            :   object_data_on_route.object_center_position_projected_on_lane_in_nominal_route_direction
     151                 :         28 :     = object_data_on_route.object_center_position_projected_on_route;
     152                 :         28 :   auto laneInNominalRouteDirection = centerPointOnRoute.queryPosition;
     153         [ -  + ]:         28 :   if (centerPointOnRoute.laneSegmentIterator->lane_interval.wrong_way)
     154                 :            :   {
     155                 :            :     // driving on the wrong lane, so we have to project to nominal route direction
     156         [ #  # ]:          0 :     ad::map::lane::projectPositionToLaneInHeadingDirection(
     157                 :            :       laneInNominalRouteDirection, object_data_on_route.route_heading, laneInNominalRouteDirection);
     158                 :            :     object_data_on_route.object_center_position_projected_on_lane_in_nominal_route_direction
     159         [ #  # ]:          0 :       = ad::map::lane::getENULanePoint(laneInNominalRouteDirection);
     160                 :            :   }
     161                 :         28 :   laneInNominalRouteDirection.parametric_offset = physics::ParametricValue(0.5);
     162                 :            :   object_data_on_route.nominal_center_position_of_lane_in_nominal_route_direction
     163         [ +  - ]:         28 :     = ad::map::lane::getENULanePoint(laneInNominalRouteDirection);
     164                 :            : 
     165                 :            :   // object center data is now filled correctly now getting to the orientation measures
     166                 :            :   auto const routeSectionAroundObject = getRouteSection(centerPointOnRoute,
     167                 :         28 :                                                         object_data.match_object.enu_position.dimension.length,
     168                 :         28 :                                                         object_data.match_object.enu_position.dimension.length,
     169                 :            :                                                         route,
     170         [ +  - ]:         28 :                                                         ad::map::route::RouteSectionCreationMode::AllRouteLanes);
     171         [ +  - ]:         28 :   auto const routeSectionBorders = ad::map::route::getENUBorderOfRoute(routeSectionAroundObject);
     172                 :            :   object_data_on_route.route_heading
     173         [ +  - ]:         28 :     = ad::map::lane::getENUHeading(routeSectionBorders,
     174                 :            :                                    object_data_on_route.object_center_position_projected_on_route,
     175         [ +  - ]:         28 :                                    object_data.match_object.enu_position.dimension.length / 2.);
     176                 :            : 
     177                 :         28 :   object_data_on_route.route_radius = ad::physics::Distance::getMax();
     178                 :            :   object_data_on_route.object_route_section_front_right
     179                 :         28 :     = object_data_on_route.object_center_position_projected_on_route;
     180                 :         28 :   object_data_on_route.object_route_section_front_left = object_data_on_route.object_center_position_projected_on_route;
     181                 :         28 :   object_data_on_route.object_route_section_back_right = object_data_on_route.object_center_position_projected_on_route;
     182                 :         28 :   object_data_on_route.object_route_section_back_left = object_data_on_route.object_center_position_projected_on_route;
     183                 :         28 :   object_data_on_route.object_turning_center = object_data_on_route.object_center_position_projected_on_route;
     184         [ +  - ]:         56 :   if (!routeSectionBorders.empty() && !routeSectionBorders.front().right.points.empty()
     185   [ +  -  +  - ]:         28 :       && !routeSectionBorders.front().left.points.empty() && !routeSectionBorders.back().right.points.empty()
     186   [ +  -  +  -  :         56 :       && !routeSectionBorders.back().left.points.empty())
                   +  - ]
     187                 :            :   {
     188                 :            :     // now we calculate the current radius of the route based on the routeSectionBorders (ignoring z-component)
     189                 :            :     // the front left and right of the borders provide two points P1, P2 of one line
     190                 :            :     // the back left and right of the borders provide two points P3, P4 of second line
     191                 :            :     // the intersection point of this line is the imaginary cicle center the route is turning around
     192                 :            :     // Formular from https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line
     193                 :         28 :     object_data_on_route.object_route_section_front_right = routeSectionBorders.front().right.points.front();
     194                 :         28 :     object_data_on_route.object_route_section_front_left = routeSectionBorders.front().left.points.front();
     195                 :         28 :     object_data_on_route.object_route_section_back_right = routeSectionBorders.back().right.points.back();
     196                 :         28 :     object_data_on_route.object_route_section_back_left = routeSectionBorders.back().left.points.back();
     197                 :            : 
     198                 :         28 :     auto const x1 = object_data_on_route.object_route_section_front_left.x;
     199                 :         28 :     auto const y1 = object_data_on_route.object_route_section_front_left.y;
     200                 :         28 :     auto const x2 = object_data_on_route.object_route_section_front_right.x;
     201                 :         28 :     auto const y2 = object_data_on_route.object_route_section_front_right.y;
     202                 :         28 :     auto const x3 = object_data_on_route.object_route_section_back_left.x;
     203                 :         28 :     auto const y3 = object_data_on_route.object_route_section_back_left.y;
     204                 :         28 :     auto const x4 = object_data_on_route.object_route_section_back_right.x;
     205                 :         28 :     auto const y4 = object_data_on_route.object_route_section_back_right.y;
     206                 :            : 
     207   [ +  -  +  -  :         28 :     auto denominator = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
          +  -  +  -  +  
             -  +  -  +  
                      - ]
     208   [ +  -  +  + ]:         28 :     if (denominator != ad::map::point::ENUCoordinate(0.))
     209                 :            :     {
     210   [ +  -  +  -  :          2 :       auto const a = x1 * y2 - y1 * x2;
                   +  - ]
     211   [ +  -  +  -  :          2 :       auto const b = x3 * y4 - y3 * x4;
                   +  - ]
     212   [ +  -  +  -  :          2 :       auto const px = (a * (x3 - x4) - b * (x1 - x2)) / denominator;
          +  -  +  -  +  
                -  +  - ]
     213   [ +  -  +  -  :          2 :       auto const py = (a * (y3 - y4) - b * (y1 - y2)) / denominator;
          +  -  +  -  +  
                -  +  - ]
     214                 :            :       object_data_on_route.object_turning_center
     215         [ +  - ]:          2 :         = ad::map::point::createENUPoint(ad::map::point::ENUCoordinate(px),
     216                 :            :                                          ad::map::point::ENUCoordinate(py),
     217                 :            :                                          object_data_on_route.object_center_position_projected_on_route.z);
     218         [ +  - ]:          2 :       object_data_on_route.route_radius = ad::map::point::distance(
     219                 :            :         object_data_on_route.object_turning_center, object_data_on_route.object_center_position_projected_on_route);
     220                 :            :       // determine the sign of the radius
     221         [ +  - ]:          2 :       auto const headingTowardsTurningCenter = ad::map::point::createENUHeading(
     222                 :            :         object_data_on_route.object_center_position_projected_on_route, object_data_on_route.object_turning_center);
     223         [ +  - ]:          2 :       auto const headingDiff = physics::normalizeAngleSigned(
     224                 :          2 :         physics::Angle(headingTowardsTurningCenter.mENUHeading - object_data_on_route.route_heading.mENUHeading));
     225   [ +  -  +  - ]:          2 :       if (headingDiff < ad::physics::Angle(0.))
     226                 :            :       {
     227         [ +  - ]:          2 :         object_data_on_route.route_radius = -object_data_on_route.route_radius;
     228                 :            :       }
     229                 :            :     }
     230                 :            :   }
     231                 :            : 
     232   [ +  -  -  + ]:         28 :   if (std::fabs(object_data_on_route.route_heading) >= ad::map::point::ENUHeading(2 * M_PI))
     233                 :            :   {
     234                 :            :     // if the projected center point is outside, lane::getENUHeading() returns 2*M_PI
     235                 :            :     // this should not happen since the projection should be within or at the borders
     236   [ #  #  #  # ]:          0 :     getLogger()->warn("calculateRssObjectDataOnRoute() projected object center point {} not part of route section "
     237                 :            :                       "around object at {}\n object_data {}, route {} and extracted section borders {}.",
     238                 :            :                       object_data_on_route.object_center_position_projected_on_route,
     239                 :            :                       centerPointOnRoute.queryPosition,
     240                 :            :                       object_data,
     241                 :            :                       route,
     242                 :            :                       routeSectionBorders);
     243                 :          0 :     object_data_on_route.is_valid = false;
     244                 :            :   }
     245                 :            : 
     246         [ +  - ]:         28 :   object_data_on_route.route_heading_delta = physics::normalizeAngleSigned(physics::Angle(
     247                 :         28 :     object_data_on_route.route_heading.mENUHeading - object_data.match_object.enu_position.heading.mENUHeading));
     248                 :            : 
     249                 :            :   object_data_on_route.route_speed_lon
     250   [ +  -  +  - ]:         28 :     = std::cos(object_data_on_route.route_heading_delta) * object_data.speed_range.maximum;
     251                 :            :   object_data_on_route.route_speed_lat
     252   [ +  -  +  - ]:         28 :     = std::sin(object_data_on_route.route_heading_delta) * object_data.speed_range.maximum;
     253                 :            : 
     254                 :         28 :   return object_data_on_route;
     255                 :         28 : }
     256                 :            : 
     257                 :            : } // namespace map
     258                 :            : } // namespace rss
     259                 :            : } // namespace ad