From 3eb577edcf9b15715dffeb95fe2b20d65d86ab5a Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 1 Jul 2024 09:57:19 +0900 Subject: [PATCH] perf(motion_velocity_planner): resample trajectory after vel smoothing (#7732) * perf(dynamic_obstacle_stop): create rtree with packing algorithm Signed-off-by: Maxime CLEMENT * Revert "perf(out_of_lane): downsample the trajectory to improve performance (#7691)" This reverts commit 8444a9eb29b32f500be3724dd5662013b9b81060. * perf(motion_velocity_planner): resample trajectory after vel smoothing Signed-off-by: Maxime CLEMENT --------- Signed-off-by: Maxime CLEMENT --- .../autoware_motion_velocity_planner_node/src/node.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 714b6fd948a32..d10672dbe6f90 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -14,6 +14,7 @@ #include "node.hpp" +#include #include #include #include @@ -365,9 +366,11 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; if (smooth_velocity_before_planning_) input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); + const auto resampled_trajectory = + autoware::motion_utils::resampleTrajectory(output_trajectory_msg, 0.5); const auto planning_results = planner_manager_.plan_velocities( - input_trajectory_points, std::make_shared(planner_data_)); + resampled_trajectory.points, std::make_shared(planner_data_)); autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors; velocity_factors.header.frame_id = "map";