From c32b71b48451b4d36a729a5483b52420cde84f7d Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 21 Dec 2023 16:00:03 +0900
Subject: [PATCH 01/15] WIP add acc constraints to discard paths

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
 .../map_based_prediction_node.hpp             | 106 ++++++++++++++++++
 .../src/map_based_prediction_node.cpp         |   8 ++
 2 files changed, 114 insertions(+)

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index bdd9ad89bc025..fd2451410319c 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -16,11 +16,13 @@
 #define MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_
 
 #include "map_based_prediction/path_generator.hpp"
+#include "tier4_autoware_utils/geometry/geometry.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/transform_listener.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
 
+#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
@@ -34,6 +36,7 @@
 #include <lanelet2_routing/Forward.h>
 #include <lanelet2_traffic_rules/TrafficRules.h>
 
+#include <algorithm>
 #include <deque>
 #include <memory>
 #include <string>
@@ -99,8 +102,111 @@ using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_perception_msgs::msg::TrackedObject;
 using autoware_auto_perception_msgs::msg::TrackedObjectKinematics;
 using autoware_auto_perception_msgs::msg::TrackedObjects;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using tier4_autoware_utils::StopWatch;
 using tier4_debug_msgs::msg::StringStamped;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+
+inline std::vector<double> calcTrajectoryCurvatureFrom3Points(
+  const TrajectoryPoints & trajectory, size_t idx_dist)
+{
+  using tier4_autoware_utils::calcCurvature;
+  using tier4_autoware_utils::getPoint;
+
+  if (trajectory.size() < 3) {
+    const std::vector<double> k_arr(trajectory.size(), 0.0);
+    return k_arr;
+  }
+
+  // if the idx size is not enough, change the idx_dist
+  const auto max_idx_dist = static_cast<size_t>(std::floor((trajectory.size() - 1) / 2.0));
+  idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist));
+
+  if (idx_dist < 1) {
+    throw std::logic_error("idx_dist less than 1 is not expected");
+  }
+
+  // calculate curvature by circle fitting from three points
+  std::vector<double> k_arr(trajectory.size(), 0.0);
+
+  for (size_t i = 1; i + 1 < trajectory.size(); i++) {
+    double curvature = 0.0;
+    const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i)));
+    const auto p1 = getPoint(trajectory.at(i));
+    const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i)));
+    try {
+      curvature = calcCurvature(p0, p1, p2);
+    } catch (std::exception const & e) {
+      // ...code that handles the error...
+      RCLCPP_WARN(
+        rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "%s",
+        e.what());
+      if (i > 1) {
+        curvature = k_arr.at(i - 1);  // previous curvature
+      } else {
+        curvature = 0.0;
+      }
+    }
+    k_arr.at(i) = curvature;
+  }
+  // copy curvatures for the last and first points;
+  k_arr.at(0) = k_arr.at(1);
+  k_arr.back() = k_arr.at((trajectory.size() - 2));
+
+  return k_arr;
+}
+
+inline TrajectoryPoints toTrajectoryPoints(const PredictedPath & path, const double velocity)
+{
+  TrajectoryPoints out_trajectory;
+  std::for_each(path.path.begin(), path.path.end(), [&out_trajectory, velocity](const auto & pose) {
+    TrajectoryPoint p;
+    p.pose = pose;
+    p.longitudinal_velocity_mps = velocity;
+    out_trajectory.push_back(p);
+  });
+  return out_trajectory;
+};
+
+inline bool isAccelerationConstraintsSatisfied(
+  const TrajectoryPoints & trajectory [[maybe_unused]], const double points_interval = 1.0)
+{
+  if (trajectory.size() < 3) return false;
+  constexpr double curvature_calculation_distance = 5.0;
+  const size_t idx_dist = static_cast<size_t>(
+    std::max(static_cast<int>((curvature_calculation_distance) / points_interval), 1));
+  // Calculate curvature assuming the trajectory points interval is constant
+  const auto curvature_v = calcTrajectoryCurvatureFrom3Points(trajectory, idx_dist);
+
+  constexpr double max_lateral_accel = 1.0;
+  // constexpr double min_curve_velocity = 3.5;
+  constexpr double deceleration_distance_before_curve = 3.5;
+  constexpr double deceleration_distance_after_curve = 2.0;
+
+  //  Decrease speed according to lateral G
+  const size_t before_decel_index =
+    static_cast<size_t>(std::round(deceleration_distance_before_curve / points_interval));
+  const size_t after_decel_index =
+    static_cast<size_t>(std::round(deceleration_distance_after_curve / points_interval));
+  const double max_lateral_accel_abs = std::fabs(max_lateral_accel);
+
+  for (size_t i = 0; i < trajectory.size(); ++i) {
+    double curvature = 0.0;
+    const size_t start = i > after_decel_index ? i - after_decel_index : 0;
+    const size_t end = std::min(trajectory.size(), i + before_decel_index + 1);
+    for (size_t j = start; j < end; ++j) {
+      if (j >= curvature_v.size()) return true;
+      curvature = std::max(curvature, std::fabs(curvature_v.at(j)));
+    }
+    double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5));
+    // v_curvature_max = std::max(v_curvature_max, min_curve_velocity);
+
+    if (trajectory.at(i).longitudinal_velocity_mps > v_curvature_max) {
+      return false;
+    }
+  }
+  return true;
+};
 
 class MapBasedPredictionNode : public rclcpp::Node
 {
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index f89c83e9c2d5a..738fd32c54fd5 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -978,6 +978,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
           if (predicted_path.path.empty()) {
             continue;
           }
+
+          // Check lat. acceleration constraints
+          if (ref_path.maneuver != Maneuver::LANE_FOLLOW) {
+            const auto trajectory_with_const_velocity =
+              toTrajectoryPoints(predicted_path, abs_obj_speed);
+            if (!isAccelerationConstraintsSatisfied(trajectory_with_const_velocity)) continue;
+          }
+
           predicted_path.confidence = ref_path.probability;
           predicted_paths.push_back(predicted_path);
         }

From 94c0364c54251a0fbe75d14d31fd36974d2a0d55 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Thu, 21 Dec 2023 18:49:56 +0900
Subject: [PATCH 02/15] Add lat acc constraints to predicted paths

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
 .../map_based_prediction_node.hpp             | 219 ++++++++++--------
 .../src/map_based_prediction_node.cpp         |  49 +++-
 2 files changed, 161 insertions(+), 107 deletions(-)

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index fd2451410319c..8fc596d742a6c 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -17,6 +17,7 @@
 
 #include "map_based_prediction/path_generator.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "tier4_autoware_utils/ros/update_param.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/transform_listener.hpp>
@@ -106,108 +107,6 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using tier4_autoware_utils::StopWatch;
 using tier4_debug_msgs::msg::StringStamped;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
