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 8ef6d628aa2f6..3d01ab96e9b62 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 @@ -176,8 +176,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 6a414c26b8806..58356f4f96ed6 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -1251,7 +1251,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; } @@ -1271,8 +1271,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) { @@ -1307,7 +1306,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) {