From 32f5cf94f6bf47c2623766b66e44dc071d335228 Mon Sep 17 00:00:00 2001 From: TaikiYamada4 Date: Wed, 27 Nov 2024 10:47:35 +0900 Subject: [PATCH] Rearrange code structure Signed-off-by: TaikiYamada4 --- .../traffic_light/traffic_light_facing.hpp | 30 +++++++++---------- .../traffic_light/traffic_light_facing.cpp | 4 +-- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/validators/traffic_light/traffic_light_facing.hpp b/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/validators/traffic_light/traffic_light_facing.hpp index eeea3d8c..458df226 100644 --- a/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/validators/traffic_light/traffic_light_facing.hpp +++ b/map/autoware_lanelet2_map_validator/src/include/lanelet2_map_validator/validators/traffic_light/traffic_light_facing.hpp @@ -55,19 +55,6 @@ class TrafficLightFacingValidator : public lanelet::validation::MapValidator lanelet::ConstLineString3d get_stop_line_from_reg_elem( const lanelet::RegulatoryElementConstPtr & reg_elem); - /** - * @brief Returns a linestring that connects both ends of the left and right bounds - * of the input lanelet. There might be two candidates (front and back) but this function - * only returns the one near to the input 'reference'. This is used to get a directional - * stop line from the lanelet here. - * - * @param lanelet - * @param reference - * @return lanelet::LineString3d - */ - lanelet::LineString3d get_starting_edge_from_lanelet( - const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineString3d & reference); - /** * @brief Returns lanelets that refers the regulatory element specified by the input id * from the input lanelet map. @@ -80,13 +67,26 @@ class TrafficLightFacingValidator : public lanelet::validation::MapValidator const lanelet::LaneletMap & map, const lanelet::Id reg_elem_id); /** - * @brief Convert lanelet::ConstLineString3d to a vector3d. The linestring must be made from two - * points. + * @brief Convert lanelet::ConstLineString3d to a Eigen::Vector3d. + * The linestring must be made only from two points. * * @param linestring * @return Eigen::Vector3d */ Eigen::Vector3d linestring_to_vector3d(const lanelet::ConstLineString3d linestring); + + /** + * @brief Returns a linestring that connects both ends of the left and right bounds + * of the input lanelet. There might be two candidates (front and back) but this function + * only returns the one near to the input 'reference'. This is used to get a directional + * stop line from the lanelet here. + * + * @param lanelet + * @param reference + * @return lanelet::LineString3d + */ + lanelet::LineString3d get_starting_edge_from_lanelet( + const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineString3d & reference); }; } // namespace lanelet::autoware::validation diff --git a/map/autoware_lanelet2_map_validator/src/validators/traffic_light/traffic_light_facing.cpp b/map/autoware_lanelet2_map_validator/src/validators/traffic_light/traffic_light_facing.cpp index 41322e30..9984e118 100644 --- a/map/autoware_lanelet2_map_validator/src/validators/traffic_light/traffic_light_facing.cpp +++ b/map/autoware_lanelet2_map_validator/src/validators/traffic_light/traffic_light_facing.cpp @@ -82,8 +82,8 @@ lanelet::validation::Issues TrafficLightFacingValidator::check_traffic_light_fac continue; } - // Assume the psuedo stop line from the first lanelet and check it is similar to the ones - // of other lanelets + // Estimate the psuedo stop line from the first lanelet and check whether it is similar to + // those of other lanelets lanelet::ConstLineString3d temp_pseudo_stop_line = get_starting_edge_from_lanelet(referring_lanelets[0], stop_line);