LCOV - code coverage report
Current view: top level - src/unstructured - TrajectoryCommon.cpp (source / functions) Hit Total Coverage
Test: ad_rss Lines: 100 111 90.1 %
Date: 2024-10-01 15:08:14 Functions: 4 4 100.0 %
Branches: 130 263 49.4 %

           Branch data     Line data    Source code
       1                 :            : 
       2                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       3                 :            : //
       4                 :            : // Copyright (C) 2020-2021 Intel Corporation
       5                 :            : //
       6                 :            : // SPDX-License-Identifier: LGPL-2.1-only
       7                 :            : //
       8                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       9                 :            : /**
      10                 :            :  * @file
      11                 :            :  */
      12                 :            : 
      13                 :            : #include "TrajectoryCommon.hpp"
      14                 :            : #include <ad/physics/Operation.hpp>
      15                 :            : #include "ad/rss/unstructured/DebugDrawing.hpp"
      16                 :            : 
      17                 :            : /*!
      18                 :            :  * @brief namespace ad
      19                 :            :  */
      20                 :            : namespace ad {
      21                 :            : /*!
      22                 :            :  * @brief namespace rss
      23                 :            :  */
      24                 :            : namespace rss {
      25                 :            : /*!
      26                 :            :  * @brief namespace unstructured
      27                 :            :  */
      28                 :            : namespace unstructured {
      29                 :            : 
      30                 :       3040 : Point getVehicleCorner(TrajectoryPoint const &point,
      31                 :            :                        ad::physics::Dimension2D const &vehicleDimension,
      32                 :            :                        VehicleCorner const corner)
      33                 :            : {
      34                 :            :   Point resultPoint;
      35         [ +  - ]:       3040 :   auto const vehicleAngle = point.angle - ad::physics::cPI_2;
      36   [ +  +  +  +  :       3040 :   switch (corner)
                      - ]
      37                 :            :   {
      38                 :        760 :     case VehicleCorner::frontLeft:
      39                 :            :       resultPoint = rotateAroundPoint(
      40   [ +  -  +  -  :        760 :         point.position, toPoint(-vehicleDimension.width / 2.0, vehicleDimension.length / 2.0), vehicleAngle);
          +  -  +  -  +  
                      - ]
      41                 :        760 :       break;
      42                 :        760 :     case VehicleCorner::frontRight:
      43                 :            :       resultPoint = rotateAroundPoint(
      44   [ +  -  +  -  :        760 :         point.position, toPoint(vehicleDimension.width / 2.0, vehicleDimension.length / 2.0), vehicleAngle);
             +  -  +  - ]
      45                 :        760 :       break;
      46                 :        760 :     case VehicleCorner::backLeft:
      47                 :            :       resultPoint = rotateAroundPoint(
      48   [ +  -  +  -  :        760 :         point.position, toPoint(-vehicleDimension.width / 2.0, -vehicleDimension.length / 2.0), vehicleAngle);
          +  -  +  -  +  
                -  +  - ]
      49                 :        760 :       break;
      50                 :        760 :     case VehicleCorner::backRight:
      51                 :            :       resultPoint = rotateAroundPoint(
      52   [ +  -  +  -  :        760 :         point.position, toPoint(vehicleDimension.width / 2.0, -vehicleDimension.length / 2.0), vehicleAngle);
          +  -  +  -  +  
                      - ]
      53                 :        760 :       break;
      54                 :          0 :     default:
      55         [ #  # ]:          0 :       throw std::runtime_error("unstructured::getVehicleCorner>> invalid corner requested");
      56                 :            :       break;
      57                 :            :   }
      58                 :       3040 :   return resultPoint;
      59                 :            : }
      60                 :            : 
      61                 :        120 : bool calculateStepPolygon(situation::VehicleState const &vehicleState,
      62                 :            :                           TrajectorySetStep const &step,
      63                 :            :                           std::string const &debugNamespace,
      64                 :            :                           Polygon &polygon,
      65                 :            :                           TrajectorySetStepVehicleLocation &stepVehicleLocation)
      66                 :            : {
      67                 :        240 :   MultiPoint frontPtsLeft;
      68                 :        240 :   MultiPoint frontPtsRight;
      69                 :            : 
      70                 :        120 :   int idx = 0;
      71         [ +  + ]:        440 :   for (auto it = step.left.cbegin(); it != step.left.cend(); ++it)
      72                 :            :   {
      73         [ +  - ]:        320 :     auto vehicleLocationLeft = TrafficParticipantLocation(*it, vehicleState);
      74   [ +  -  +  -  :        320 :     DEBUG_DRAWING_POLYGON(vehicleLocationLeft.toPolygon(), "blue", debugNamespace + "_left_" + std::to_string(idx));
          +  -  +  -  +  
             -  +  -  +  
                      - ]
      75   [ +  -  +  - ]:        320 :     boost::geometry::append(frontPtsLeft, vehicleLocationLeft.toMultiPoint());
      76         [ +  + ]:        320 :     if (it == step.left.end() - 1)
      77                 :            :     {
      78                 :        120 :       stepVehicleLocation.left = vehicleLocationLeft;
      79                 :            :     }
      80                 :        320 :     ++idx;
      81                 :            :   }
      82                 :            : 
      83                 :            :   // center
      84         [ +  - ]:        120 :   auto vehicleLocationCenter = TrafficParticipantLocation(step.center, vehicleState);
      85   [ +  -  +  -  :        120 :   DEBUG_DRAWING_POLYGON(vehicleLocationCenter.toPolygon(), "blue", debugNamespace + "_center");
          +  -  +  -  +  
                      - ]
      86   [ +  -  +  - ]:        120 :   boost::geometry::append(frontPtsLeft, vehicleLocationCenter.toMultiPoint());
      87                 :        120 :   stepVehicleLocation.center = vehicleLocationCenter;
      88   [ +  -  +  - ]:        120 :   boost::geometry::append(frontPtsRight, vehicleLocationCenter.toMultiPoint());
      89                 :            : 
      90                 :        120 :   idx = 0;
      91         [ +  + ]:        440 :   for (auto it = step.right.cbegin(); it != step.right.cend(); ++it)
      92                 :            :   {
      93         [ +  - ]:        320 :     auto vehicleLocationRight = TrafficParticipantLocation(*it, vehicleState);
      94   [ +  -  +  -  :        320 :     DEBUG_DRAWING_POLYGON(vehicleLocationRight.toPolygon(), "blue", debugNamespace + "_right_" + std::to_string(idx));
          +  -  +  -  +  
             -  +  -  +  
                      - ]
      95   [ +  -  +  - ]:        320 :     boost::geometry::append(frontPtsRight, vehicleLocationRight.toMultiPoint());
      96         [ +  + ]:        320 :     if (it == step.right.begin())
      97                 :            :     {
      98                 :        120 :       stepVehicleLocation.right = vehicleLocationRight;
      99                 :            :     }
     100                 :        320 :     ++idx;
     101                 :            :   }
     102                 :            : 
     103         [ +  - ]:        240 :   Polygon hullLeft;
     104         [ +  - ]:        120 :   Polygon hullRight;
     105         [ +  - ]:        120 :   boost::geometry::convex_hull(frontPtsLeft, hullLeft);
     106         [ +  - ]:        120 :   boost::geometry::convex_hull(frontPtsRight, hullRight);
     107         [ +  - ]:        120 :   auto result = combinePolygon(hullLeft, hullRight, polygon);
     108                 :        240 :   return result;
     109                 :            : }
     110                 :            : 
     111                 :        100 : bool calculateEstimationBetweenSteps(Polygon &polygon,
     112                 :            :                                      TrajectorySetStepVehicleLocation const &previousStepVehicleLocation,
     113                 :            :                                      TrajectorySetStepVehicleLocation const &currentStepVehicleLocation,
     114                 :            :                                      std::string const &debugNamespace)
     115                 :            : {
     116                 :            :   // Fill potential gap between two calculation steps by using the previous and current step
     117   [ +  -  +  + ]:        100 :   if (previousStepVehicleLocation == currentStepVehicleLocation)
     118                 :            :   {
     119                 :         40 :     return true;
     120                 :            :   }
     121                 :            : 
     122         [ +  - ]:        120 :   Polygon hull;
     123                 :        120 :   MultiPoint interimPtsLeft;
     124   [ +  -  +  - ]:         60 :   boost::geometry::append(interimPtsLeft, previousStepVehicleLocation.left.toMultiPoint());
     125   [ +  -  +  - ]:         60 :   boost::geometry::append(interimPtsLeft, previousStepVehicleLocation.center.toMultiPoint());
     126   [ +  -  +  - ]:         60 :   boost::geometry::append(interimPtsLeft, currentStepVehicleLocation.left.toMultiPoint());
     127   [ +  -  +  - ]:         60 :   boost::geometry::append(interimPtsLeft, currentStepVehicleLocation.center.toMultiPoint());
     128         [ +  - ]:        120 :   Polygon hullLeft;
     129         [ +  - ]:         60 :   boost::geometry::convex_hull(interimPtsLeft, hullLeft);
     130                 :            : 
     131                 :        120 :   MultiPoint interimPtsRight;
     132   [ +  -  +  - ]:         60 :   boost::geometry::append(interimPtsRight, previousStepVehicleLocation.right.toMultiPoint());
     133   [ +  -  +  - ]:         60 :   boost::geometry::append(interimPtsRight, previousStepVehicleLocation.center.toMultiPoint());
     134   [ +  -  +  - ]:         60 :   boost::geometry::append(interimPtsRight, currentStepVehicleLocation.right.toMultiPoint());
     135   [ +  -  +  - ]:         60 :   boost::geometry::append(interimPtsRight, currentStepVehicleLocation.center.toMultiPoint());
     136         [ +  - ]:         60 :   Polygon hullRight;
     137         [ +  - ]:         60 :   boost::geometry::convex_hull(interimPtsRight, hullRight);
     138         [ +  - ]:         60 :   auto result = combinePolygon(hullRight, hullLeft, hull);
     139         [ -  + ]:         60 :   if (!result)
     140                 :            :   {
     141         [ #  # ]:          0 :     spdlog::debug("unstructured::calculateEstimationBetweenSteps>> Could not create estimation polygon ({})."
     142                 :            :                   "left {}, right {}",
     143                 :            :                   debugNamespace,
     144         [ #  # ]:          0 :                   std::to_string(hullLeft),
     145         [ #  # ]:          0 :                   std::to_string(hullRight));
     146                 :            :   }
     147                 :            : 
     148         [ +  - ]:         60 :   if (result)
     149                 :            :   {
     150   [ +  -  +  -  :         60 :     DEBUG_DRAWING_POLYGON(hull, "yellow", debugNamespace + "_hull_front");
             +  -  +  - ]
     151         [ +  - ]:         60 :     result = combinePolygon(polygon, hull, polygon);
     152         [ -  + ]:         60 :     if (!result)
     153                 :            :     {
     154         [ #  # ]:          0 :       spdlog::warn("unstructured::calculateEstimationBetweenSteps>> Could not combine front estimation polygon ({}) "
     155                 :            :                    "with final polygon."
     156                 :            :                    "polygon {}, hull {}",
     157                 :            :                    debugNamespace,
     158         [ #  # ]:          0 :                    std::to_string(polygon),
     159         [ #  # ]:          0 :                    std::to_string(hull));
     160                 :            :     }
     161                 :            :   }
     162                 :         60 :   return result;
     163                 :            : }
     164                 :            : 
     165                 :         40 : bool calculateFrontAndSidePolygon(situation::VehicleState const &vehicleState,
     166                 :            :                                   TrajectorySetStepVehicleLocation const &initialStepVehicleLocation,
     167                 :            :                                   std::vector<TrajectorySetStep> const &sideSteps,
     168                 :            :                                   TrajectorySetStep const &front,
     169                 :            :                                   std::string const &debugNamespace,
     170                 :            :                                   Polygon &resultPolygon,
     171                 :            :                                   TrajectorySetStepVehicleLocation &frontSideStepVehicleLocation)
     172                 :            : {
     173                 :         40 :   auto result = true;
     174                 :         40 :   auto previousStepVehicleLocation = initialStepVehicleLocation;
     175                 :            :   //-------------
     176                 :            :   // sides
     177                 :            :   //-------------
     178                 :         40 :   auto idx = 0;
     179   [ +  +  +  -  :        100 :   for (auto sideStepIt = sideSteps.cbegin(); (sideStepIt != sideSteps.cend()) && result; ++sideStepIt)
                   +  + ]
     180                 :            :   {
     181         [ +  - ]:         60 :     Polygon stepPolygon;
     182                 :         60 :     TrajectorySetStepVehicleLocation currentStepVehicleLocation;
     183         [ +  - ]:         60 :     result = calculateStepPolygon(
     184   [ +  -  +  -  :        120 :       vehicleState, *sideStepIt, debugNamespace + "_" + std::to_string(idx), stepPolygon, currentStepVehicleLocation);
                   +  - ]
     185         [ -  + ]:         60 :     if (!result)
     186                 :            :     {
     187         [ #  # ]:          0 :       spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate step polygon");
     188                 :            :     }
     189         [ +  - ]:         60 :     if (result)
     190                 :            :     {
     191         [ +  - ]:         60 :       result = calculateEstimationBetweenSteps(resultPolygon,
     192                 :            :                                                previousStepVehicleLocation,
     193                 :            :                                                currentStepVehicleLocation,
     194   [ +  -  +  -  :        120 :                                                debugNamespace + "_step_estimation_" + std::to_string(idx));
                   +  - ]
     195         [ -  + ]:         60 :       if (!result)
     196                 :            :       {
     197         [ #  # ]:          0 :         spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate between steps");
     198                 :            :       }
     199                 :         60 :       previousStepVehicleLocation = currentStepVehicleLocation;
     200                 :            :     }
     201                 :            : 
     202         [ +  - ]:         60 :     if (result)
     203                 :            :     {
     204         [ +  - ]:         60 :       result = combinePolygon(resultPolygon, stepPolygon, resultPolygon);
     205                 :            :     }
     206                 :         60 :     idx++;
     207                 :            :   }
     208                 :            : 
     209                 :            :   //-------------
     210                 :            :   // front
     211                 :            :   //-------------
     212         [ +  - ]:         40 :   Polygon frontPolygon;
     213         [ +  - ]:         40 :   if (result)
     214                 :            :   {
     215         [ +  - ]:         40 :     result = calculateStepPolygon(
     216         [ +  - ]:         80 :       vehicleState, front, debugNamespace + "_front", frontPolygon, frontSideStepVehicleLocation);
     217                 :            :   }
     218         [ +  - ]:         40 :   if (result)
     219                 :            :   {
     220         [ +  - ]:         40 :     result = calculateEstimationBetweenSteps(
     221         [ +  - ]:         80 :       resultPolygon, previousStepVehicleLocation, frontSideStepVehicleLocation, debugNamespace + "_front_estimation");
     222         [ -  + ]:         40 :     if (!result)
     223                 :            :     {
     224         [ #  # ]:          0 :       spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate between last step and "
     225                 :            :                     "front polygon.");
     226                 :            :     }
     227                 :            :   }
     228         [ +  - ]:         40 :   if (result)
     229                 :            :   {
     230         [ +  - ]:         40 :     result = combinePolygon(resultPolygon, frontPolygon, resultPolygon);
     231                 :            :   }
     232                 :         80 :   return result;
     233                 :            : }
     234                 :            : 
     235                 :            : } // namespace unstructured
     236                 :            : } // namespace rss
     237                 :            : } // namespace ad

Generated by: LCOV version 1.14