feat(autoware_map_based_prediction): improve frenet path generation (a…

…utowarefoundation#8602) * feat: calculate terminal d position based on playable width in path_generator.cpp Signed-off-by: Taekjin LEE <[email protected]> * feat: Add width parameter path generations refactor(path_generator): improve backlash width calculation Signed-off-by: Taekjin LEE <[email protected]> refactor(path_generator): improve backlash width calculation Signed-off-by: Taekjin LEE <[email protected]> * fix: set initial point of Frenet Path to Cartesian Path conversion Signed-off-by: Taekjin LEE <[email protected]> refactor: limit the d value to the radius for curved reference paths refactor: limit d value to curve limit for curved reference paths refactor: extend base_path_s with extrapolated base_path_x, base_path_y, base_path_z if min_s is negative refactor: linear path when object is moving backward feat: Update getFrenetPoint function to include target_path parameter The getFrenetPoint function in path_generator.hpp and path_generator.cpp has been updated to include a new parameter called target_path. This parameter is used to trim the reference path based on the starting segment index, allowing for more accurate calculations. * feat: Add interpolationLerp function for linear interpolation Signed-off-by: Taekjin LEE <[email protected]> * Update starting_segment_idx type in getFrenetPoint function Signed-off-by: Taekjin LEE <[email protected]> refactor: Update starting_segment_idx type in getFrenetPoint function refactor: Update getFrenetPoint function to include target_path parameter refactor: exclude target path determination logic from getFrenetPoint refactor: Add interpolationLerp function for quaternion linear interpolation refactor: remove redundant yaw height update refactor: Update path_generator.cpp to include object height in predicted_pose fix: comment out optimum target searcher * feat: implement a new optimization of target ref path search Signed-off-by: Taekjin LEE <[email protected]> refactor: Update path_generator.cpp to include object height in predicted_pose refactor: measure performance refactor: remove comment-outs, measure times style(pre-commit): autofix refactor: move starting point search function to getPredictedReferencePath refactor: target segment index search parameter adjust * fix: replace nearest search to custom one for efficiency Signed-off-by: Taekjin LEE <[email protected]> feat: Update CLOSE_LANELET_THRESHOLD and CLOSE_PATH_THRESHOLD values Signed-off-by: Taekjin LEE <[email protected]> * refactor: getFrenetPoint blocks Signed-off-by: Taekjin LEE <[email protected]> * chore: add comments Signed-off-by: Taekjin LEE <[email protected]> * feat: Trim reference paths if optimum position is not found Signed-off-by: Taekjin LEE <[email protected]> style(pre-commit): autofix chore: remove comment Signed-off-by: Taekjin LEE <[email protected]> * fix: shadowVariable of time keeper pointers Signed-off-by: Taekjin LEE <[email protected]> * refactor: improve backlash width calculation, parameter adjustment Signed-off-by: Taekjin LEE <[email protected]> * fix: cylinder type object do not have y dimension, use x dimension Signed-off-by: Taekjin LEE <[email protected]> * chore: add comment to explain an internal parameter 'margin' Signed-off-by: Taekjin LEE <[email protected]> * chore: add comment of backlash calculation shortcut Signed-off-by: Taekjin LEE <[email protected]> * chore: Improve readability of backlash to target shift model Signed-off-by: Taekjin LEE <[email protected]> * feat: set the return width by the path width Signed-off-by: Taekjin LEE <[email protected]> * refactor: separate a logic to searchProperStartingRefPathIndex function Signed-off-by: Taekjin LEE <[email protected]> * refactor: search starting ref path using optional for return type Signed-off-by: Taekjin LEE <[email protected]> --------- Signed-off-by: Taekjin LEE <[email protected]>
shtokuda · Sep 9, 2024 · 67265bb · 67265bb
1 parent 55d0905
commit 67265bb
Show file tree

Hide file tree

Showing 4 changed files with 384 additions and 84 deletions.
diff --git a/.../autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/.../autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -127,6 +127,7 @@ struct PredictedRefPath
 {
   float probability;
   double speed_limit;
+  double width;
   PosePath path;
   Maneuver maneuver;
 };
@@ -291,6 +292,8 @@ class MapBasedPredictionNode : public rclcpp::Node
     const std::string & object_id);
   std::string tryMatchNewObjectToDisappeared(
     const std::string & object_id, std::unordered_map<std::string, TrackedObject> & current_users);
