Skip to content

Commit 637be55

Browse files
maxime-clemmaxime-clem
committed
perf(out_of_lane): downsample the trajectory to improve performance (autowarefoundation#7691)
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
1 parent 970c6f3 commit 637be55

File tree

6 files changed

+54
-57
lines changed

6 files changed

+54
-57
lines changed

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp

+1-11
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@ size_t calculateStartIndex(
5050
/// @param[in] start_idx starting index of the input trajectory
5151
/// @param[in] factor factor used for downsampling
5252
/// @return downsampled trajectory
53-
TrajectoryPoints downsampleTrajectory(
53+
TrajectoryPoints downsample_trajectory(
5454
const TrajectoryPoints & trajectory, const size_t start_idx, const int factor);
5555

5656
/// @brief recalculate the steering angle of the trajectory
@@ -123,16 +123,6 @@ std::vector<autoware::motion_velocity_planner::SlowdownInterval> calculate_slowd
123123
const std::vector<multi_linestring_t> & projections, const std::vector<polygon_t> & footprints,
124124
ProjectionParameters & projection_params, const VelocityParameters & velocity_params,
125125
autoware::motion_utils::VirtualWalls & virtual_walls);
126-
127-
/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory
128-
/// @param[in] downsampled_trajectory downsampled trajectory
129-
/// @param[in] trajectory input trajectory
130-
/// @param[in] start_idx starting index of the downsampled trajectory relative to the input
131-
/// @param[in] factor downsampling factor
132-
/// @return input trajectory with the velocity profile of the downsampled trajectory
133-
TrajectoryPoints copyDownsampledVelocity(
134-
const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx,
135-
const int factor);
136126
} // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
137127

138128
#endif // OBSTACLE_VELOCITY_LIMITER_HPP_

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323

2424
#include <autoware/motion_utils/marker/virtual_wall_marker_creator.hpp>
2525
#include <autoware/motion_utils/trajectory/trajectory.hpp>
26+
#include <autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp>
2627
#include <autoware/universe_utils/ros/update_param.hpp>
2728
#include <autoware/universe_utils/system/stop_watch.hpp>
2829
#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
@@ -154,7 +155,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan(
154155
const auto end_idx = obstacle_velocity_limiter::calculateEndIndex(
155156
original_traj_points, start_idx, preprocessing_params_.max_length,
156157
preprocessing_params_.max_duration);
157-
auto downsampled_traj_points = obstacle_velocity_limiter::downsampleTrajectory(
158+
auto downsampled_traj_points = downsample_trajectory(
158159
original_traj_points, start_idx, end_idx, preprocessing_params_.downsample_factor);
159160
obstacle_velocity_limiter::ObstacleMasks obstacle_masks;
160161
const auto preprocessing_us = stopwatch.toc("preprocessing");

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp

-23
Original file line numberDiff line numberDiff line change
@@ -55,17 +55,6 @@ size_t calculateEndIndex(
5555
return idx;
5656
}
5757

58-
TrajectoryPoints downsampleTrajectory(
59-
const TrajectoryPoints & trajectory, const size_t start_idx, const size_t end_idx,
60-
const int factor)
61-
{
62-
if (factor < 1) return trajectory;
63-
TrajectoryPoints downsampled_traj;
64-
downsampled_traj.reserve((end_idx - start_idx) / factor);
65-
for (size_t i = start_idx; i <= end_idx; i += factor) downsampled_traj.push_back(trajectory[i]);
66-
return downsampled_traj;
67-
}
68-
6958
void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_base)
7059
{
7160
auto t = 0.0;
@@ -84,16 +73,4 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b
8473
std::atan2(wheel_base * d_heading, point.longitudinal_velocity_mps * dt);
8574
}
8675
}
87-
88-
TrajectoryPoints copyDownsampledVelocity(
89-
const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx,
90-
const int factor)
91-
{
92-
const auto size = std::min(downsampled_traj.size(), trajectory.size());
93-
for (size_t i = 0; i < size; ++i) {
94-
trajectory[start_idx + i * factor].longitudinal_velocity_mps =
95-
downsampled_traj[i].longitudinal_velocity_mps;
96-
}
97-
return trajectory;
98-
}
9976
} // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter

‎planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp

