perf(motion_velocity_planner): resample trajectory after vel smoothing (autowarefoundation#7732)

* perf(dynamic_obstacle_stop): create rtree with packing algorithm

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Revert "perf(out_of_lane): downsample the trajectory to improve performance (autowarefoundation#7691)"

This reverts commit 8444a9e.

* perf(motion_velocity_planner): resample trajectory after vel smoothing

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

---------

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp

@@ -14,6 +14,7 @@
 
 #include "node.hpp"
 
+#include <autoware/motion_utils/resample/resample.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/ros/update_param.hpp>
 #include <autoware/universe_utils/ros/wait_for_param.hpp>
@@ -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<const PlannerData>(planner_data_));
+    resampled_trajectory.points, std::make_shared<const PlannerData>(planner_data_));
 
   autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors;
   velocity_factors.header.frame_id = "map";