feat(map_based_prediction): prediction with acc constraints

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
 
@@ -29,6 +32,6 @@
         diff_dist_threshold_to_left_bound: 0.29 #[m]
         diff_dist_threshold_to_right_bound: -0.29 #[m]
       num_continuous_state_transition: 3
       consider_only_routable_neighbours: false
 
     reference_path_resolution: 0.5 #[m]

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::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 @@
   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 @@
   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 @@
   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 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 > 3.14159) {
+        delta_theta -= 2.0 * 3.14159;
+      } else if (delta_theta < -3.14159) {
+        delta_theta += 2.0 * 3.14159;
+      }
+
+      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

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -740,6 +740,12 @@
   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 @@
   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,55 @@
         }
         // 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;
-          predicted_paths.push_back(predicted_path);
+
+          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(
+                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;