diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 02b11bed002..f11a2ddfaa3 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -86,7 +86,6 @@ HdMapUtils::HdMapUtils( // If route is not specified, the lanelet_id with the lowest array index is used as a candidate for // canonicalize destination. - auto HdMapUtils::countLaneChanges( const traffic_simulator_msgs::msg::LaneletPose & from, const traffic_simulator_msgs::msg::LaneletPose & to, @@ -102,20 +101,18 @@ auto HdMapUtils::countLaneChanges( for (std::size_t i = 1; i < route.size(); ++i) { const auto & previous = route[i - 1]; const auto & current = route[i]; - - if (auto followings = - lanelet_map::nextLaneletIds(previous, routing_configuration.routing_graph_type); - std::find(followings.begin(), followings.end(), current) == followings.end()) { - if (auto lefts = pose::leftLaneletIds( - previous, routing_configuration.routing_graph_type, include_opposite_direction); - std::find(lefts.begin(), lefts.end(), current) != lefts.end()) { - lane_changes.first++; - } else if (auto rights = pose::rightLaneletIds( - previous, routing_configuration.routing_graph_type, - include_opposite_direction); - std::find(rights.begin(), rights.end(), current) != rights.end()) { - lane_changes.second++; - } + if (const auto followings = lanelet_map::nextLaneletIds( + previous, routing_configuration.routing_graph_type, include_opposite_direction); + std::find(followings.begin(), followings.end(), current) != followings.end()) { + continue; + } else if (const auto lefts = pose::leftLaneletIds( + previous, routing_configuration.routing_graph_type, include_opposite_direction); + std::find(lefts.begin(), lefts.end(), current) != lefts.end()) { + lane_changes.first++; + } else if (const auto rights = pose::rightLaneletIds( + previous, routing_configuration.routing_graph_type, include_opposite_direction); + std::find(rights.begin(), rights.end(), current) != rights.end()) { + lane_changes.second++; } } return lane_changes;