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 aeeaabeb9b510..af63b50bceb39 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 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 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 2b78a2f1495bf..0c3d9070541ed 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,6 +23,7 @@ #include #include +#include #include #include #include @@ -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"); 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 7498568be99fb..dc7f3c6f098df 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,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 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 cb9fe40e64907..e098ee6abe822 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,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_ 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 f6d81f6f036d0..ecb04c3cc2a6f 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,6 +25,7 @@ #include #include +#include #include #include #include @@ -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"); 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 new file mode 100644 index 0000000000000..95728c4738c5b --- /dev/null +++ b/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 + +#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_