From b5d83826ebee4394782cf3b3d7cbda4300a9a51a Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 15 May 2024 15:16:30 +0900 Subject: [PATCH 1/2] perf(map_height_fitter): more efficient query to find closest lanelet Signed-off-by: Maxime CLEMENT --- map/map_height_fitter/src/map_height_fitter.cpp | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 095574125d9a0..a94a7119d1b98 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -200,20 +200,12 @@ double MapHeightFitter::Impl::get_ground_height(const Point & point) const } } } else if (fit_target_ == "vector_map") { - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(vector_map_); - - geometry_msgs::msg::Pose pose; - pose.position.x = x; - pose.position.y = y; - pose.position.z = 0.0; - lanelet::ConstLanelet closest_lanelet; - const bool result = - lanelet::utils::query::getClosestLanelet(all_lanelets, pose, &closest_lanelet); - if (!result) { + const auto closest_lanelets = vector_map_->laneletLayer.nearest(lanelet::BasicPoint2d{x, y}, 1); + if (closest_lanelets.empty()) { RCLCPP_WARN_STREAM(logger, "failed to get closest lanelet"); return point.z; } - height = closest_lanelet.centerline().back().z(); + height = closest_lanelets.front().centerline().back().z(); } return std::isfinite(height) ? height : point.z; From 957ac7f1949a8d8c32b84391f7d4c968a7a6ae90 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Wed, 15 May 2024 21:49:36 +0900 Subject: [PATCH 2/2] switch to query points instead of lanelets Signed-off-by: Maxime CLEMENT --- map/map_height_fitter/src/map_height_fitter.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index a94a7119d1b98..0c99d33772aea 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -200,12 +200,12 @@ double MapHeightFitter::Impl::get_ground_height(const Point & point) const } } } else if (fit_target_ == "vector_map") { - const auto closest_lanelets = vector_map_->laneletLayer.nearest(lanelet::BasicPoint2d{x, y}, 1); - if (closest_lanelets.empty()) { + const auto closest_points = vector_map_->pointLayer.nearest(lanelet::BasicPoint2d{x, y}, 1); + if (closest_points.empty()) { RCLCPP_WARN_STREAM(logger, "failed to get closest lanelet"); return point.z; } - height = closest_lanelets.front().centerline().back().z(); + height = closest_points.front().z(); } return std::isfinite(height) ? height : point.z;