From 8b4c72678af56daf0fc74837504a977d5976fd5d Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Fri, 29 Jul 2022 12:04:19 +0900
Subject: [PATCH] feat(behavior_velocity_planner): apply smoother to reference
 trajectory for ttc (#1405)

* feat(behavior_velocity_smoother): simply apply smoother to existing reference trajectory

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* avoid zero division

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* initialize smoothed_path with reference_path and WARN if smoothPath failed

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

* added another version for calcDist2d

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
 .../include/utilization/util.hpp              | 31 +++++++++++++++++
 .../intersection/scene_intersection.cpp       | 34 ++++++++++++++++---
 2 files changed, 60 insertions(+), 5 deletions(-)

diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp
index 2f0c4615e2896..b85680518aa43 100644
--- a/planning/behavior_velocity_planner/include/utilization/util.hpp
+++ b/planning/behavior_velocity_planner/include/utilization/util.hpp
@@ -120,6 +120,37 @@ void insertVelocity(
   size_t & insert_index, const double min_distance = 0.001);
 inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); }
 
+inline double square(const double & a) { return a * a; }
+double normalizeEulerAngle(double euler);
+geometry_msgs::msg::Quaternion getQuaternionFromYaw(double yaw);
+
+template <class T1, class T2>
+double calcSquaredDist2d(const T1 & a, const T2 & b)
+{
+  return square(getPoint(a).x - getPoint(b).x) + square(getPoint(a).y - getPoint(b).y);
+}
+
+template <class T1, class T2>
+double calcDist2d(const T1 & a, const T2 & b)
+{
+  return std::sqrt(calcSquaredDist2d<T1, T2>(a, b));
+}
+
+template <class T>
+double calcDist2d(const T & a, const T & b)
+{
+  return std::sqrt(calcSquaredDist2d<T, T>(a, b));
+}
+
+template <class T>
+bool calcClosestIndex(
+  const T & path, const geometry_msgs::msg::Pose & pose, int & closest, double dist_thr = 3.0,
+  double angle_thr = M_PI_4);
+
+template <class T>
+bool calcClosestIndex(
+  const T & path, const geometry_msgs::msg::Point & point, int & closest, double dist_thr = 3.0);
+
 geometry_msgs::msg::Pose transformRelCoordinate2D(
   const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin);
 geometry_msgs::msg::Pose transformAbsCoordinate2D(
diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
index 41e869f1ec7c2..9825742f20dae 100644
--- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp
@@ -20,6 +20,7 @@
 #include <scene_module/intersection/util.hpp>
 #include <utilization/boost_geometry_helper.hpp>
 #include <utilization/path_utilization.hpp>
+#include <utilization/trajectory_utils.hpp>
 #include <utilization/util.hpp>
 
 #include <lanelet2_core/geometry/Polygon.h>
@@ -455,14 +456,14 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
   const int objective_lane_id) const
 {
-  TimeDistanceArray time_distance_array{};
   double closest_vel =
     (std::max(1e-01, std::fabs(planner_data_->current_velocity->twist.linear.x)));
   double dist_sum = 0.0;
-  double passing_time = 0.0;
-  time_distance_array.emplace_back(passing_time, dist_sum);
   int assigned_lane_found = false;
 
+  PathWithLaneId reference_path;
+  reference_path.points.push_back(path.points.at(closest_idx));
+  reference_path.points.at(0).point.longitudinal_velocity_mps = closest_vel;
   for (size_t i = closest_idx + 1; i < path.points.size(); ++i) {
     const double dist =
       tier4_autoware_utils::calcDistance2d(path.points.at(i - 1), path.points.at(i));
@@ -474,8 +475,12 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime(
     // calc average vel in idx i~i+1
     const double average_vel =
       std::min((closest_vel + next_vel) / 2.0, planner_param_.intersection_velocity);
-    passing_time += dist / average_vel;
-    time_distance_array.emplace_back(passing_time, dist_sum);
+    // passing_time += dist / average_vel;
+    // time_distance_array.emplace_back(passing_time, dist_sum);
+    auto reference_point = path.points[i];
+    reference_point.point.longitudinal_velocity_mps = average_vel;
+    reference_path.points.push_back(reference_point);
+
     closest_vel = next_vel;
 
     bool has_objective_lane_id = util::hasLaneId(path.points.at(i), objective_lane_id);
@@ -489,6 +494,25 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime(
     return {{0.0, 0.0}};  // has already passed the intersection.
   }
 
+  PathWithLaneId smoothed_reference_path = reference_path;
+  if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) {
+    RCLCPP_WARN(logger_, "smoothPath failed");
+  }
+
+  TimeDistanceArray time_distance_array{};
+  dist_sum = 0.0;
+  double passing_time = 0.0;
+  time_distance_array.emplace_back(passing_time, dist_sum);
+  for (size_t i = 1; i < smoothed_reference_path.points.size(); ++i) {
+    const double dist = planning_utils::calcDist2d(
+      smoothed_reference_path.points.at(i - 1), smoothed_reference_path.points.at(i));
+    dist_sum += dist;
+    // to avoid zero division
+    passing_time +=
+      (dist / std::max<double>(
+                0.01, smoothed_reference_path.points.at(i - 1).point.longitudinal_velocity_mps));
+    time_distance_array.emplace_back(passing_time, dist_sum);
+  }
   RCLCPP_DEBUG(logger_, "intersection dist = %f, passing_time = %f", dist_sum, passing_time);
 
   return time_distance_array;