feat(map_based_prediction): enable prediction to/from shoulder lanelet (lanelet not in routing graph) (#4353)

* enable prediction to adjacent lanelet

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* add function to get unconnected lanelet in graph

Signed-off-by: Yoshi, Ri <yoshiyoshidetteiu@gmail.com>

* add function to generate possiblepath from isolated lanelets

Signed-off-by: Yoshi, Ri <yoshiyoshidetteiu@gmail.com>

* precommit fix

Signed-off-by: Yoshi, Ri <yoshiyoshidetteiu@gmail.com>

* fix typo and unnecessary descriptions

Signed-off-by: Yoshi, Ri <yoshiyoshidetteiu@gmail.com>

* use lambda function to define lanelet path extractor

Signed-off-by: Yoshi, Ri <yoshiyoshidetteiu@gmail.com>

* add lambda function to get left/right lanelet

Signed-off-by: Yoshi, Ri <yoshiyoshidetteiu@gmail.com>

---------

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
Signed-off-by: Yoshi, Ri <yoshiyoshidetteiu@gmail.com>

Author: YoshiRi

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -139,6 +139,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   // Parameters
   bool enable_delay_compensation_;
   double prediction_time_horizon_;
+  double prediction_time_horizon_rate_for_validate_lane_length_;
   double prediction_sampling_time_interval_;
   double min_velocity_for_map_based_prediction_;
   double min_crosswalk_user_velocity_;

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -34,6 +34,7 @@
 #include <algorithm>
 #include <chrono>
 #include <cmath>
+#include <functional>
 #include <limits>
 
 namespace map_based_prediction
@@ -207,6 +208,114 @@ double calcAbsYawDiffBetweenLaneletAndObject(
   return abs_norm_delta;
 }
 
+/**
+ * @brief Get the Right LineSharing Lanelets object
+ *
+ * @param current_lanelet
+ * @param lanelet_map_ptr
+ * @return lanelet::ConstLanelets
+ */
+lanelet::ConstLanelets getRightLineSharingLanelets(
+  const lanelet::ConstLanelet & current_lanelet, const lanelet::LaneletMapPtr & lanelet_map_ptr)
+{
+  lanelet::ConstLanelets
+    output_lanelets;  // create an empty container of type lanelet::ConstLanelets
+
+  // step1: look for lane sharing current right bound
+  lanelet::Lanelets right_lane_candidates =
+    lanelet_map_ptr->laneletLayer.findUsages(current_lanelet.rightBound());
+  for (auto & candidate : right_lane_candidates) {
+    // exclude self lanelet
+    if (candidate == current_lanelet) continue;
+    // if candidate has linestring as leftbound, assign it to output
+    if (candidate.leftBound() == current_lanelet.rightBound()) {
+      output_lanelets.push_back(candidate);
+    }
+  }
+  return output_lanelets;  // return empty
+}
+
+/**
+ * @brief Get the Left LineSharing Lanelets object
+ *
+ * @param current_lanelet
+ * @param lanelet_map_ptr
+ * @return lanelet::ConstLanelets
+ */
+lanelet::ConstLanelets getLeftLineSharingLanelets(
+  const lanelet::ConstLanelet & current_lanelet, const lanelet::LaneletMapPtr & lanelet_map_ptr)
+{
+  lanelet::ConstLanelets
+    output_lanelets;  // create an empty container of type lanelet::ConstLanelets
+
+  // step1: look for lane sharing current left bound
+  lanelet::Lanelets left_lane_candidates =
+    lanelet_map_ptr->laneletLayer.findUsages(current_lanelet.leftBound());
+  for (auto & candidate : left_lane_candidates) {
+    // exclude self lanelet
+    if (candidate == current_lanelet) continue;
+    // if candidate has linestring as rightbound, assign it to output
+    if (candidate.rightBound() == current_lanelet.leftBound()) {
+      output_lanelets.push_back(candidate);
+    }
+  }
+  return output_lanelets;  // return empty
+}
+
+/**
+ * @brief Check if the lanelet is isolated in routing graph
+ * @param current_lanelet
+ * @param lanelet_map_ptr
+ */
+bool isIsolatedLanelet(
+  const lanelet::ConstLanelet & lanelet, lanelet::routing::RoutingGraphPtr & graph)
+{
+  const auto & following_lanelets = graph->following(lanelet);
+  const auto & left_lanelets = graph->lefts(lanelet);
+  const auto & right_lanelets = graph->rights(lanelet);
+  return left_lanelets.empty() && right_lanelets.empty() && following_lanelets.empty();
+}
+
+/**
+ * @brief Get the Possible Paths For Isolated Lanelet object
+ * @param lanelet
+ * @return lanelet::routing::LaneletPaths
+ */
+lanelet::routing::LaneletPaths getPossiblePathsForIsolatedLanelet(
+  const lanelet::ConstLanelet & lanelet)
+{
+  lanelet::ConstLanelets possible_lanelets;
+  possible_lanelets.push_back(lanelet);
+  lanelet::routing::LaneletPaths possible_paths;
+  // need to init path with constlanelets
+  lanelet::routing::LaneletPath possible_path(possible_lanelets);
+  possible_paths.push_back(possible_path);
+  return possible_paths;
+}
+
+/**
+ * @brief validate isolated lanelet length has enough length for prediction
+ * @param lanelet
+ * @param object: object information for calc length threshold
+ * @param prediction_time: time horizon[s] for calc length threshold
+ * @return bool
+ */
+bool validateIsolatedLaneletLength(
+  const lanelet::ConstLanelet & lanelet, const TrackedObject & object, const double prediction_time)
+{
+  // get closest center line point to object
+  const auto & center_line = lanelet.centerline2d();
+  const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
+  const lanelet::BasicPoint2d obj_point(obj_pos.x, obj_pos.y);
+  // get end point of the center line
+  const auto & end_point = center_line.back();
+  // calc approx distance between closest point and end point
+  const double approx_distance = lanelet::geometry::distance2d(obj_point, end_point);
+  const double min_length =
+    object.kinematics.twist_with_covariance.twist.linear.x * prediction_time;
+  return approx_distance > min_length;
+}
+
 lanelet::ConstLanelets getLanelets(const map_based_prediction::LaneletsData & data)
 {
   lanelet::ConstLanelets lanelets;
@@ -522,6 +631,10 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
       declare_parameter<int>("lane_change_detection.num_continuous_state_transition");
   }
   reference_path_resolution_ = declare_parameter("reference_path_resolution", 0.5);
+  /* prediction path will disabled when the estimated path length exceeds lanelet length. This
+   * parameter control the estimated path length = vx * th * (rate)  */
+  prediction_time_horizon_rate_for_validate_lane_length_ =
+    declare_parameter("prediction_time_horizon_rate_for_validate_lane_length", 0.8);
 
   path_generator_ = std::make_shared<PathGenerator>(
     prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_);
@@ -1119,25 +1232,69 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
     const double search_dist = prediction_time_horizon_ * obj_vel +
                                lanelet::utils::getLaneletLength3d(current_lanelet_data.lanelet);
     lanelet::routing::PossiblePathsParams possible_params{search_dist, {}, 0, false, true};
+    const double validate_time_horizon =
+      prediction_time_horizon_ * prediction_time_horizon_rate_for_validate_lane_length_;
+
+    // lambda function to get possible paths for isolated lanelet
+    // isolated is often caused by lanelet with no connection e.g. shoulder-lane
+    auto getPathsForNormalOrIsolatedLanelet = [&](const lanelet::ConstLanelet & lanelet) {
+      // if lanelet is not isolated, return normal possible paths
+      if (!isIsolatedLanelet(lanelet, routing_graph_ptr_)) {
+        return routing_graph_ptr_->possiblePaths(lanelet, possible_params);
+      }
+      // if lanelet is isolated, check if it has enough length
+      if (!validateIsolatedLaneletLength(lanelet, object, validate_time_horizon)) {
+        return lanelet::routing::LaneletPaths{};
+      } else {
+        // if lanelet has enough length, return possible paths
+        return getPossiblePathsForIsolatedLanelet(lanelet);
+      }
+    };
+
+    // lambda function to extract left/right lanelets
+    auto getLeftOrRightLanelets = [&](
+                                    const lanelet::ConstLanelet & lanelet,
+                                    const bool get_left) -> std::optional<lanelet::ConstLanelet> {
+      const auto opt =
+        get_left ? routing_graph_ptr_->left(lanelet) : routing_graph_ptr_->right(lanelet);
+      if (!!opt) {
+        return *opt;
+      }
+      const auto adjacent = get_left ? routing_graph_ptr_->adjacentLeft(lanelet)
+                                     : routing_graph_ptr_->adjacentRight(lanelet);
+      if (!!adjacent) {
+        return *adjacent;
+      }
+      // search for unconnected lanelet
+      const auto unconnected_lanelets = get_left
+                                          ? getLeftLineSharingLanelets(lanelet, lanelet_map_ptr_)
+                                          : getRightLineSharingLanelets(lanelet, lanelet_map_ptr_);
+      // just return first candidate of unconnected lanelet for now
+      if (!unconnected_lanelets.empty()) {
+        return unconnected_lanelets.front();
+      }
+      // if no candidate lanelet found, return empty
+      return std::nullopt;
+    };
 
     // Step1. Get the path
     // Step1.1 Get the left lanelet
     lanelet::routing::LaneletPaths left_paths;
-    auto opt_left = routing_graph_ptr_->left(current_lanelet_data.lanelet);
-    if (!!opt_left) {
-      left_paths = routing_graph_ptr_->possiblePaths(*opt_left, possible_params);
+    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
     lanelet::routing::LaneletPaths right_paths;
-    auto opt_right = routing_graph_ptr_->right(current_lanelet_data.lanelet);
-    if (!!opt_right) {
-      right_paths = routing_graph_ptr_->possiblePaths(*opt_right, possible_params);
+    const auto right_lanelet = getLeftOrRightLanelets(current_lanelet_data.lanelet, false);
+    if (!!right_lanelet) {
+      right_paths = getPathsForNormalOrIsolatedLanelet(right_lanelet.value());
     }
 
     // Step1.3 Get the centerline
     lanelet::routing::LaneletPaths center_paths =
-      routing_graph_ptr_->possiblePaths(current_lanelet_data.lanelet, possible_params);
+      getPathsForNormalOrIsolatedLanelet(current_lanelet_data.lanelet);
 
     // Skip calculations if all paths are empty
     if (left_paths.empty() && right_paths.empty() && center_paths.empty()) {