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]>

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp

@@ -50,7 +50,7 @@ size_t calculateStartIndex(
 /// @param[in] start_idx starting index of the input trajectory
 /// @param[in] factor factor used for downsampling
 /// @return downsampled trajectory
-TrajectoryPoints downsample_trajectory(
+TrajectoryPoints downsampleTrajectory(
   const TrajectoryPoints & trajectory, const size_t start_idx, const int factor);
 
 /// @brief recalculate the steering angle of the trajectory
@@ -123,6 +123,16 @@ std::vector<autoware::motion_velocity_planner::SlowdownInterval> calculate_slowd
   const std::vector<multi_linestring_t> & projections, const std::vector<polygon_t> & footprints,
   ProjectionParameters & projection_params, const VelocityParameters & velocity_params,
   autoware::motion_utils::VirtualWalls & virtual_walls);
+
+/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory
+/// @param[in] downsampled_trajectory downsampled trajectory
+/// @param[in] trajectory input trajectory
+/// @param[in] start_idx starting index of the downsampled trajectory relative to the input
+/// @param[in] factor downsampling factor
+/// @return input trajectory with the velocity profile of the downsampled trajectory
+TrajectoryPoints copyDownsampledVelocity(
+  const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx,
+  const int factor);
 }  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 
 #endif  // OBSTACLE_VELOCITY_LIMITER_HPP_

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp

@@ -23,7 +23,6 @@
 
 #include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
-#include <autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp>
 #include <autoware/universe_utils/ros/update_param.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>
@@ -160,7 +159,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan(
   const auto end_idx = obstacle_velocity_limiter::calculateEndIndex(
     original_traj_points, start_idx, preprocessing_params_.max_length,
     preprocessing_params_.max_duration);
-  auto downsampled_traj_points = downsample_trajectory(
+  auto downsampled_traj_points = obstacle_velocity_limiter::downsampleTrajectory(
     original_traj_points, start_idx, end_idx, preprocessing_params_.downsample_factor);
   obstacle_velocity_limiter::ObstacleMasks obstacle_masks;
   const auto preprocessing_us = stopwatch.toc("preprocessing");

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp

@@ -55,6 +55,17 @@ size_t calculateEndIndex(
   return idx;
 }
 
+TrajectoryPoints downsampleTrajectory(
+  const TrajectoryPoints & trajectory, const size_t start_idx, const size_t end_idx,
+  const int factor)
+{
+  if (factor < 1) return trajectory;
+  TrajectoryPoints downsampled_traj;
+  downsampled_traj.reserve((end_idx - start_idx) / factor);
+  for (size_t i = start_idx; i <= end_idx; i += factor) downsampled_traj.push_back(trajectory[i]);
+  return downsampled_traj;
+}
+
 void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_base)
 {
   auto prev_point = trajectory.front();
@@ -71,4 +82,16 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b
       std::atan2(wheel_base * d_heading, point.longitudinal_velocity_mps * dt);
   }
 }
+
+TrajectoryPoints copyDownsampledVelocity(
+  const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx,
+  const int factor)
+{
+  const auto size = std::min(downsampled_traj.size(), trajectory.size());
+  for (size_t i = 0; i < size; ++i) {
+    trajectory[start_idx + i * factor].longitudinal_velocity_mps =
+      downsampled_traj[i].longitudinal_velocity_mps;
+  }
+  return trajectory;
+}
 }  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp

@@ -37,11 +37,31 @@ size_t calculateEndIndex(
   const TrajectoryPoints & trajectory, const size_t start_idx, const double max_length,
   const double max_duration);
 
+/// @brief downsample a trajectory, reducing its number of points by the given factor
+/// @param[in] trajectory input trajectory
+/// @param[in] start_idx starting index of the input trajectory
+/// @param[in] end_idx ending index of the input trajectory
+/// @param[in] factor factor used for downsampling
+/// @return downsampled trajectory
+TrajectoryPoints downsampleTrajectory(
+  const TrajectoryPoints & trajectory, const size_t start_idx, const size_t end_idx,
+  const int factor);
+
 /// @brief recalculate the steering angle of the trajectory
 /// @details uses the change in headings for calculation
 /// @param[inout] trajectory input trajectory
 /// @param[in] wheel_base wheel base of the vehicle
 void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_base);
+
+/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory
+/// @param[in] downsampled_trajectory downsampled trajectory
+/// @param[in] trajectory input trajectory
+/// @param[in] start_idx starting index of the downsampled trajectory relative to the input
+/// @param[in] factor downsampling factor
+/// @return input trajectory with the velocity profile of the downsampled trajectory
+TrajectoryPoints copyDownsampledVelocity(
+  const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx,
+  const int factor);
 }  // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 
 #endif  // TRAJECTORY_PREPROCESSING_HPP_

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -25,7 +25,6 @@
 
 #include <autoware/motion_utils/trajectory/interpolation.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
-#include <autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp>
 #include <autoware/universe_utils/ros/parameter.hpp>
 #include <autoware/universe_utils/ros/update_param.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
@@ -161,11 +160,9 @@ VelocityPlanningResult OutOfLaneModule::plan(
   stopwatch.tic();
   out_of_lane::EgoData ego_data;
   ego_data.pose = planner_data->current_odometry.pose.pose;
-  const auto start_idx =
+  ego_data.trajectory_points = ego_trajectory_points;
+  ego_data.first_trajectory_idx =
     autoware::motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position);
-  ego_data.trajectory_points =
-    downsample_trajectory(ego_trajectory_points, start_idx, ego_trajectory_points.size(), 10);
-  ego_data.first_trajectory_idx = 0;
   ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x;
   ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel();
   stopwatch.tic("calculate_trajectory_footprints");

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>
@@ -380,9 +381,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";