From c90ab0153bd8921dfe19c4f2ac4f6dda7f456610 Mon Sep 17 00:00:00 2001 From: Mehmet Dogru Date: Fri, 24 Nov 2023 15:20:08 +0900 Subject: [PATCH] feat(map_based_prediction): consider only routable neighbours for lane change Signed-off-by: Mehmet Dogru --- .../config/map_based_prediction.param.yaml | 1 + .../map_based_prediction_node.hpp | 1 + .../src/map_based_prediction_node.cpp | 30 +++++++++++-------- 3 files changed, 20 insertions(+), 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 index 260ae45da2e05..69ff96a263f4b 100644 --- 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 index 01f39057aef36..bdd9ad89bc025 100644 --- 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 index 2ceb22fd6e7b9..b2f1296cc85b9 100644 --- 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("lane_change_detection.num_continuous_state_transition"); + + consider_only_routable_neighbours_ = + declare_parameter("lane_change_detection.consider_only_routable_neighbours"); } reference_path_resolution_ = declare_parameter("reference_path_resolution"); /* prediction path will disabled when the estimated path length exceeds lanelet length. This @@ -1514,19 +1517,22 @@ std::vector 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; };