perf(out_of_lane): downsample the trajectory to improve performance (a…

…utowarefoundation#7691)

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 downsampleTrajectory(
+TrajectoryPoints downsample_trajectory(
   const TrajectoryPoints & trajectory, const size_t start_idx, const int factor);
 
 /// @brief recalculate the steering angle of the trajectory
@@ -123,16 +123,6 @@ 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,6 +23,7 @@
 
 #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_vehicle_info_utils/vehicle_info_utils.hpp>
@@ -154,7 +155,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 = obstacle_velocity_limiter::downsampleTrajectory(
+  auto downsampled_traj_points = downsample_trajectory(
     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,17 +55,6 @@ 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 t = 0.0;
@@ -84,16 +73,4 @@ 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,31 +37,11 @@ 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,6 +25,7 @@
 
 #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>
@@ -155,9 +156,11 @@ VelocityPlanningResult OutOfLaneModule::plan(
   stopwatch.tic();
   out_of_lane::EgoData ego_data;
   ego_data.pose = planner_data->current_odometry.pose.pose;
-  ego_data.trajectory_points = ego_trajectory_points;
-  ego_data.first_trajectory_idx =
+  const auto start_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_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp

@@ -0,0 +1,46 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_
+#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_
+
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
+
+#include <vector>
+
+namespace autoware::motion_velocity_planner
+{
+/// @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
+std::vector<autoware_planning_msgs::msg::TrajectoryPoint> downsample_trajectory(
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const size_t start_idx, const size_t end_idx, const int factor)
+{
+  if (factor < 1) {
+    return trajectory;
+  }
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> downsampled_traj;
+  downsampled_traj.reserve((end_idx - start_idx) / factor + 1);
+  for (size_t i = start_idx; i <= end_idx; i += factor) {
+    downsampled_traj.push_back(trajectory[i]);
+  }
+  return downsampled_traj;
+}
+}  // namespace autoware::motion_velocity_planner
+
+#endif  // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_