-
-inline std::vector<double> calcTrajectoryCurvatureFrom3Points(
-  const TrajectoryPoints & trajectory, size_t idx_dist)
-{
-  using tier4_autoware_utils::calcCurvature;
-  using tier4_autoware_utils::getPoint;
-
-  if (trajectory.size() < 3) {
-    const std::vector<double> k_arr(trajectory.size(), 0.0);
-    return k_arr;
-  }
-
-  // if the idx size is not enough, change the idx_dist
-  const auto max_idx_dist = static_cast<size_t>(std::floor((trajectory.size() - 1) / 2.0));
-  idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist));
-
-  if (idx_dist < 1) {
-    throw std::logic_error("idx_dist less than 1 is not expected");
-  }
-
-  // calculate curvature by circle fitting from three points
-  std::vector<double> k_arr(trajectory.size(), 0.0);
-
-  for (size_t i = 1; i + 1 < trajectory.size(); i++) {
-    double curvature = 0.0;
-    const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i)));
-    const auto p1 = getPoint(trajectory.at(i));
-    const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i)));
-    try {
-      curvature = calcCurvature(p0, p1, p2);
-    } catch (std::exception const & e) {
-      // ...code that handles the error...
-      RCLCPP_WARN(
-        rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "%s",
-        e.what());
-      if (i > 1) {
-        curvature = k_arr.at(i - 1);  // previous curvature
-      } else {
-        curvature = 0.0;
-      }
-    }
-    k_arr.at(i) = curvature;
-  }
-  // copy curvatures for the last and first points;
-  k_arr.at(0) = k_arr.at(1);
-  k_arr.back() = k_arr.at((trajectory.size() - 2));
-
-  return k_arr;
-}
-
-inline TrajectoryPoints toTrajectoryPoints(const PredictedPath & path, const double velocity)
-{
-  TrajectoryPoints out_trajectory;
-  std::for_each(path.path.begin(), path.path.end(), [&out_trajectory, velocity](const auto & pose) {
-    TrajectoryPoint p;
-    p.pose = pose;
-    p.longitudinal_velocity_mps = velocity;
-    out_trajectory.push_back(p);
-  });
-  return out_trajectory;
-};
-
-inline bool isAccelerationConstraintsSatisfied(
-  const TrajectoryPoints & trajectory [[maybe_unused]], const double points_interval = 1.0)
-{
-  if (trajectory.size() < 3) return false;
-  constexpr double curvature_calculation_distance = 5.0;
-  const size_t idx_dist = static_cast<size_t>(
-    std::max(static_cast<int>((curvature_calculation_distance) / points_interval), 1));
-  // Calculate curvature assuming the trajectory points interval is constant
-  const auto curvature_v = calcTrajectoryCurvatureFrom3Points(trajectory, idx_dist);
-
-  constexpr double max_lateral_accel = 1.0;
-  // constexpr double min_curve_velocity = 3.5;
-  constexpr double deceleration_distance_before_curve = 3.5;
-  constexpr double deceleration_distance_after_curve = 2.0;
-
-  //  Decrease speed according to lateral G
-  const size_t before_decel_index =
-    static_cast<size_t>(std::round(deceleration_distance_before_curve / points_interval));
-  const size_t after_decel_index =
-    static_cast<size_t>(std::round(deceleration_distance_after_curve / points_interval));
-  const double max_lateral_accel_abs = std::fabs(max_lateral_accel);
-
-  for (size_t i = 0; i < trajectory.size(); ++i) {
-    double curvature = 0.0;
-    const size_t start = i > after_decel_index ? i - after_decel_index : 0;
-    const size_t end = std::min(trajectory.size(), i + before_decel_index + 1);
-    for (size_t j = start; j < end; ++j) {
-      if (j >= curvature_v.size()) return true;
-      curvature = std::max(curvature, std::fabs(curvature_v.at(j)));
-    }
-    double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5));
-    // v_curvature_max = std::max(v_curvature_max, min_curve_velocity);
-
-    if (trajectory.at(i).longitudinal_velocity_mps > v_curvature_max) {
-      return false;
-    }
-  }
-  return true;
-};
-
 class MapBasedPredictionNode : public rclcpp::Node
 {
 public:
@@ -229,6 +128,11 @@ class MapBasedPredictionNode : public rclcpp::Node
   std::shared_ptr<lanelet::routing::RoutingGraph> routing_graph_ptr_;
   std::shared_ptr<lanelet::traffic_rules::TrafficRules> traffic_rules_ptr_;
 
+  // parameter update
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult onParam(
+    const std::vector<rclcpp::Parameter> & parameters);
+
   // Pose Transform Listener
   tier4_autoware_utils::TransformListener transform_listener_{this};
 
@@ -266,6 +170,10 @@ class MapBasedPredictionNode : public rclcpp::Node
   bool consider_only_routable_neighbours_;
   double reference_path_resolution_;
 
+  double max_lateral_accel_;
+  double deceleration_distance_before_curve_;
+  double deceleration_distance_after_curve_;
+
   // Stop watch
   StopWatch<std::chrono::milliseconds> stop_watch_;
 
@@ -343,6 +251,113 @@ class MapBasedPredictionNode : public rclcpp::Node
   Maneuver predictObjectManeuverByLatDiffDistance(
     const TrackedObject & object, const LaneletData & current_lanelet_data,
     const double object_detected_time);
+
+  inline std::vector<double> calcTrajectoryCurvatureFrom3Points(
+    const TrajectoryPoints & trajectory, size_t idx_dist)
+  {
+    using tier4_autoware_utils::calcCurvature;
+    using tier4_autoware_utils::getPoint;
+
+    if (trajectory.size() < 3) {
+      const std::vector<double> k_arr(trajectory.size(), 0.0);
+      return k_arr;
+    }
+
+    // if the idx size is not enough, change the idx_dist
+    const auto max_idx_dist = static_cast<size_t>(std::floor((trajectory.size() - 1) / 2.0));
+    idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist));
+
+    if (idx_dist < 1) {
+      throw std::logic_error("idx_dist less than 1 is not expected");
+    }
+
+    // calculate curvature by circle fitting from three points
+    std::vector<double> k_arr(trajectory.size(), 0.0);
+
+    for (size_t i = 1; i + 1 < trajectory.size(); i++) {
+      double curvature = 0.0;
+      const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i)));
+      const auto p1 = getPoint(trajectory.at(i));
+      const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i)));
+      try {
+        curvature = calcCurvature(p0, p1, p2);
+      } catch (std::exception const & e) {
+        // ...code that handles the error...
+        RCLCPP_WARN(
+          rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "%s",
+          e.what());
+        if (i > 1) {
+          curvature = k_arr.at(i - 1);  // previous curvature
+        } else {
+          curvature = 0.0;
+        }
+      }
+      k_arr.at(i) = curvature;
+    }
+    // copy curvatures for the last and first points;
+    k_arr.at(0) = k_arr.at(1);
+    k_arr.back() = k_arr.at((trajectory.size() - 2));
+
+    return k_arr;
+  }
+
+  inline TrajectoryPoints toTrajectoryPoints(const PredictedPath & path, const double velocity)
+  {
+    TrajectoryPoints out_trajectory;
+    std::for_each(
+      path.path.begin(), path.path.end(), [&out_trajectory, velocity](const auto & pose) {
+        TrajectoryPoint p;
+        p.pose = pose;
+        p.longitudinal_velocity_mps = velocity;
+        out_trajectory.push_back(p);
+      });
+    return out_trajectory;
+  };
+
+  inline bool isAccelerationConstraintsSatisfied(
+    const TrajectoryPoints & trajectory [[maybe_unused]], const double points_interval = 1.0)
+  {
+    if (trajectory.size() < 3) return false;
+    constexpr double curvature_calculation_distance = 5.0;
+    const size_t idx_dist = static_cast<size_t>(
+      std::max(static_cast<int>((curvature_calculation_distance) / points_interval), 1));
+    // Calculate curvature assuming the trajectory points interval is constant
+    const auto curvature_v = calcTrajectoryCurvatureFrom3Points(trajectory, idx_dist);
+
+    // constexpr double min_curve_velocity = 3.5;
+    // constexpr double deceleration_distance_before_curve = 3.5;
+    // constexpr double deceleration_distance_after_curve = 2.0;
+
+    //  Decrease speed according to lateral G
+    const size_t before_decel_index =
+      static_cast<size_t>(std::round(deceleration_distance_before_curve_ / points_interval));
+    const size_t after_decel_index =
+      static_cast<size_t>(std::round(deceleration_distance_after_curve_ / points_interval));
+    const double max_lateral_accel_abs = std::fabs(max_lateral_accel_);
+
+    double min_curv_thing = 0.0;
+    for (size_t i = 0; i < trajectory.size(); ++i) {
+      double curvature = 0.0;
+      const size_t start = i > after_decel_index ? i - after_decel_index : 0;
+      const size_t end = std::min(trajectory.size(), i + before_decel_index + 1);
+      for (size_t j = start; j < end; ++j) {
+        if (j >= curvature_v.size()) return true;
+        curvature = std::max(curvature, std::fabs(curvature_v.at(j)));
+      }
+      double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5));
+      // v_curvature_max = std::max(v_curvature_max, min_curve_velocity);
+      min_curv_thing = std::min(v_curvature_max, min_curv_thing);
+      if (trajectory.at(i).longitudinal_velocity_mps > v_curvature_max) {
+        std::cerr << "False Min curvature thing " << min_curv_thing << "\n";
+        std::cerr << "Velocity " << trajectory.at(i).longitudinal_velocity_mps << "\n";
+        return false;
+      }
+    }
+    std::cerr << "True Min curvature thing " << min_curv_thing << "\n";
+    std::cerr << "Velocity " << trajectory.back().longitudinal_velocity_mps << "\n";
+
+    return true;
+  };
 };
 }  // namespace map_based_prediction
 
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 738fd32c54fd5..859b54a98f8e7 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -740,6 +740,13 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   sigma_yaw_angle_deg_ = declare_parameter<double>("sigma_yaw_angle_deg");
   object_buffer_time_length_ = declare_parameter<double>("object_buffer_time_length");
   history_time_length_ = declare_parameter<double>("history_time_length");