+  std::optional<size_t> searchProperStartingRefPathIndex(
+    const TrackedObject & object, const PosePath & pose_path) const;
   std::vector<PredictedRefPath> getPredictedReferencePath(
     const TrackedObject & object, const LaneletsData & current_lanelets_data,
     const double object_detected_time, const double time_horizon);
@@ -315,9 +318,11 @@ class MapBasedPredictionNode : public rclcpp::Node
     const Maneuver & maneuver, std::vector<PredictedRefPath> & reference_paths,
     const double speed_limit = 0.0);
 
-  mutable universe_utils::LRUCache<lanelet::routing::LaneletPaths, std::vector<PosePath>>
+  mutable universe_utils::LRUCache<
+    lanelet::routing::LaneletPaths, std::vector<std::pair<PosePath, double>>>
     lru_cache_of_convert_path_type_{1000};
-  std::vector<PosePath> convertPathType(const lanelet::routing::LaneletPaths & paths) const;
+  std::vector<std::pair<PosePath, double>> convertPathType(
+    const lanelet::routing::LaneletPaths & paths) const;
 
   void updateFuturePossibleLanelets(
     const TrackedObject & object, const lanelet::routing::LaneletPaths & paths);

diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp
@@ -23,7 +23,11 @@
 #include <autoware_perception_msgs/msg/tracked_objects.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
 #include <geometry_msgs/msg/twist.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <tf2/LinearMath/Quaternion.h>
 
 #include <memory>
 #include <utility>
@@ -95,8 +99,9 @@ class PathGenerator
     const TrackedObject & object, const double duration) const;
 
   PredictedPath generatePathForOnLaneVehicle(
-    const TrackedObject & object, const PosePath & ref_paths, const double duration,
-    const double lateral_duration, const double speed_limit = 0.0) const;
+    const TrackedObject & object, const PosePath & ref_path, const double duration,
+    const double lateral_duration, const double path_width = 0.0,
+    const double speed_limit = 0.0) const;
 
   PredictedPath generatePathForCrosswalkUser(
     const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk,
@@ -129,7 +134,8 @@ class PathGenerator
 
   PredictedPath generatePolynomialPath(
     const TrackedObject & object, const PosePath & ref_path, const double duration,
-    const double lateral_duration, const double speed_limit = 0.0) const;
+    const double lateral_duration, const double path_width, const double backlash_width,
+    const double speed_limit = 0.0) const;
 
   FrenetPath generateFrenetPath(
     const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length,
@@ -139,6 +145,13 @@ class PathGenerator
   Eigen::Vector2d calcLonCoefficients(
     const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const;
 
+  std::vector<double> interpolationLerp(
+    const std::vector<double> & base_keys, const std::vector<double> & base_values,
+    const std::vector<double> & query_keys) const;
+  std::vector<tf2::Quaternion> interpolationLerp(
+    const std::vector<double> & base_keys, const std::vector<tf2::Quaternion> & base_values,
+    const std::vector<double> & query_keys) const;
+
   PosePath interpolateReferencePath(
     const PosePath & base_path, const FrenetPath & frenet_predicted_path) const;
 
@@ -147,7 +160,7 @@ class PathGenerator
     const PosePath & ref_path) const;
 
   FrenetPoint getFrenetPoint(
-    const TrackedObject & object, const PosePath & ref_path, const double duration,
+    const TrackedObject & object, const geometry_msgs::msg::Pose & ref_pose, const double duration,
     const double speed_limit = 0.0) const;
 };
 }  // namespace autoware::map_based_prediction

diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
@@ -23,6 +23,7 @@
 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>
 #include <autoware_lanelet2_extension/utility/query.hpp>
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
+#include <autoware_utils/autoware_utils.hpp>
 #include <interpolation/linear_interpolation.hpp>
 
 #include <autoware_perception_msgs/msg/detected_objects.hpp>
@@ -1114,7 +1115,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
 
         // If predicted reference path is empty, assume this object is out of the lane
         if (ref_paths.empty()) {
-          PredictedPath predicted_path = path_generator_->generatePathForLowSpeedVehicle(
+          PredictedPath predicted_path = path_generator_->generatePathForOffLaneVehicle(
             transformed_object, prediction_time_horizon_.vehicle);
           predicted_path.confidence = 1.0;
           if (predicted_path.path.empty()) break;
@@ -1153,7 +1154,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, prediction_time_horizon_.vehicle,
-            lateral_control_time_horizon_, ref_path.speed_limit);
+            lateral_control_time_horizon_, ref_path.width, ref_path.speed_limit);
           if (predicted_path.path.empty()) continue;
 
           if (!check_lateral_acceleration_constraints_) {
@@ -1844,13 +1845,108 @@ void MapBasedPredictionNode::updateRoadUsersHistory(
   }
 }
 
+std::optional<size_t> MapBasedPredictionNode::searchProperStartingRefPathIndex(
+  const TrackedObject & object, const PosePath & pose_path) const
+{
+  std::unique_ptr<ScopedTimeTrack> st1_ptr;
+  if (time_keeper_) st1_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
+  bool is_position_found = false;
+  std::optional<size_t> opt_index{std::nullopt};
+  auto & index = opt_index.emplace();
+
+  // starting segment index is a segment close enough to the object
+  const auto obj_point = object.kinematics.pose_with_covariance.pose.position;
+  {
+    std::unique_ptr<ScopedTimeTrack> st2_ptr;
+    if (time_keeper_)
+      st2_ptr = std::make_unique<ScopedTimeTrack>("find_close_segment_index", *time_keeper_);
+    double min_dist_sq = std::numeric_limits<double>::max();
+    constexpr double acceptable_dist_sq = 1.0;  // [m2]
+    for (size_t i = 0; i < pose_path.size(); i++) {
+      const double dx = pose_path.at(i).position.x - obj_point.x;
+      const double dy = pose_path.at(i).position.y - obj_point.y;
+      const double dist_sq = dx * dx + dy * dy;
+      if (dist_sq < min_dist_sq) {
+        min_dist_sq = dist_sq;
+        index = i;
+      }
+      if (dist_sq < acceptable_dist_sq) {
+        break;
+      }
+    }
+  }
+
+  // calculate score that object can reach the target path smoothly, and search the
+  // starting segment index that have the best score
+  size_t idx = 0;
+  {  // find target segmentation index
+    std::unique_ptr<ScopedTimeTrack> st3_ptr;
+    if (time_keeper_)
+      st3_ptr = std::make_unique<ScopedTimeTrack>("find_target_seg_index", *time_keeper_);
+
+    constexpr double search_distance = 22.0;       // [m]
+    constexpr double yaw_diff_limit = M_PI / 3.0;  // 60 degrees
+
+    const double obj_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
+    const size_t search_segment_count =
+      static_cast<size_t>(std::floor(search_distance / reference_path_resolution_));
+    const size_t search_segment_num =
+      std::min(search_segment_count, static_cast<size_t>(pose_path.size() - index));
+
+    // search for the best score, which is the smallest
+    double best_score = 1e9;  // initial value is large enough
+    for (size_t i = 0; i < search_segment_num; ++i) {
+      const auto & path_pose = pose_path.at(index + i);
+      // yaw difference
+      const double path_yaw = tf2::getYaw(path_pose.orientation);
+      const double relative_path_yaw = autoware_utils::normalize_radian(path_yaw - obj_yaw);
+      if (std::abs(relative_path_yaw) > yaw_diff_limit) {
+        continue;
+      }
+
+      const double dx = path_pose.position.x - obj_point.x;
+      const double dy = path_pose.position.y - obj_point.y;
+      const double dx_cp = std::cos(obj_yaw) * dx + std::sin(obj_yaw) * dy;
+      const double dy_cp = -std::sin(obj_yaw) * dx + std::cos(obj_yaw) * dy;
+      const double neutral_yaw = std::atan2(dy_cp, dx_cp) * 2.0;
+      const double delta_yaw = autoware_utils::normalize_radian(path_yaw - obj_yaw - neutral_yaw);
+      if (std::abs(delta_yaw) > yaw_diff_limit) {
+        continue;
+      }
+
+      // objective function score
+      constexpr double weight_ratio = 0.01;
+      double score = delta_yaw * delta_yaw + weight_ratio * neutral_yaw * neutral_yaw;
+      constexpr double acceptable_score = 1e-3;
+
+      if (score < best_score) {
+        best_score = score;
+        idx = i;
+        is_position_found = true;
+        if (score < acceptable_score) {
+          // if the score is small enough, we can break the loop
+          break;
+        }
+      }
+    }
+  }
+
+  // update starting segment index
+  index += idx;
+  index = std::clamp(index, 0ul, pose_path.size() - 1);
+
+  return is_position_found ? opt_index : std::nullopt;
+}
+
 std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
   const TrackedObject & object, const LaneletsData & current_lanelets_data,
   const double object_detected_time, const double time_horizon)
 {
   std::unique_ptr<ScopedTimeTrack> st_ptr;
   if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
+  // Step 1. Set conditions for the prediction
   const double obj_vel = std::hypot(
     object.kinematics.twist_with_covariance.twist.linear.x,
     object.kinematics.twist_with_covariance.twist.linear.y);
@@ -1891,9 +1987,10 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
     return search_dist;
   };
 
