|
17 | 17 | #include <pcl/filters/voxel_grid.h>
|
18 | 18 |
|
19 | 19 | #include <algorithm>
|
| 20 | +#include <limits> |
20 | 21 | #include <string>
|
21 | 22 |
|
22 | 23 | namespace behavior_velocity_planner
|
@@ -303,6 +304,40 @@ void calculateMinAndMaxVelFromCovariance(
|
303 | 304 | dynamic_obstacle.max_velocity_mps = max_velocity;
|
304 | 305 | }
|
305 | 306 |
|
| 307 | +double convertDurationToDouble(const builtin_interfaces::msg::Duration & duration) |
| 308 | +{ |
| 309 | + return duration.sec + duration.nanosec / 1e9; |
| 310 | +} |
| 311 | + |
| 312 | +// Create a path leading up to a specified prediction time |
| 313 | +std::vector<geometry_msgs::msg::Pose> createPathToPredictionTime( |
| 314 | + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, double prediction_time) |
| 315 | +{ |
| 316 | + // Calculate the number of poses to include based on the prediction time and the time step between |
| 317 | + // poses |
| 318 | + const double time_step_seconds = convertDurationToDouble(predicted_path.time_step); |
| 319 | + if (time_step_seconds < std::numeric_limits<double>::epsilon()) { |
| 320 | + // Handle the case where time_step_seconds is zero or too close to zero |
| 321 | + RCLCPP_WARN_STREAM( |
| 322 | + rclcpp::get_logger("run_out: createPathToPredictionTime"), |
| 323 | + "time_step of the path is too close to zero. Use the input path"); |
| 324 | + const std::vector<geometry_msgs::msg::Pose> input_path( |
| 325 | + predicted_path.path.begin(), predicted_path.path.end()); |
| 326 | + return input_path; |
| 327 | + } |
| 328 | + const size_t poses_to_include = |
| 329 | + std::min(static_cast<size_t>(prediction_time / time_step_seconds), predicted_path.path.size()); |
| 330 | + |
| 331 | + // Construct the path to the specified prediction time |
| 332 | + std::vector<geometry_msgs::msg::Pose> path_to_prediction_time; |
| 333 | + path_to_prediction_time.reserve(poses_to_include); |
| 334 | + for (size_t i = 0; i < poses_to_include; ++i) { |
| 335 | + path_to_prediction_time.push_back(predicted_path.path[i]); |
| 336 | + } |
| 337 | + |
| 338 | + return path_to_prediction_time; |
| 339 | +} |
| 340 | + |
306 | 341 | } // namespace
|
307 | 342 |
|
308 | 343 | DynamicObstacleCreatorForObject::DynamicObstacleCreatorForObject(
|
@@ -334,8 +369,7 @@ std::vector<DynamicObstacle> DynamicObstacleCreatorForObject::createDynamicObsta
|
334 | 369 | for (const auto & path : predicted_object.kinematics.predicted_paths) {
|
335 | 370 | PredictedPath predicted_path;
|
336 | 371 | predicted_path.confidence = path.confidence;
|
337 |
| - predicted_path.path.resize(path.path.size()); |
338 |
| - std::copy(path.path.cbegin(), path.path.cend(), predicted_path.path.begin()); |
| 372 | + predicted_path.path = createPathToPredictionTime(path, param_.max_prediction_time); |
339 | 373 |
|
340 | 374 | dynamic_obstacle.predicted_paths.emplace_back(predicted_path);
|
341 | 375 | }
|
|
0 commit comments