+
+  max_lateral_accel_ = declare_parameter<double>("max_lateral_accel");
+  deceleration_distance_before_curve_ =
+    declare_parameter<double>("deceleration_distance_before_curve");
+  deceleration_distance_after_curve_ =
+    declare_parameter<double>("deceleration_distance_after_curve");
+
   {  // lane change detection
     lane_change_detection_method_ = declare_parameter<std::string>("lane_change_detection.method");
 
@@ -789,6 +796,30 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   pub_debug_markers_ =
     this->create_publisher<visualization_msgs::msg::MarkerArray>("maneuver", rclcpp::QoS{1});
   pub_calculation_time_ = create_publisher<StringStamped>("~/debug/calculation_time", 1);
+
+  set_param_res_ = this->add_on_set_parameters_callback(
+    std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1));
+}
+
+rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam(
+  const std::vector<rclcpp::Parameter> & parameters)
+{
+  using tier4_autoware_utils::updateParam;
+
+  updateParam(parameters, "max_lateral_accel", max_lateral_accel_);
+  updateParam(
+    parameters, "deceleration_distance_before_curve", deceleration_distance_before_curve_);
+  updateParam(parameters, "deceleration_distance_after_curve", deceleration_distance_after_curve_);
+
+  std::cerr << "Params updated! \n";
+  std::cerr << "max_lateral_accel" << max_lateral_accel_ << "\n";
+  std::cerr << "deceleration_distance_before_curve" << deceleration_distance_before_curve_ << "\n";
+  std::cerr << "deceleration_distance_after_curve" << deceleration_distance_after_curve_ << "\n";
+
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  result.reason = "success";
+  return result;
 }
 
 PredictedObjectKinematics MapBasedPredictionNode::convertToPredictedKinematics(
@@ -971,6 +1002,10 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
           replaceObjectYawWithLaneletsYaw(current_lanelets, yaw_fixed_transformed_object);
         }
         // Generate Predicted Path
+        // Check lat. acceleration constraints
+        std::cerr << "---------- Object " << tier4_autoware_utils::toHexString(object.object_id)
+                  << "---------\n";
+        std::cerr << "Object speed " << abs_obj_speed << "\n";
         std::vector<PredictedPath> predicted_paths;
         for (const auto & ref_path : ref_paths) {
           PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
@@ -979,12 +1014,16 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
             continue;
           }
 
-          // Check lat. acceleration constraints
-          if (ref_path.maneuver != Maneuver::LANE_FOLLOW) {
-            const auto trajectory_with_const_velocity =
-              toTrajectoryPoints(predicted_path, abs_obj_speed);
-            if (!isAccelerationConstraintsSatisfied(trajectory_with_const_velocity)) continue;
+          // if (ref_path.maneuver != Maneuver::LANE_FOLLOW) {
+          const auto trajectory_with_const_velocity =
+            toTrajectoryPoints(predicted_path, abs_obj_speed);
+          if (!isAccelerationConstraintsSatisfied(trajectory_with_const_velocity)) {
+            std::cerr << "Path eliminated by Lat acc constraints\n";
+            continue;
           }
+          // }
+          std::cerr << "---------- Object " << tier4_autoware_utils::toHexString(object.object_id)
+                    << "---------\n";
 
           predicted_path.confidence = ref_path.probability;
           predicted_paths.push_back(predicted_path);

From 69e59fe62ce9c61f99663393f5900af2f5a1c0f3 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 22 Dec 2023 15:00:52 +0900
Subject: [PATCH 03/15] Delete debug print

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
 .../include/map_based_prediction/map_based_prediction_node.hpp  | 2 --
 1 file changed, 2 deletions(-)

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index 8fc596d742a6c..dde97c53c9bc5 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -346,14 +346,12 @@ class MapBasedPredictionNode : public rclcpp::Node
       }
       double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5));
       // v_curvature_max = std::max(v_curvature_max, min_curve_velocity);
