feat(map_based_prediction): consider only routable neighbours for lan…

…e change Signed-off-by: Mehmet Dogru <[email protected]>
mehmetdogru · Nov 24, 2023 · c90ab01 · c90ab01
1 parent 5f9608d
commit c90ab01
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 12 deletions.
diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml
@@ -29,5 +29,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]
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
@@ -157,6 +157,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   double diff_dist_threshold_to_left_bound_;
   double diff_dist_threshold_to_right_bound_;
   int num_continuous_state_transition_;
+  bool consider_only_routable_neighbours_;
   double reference_path_resolution_;
 
   // 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
@@ -760,6 +760,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
 
     num_continuous_state_transition_ =
       declare_parameter<int>("lane_change_detection.num_continuous_state_transition");
+
+    consider_only_routable_neighbours_ =
+      declare_parameter<bool>("lane_change_detection.consider_only_routable_neighbours");
   }
   reference_path_resolution_ = declare_parameter<double>("reference_path_resolution");
   /* prediction path will disabled when the estimated path length exceeds lanelet length. This
@@ -1514,19 +1517,22 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
       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 (!consider_only_routable_neighbours_) {
+        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;
     };