+  // Step 2. Get possible paths for each lanelet
   std::vector<PredictedRefPath> all_ref_paths;
-
   for (const auto & current_lanelet_data : current_lanelets_data) {
+    // Set condition on each lanelet
     const lanelet::traffic_rules::SpeedLimitInformation limit =
       traffic_rules_ptr_->speedLimit(current_lanelet_data.lanelet);
     const double legal_speed_limit = static_cast<double>(limit.speedLimit.value());
@@ -1959,22 +2056,21 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
       return std::nullopt;
     };
 
-    // Step1. Get the path
-    // Step1.1 Get the left lanelet
+    // a-1. Get the left lanelet
     lanelet::routing::LaneletPaths left_paths;
     const auto left_lanelet = getLeftOrRightLanelets(current_lanelet_data.lanelet, true);
     if (!!left_lanelet) {
       left_paths = getPathsForNormalOrIsolatedLanelet(left_lanelet.value());
     }
 
-    // Step1.2 Get the right lanelet
+    // a-2. Get the right lanelet
     lanelet::routing::LaneletPaths right_paths;
     const auto right_lanelet = getLeftOrRightLanelets(current_lanelet_data.lanelet, false);
     if (!!right_lanelet) {
       right_paths = getPathsForNormalOrIsolatedLanelet(right_lanelet.value());
     }
 
-    // Step1.3 Get the centerline
+    // a-3. Get the center lanelet
     lanelet::routing::LaneletPaths center_paths =
       getPathsForNormalOrIsolatedLanelet(current_lanelet_data.lanelet);
 
@@ -1983,15 +2079,15 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
       continue;
     }
 
-    // Step2. Predict Object Maneuver
+    // b. Predict Object Maneuver
     const Maneuver predicted_maneuver =
       predictObjectManeuver(object, current_lanelet_data, object_detected_time);
 
-    // Step3. Allocate probability for each predicted maneuver
+    // c. Allocate probability for each predicted maneuver
     const auto maneuver_prob =
       calculateManeuverProbability(predicted_maneuver, left_paths, right_paths, center_paths);
 