-      min_curv_thing = std::min(v_curvature_max, min_curv_thing);
       if (trajectory.at(i).longitudinal_velocity_mps > v_curvature_max) {
         std::cerr << "False Min curvature thing " << min_curv_thing << "\n";
         std::cerr << "Velocity " << trajectory.at(i).longitudinal_velocity_mps << "\n";
         return false;
       }
     }
-    std::cerr << "True Min curvature thing " << min_curv_thing << "\n";
     std::cerr << "Velocity " << trajectory.back().longitudinal_velocity_mps << "\n";
 
     return true;

From 84d4a90a1c05e74aced3397dc9fa9c197fd222bd Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 22 Dec 2023 15:05:24 +0900
Subject: [PATCH 04/15] delete unused var

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
 .../include/map_based_prediction/map_based_prediction_node.hpp   | 1 -
 1 file changed, 1 deletion(-)

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index dde97c53c9bc5..b7831bb55798c 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -347,7 +347,6 @@ class MapBasedPredictionNode : public rclcpp::Node
       double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5));
       // v_curvature_max = std::max(v_curvature_max, min_curve_velocity);
       if (trajectory.at(i).longitudinal_velocity_mps > v_curvature_max) {
-        std::cerr << "False Min curvature thing " << min_curv_thing << "\n";
         std::cerr << "Velocity " << trajectory.at(i).longitudinal_velocity_mps << "\n";
         return false;
       }

From 374c6f16acea4e1e929d8bef9bc7d30e8cbb3a36 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 22 Dec 2023 15:05:58 +0900
Subject: [PATCH 05/15] delete unused var

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
 .../include/map_based_prediction/map_based_prediction_node.hpp   | 1 -
 1 file changed, 1 deletion(-)

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index b7831bb55798c..eeb8a21375bf5 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -335,7 +335,6 @@ class MapBasedPredictionNode : public rclcpp::Node
       static_cast<size_t>(std::round(deceleration_distance_after_curve_ / points_interval));
     const double max_lateral_accel_abs = std::fabs(max_lateral_accel_);
 
-    double min_curv_thing = 0.0;
     for (size_t i = 0; i < trajectory.size(); ++i) {
       double curvature = 0.0;
       const size_t start = i > after_decel_index ? i - after_decel_index : 0;

From 3c514986786464cc8642d470796abf7e90b3acc6 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <danielsanchezaran@gmail.com>
Date: Fri, 22 Dec 2023 17:31:40 +0900
Subject: [PATCH 06/15] WIP

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
---
 .../map_based_prediction_node.hpp             | 25 ++++++++++++++++---
 .../src/map_based_prediction_node.cpp         |  7 ++----
 2 files changed, 24 insertions(+), 8 deletions(-)

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index eeb8a21375bf5..379beb2b75f16 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -173,6 +173,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   double max_lateral_accel_;
   double deceleration_distance_before_curve_;
   double deceleration_distance_after_curve_;
+  double min_acceleration_before_curve_;
 
   // Stop watch
   StopWatch<std::chrono::milliseconds> stop_watch_;
@@ -334,8 +335,12 @@ class MapBasedPredictionNode : public rclcpp::Node
     const size_t after_decel_index =
       static_cast<size_t>(std::round(deceleration_distance_after_curve_ / points_interval));
     const double max_lateral_accel_abs = std::fabs(max_lateral_accel_);
+    double arc_length = 0.0;
 
     for (size_t i = 0; i < trajectory.size(); ++i) {
+      if (i > 0)
+        arc_length +=
+          tier4_autoware_utils::calcDistance2d(trajectory.at(i).pose, trajectory.at(i - 1).pose);
       double curvature = 0.0;
       const size_t start = i > after_decel_index ? i - after_decel_index : 0;
       const size_t end = std::min(trajectory.size(), i + before_decel_index + 1);
@@ -343,11 +348,25 @@ class MapBasedPredictionNode : public rclcpp::Node
         if (j >= curvature_v.size()) return true;
         curvature = std::max(curvature, std::fabs(curvature_v.at(j)));
       }
+      // double curvature = curvature_v.at(i);
       double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5));
       // v_curvature_max = std::max(v_curvature_max, min_curve_velocity);
-      if (trajectory.at(i).longitudinal_velocity_mps > v_curvature_max) {
-        std::cerr << "Velocity " << trajectory.at(i).longitudinal_velocity_mps << "\n";
-        return false;
+      const double current_speed = trajectory.at(i).longitudinal_velocity_mps;
+      if (current_speed > v_curvature_max) {
+        // v_curvature_max = current_speed + a*t
+        std::cerr << "Current path index " << i << "\n";
+        std::cerr << "current_speed > v_curvature_max  checking if deceleration is possible\n";
+        const double t =
+          (v_curvature_max - current_speed) / min_acceleration_before_curve_;  // acc is negative
+        const double distance_to_slow_down =
+          current_speed * t + 0.5 * min_acceleration_before_curve_ * std::pow(t, 2);
+        std::cerr << "Decel time " << t << "\n";
+        std::cerr << "Decel distance " << distance_to_slow_down << "\n";
+        std::cerr << "Arc length " << arc_length << "\n";
+        std::cerr << "Curvature " << curvature << "\n";
+        bool c = distance_to_slow_down > arc_length;
+        std::cerr << "Check is " << c << "\n";
+        if (distance_to_slow_down > arc_length) return false;
       }
     }
     std::cerr << "Velocity " << trajectory.back().longitudinal_velocity_mps << "\n";
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 859b54a98f8e7..83914c034592d 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -746,6 +746,7 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
     declare_parameter<double>("deceleration_distance_before_curve");
   deceleration_distance_after_curve_ =
     declare_parameter<double>("deceleration_distance_after_curve");
