From 46bd47f9974b291c854d5cec33d0ecebcffd2b5e Mon Sep 17 00:00:00 2001
From: Tomohito ANDO <tomohito.ando@tier4.jp>
Date: Tue, 19 Mar 2024 16:45:10 +0900
Subject: [PATCH] =?UTF-8?q?fix(behavior=5Fvelocity=5Frun=5Fout):=20constru?=
 =?UTF-8?q?ct=20predicted=20path=20up=20to=20max=20pr=E2=80=A6=20(#6650)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* fix(behavior_velocity_run_out): construct predicted path up to max prediction time in object method

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* handle division by zero

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

* add missing include

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>

---------

Signed-off-by: Tomohito Ando <tomohito.ando@tier4.jp>
---
 .../src/dynamic_obstacle.cpp                  | 38 ++++++++++++++++++-
 1 file changed, 36 insertions(+), 2 deletions(-)

diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp
index a1b53d6adc966..5a96e6be4d7c3 100644
--- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp
+++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp
@@ -17,6 +17,7 @@
 #include <pcl/filters/voxel_grid.h>
 
 #include <algorithm>
+#include <limits>
 #include <string>
 
 namespace behavior_velocity_planner
@@ -303,6 +304,40 @@ void calculateMinAndMaxVelFromCovariance(
   dynamic_obstacle.max_velocity_mps = max_velocity;
 }
 
+double convertDurationToDouble(const builtin_interfaces::msg::Duration & duration)
+{
+  return duration.sec + duration.nanosec / 1e9;
+}
+
+// Create a path leading up to a specified prediction time
+std::vector<geometry_msgs::msg::Pose> createPathToPredictionTime(
+  const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, double prediction_time)
+{
+  // Calculate the number of poses to include based on the prediction time and the time step between
+  // poses
+  const double time_step_seconds = convertDurationToDouble(predicted_path.time_step);
+  if (time_step_seconds < std::numeric_limits<double>::epsilon()) {
+    // Handle the case where time_step_seconds is zero or too close to zero
+    RCLCPP_WARN_STREAM(
+      rclcpp::get_logger("run_out: createPathToPredictionTime"),
+      "time_step of the path is too close to zero. Use the input path");
+    const std::vector<geometry_msgs::msg::Pose> input_path(
+      predicted_path.path.begin(), predicted_path.path.end());
+    return input_path;
+  }
+  const size_t poses_to_include =
+    std::min(static_cast<size_t>(prediction_time / time_step_seconds), predicted_path.path.size());
+
+  // Construct the path to the specified prediction time
+  std::vector<geometry_msgs::msg::Pose> path_to_prediction_time;
+  path_to_prediction_time.reserve(poses_to_include);
+  for (size_t i = 0; i < poses_to_include; ++i) {
+    path_to_prediction_time.push_back(predicted_path.path[i]);
+  }
+
+  return path_to_prediction_time;
+}
+
 }  // namespace
 
 DynamicObstacleCreatorForObject::DynamicObstacleCreatorForObject(
@@ -334,8 +369,7 @@ std::vector<DynamicObstacle> DynamicObstacleCreatorForObject::createDynamicObsta
     for (const auto & path : predicted_object.kinematics.predicted_paths) {
       PredictedPath predicted_path;
       predicted_path.confidence = path.confidence;
-      predicted_path.path.resize(path.path.size());
-      std::copy(path.path.cbegin(), path.path.cend(), predicted_path.path.begin());
+      predicted_path.path = createPathToPredictionTime(path, param_.max_prediction_time);
 
       dynamic_obstacle.predicted_paths.emplace_back(predicted_path);
     }