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 <[email protected]> * 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 <[email protected]> --------- Signed-off-by: Maxime CLEMENT <[email protected]>
tier4 · Jul 1, 2024 · 3eb577e · 3eb577e
1 parent 5a424ba
commit 3eb577e
Showing 1 changed file with 4 additions and 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
@@ -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";