+  min_acceleration_before_curve_ = declare_parameter<double>("min_acceleration_before_curve");
 
   {  // lane change detection
     lane_change_detection_method_ = declare_parameter<std::string>("lane_change_detection.method");
@@ -810,11 +811,7 @@ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam(
   updateParam(
     parameters, "deceleration_distance_before_curve", deceleration_distance_before_curve_);
   updateParam(parameters, "deceleration_distance_after_curve", deceleration_distance_after_curve_);
-
-  std::cerr << "Params updated! \n";
-  std::cerr << "max_lateral_accel" << max_lateral_accel_ << "\n";
-  std::cerr << "deceleration_distance_before_curve" << deceleration_distance_before_curve_ << "\n";
-  std::cerr << "deceleration_distance_after_curve" << deceleration_distance_after_curve_ << "\n";
+  updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_);
 
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;

From c046797aa8cb72d30af6061d390e9ca7a34e76c5 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Mon, 25 Dec 2023 14:28:21 +0900
Subject: [PATCH 07/15] make lat acc calculation with yaw rate

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
 .../map_based_prediction_node.hpp             | 98 +++++++++----------
 .../src/map_based_prediction_node.cpp         | 12 ++-
 2 files changed, 57 insertions(+), 53 deletions(-)

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index 379beb2b75f16..7d5182a9a537b 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -16,6 +16,7 @@
 #define MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_
 
 #include "map_based_prediction/path_generator.hpp"
+#include "tf2/LinearMath/Quaternion.h"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
 #include "tier4_autoware_utils/ros/update_param.hpp"
 
@@ -316,61 +317,58 @@ class MapBasedPredictionNode : public rclcpp::Node
   };
 
   inline bool isAccelerationConstraintsSatisfied(
-    const TrajectoryPoints & trajectory [[maybe_unused]], const double points_interval = 1.0)
+    const TrajectoryPoints & trajectory [[maybe_unused]], const double delta_time)
   {
-    if (trajectory.size() < 3) return false;
-    constexpr double curvature_calculation_distance = 5.0;
-    const size_t idx_dist = static_cast<size_t>(
-      std::max(static_cast<int>((curvature_calculation_distance) / points_interval), 1));
-    // Calculate curvature assuming the trajectory points interval is constant
-    const auto curvature_v = calcTrajectoryCurvatureFrom3Points(trajectory, idx_dist);
-
-    // constexpr double min_curve_velocity = 3.5;
-    // constexpr double deceleration_distance_before_curve = 3.5;
-    // constexpr double deceleration_distance_after_curve = 2.0;
-
-    //  Decrease speed according to lateral G
-    const size_t before_decel_index =
-      static_cast<size_t>(std::round(deceleration_distance_before_curve_ / points_interval));
-    const size_t after_decel_index =
-      static_cast<size_t>(std::round(deceleration_distance_after_curve_ / points_interval));
+    if (trajectory.size() < 3) return true;
     const double max_lateral_accel_abs = std::fabs(max_lateral_accel_);
-    double arc_length = 0.0;
 
-    for (size_t i = 0; i < trajectory.size(); ++i) {
-      if (i > 0)
-        arc_length +=
-          tier4_autoware_utils::calcDistance2d(trajectory.at(i).pose, trajectory.at(i - 1).pose);
-      double curvature = 0.0;
-      const size_t start = i > after_decel_index ? i - after_decel_index : 0;
-      const size_t end = std::min(trajectory.size(), i + before_decel_index + 1);
-      for (size_t j = start; j < end; ++j) {
-        if (j >= curvature_v.size()) return true;
-        curvature = std::max(curvature, std::fabs(curvature_v.at(j)));
-      }
-      // double curvature = curvature_v.at(i);
-      double v_curvature_max = std::sqrt(max_lateral_accel_abs / std::max(curvature, 1.0E-5));
-      // v_curvature_max = std::max(v_curvature_max, min_curve_velocity);
-      const double current_speed = trajectory.at(i).longitudinal_velocity_mps;
-      if (current_speed > v_curvature_max) {
-        // v_curvature_max = current_speed + a*t
-        std::cerr << "Current path index " << i << "\n";
-        std::cerr << "current_speed > v_curvature_max  checking if deceleration is possible\n";
-        const double t =
-          (v_curvature_max - current_speed) / min_acceleration_before_curve_;  // acc is negative
-        const double distance_to_slow_down =
-          current_speed * t + 0.5 * min_acceleration_before_curve_ * std::pow(t, 2);
-        std::cerr << "Decel time " << t << "\n";
-        std::cerr << "Decel distance " << distance_to_slow_down << "\n";
-        std::cerr << "Arc length " << arc_length << "\n";
-        std::cerr << "Curvature " << curvature << "\n";
-        bool c = distance_to_slow_down > arc_length;
-        std::cerr << "Check is " << c << "\n";
-        if (distance_to_slow_down > arc_length) return false;
+    double arc_length = 0.0;
+    for (size_t i = 1; i < trajectory.size(); ++i) {
+      const auto current_pose = trajectory.at(i).pose;
+      const auto next_pose = trajectory.at(i - 1).pose;
+      // Compute distance between poses
+      const double delta_s = std::hypot(
+        next_pose.position.x - current_pose.position.x,
+        next_pose.position.y - current_pose.position.y);
+      arc_length += delta_s;
+
+      // Compute change in heading
+      tf2::Quaternion q_current, q_next;
+      tf2::convert(current_pose.orientation, q_current);
+      tf2::convert(next_pose.orientation, q_next);
+      double delta_theta = q_current.angleShortestPath(q_next);
+      // Handle wrap-around
+      if (delta_theta > 3.14159) {
+        delta_theta -= 2.0 * 3.14159;
+      } else if (delta_theta < -3.14159) {
+        delta_theta += 2.0 * 3.14159;
       }
-    }
-    std::cerr << "Velocity " << trajectory.back().longitudinal_velocity_mps << "\n";
 
+      const double yaw_rate = std::max(delta_theta / delta_time, 1.0E-5);
+
+      const double current_speed = std::abs(trajectory.at(i).longitudinal_velocity_mps);
+      // Compute lateral acceleration
+      const double lateral_acceleration = std::abs(current_speed * yaw_rate);
+      if (lateral_acceleration < max_lateral_accel_abs) continue;
+
+      const double v_curvature_max = std::sqrt(max_lateral_accel_abs / yaw_rate);
+      const double t =
+        (v_curvature_max - current_speed) / min_acceleration_before_curve_;  // acc is negative
+      const double distance_to_slow_down =
+        current_speed * t + 0.5 * min_acceleration_before_curve_ * std::pow(t, 2);
+
+      std::cerr << "current_speed > v_curvature_max  checking if deceleration is possible\n";
+      std::cerr << "Current path index " << i << "\n";
+      std::cerr << "Decel time " << t << "\n";
+      std::cerr << "Decel distance " << distance_to_slow_down << "\n";
+      std::cerr << "Arc length " << arc_length << "\n";
+      std::cerr << "yaw_rate " << yaw_rate << "\n";
+      std::cerr << "current_speed " << current_speed << "\n";
+      std::cerr << "v_curvature_max " << v_curvature_max << "\n";
+      bool c = distance_to_slow_down > arc_length;
+      std::cerr << "Check is " << c << "\n";
+      if (distance_to_slow_down > arc_length) return false;
+    }
     return true;
   };
 };
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 83914c034592d..1c262fd87cfa3 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -1004,27 +1004,33 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
                   << "---------\n";
         std::cerr << "Object speed " << abs_obj_speed << "\n";
         std::vector<PredictedPath> predicted_paths;
+        int count = 0;
         for (const auto & ref_path : ref_paths) {
+          std::cerr << "---------------Path # -------------" << count << "\n";
           PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
             yaw_fixed_transformed_object, ref_path.path);
           if (predicted_path.path.empty()) {
             continue;
           }
+          std::cerr << "path prob " << predicted_path.confidence << "\n";
 
           // if (ref_path.maneuver != Maneuver::LANE_FOLLOW) {
           const auto trajectory_with_const_velocity =
             toTrajectoryPoints(predicted_path, abs_obj_speed);
