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