-20
Original file line numberDiff line numberDiff line change
@@ -37,31 +37,11 @@ size_t calculateEndIndex(
3737
const TrajectoryPoints & trajectory, const size_t start_idx, const double max_length,
3838
const double max_duration);
3939

40-
/// @brief downsample a trajectory, reducing its number of points by the given factor
41-
/// @param[in] trajectory input trajectory
42-
/// @param[in] start_idx starting index of the input trajectory
43-
/// @param[in] end_idx ending index of the input trajectory
44-
/// @param[in] factor factor used for downsampling
45-
/// @return downsampled trajectory
46-
TrajectoryPoints downsampleTrajectory(
47-
const TrajectoryPoints & trajectory, const size_t start_idx, const size_t end_idx,
48-
const int factor);
49-
5040
/// @brief recalculate the steering angle of the trajectory
5141
/// @details uses the change in headings for calculation
5242
/// @param[inout] trajectory input trajectory
5343
/// @param[in] wheel_base wheel base of the vehicle
5444
void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_base);
55-
56-
/// @brief copy the velocity profile of a downsampled trajectory to the original trajectory
57-
/// @param[in] downsampled_trajectory downsampled trajectory
58-
/// @param[in] trajectory input trajectory
59-
/// @param[in] start_idx starting index of the downsampled trajectory relative to the input
60-
/// @param[in] factor downsampling factor
61-
/// @return input trajectory with the velocity profile of the downsampled trajectory
62-
TrajectoryPoints copyDownsampledVelocity(
63-
const TrajectoryPoints & downsampled_traj, TrajectoryPoints trajectory, const size_t start_idx,
64-
const int factor);
6545
} // namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
6646

6747
#endif // TRAJECTORY_PREPROCESSING_HPP_

‎planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525

2626
#include <autoware/motion_utils/trajectory/interpolation.hpp>
2727
#include <autoware/motion_utils/trajectory/trajectory.hpp>
28+
#include <autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp>
2829
#include <autoware/universe_utils/ros/parameter.hpp>
2930
#include <autoware/universe_utils/ros/update_param.hpp>
3031
#include <autoware/universe_utils/system/stop_watch.hpp>
@@ -155,9 +156,11 @@ VelocityPlanningResult OutOfLaneModule::plan(
155156
stopwatch.tic();
156157
out_of_lane::EgoData ego_data;
157158
ego_data.pose = planner_data->current_odometry.pose.pose;
158-
ego_data.trajectory_points = ego_trajectory_points;
159-
ego_data.first_trajectory_idx =
159+
const auto start_idx =
160160
autoware::motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position);
161+
ego_data.trajectory_points =
162+
downsample_trajectory(ego_trajectory_points, start_idx, ego_trajectory_points.size(), 10);
163+
ego_data.first_trajectory_idx = 0;
161164
ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x;
162165
ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel();
163166
stopwatch.tic("calculate_trajectory_footprints");

‎planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp

+46
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_
16+
#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_
17+
18+
#include <autoware_planning_msgs/msg/trajectory_point.hpp>
19+
20+
#include <vector>
21+
22+
namespace autoware::motion_velocity_planner
23+
{
24+
/// @brief downsample a trajectory, reducing its number of points by the given factor
25+
/// @param[in] trajectory input trajectory
26+
/// @param[in] start_idx starting index of the input trajectory
27+
/// @param[in] end_idx ending index of the input trajectory
28+
/// @param[in] factor factor used for downsampling
29+
/// @return downsampled trajectory
30+
std::vector<autoware_planning_msgs::msg::TrajectoryPoint> downsample_trajectory(
31+
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
32+
const size_t start_idx, const size_t end_idx, const int factor)
33+
{
34+
if (factor < 1) {
35+
return trajectory;
36+
}
37+
std::vector<autoware_planning_msgs::msg::TrajectoryPoint> downsampled_traj;
38+
downsampled_traj.reserve((end_idx - start_idx) / factor + 1);
39+
for (size_t i = start_idx; i <= end_idx; i += factor) {
40+
downsampled_traj.push_back(trajectory[i]);
41+
}
42+
return downsampled_traj;
43+
}
44+
} // namespace autoware::motion_velocity_planner
45+
46+
#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__TRAJECTORY_PREPROCESSING_HPP_

0 commit comments

Comments
 (0)