-          if (!isAccelerationConstraintsSatisfied(trajectory_with_const_velocity)) {
+          if (!isAccelerationConstraintsSatisfied(
+                trajectory_with_const_velocity, prediction_sampling_time_interval_)) {
             std::cerr << "Path eliminated by Lat acc constraints\n";
+            std::cerr << "---------------Path # -------------" << count++ << "\n";
             continue;
           }
           // }
-          std::cerr << "---------- Object " << tier4_autoware_utils::toHexString(object.object_id)
-                    << "---------\n";
 
           predicted_path.confidence = ref_path.probability;
           predicted_paths.push_back(predicted_path);
+          std::cerr << "---------------Path # -------------" << count++ << "\n";
         }
+        std::cerr << "---------- Object " << tier4_autoware_utils::toHexString(object.object_id)
+                  << "---------\n";
 
         // Normalize Path Confidence and output the predicted object
 

From 4b0cf86decfe647f5eda21286d21c9cf740ea8f9 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Mon, 25 Dec 2023 14:46:12 +0900
Subject: [PATCH 08/15] eliminate debug prints

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
 .../map_based_prediction_node.hpp                   | 10 ----------
 .../src/map_based_prediction_node.cpp               | 13 +------------
 2 files changed, 1 insertion(+), 22 deletions(-)

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index 7d5182a9a537b..df902610a4484 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -357,16 +357,6 @@ class MapBasedPredictionNode : public rclcpp::Node
       const double distance_to_slow_down =
         current_speed * t + 0.5 * min_acceleration_before_curve_ * std::pow(t, 2);
 
-      std::cerr << "current_speed > v_curvature_max  checking if deceleration is possible\n";
-      std::cerr << "Current path index " << i << "\n";
-      std::cerr << "Decel time " << t << "\n";
-      std::cerr << "Decel distance " << distance_to_slow_down << "\n";
-      std::cerr << "Arc length " << arc_length << "\n";
-      std::cerr << "yaw_rate " << yaw_rate << "\n";
-      std::cerr << "current_speed " << current_speed << "\n";
-      std::cerr << "v_curvature_max " << v_curvature_max << "\n";
-      bool c = distance_to_slow_down > arc_length;
-      std::cerr << "Check is " << c << "\n";
       if (distance_to_slow_down > arc_length) return false;
     }
     return true;
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 1c262fd87cfa3..fb57407e9d859 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -1000,37 +1000,26 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
         }
         // Generate Predicted Path
         // Check lat. acceleration constraints
-        std::cerr << "---------- Object " << tier4_autoware_utils::toHexString(object.object_id)
-                  << "---------\n";
-        std::cerr << "Object speed " << abs_obj_speed << "\n";
+
         std::vector<PredictedPath> predicted_paths;
         int count = 0;
         for (const auto & ref_path : ref_paths) {
-          std::cerr << "---------------Path # -------------" << count << "\n";
           PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
             yaw_fixed_transformed_object, ref_path.path);
           if (predicted_path.path.empty()) {
             continue;
           }
-          std::cerr << "path prob " << predicted_path.confidence << "\n";
 
-          // if (ref_path.maneuver != Maneuver::LANE_FOLLOW) {
           const auto trajectory_with_const_velocity =
             toTrajectoryPoints(predicted_path, abs_obj_speed);
           if (!isAccelerationConstraintsSatisfied(
                 trajectory_with_const_velocity, prediction_sampling_time_interval_)) {
-            std::cerr << "Path eliminated by Lat acc constraints\n";
-            std::cerr << "---------------Path # -------------" << count++ << "\n";
             continue;
           }
-          // }
 
           predicted_path.confidence = ref_path.probability;
           predicted_paths.push_back(predicted_path);
-          std::cerr << "---------------Path # -------------" << count++ << "\n";
         }
-        std::cerr << "---------- Object " << tier4_autoware_utils::toHexString(object.object_id)
-                  << "---------\n";
 
         // Normalize Path Confidence and output the predicted object
 

From e46d1ff75f4cd18149c6e409e1cb138e763e62db Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Mon, 25 Dec 2023 16:08:20 +0900
Subject: [PATCH 09/15] delete unused variable

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
 .../map_based_prediction/src/map_based_prediction_node.cpp       | 1 -
 1 file changed, 1 deletion(-)

diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index fb57407e9d859..ebc0532ecf461 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -1002,7 +1002,6 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
         // Check lat. acceleration constraints
 
         std::vector<PredictedPath> predicted_paths;
-        int count = 0;
         for (const auto & ref_path : ref_paths) {
           PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
             yaw_fixed_transformed_object, ref_path.path);

From b2a4686b3b3680e4b8d76975f86f8b7cac83c062 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 26 Dec 2023 13:51:34 +0900
Subject: [PATCH 10/15] refactor rename function

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
 .../map_based_prediction/map_based_prediction_node.hpp      | 2 +-
 .../map_based_prediction/src/map_based_prediction_node.cpp  | 6 ++----
 2 files changed, 3 insertions(+), 5 deletions(-)

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index df902610a4484..dcc86556ff533 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -316,7 +316,7 @@ class MapBasedPredictionNode : public rclcpp::Node
     return out_trajectory;
   };
 
