feat(map_based_prediction): prediction with acc constraints (#5960)

* WIP add acc constraints to discard paths

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* Add lat acc constraints to predicted paths

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* Delete debug print

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* delete unused var

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* delete unused var

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* WIP

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* make lat acc calculation with yaw rate

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* eliminate debug prints

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* delete unused variable

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* refactor rename function

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* keep a copy of the straightest path in case all paths are cleared

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* refactor and add constraints check parameter

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* update documentation

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* add braces for readability (style guide)

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

* fix review issues

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>

---------

Signed-off-by: Daniel Sanchez <daniel.sanchez@tier4.jp>
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

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:

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
 

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -16,11 +16,15 @@
 #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"
 
 #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 +38,7 @@
 #include <lanelet2_routing/Forward.h>
 #include <lanelet2_traffic_rules/TrafficRules.h>
 
+#include <algorithm>
 #include <deque>
 #include <memory>
 #include <string>
@@ -99,9 +104,10 @@ 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>;
 class MapBasedPredictionNode : public rclcpp::Node
 {
 public:
@@ -123,6 +129,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};
 
@@ -160,6 +171,10 @@ class MapBasedPredictionNode : public rclcpp::Node
   bool consider_only_routable_neighbours_;
   double reference_path_resolution_;
 
+  bool check_lateral_acceleration_constraints_;
+  double max_lateral_accel_;
+  double min_acceleration_before_curve_;
+
   // Stop watch
   StopWatch<std::chrono::milliseconds> stop_watch_;
 
@@ -237,6 +252,114 @@ class MapBasedPredictionNode : public rclcpp::Node
   Maneuver predictObjectManeuverByLatDiffDistance(
     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)
+  {
+    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("map_based_prediction"), "%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 isLateralAccelerationConstraintSatisfied(
+    const TrajectoryPoints & trajectory [[maybe_unused]], const double delta_time)
+  {
+    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 = 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 > 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);
+
+      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);
+
+      if (distance_to_slow_down > arc_length) return false;
+    }
+    return true;
+  };
 };
 }  // namespace map_based_prediction
 

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -740,6 +740,12 @@ 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");
+
+  check_lateral_acceleration_constraints_ =
+    declare_parameter<bool>("check_lateral_acceleration_constraints");
+  max_lateral_accel_ = declare_parameter<double>("max_lateral_accel");
+  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");
 
@@ -789,6 +795,25 @@ 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, "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;
+  result.reason = "success";
+  return result;
 }
 
 PredictedObjectKinematics MapBasedPredictionNode::convertToPredictedKinematics(
@@ -972,16 +997,53 @@ 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()) {
+          if (predicted_path.path.empty()) continue;
+
+          if (!check_lateral_acceleration_constraints_) {
+            predicted_path.confidence = ref_path.probability;
+            predicted_paths.push_back(predicted_path);
             continue;
           }
-          predicted_path.confidence = ref_path.probability;
-          predicted_paths.push_back(predicted_path);
+
+          // 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_)) {
+            predicted_path.confidence = ref_path.probability;
+            predicted_paths.push_back(predicted_path);
+            continue;
+          }
+
+          // 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
 
         float sum_confidence = 0.0;