-    // Step4. add candidate reference paths to the all_ref_paths
+    // d. add candidate reference paths to the all_ref_paths
     const float path_prob = current_lanelet_data.probability;
     const auto addReferencePathsLocal = [&](const auto & paths, const auto & maneuver) {
       addReferencePaths(
@@ -2002,6 +2098,31 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
     addReferencePathsLocal(center_paths, Maneuver::LANE_FOLLOW);
   }
 
+  // Step 3. Search starting point for each reference path
+  for (auto it = all_ref_paths.begin(); it != all_ref_paths.end();) {
+    std::unique_ptr<ScopedTimeTrack> st1_ptr;
+    if (time_keeper_)
+      st1_ptr =
+        std::make_unique<ScopedTimeTrack>("searching_refpath_starting_point", *time_keeper_);
+
+    auto & pose_path = it->path;
+    if (pose_path.empty()) {
+      continue;
+    }
+
+    const std::optional<size_t> opt_starting_idx =
+      searchProperStartingRefPathIndex(object, pose_path);
+
+    if (opt_starting_idx.has_value()) {
+      // Trim the reference path
+      pose_path.erase(pose_path.begin(), pose_path.begin() + opt_starting_idx.value());
+      ++it;
+    } else {
+      // Proper starting point is not found, remove the reference path
+      it = all_ref_paths.erase(it);
+    }
+  }
+
   return all_ref_paths;
 }
 
@@ -2341,7 +2462,8 @@ void MapBasedPredictionNode::addReferencePaths(
     for (const auto & converted_path : converted_paths) {
       PredictedRefPath predicted_path;
       predicted_path.probability = maneuver_probability.at(maneuver) * path_probability;
-      predicted_path.path = converted_path;
+      predicted_path.path = converted_path.first;
+      predicted_path.width = converted_path.second;
       predicted_path.maneuver = maneuver;
       predicted_path.speed_limit = speed_limit;
       reference_paths.push_back(predicted_path);
@@ -2416,7 +2538,7 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability(
   return maneuver_prob;
 }
 
-std::vector<PosePath> MapBasedPredictionNode::convertPathType(
+std::vector<std::pair<PosePath, double>> MapBasedPredictionNode::convertPathType(
   const lanelet::routing::LaneletPaths & paths) const
 {
   std::unique_ptr<ScopedTimeTrack> st_ptr;
@@ -2426,9 +2548,10 @@ std::vector<PosePath> MapBasedPredictionNode::convertPathType(
     return *lru_cache_of_convert_path_type_.get(paths);
   }
 
-  std::vector<PosePath> converted_paths;
+  std::vector<std::pair<PosePath, double>> converted_paths;
   for (const auto & path : paths) {
     PosePath converted_path;
+    double width = 10.0;  // Initialize with a large value
 
     // Insert Positions. Note that we start inserting points from previous lanelet
     if (!path.empty()) {
@@ -2446,6 +2569,7 @@ std::vector<PosePath> MapBasedPredictionNode::convertPathType(
             continue;
           }
 
+          // only considers yaw of the lanelet
           const double lane_yaw = std::atan2(
             current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x);
           const double sin_yaw_half = std::sin(lane_yaw / 2.0);
@@ -2495,6 +2619,14 @@ std::vector<PosePath> MapBasedPredictionNode::convertPathType(
         converted_path.push_back(current_p);
         prev_p = current_p;
       }
+
+      // Update minimum width
+      const auto left_bound = lanelet.leftBound2d();
+      const auto right_bound = lanelet.rightBound2d();
+      const double lanelet_width_front = std::hypot(
+        left_bound.front().x() - right_bound.front().x(),
+        left_bound.front().y() - right_bound.front().y());
+      width = std::min(width, lanelet_width_front);
     }
 
     // Resample Path
@@ -2506,7 +2638,7 @@ std::vector<PosePath> MapBasedPredictionNode::convertPathType(
     // interpolation for xy
     const auto resampled_converted_path = autoware::motion_utils::resamplePoseVector(
       converted_path, reference_path_resolution_, use_akima_spline_for_xy, use_lerp_for_z);
-    converted_paths.push_back(resampled_converted_path);
+    converted_paths.push_back(std::make_pair(resampled_converted_path, width));
   }
 
   lru_cache_of_convert_path_type_.put(paths, converted_paths);