-  inline bool isAccelerationConstraintsSatisfied(
+  inline bool isLateralAccelerationConstraintSatisfied(
     const TrajectoryPoints & trajectory [[maybe_unused]], const double delta_time)
   {
     if (trajectory.size() < 3) return true;
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index ebc0532ecf461..520f089dbba9f 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -999,8 +999,6 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
           replaceObjectYawWithLaneletsYaw(current_lanelets, yaw_fixed_transformed_object);
         }
         // Generate Predicted Path
-        // Check lat. acceleration constraints
-
         std::vector<PredictedPath> predicted_paths;
         for (const auto & ref_path : ref_paths) {
           PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
@@ -1008,10 +1006,10 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
           if (predicted_path.path.empty()) {
             continue;
           }
-
+          // Check lat. acceleration constraints
           const auto trajectory_with_const_velocity =
             toTrajectoryPoints(predicted_path, abs_obj_speed);
-          if (!isAccelerationConstraintsSatisfied(
+          if (!isLateralAccelerationConstraintSatisfied(
                 trajectory_with_const_velocity, prediction_sampling_time_interval_)) {
             continue;
           }

From 25f6fc3637ad10d7ad72d82d4c727b3965ebce0f Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 26 Dec 2023 14:17:25 +0900
Subject: [PATCH 11/15] keep a copy of the straightest path in case all paths
 are cleared

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
 .../src/map_based_prediction_node.cpp         | 25 ++++++++++++++++---
 1 file changed, 22 insertions(+), 3 deletions(-)

diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 520f089dbba9f..cbe920d9ce52b 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -1000,24 +1000,43 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
         }
         // Generate Predicted Path
         std::vector<PredictedPath> predicted_paths;
+        double min_avg_curvature = std::numeric_limits<double>::max();
+        PredictedPath path_with_smallest_avg_curvature;
+
         for (const auto & ref_path : ref_paths) {
           PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
             yaw_fixed_transformed_object, ref_path.path);
           if (predicted_path.path.empty()) {
             continue;
           }
+          predicted_path.confidence = ref_path.probability;
+
           // Check lat. acceleration constraints
           const auto trajectory_with_const_velocity =
             toTrajectoryPoints(predicted_path, abs_obj_speed);
+
           if (!isLateralAccelerationConstraintSatisfied(
                 trajectory_with_const_velocity, prediction_sampling_time_interval_)) {
+            constexpr double curvature_calculation_distance = 2.0;
+            constexpr double points_interval = 1.0;
+            const size_t idx_dist = static_cast<size_t>(
+              std::max(static_cast<int>((curvature_calculation_distance) / points_interval), 1));
+            // Calculate curvature assuming the trajectory points interval is constant
+            const auto curvature_v =
+              calcTrajectoryCurvatureFrom3Points(trajectory_with_const_velocity, idx_dist);
+            if (curvature_v.empty()) continue;
+            const auto curvature_avg =
+              std::accumulate(curvature_v.begin(), curvature_v.end(), 0.0) / curvature_v.size();
+            if (curvature_avg < min_avg_curvature) {
+              // In case all paths are deleted, a copy of the straightest path is kept
+              min_avg_curvature = curvature_avg;
+              path_with_smallest_avg_curvature = predicted_path;
+            }
             continue;
           }
-
-          predicted_path.confidence = ref_path.probability;
           predicted_paths.push_back(predicted_path);
         }
-
+        if (predicted_paths.empty()) predicted_paths.push_back(path_with_smallest_avg_curvature);
         // Normalize Path Confidence and output the predicted object
 
         float sum_confidence = 0.0;

From e6190ffae4164021270af4f8c8f659a5bed37449 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 26 Dec 2023 16:14:31 +0900
Subject: [PATCH 12/15] refactor and add constraints check parameter

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
 .../map_based_prediction_node.hpp             |  3 +-
 .../src/map_based_prediction_node.cpp         | 57 +++++++++++--------
 2 files changed, 33 insertions(+), 27 deletions(-)

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index dcc86556ff533..3dea54f66e094 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -171,9 +171,8 @@ class MapBasedPredictionNode : public rclcpp::Node
   bool consider_only_routable_neighbours_;
   double reference_path_resolution_;
 
+  bool check_lateral_acceleration_constraints_;
   double max_lateral_accel_;
-  double deceleration_distance_before_curve_;
-  double deceleration_distance_after_curve_;
   double min_acceleration_before_curve_;
 
   // Stop watch
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index cbe920d9ce52b..e671277b1ff80 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -741,11 +741,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   object_buffer_time_length_ = declare_parameter<double>("object_buffer_time_length");
   history_time_length_ = declare_parameter<double>("history_time_length");
 
+  check_lateral_acceleration_constraints_ =
+    declare_parameter<bool>("check_lateral_acceleration_constraints");
   max_lateral_accel_ = declare_parameter<double>("max_lateral_accel");
-  deceleration_distance_before_curve_ =
-    declare_parameter<double>("deceleration_distance_before_curve");
-  deceleration_distance_after_curve_ =
-    declare_parameter<double>("deceleration_distance_after_curve");
   min_acceleration_before_curve_ = declare_parameter<double>("min_acceleration_before_curve");
 
   {  // lane change detection
@@ -808,10 +806,9 @@ rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam(
   using tier4_autoware_utils::updateParam;
 
   updateParam(parameters, "max_lateral_accel", max_lateral_accel_);
-  updateParam(
-    parameters, "deceleration_distance_before_curve", deceleration_distance_before_curve_);
-  updateParam(parameters, "deceleration_distance_after_curve", deceleration_distance_after_curve_);
   updateParam(parameters, "min_acceleration_before_curve", min_acceleration_before_curve_);
+  updateParam(
+    parameters, "check_lateral_acceleration_constraints", check_lateral_acceleration_constraints_);
 
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
@@ -1009,33 +1006,43 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
           if (predicted_path.path.empty()) {
             continue;
           }
-          predicted_path.confidence = ref_path.probability;
+
+          if (!check_lateral_acceleration_constraints_) {
+            predicted_path.confidence = ref_path.probability;
+            predicted_paths.push_back(predicted_path);
+            continue;
+          }
 
           // Check lat. acceleration constraints
           const auto trajectory_with_const_velocity =
             toTrajectoryPoints(predicted_path, abs_obj_speed);
 
-          if (!isLateralAccelerationConstraintSatisfied(
+          if (isLateralAccelerationConstraintSatisfied(
                 trajectory_with_const_velocity, prediction_sampling_time_interval_)) {
-            constexpr double curvature_calculation_distance = 2.0;
-            constexpr double points_interval = 1.0;
-            const size_t idx_dist = static_cast<size_t>(
-              std::max(static_cast<int>((curvature_calculation_distance) / points_interval), 1));
-            // Calculate curvature assuming the trajectory points interval is constant
-            const auto curvature_v =
-              calcTrajectoryCurvatureFrom3Points(trajectory_with_const_velocity, idx_dist);
-            if (curvature_v.empty()) continue;
-            const auto curvature_avg =
-              std::accumulate(curvature_v.begin(), curvature_v.end(), 0.0) / curvature_v.size();
-            if (curvature_avg < min_avg_curvature) {
-              // In case all paths are deleted, a copy of the straightest path is kept
-              min_avg_curvature = curvature_avg;
-              path_with_smallest_avg_curvature = predicted_path;
-            }
+            predicted_path.confidence = ref_path.probability;
+            predicted_paths.push_back(predicted_path);
             continue;
           }
-          predicted_paths.push_back(predicted_path);
+
+          // Calculate curvature assuming the trajectory points interval is constant
+          // In case all paths are deleted, a copy of the straightest path is kept
+
+          constexpr double curvature_calculation_distance = 2.0;
+          constexpr double points_interval = 1.0;
+          const size_t idx_dist = static_cast<size_t>(
+            std::max(static_cast<int>((curvature_calculation_distance) / points_interval), 1));
+          const auto curvature_v =
+            calcTrajectoryCurvatureFrom3Points(trajectory_with_const_velocity, idx_dist);
+          if (curvature_v.empty()) continue;
+          const auto curvature_avg =
+            std::accumulate(curvature_v.begin(), curvature_v.end(), 0.0) / curvature_v.size();
+          if (curvature_avg < min_avg_curvature) {
+            min_avg_curvature = curvature_avg;
+            path_with_smallest_avg_curvature = predicted_path;
+            path_with_smallest_avg_curvature.confidence = ref_path.probability;
+          }
         }
+
         if (predicted_paths.empty()) predicted_paths.push_back(path_with_smallest_avg_curvature);
         // Normalize Path Confidence and output the predicted object
 

From 9aa79abdd810ed6ba849f8f0910c293e464550ed Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 26 Dec 2023 17:00:18 +0900
Subject: [PATCH 13/15] update documentation

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
 perception/map_based_prediction/README.md     | 20 ++++++++++++++++++-
 .../config/map_based_prediction.param.yaml    |  3 +++
 2 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md
index 6d3d7f5e035a9..ec2f57963332f 100644
--- a/perception/map_based_prediction/README.md
+++ b/perception/map_based_prediction/README.md
@@ -66,7 +66,7 @@ Lane change logics is illustrated in the figure below.An example of how to tune
 
 ### Tuning lane change detection logic
 
-Currently we provide two parameters to tune lane change detection:
+Currently we provide three parameters to tune lane change detection:
 
 - `dist_threshold_to_bound_`: maximum distance from lane boundary allowed for lane changing vehicle
 - `time_threshold_to_bound_`: maximum time allowed for lane change vehicle to reach the boundary
@@ -124,6 +124,24 @@ See paper [2] for more details.
 
 `lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.)
 
+#### Pruning predicted paths with lateral acceleration constraint
+
+It is possible to apply a maximum lateral acceleration constraint to generated vehicle paths. This check verifies if it is possible for the vehicle to perform the predicted path without surpassing a lateral acceleration threshold `max_lateral_accel` when taking a curve. If it is not possible, it checks if the vehicle can slow down on time to take the curve with a deceleration of `min_acceleration_before_curve` and comply with the constraint. If that is also not possible, the path is eliminated.
+
+Currently we provide three parameters to tune the lateral acceleration constraint:
+
+- `check_lateral_acceleration_constraints_`: to enable the constraint check.
+- `max_lateral_accel_`: max acceptable lateral acceleration for predicted paths (absolute value).
+- `min_acceleration_before_curve_`: the minimum acceleration the vehicle would theoretically use to slow down before a curve is taken (must be negative).
+
+You can change these parameters in rosparam in the table below.
+
+| param name                                | default value  |
+| ----------------------------------------- | -------------- |
+| `check_lateral_acceleration_constraints_` | `false` [bool] |
+| `max_lateral_accel`                       | `2.0` [m/s^2]  |
+| `min_acceleration_before_curve`           | `-2.0` [m/s^2] |
+
 ### Path prediction for crosswalk users
 
 This module treats **Pedestrians** and **Bicycles** as objects using the crosswalk, and outputs prediction path based on map and estimated object's velocity, assuming the object has intention to cross the crosswalk, if the objects satisfies at least one of the following conditions:
diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml
index 69ff96a263f4b..fe4d2a51ec6f3 100644
--- a/perception/map_based_prediction/config/map_based_prediction.param.yaml
+++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml
@@ -13,6 +13,9 @@
     sigma_yaw_angle_deg: 5.0 #[angle degree]
     object_buffer_time_length: 2.0 #[s]
     history_time_length: 1.0 #[s]
+    check_lateral_acceleration_constraints: false # whether to check if the predicted path complies with lateral acceleration constraints
+    max_lateral_accel: 2.0 # [m/ss] max acceptable lateral acceleration for predicted vehicle paths
+    min_acceleration_before_curve: -2.0 # [m/ss] min acceleration a vehicle might take to decelerate before a curve
     # parameter for shoulder lane prediction
     prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8
 

From 1f8d535ad909c161477ea0077dd25555256b6a98 Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Tue, 26 Dec 2023 17:42:53 +0900
Subject: [PATCH 14/15] add braces for readability (style guide)

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
 .../map_based_prediction/src/map_based_prediction_node.cpp    | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index e671277b1ff80..28ef17cea4705 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -1033,7 +1033,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
             std::max(static_cast<int>((curvature_calculation_distance) / points_interval), 1));
           const auto curvature_v =
             calcTrajectoryCurvatureFrom3Points(trajectory_with_const_velocity, idx_dist);
-          if (curvature_v.empty()) continue;
+          if (curvature_v.empty()) {
+            continue;
+          }
           const auto curvature_avg =
             std::accumulate(curvature_v.begin(), curvature_v.end(), 0.0) / curvature_v.size();
           if (curvature_avg < min_avg_curvature) {

From 1eae7141b0a272c4c6009bf8903c24eb353a60de Mon Sep 17 00:00:00 2001
From: Daniel Sanchez <daniel.sanchez@tier4.jp>
Date: Wed, 27 Dec 2023 10:43:28 +0900
Subject: [PATCH 15/15] fix review issues

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
---
 .../map_based_prediction_node.hpp                  | 14 +++++++-------
 .../src/map_based_prediction_node.cpp              |  4 +---
 2 files changed, 8 insertions(+), 10 deletions(-)

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index 3dea54f66e094..02ca62a61717c 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -253,6 +253,8 @@ class MapBasedPredictionNode : public rclcpp::Node
     const TrackedObject & object, const LaneletData & current_lanelet_data,
     const double object_detected_time);
 
+  // NOTE: This function is copied from the motion_velocity_smoother package.
+  // TODO(someone): Consolidate functions and move them to a common
   inline std::vector<double> calcTrajectoryCurvatureFrom3Points(
     const TrajectoryPoints & trajectory, size_t idx_dist)
   {
@@ -284,9 +286,7 @@ class MapBasedPredictionNode : public rclcpp::Node
         curvature = calcCurvature(p0, p1, p2);
       } catch (std::exception const & e) {
         // ...code that handles the error...
-        RCLCPP_WARN(
-          rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "%s",
-          e.what());
+        RCLCPP_WARN(rclcpp::get_logger("map_based_prediction"), "%s", e.what());
         if (i > 1) {
           curvature = k_arr.at(i - 1);  // previous curvature
         } else {
@@ -337,10 +337,10 @@ class MapBasedPredictionNode : public rclcpp::Node
       tf2::convert(next_pose.orientation, q_next);
       double delta_theta = q_current.angleShortestPath(q_next);
       // Handle wrap-around
-      if (delta_theta > 3.14159) {
-        delta_theta -= 2.0 * 3.14159;
-      } else if (delta_theta < -3.14159) {
-        delta_theta += 2.0 * 3.14159;
+      if (delta_theta > M_PI) {
+        delta_theta -= 2.0 * M_PI;
+      } else if (delta_theta < -M_PI) {
+        delta_theta += 2.0 * M_PI;
       }
 
       const double yaw_rate = std::max(delta_theta / delta_time, 1.0E-5);
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 28ef17cea4705..0091fc6bc38cb 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -1003,9 +1003,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
         for (const auto & ref_path : ref_paths) {
           PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(
             yaw_fixed_transformed_object, ref_path.path);
-          if (predicted_path.path.empty()) {
-            continue;
-          }
+          if (predicted_path.path.empty()) continue;
 
           if (!check_lateral_acceleration_constraints_) {
             predicted_path.confidence = ref_path.probability;