Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2018-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "ad/rss/map/RssObjectConversion.hpp"
      10                 :            : 
      11                 :            : #include <ad/map/point/HeadingOperation.hpp>
      12                 :            : #include <ad/map/route/RouteOperation.hpp>
      13                 :            : #include <ad/physics/Operation.hpp>
      14                 :            : #include <ad/rss/situation/RssFormulas.hpp>
      15                 :            : #include <algorithm>
      16                 :            : #include "ad/rss/map/Logging.hpp"
      17                 :            : #include "ad/rss/map/RssSceneCreator.hpp"
      18                 :            : 
      19                 :            : namespace ad {
      20                 :            : namespace rss {
      21                 :            : namespace map {
      22                 :            : 
      23                 :        430 : RssObjectConversion::RssObjectConversion(RssObjectData const &objectData)
      24                 :        430 :   : mObjectMapMatchedPosition(&objectData.matchObject)
      25                 :            :   , mMaxSpeedOnAcceleration(0.)
      26                 :            :   , mOriginalObjectSpeed(objectData.speed)
      27                 :        431 :   , mRssDynamics(objectData.rssDynamics)
      28                 :            : {
      29                 :        430 :   initializeRssObject(objectData.id,
      30         [ +  + ]:        430 :                       objectData.type,
      31                 :        431 :                       ::ad::rss::world::OccupiedRegionVector(),
      32                 :        430 :                       objectData.matchObject.enuPosition,
      33                 :        430 :                       objectData.speed,
      34                 :        430 :                       objectData.yawRate,
      35                 :        430 :                       objectData.steeringAngle);
      36                 :        429 : }
      37                 :            : 
      38                 :          6 : RssObjectConversion::RssObjectConversion(RssObjectData const &objectData,
      39                 :          6 :                                          ::ad::rss::world::OccupiedRegionVector const &objectOccupiedRegions)
      40                 :            :   : mObjectMapMatchedPosition(nullptr)
      41                 :            :   , mMaxSpeedOnAcceleration(0.)
      42                 :            :   , mOriginalObjectSpeed(objectData.speed)
      43                 :          6 :   , mRssDynamics(objectData.rssDynamics)
      44                 :            : {
      45                 :          6 :   initializeRssObject(objectData.id,
      46                 :          6 :                       objectData.type,
      47                 :            :                       objectOccupiedRegions,
      48                 :          6 :                       objectData.matchObject.enuPosition,
      49                 :          6 :                       objectData.speed,
      50                 :          6 :                       objectData.yawRate,
      51         [ +  - ]:          6 :                       objectData.steeringAngle);
      52                 :          6 : }
      53                 :            : 
      54                 :        436 : void RssObjectConversion::initializeRssObject(::ad::rss::world::ObjectId const &objectId,
      55                 :            :                                               ::ad::rss::world::ObjectType const &objectType,
      56                 :            :                                               ::ad::rss::world::OccupiedRegionVector const &objectOccupiedRegions,
      57                 :            :                                               ::ad::map::match::ENUObjectPosition const &objectEnuPosition,
      58                 :            :                                               ::ad::physics::Speed const &objectSpeed,
      59                 :            :                                               ::ad::physics::AngularVelocity const &objectYawRate,
      60                 :            :                                               ::ad::physics::Angle const &objectSteeringAngle)
      61                 :            : {
      62                 :        436 :   mRssObject.objectId = objectId;
      63                 :        436 :   mRssObject.objectType = objectType;
      64                 :        436 :   mRssObject.velocity.speedLonMin = ::ad::physics::Speed(0.);
      65                 :        436 :   mRssObject.velocity.speedLonMax = ::ad::physics::Speed(0.);
      66                 :        436 :   mRssObject.velocity.speedLatMin = ::ad::physics::Speed(0.);
      67                 :        436 :   mRssObject.velocity.speedLatMax = ::ad::physics::Speed(0.);
      68                 :        436 :   mRssObject.occupiedRegions = objectOccupiedRegions;
      69                 :            : 
      70                 :        436 :   mRssObject.state.yaw = ::ad::physics::Angle(static_cast<double>(objectEnuPosition.heading));
      71                 :        436 :   mRssObject.state.centerPoint.x = ::ad::physics::Distance(static_cast<double>(objectEnuPosition.centerPoint.x));
      72                 :        436 :   mRssObject.state.centerPoint.y = ::ad::physics::Distance(static_cast<double>(objectEnuPosition.centerPoint.y));
      73                 :        436 :   mRssObject.state.dimension.width = ::ad::physics::Distance(static_cast<double>(objectEnuPosition.dimension.width));
      74                 :        436 :   mRssObject.state.dimension.length = ::ad::physics::Distance(static_cast<double>(objectEnuPosition.dimension.length));
      75                 :        436 :   mRssObject.state.steeringAngle = objectSteeringAngle;
      76                 :            : 
      77                 :            :   // restrict to positive speeds
      78         [ +  + ]:        436 :   mRssObject.state.speed = std::max(::ad::physics::Speed(0.), objectSpeed);
      79   [ +  -  +  + ]:        435 :   if (mRssObject.state.speed == ::ad::physics::Speed(0.))
      80                 :            :   {
      81                 :            :     // ensure angular speed is also zero if speed is zero
      82                 :          6 :     mRssObject.state.yawRate = ::ad::physics::AngularVelocity(0.);
      83                 :            :   }
      84                 :            :   else
      85                 :            :   {
      86                 :        429 :     mRssObject.state.yawRate = objectYawRate;
      87                 :            :   }
      88                 :        435 : }
      89                 :            : 
      90                 :        452 : bool RssObjectConversion::isOriginalSpeedAcceptable(::ad::physics::Speed const acceptableNegativeSpeed) const
      91                 :            : {
      92         [ +  - ]:        452 :   if (mRssObject.state.speed == mOriginalObjectSpeed)
      93                 :            :   {
      94                 :        452 :     return true;
      95                 :            :   }
      96         [ #  # ]:          0 :   if (mOriginalObjectSpeed >= acceptableNegativeSpeed)
      97                 :            :   {
      98                 :          0 :     return true;
      99                 :            :   }
     100                 :          0 :   return false;
     101                 :            : }
     102                 :            : 
     103                 :        564 : ::ad::rss::world::Object const &RssObjectConversion::getRssObject() const
     104                 :            : {
     105                 :        564 :   return mRssObject;
     106                 :            : }
     107                 :            : 
     108                 :       1988 : ::ad::rss::world::ObjectId RssObjectConversion::getId() const
     109                 :            : {
     110                 :       1988 :   return mRssObject.objectId;
     111                 :            : }
     112                 :            : 
     113                 :        564 : ::ad::rss::world::RssDynamics RssObjectConversion::getRssDynamics() const
     114                 :            : {
     115                 :        564 :   ::ad::rss::world::RssDynamics resultDynamics(mRssDynamics);
     116   [ +  -  +  + ]:        564 :   if (mMaxSpeedOnAcceleration != ::ad::physics::Speed(0.))
     117                 :            :   {
     118                 :        330 :     resultDynamics.maxSpeedOnAcceleration = mMaxSpeedOnAcceleration;
     119                 :            :   }
     120                 :        564 :   return resultDynamics;
     121                 :            : }
     122                 :            : 
     123                 :        213 : bool RssObjectConversion::calculateMinStoppingDistance(::ad::physics::Distance &minStoppingDistance) const
     124                 :            : {
     125                 :            :   auto const result
     126         [ +  - ]:        213 :     = situation::calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern(mRssObject.state.speed,
     127                 :          0 :                                                                               physics::Speed::getMax(),
     128                 :        213 :                                                                               mRssDynamics.responseTime,
     129                 :        213 :                                                                               mRssDynamics.alphaLon.accelMax,
     130                 :        213 :                                                                               mRssDynamics.alphaLon.brakeMinCorrect,
     131                 :            :                                                                               minStoppingDistance);
     132                 :            : 
     133         [ -  + ]:        213 :   if (!result)
     134                 :            :   {
     135         [ #  # ]:          0 :     getLogger()->error("RssObjectConversion::calculateminStoppingDistance[ {} ]>> error calculating longitudinal "
     136                 :            :                        "distance offset after stated braking pattern {} {}",
     137                 :          0 :                        mRssObject.objectId,
     138                 :          0 :                        mRssObject.state.speed,
     139                 :            :                        mRssDynamics);
     140                 :            :   }
     141                 :        213 :   return result;
     142                 :            : }
     143                 :            : 
     144                 :       1276 : void RssObjectConversion::updateSpeedLimit(::ad::physics::Speed const &maxSpeedOnAcceleration)
     145                 :            : {
     146                 :       1276 :   mMaxSpeedOnAcceleration = std::max(mMaxSpeedOnAcceleration, maxSpeedOnAcceleration);
     147                 :       1276 : }
     148                 :            : 
     149                 :        544 : void RssObjectConversion::addRestrictedOccupiedRegion(::ad::map::match::LaneOccupiedRegion const &laneOccupiedRegion,
     150                 :            :                                                       ::ad::map::route::LaneInterval const &laneInterval)
     151                 :            : {
     152         [ +  - ]:        544 :   ::ad::rss::world::OccupiedRegion occupiedRegion;
     153                 :        544 :   occupiedRegion.segmentId = ::ad::rss::world::LaneSegmentId(static_cast<uint64_t>(laneOccupiedRegion.laneId));
     154                 :            : 
     155                 :        544 :   ::ad::physics::ParametricValue cutAtStart;
     156   [ +  -  +  + ]:        544 :   if (::ad::map::route::isRouteDirectionNegative(laneInterval))
     157                 :            :   {
     158         [ +  - ]:        177 :     occupiedRegion.lonRange.maximum = ::ad::physics::ParametricValue(1.) - laneOccupiedRegion.longitudinalRange.minimum;
     159         [ +  - ]:        177 :     occupiedRegion.lonRange.minimum = ::ad::physics::ParametricValue(1.) - laneOccupiedRegion.longitudinalRange.maximum;
     160                 :            : 
     161         [ +  - ]:        177 :     occupiedRegion.latRange.maximum = ::ad::physics::ParametricValue(1.) - laneOccupiedRegion.lateralRange.minimum;
     162         [ +  - ]:        177 :     occupiedRegion.latRange.minimum = ::ad::physics::ParametricValue(1.) - laneOccupiedRegion.lateralRange.maximum;
     163         [ +  - ]:        177 :     cutAtStart = ::ad::physics::ParametricValue(1.) - laneInterval.start;
     164                 :            :   }
     165                 :            :   else
     166                 :            :   {
     167                 :        367 :     occupiedRegion.lonRange = laneOccupiedRegion.longitudinalRange;
     168                 :        367 :     occupiedRegion.latRange = laneOccupiedRegion.lateralRange;
     169                 :        367 :     cutAtStart = laneInterval.start;
     170                 :            :   }
     171                 :            : 
     172                 :            :   // scale region to the lane interval sizes
     173         [ +  - ]:        544 :   auto const intervalLength = calcParametricLength(laneInterval);
     174   [ +  -  +  + ]:        544 :   if (intervalLength == ::ad::physics::ParametricValue(0.))
     175                 :            :   {
     176                 :         76 :     occupiedRegion.lonRange.minimum = ::ad::physics::ParametricValue(0.);
     177                 :         76 :     occupiedRegion.lonRange.maximum = ::ad::physics::ParametricValue(1.);
     178                 :            :   }
     179                 :            :   else
     180                 :            :   {
     181                 :            :     // move region
     182         [ +  - ]:        468 :     occupiedRegion.lonRange.minimum -= cutAtStart;
     183         [ +  - ]:        468 :     occupiedRegion.lonRange.maximum -= cutAtStart;
     184                 :            :     // scale region
     185         [ +  - ]:        468 :     occupiedRegion.lonRange.minimum = occupiedRegion.lonRange.minimum / static_cast<double>(intervalLength);
     186         [ +  - ]:        468 :     occupiedRegion.lonRange.maximum = occupiedRegion.lonRange.maximum / static_cast<double>(intervalLength);
     187                 :            :     // restrict to segment
     188                 :            :     occupiedRegion.lonRange.minimum
     189                 :        468 :       = std::min(std::max(occupiedRegion.lonRange.minimum, ::ad::physics::ParametricValue(0.)),
     190   [ +  -  +  - ]:        936 :                  ::ad::physics::ParametricValue(1.));
     191                 :            :     occupiedRegion.lonRange.maximum
     192                 :        468 :       = std::min(std::max(occupiedRegion.lonRange.maximum, ::ad::physics::ParametricValue(0.)),
     193   [ +  -  +  - ]:        936 :                  ::ad::physics::ParametricValue(1.));
     194                 :            :   }
     195   [ +  -  +  - ]:       1088 :   getLogger()->trace("RssObjectConversion::addRestrictedOccupiedRegion[ {} ]>>laneOccupiedRegion {} laneInterval {} -> "
     196                 :            :                      "occupiedRegion {}",
     197         [ +  - ]:       1088 :                      getId(),
     198                 :            :                      laneOccupiedRegion,
     199                 :            :                      laneInterval,
     200                 :            :                      occupiedRegion);
     201                 :            : 
     202         [ +  - ]:        544 :   mRssObject.occupiedRegions.push_back(occupiedRegion);
     203                 :        544 : }
     204                 :            : 
     205                 :       1276 : void RssObjectConversion::laneIntervalAdded(::ad::map::route::LaneInterval const &laneInterval)
     206                 :            : {
     207         [ +  - ]:       1276 :   if (mObjectMapMatchedPosition != nullptr)
     208                 :            :   {
     209                 :            :     auto findLaneIntervalResult
     210                 :       1276 :       = std::find_if(mObjectMapMatchedPosition->mapMatchedBoundingBox.laneOccupiedRegions.begin(),
     211                 :       1276 :                      mObjectMapMatchedPosition->mapMatchedBoundingBox.laneOccupiedRegions.end(),
     212                 :       1992 :                      [laneInterval](::ad::map::match::LaneOccupiedRegion const &occupiedRegion) {
     213                 :       1992 :                        return laneInterval.laneId == occupiedRegion.laneId;
     214         [ +  - ]:       1276 :                      });
     215         [ +  + ]:       1276 :     if (findLaneIntervalResult != mObjectMapMatchedPosition->mapMatchedBoundingBox.laneOccupiedRegions.end())
     216                 :            :     {
     217         [ +  - ]:        544 :       addRestrictedOccupiedRegion(*findLaneIntervalResult, laneInterval);
     218                 :            :     }
     219                 :            :   }
     220                 :       1276 : }
     221                 :            : 
     222                 :        493 : void RssObjectConversion::updateVelocityOnRoute(::ad::map::route::FullRoute const &route)
     223                 :            : {
     224                 :        493 :   double headingDiff = 0.;
     225         [ +  - ]:        493 :   if (mObjectMapMatchedPosition != nullptr)
     226                 :            :   {
     227         [ +  - ]:        493 :     auto const routeHeading = getENUHeadingOfRoute(*mObjectMapMatchedPosition, route);
     228                 :        493 :     headingDiff = static_cast<double>(
     229   [ +  -  +  - ]:        986 :       ::ad::map::point::normalizeENUHeading(routeHeading - mObjectMapMatchedPosition->enuPosition.heading));
     230                 :            :   }
     231                 :            : 
     232         [ +  - ]:        493 :   mRssObject.velocity.speedLonMin = std::fabs(std::cos(headingDiff)) * mRssObject.state.speed;
     233                 :        493 :   mRssObject.velocity.speedLonMax = mRssObject.velocity.speedLonMin;
     234                 :            : 
     235         [ +  - ]:        493 :   mRssObject.velocity.speedLatMin = std::sin(headingDiff) * mRssObject.state.speed;
     236                 :        493 :   mRssObject.velocity.speedLatMax = mRssObject.velocity.speedLatMin;
     237                 :        493 : }
     238                 :            : 
     239                 :            : } // namespace map
     240                 :            : } // namespace rss
     241                 :            : } // namespace ad