File tree 1 file changed +3
-11
lines changed
map/map_height_fitter/src
1 file changed +3
-11
lines changed Original file line number Diff line number Diff line change @@ -200,20 +200,12 @@ double MapHeightFitter::Impl::get_ground_height(const Point & point) const
200
200
}
201
201
}
202
202
} else if (fit_target_ == " vector_map" ) {
203
- lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer (vector_map_);
204
-
205
- geometry_msgs::msg::Pose pose;
206
- pose.position .x = x;
207
- pose.position .y = y;
208
- pose.position .z = 0.0 ;
209
- lanelet::ConstLanelet closest_lanelet;
210
- const bool result =
211
- lanelet::utils::query::getClosestLanelet (all_lanelets, pose, &closest_lanelet);
212
- if (!result) {
203
+ const auto closest_points = vector_map_->pointLayer .nearest (lanelet::BasicPoint2d{x, y}, 1 );
204
+ if (closest_points.empty ()) {
213
205
RCLCPP_WARN_STREAM (logger, " failed to get closest lanelet" );
214
206
return point.z ;
215
207
}
216
- height = closest_lanelet. centerline (). back ().z ();
208
+ height = closest_points. front ().z ();
217
209
}
218
210
219
211
return std::isfinite (height) ? height : point.z ;
You can’t perform that action at this time.
0 commit comments