diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp index af63b50bceb39..aeeaabeb9b510 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp +++ b/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 calculate_slowd const std::vector & projections, const std::vector & 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_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index efbdcdaf464e2..eb8a33ede2c72 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -23,7 +23,6 @@ #include #include -#include #include #include #include @@ -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"); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp index 73e05bed88c37..f4b1304d67e5e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.cpp +++ b/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 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp index e098ee6abe822..cb9fe40e64907 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/trajectory_preprocessing.hpp +++ b/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_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 8facb77932aa5..8397d0c116090 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -25,7 +25,6 @@ #include #include -#include #include #include #include @@ -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"); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp deleted file mode 100644 index 95728c4738c5b..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/trajectory_preprocessing.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// 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 - -#include - -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 downsample_trajectory( - const std::vector & trajectory, - const size_t start_idx, const size_t end_idx, const int factor) -{ - if (factor < 1) { - return trajectory; - } - std::vector 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_