LCOV - code coverage report
Current view: top level - src/perception/filters/ray_ground_classifier/src - ray_ground_classifier_types.cpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 30 31 96.8 %
Date: 2023-03-03 05:44:19 Functions: 6 6 100.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 26 34 76.5 %

           Branch data     Line data    Source code
       1                 :            : // Copyright 2017-2020 the Autoware Foundation, Arm Limited
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : //
      15                 :            : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
      16                 :            : 
      17                 :            : #include <cmath>
      18                 :            : #include <cstdint>
      19                 :            : #include <stdexcept>
      20                 :            : 
      21                 :            : #include "common/types.hpp"
      22                 :            : #include "ray_ground_classifier/ray_ground_point_classifier.hpp"
      23                 :            : 
      24                 :            : namespace autoware
      25                 :            : {
      26                 :            : namespace perception
      27                 :            : {
      28                 :            : namespace filters
      29                 :            : {
      30                 :            : namespace ray_ground_classifier
      31                 :            : {
      32                 :            : 
      33                 :            : using autoware::common::types::PointXYZIF;
      34                 :            : using autoware::common::types::float32_t;
      35                 :            : 
      36                 :         44 : Config::Config(
      37                 :            :   const float32_t sensor_height_m,
      38                 :            :   const float32_t max_local_slope_deg,
      39                 :            :   const float32_t max_global_slope_deg,
      40                 :            :   const float32_t nonground_retro_thresh_deg,
      41                 :            :   const float32_t min_height_thresh_m,
      42                 :            :   const float32_t max_global_height_thresh_m,
      43                 :            :   const float32_t max_last_local_ground_thresh_m,
      44                 :         44 :   const float32_t max_provisional_ground_distance_m)
      45                 :         44 : : m_ground_z_m(-sensor_height_m),
      46                 :         44 :   m_max_local_slope(tanf(static_cast<float32_t>(deg2rad(max_local_slope_deg)))),
      47                 :         44 :   m_max_global_slope(tanf(static_cast<float32_t>(deg2rad(max_global_slope_deg)))),
      48                 :         44 :   m_nonground_retro_thresh(tanf(static_cast<float32_t>(deg2rad(nonground_retro_thresh_deg)))),
      49                 :            :   m_min_height_thresh_m(min_height_thresh_m),
      50                 :            :   m_max_global_height_thresh_m(max_global_height_thresh_m),
      51                 :            :   m_max_last_local_ground_thresh_m(max_last_local_ground_thresh_m),
      52         [ +  + ]:         44 :   m_max_provisional_ground_distance_m(max_provisional_ground_distance_m)
      53                 :            : {
      54                 :            :   // TODO(c.ho) nan check
      55   [ +  +  +  + ]:         44 :   if ((max_local_slope_deg <= 0.0F) ||
      56         [ +  + ]:         42 :     (max_global_slope_deg <= 0.0F) ||
      57                 :            :     (nonground_retro_thresh_deg <= 0.0F))
      58                 :            :   {
      59         [ +  - ]:          3 :     throw std::runtime_error("ray ground classifier: config angles must be positive");
      60                 :            :   }
      61   [ +  +  +  + ]:         41 :   if ((max_local_slope_deg > 90.0F) ||
      62         [ +  + ]:         39 :     (max_global_slope_deg > 90.0F) ||
      63                 :            :     (nonground_retro_thresh_deg > 90.0F))
      64                 :            :   {
      65         [ +  - ]:          4 :     throw std::runtime_error("ray ground classifier: config angles must be < 90");
      66                 :            :   }
      67                 :            : 
      68   [ +  -  +  + ]:         37 :   if ((m_min_height_thresh_m <= 0.0F) ||
      69                 :            :     (m_max_global_height_thresh_m <= 0.0F))
      70                 :            :   {
      71         [ +  - ]:          1 :     throw std::runtime_error("ray ground classifier: config distances must be positive");
      72                 :            :   }
      73   [ +  +  +  + ]:         36 :   if ((max_local_slope_deg >= nonground_retro_thresh_deg) ||
      74                 :            :     (max_global_slope_deg >= nonground_retro_thresh_deg))
      75                 :            :   {
      76                 :            :     throw std::runtime_error(
      77                 :            :             "ray ground classifier: retro nonground classification must "
      78         [ +  - ]:          3 :             "be greater than other angle thresholds");
      79                 :            :   }
      80         [ -  + ]:         33 :   if ((m_max_last_local_ground_thresh_m < m_max_global_height_thresh_m)) {
      81                 :            :     throw std::runtime_error(
      82                 :            :             "ray ground classifier: max local last ground thresh must "
      83         [ #  # ]:          0 :             "be greater than max_global_height_m");
      84                 :            :   }
      85                 :         33 : }
      86                 :            : ////////////////////////////////////////////////////////////////////////////////
      87                 :        489 : float32_t Config::get_sensor_height() const
      88                 :            : {
      89                 :        489 :   return -m_ground_z_m;
      90                 :            : }
      91                 :            : ////////////////////////////////////////////////////////////////////////////////
      92                 :     360156 : PointXYZIFR::PointXYZIFR(const PointXYZIF * pt)
      93                 :            : : m_point(pt),
      94                 :     360156 :   m_r_xy(sqrtf((pt->x * pt->x) + (pt->y * pt->y)))
      95                 :            : {
      96                 :     360156 : }
      97                 :            : ////////////////////////////////////////////////////////////////////////////////
      98                 :     359459 : float32_t PointXYZIFR::get_r() const
      99                 :            : {
     100                 :     359459 :   return m_r_xy;
     101                 :            : }
     102                 :            : ////////////////////////////////////////////////////////////////////////////////
     103                 :     359459 : float32_t PointXYZIFR::get_z() const
     104                 :            : {
     105                 :     359459 :   return m_point->z;
     106                 :            : }
     107                 :            : ////////////////////////////////////////////////////////////////////////////////
     108                 :     361487 : const PointXYZIF * PointXYZIFR::get_point_pointer() const
     109                 :            : {
     110                 :     361487 :   return m_point;
     111                 :            : }
     112                 :            : 
     113                 :            : }  // namespace ray_ground_classifier
     114                 :            : }  // namespace filters
     115                 :            : }  // namespace perception
     116                 :            : }  // namespace autoware

Generated by: LCOV version 1.14