From 1b3d855f9dc31e4f45959e0d38918b239f606791 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 3 Oct 2023 06:57:08 +0900 Subject: [PATCH] fix(map_based_prediction): filter by distance for opposite lanes as well (#5195) * fix(map_based_prediction): filter by distance for opposite lanes as well Signed-off-by: kminoda * update Signed-off-by: kminoda --------- Signed-off-by: kminoda --- .../map_based_prediction/map_based_prediction_node.hpp | 3 +-- .../map_based_prediction/src/map_based_prediction_node.cpp | 7 +++---- 2 files changed, 4 insertions(+), 6 deletions(-) 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 7364e3d11b650..ff0d2a54beaef 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 @@ -184,8 +184,7 @@ class MapBasedPredictionNode : public rclcpp::Node LaneletsData getCurrentLanelets(const TrackedObject & object); bool checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const bool check_distance = true); + const std::pair & lanelet, const TrackedObject & object); float calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const; void updateObjectData(TrackedObject & object); 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 259fbae1cef91..995ce605872b7 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -1240,7 +1240,7 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob for (const auto & lanelet : surrounding_opposite_lanelets) { // Check if the close lanelets meet the necessary condition for start lanelets // except for distance checking - if (!checkCloseLaneletCondition(lanelet, object, false)) { + if (!checkCloseLaneletCondition(lanelet, object)) { continue; } @@ -1260,8 +1260,7 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob } bool MapBasedPredictionNode::checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const bool check_distance) + const std::pair & lanelet, const TrackedObject & object) { // Step1. If we only have one point in the centerline, we will ignore the lanelet if (lanelet.second.centerline().size() <= 1) { @@ -1296,7 +1295,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; const bool is_yaw_reversed = M_PI - delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta && object_vel < 0.0; - if (check_distance && dist_threshold_for_searching_lanelet_ < lanelet.first) { + if (dist_threshold_for_searching_lanelet_ < lanelet.first) { return false; } if (!is_yaw_reversed && delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta) {