From f2115654830b68d4ab42225c72242b12cbb13b27 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Thu, 27 Jun 2024 22:16:12 +0900 Subject: [PATCH] perf(dynamic_obstacle_stop): create rtree with packing algorithm Signed-off-by: Maxime CLEMENT --- .../src/footprint.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index 9695146a2ad38..73189e732b312 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -74,11 +74,14 @@ void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) for (const auto & p : ego_data.trajectory) ego_data.trajectory_footprints.push_back(autoware::universe_utils::toFootprint( p.pose, params.ego_longitudinal_offset, 0.0, params.ego_lateral_offset * 2.0)); + std::vector rtree_nodes; + rtree_nodes.reserve(ego_data.trajectory_footprints.size()); for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { const auto box = boost::geometry::return_envelope( ego_data.trajectory_footprints[i]); - ego_data.rtree.insert(std::make_pair(box, i)); + rtree_nodes.push_back(std::make_pair(box, i)); } + ego_data.rtree = Rtree(rtree_nodes); } } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop