From af8cd266f04df50579bd059b315945ae57fcf881 Mon Sep 17 00:00:00 2001 From: Lukasz Chojnacki Date: Tue, 10 Oct 2023 11:29:51 +0200 Subject: [PATCH 1/2] fix(action_node): check if is not the same right of way for two diffrent lanelets Signed-off-by: Lukasz Chojnacki --- simulation/behavior_tree_plugin/src/action_node.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 14b9e889ff5..a7a52c93ba0 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -130,6 +130,14 @@ auto ActionNode::getYieldStopDistance(const lanelet::Ids & following_lanelets) c auto ActionNode::getRightOfWayEntities(const lanelet::Ids & following_lanelets) const -> std::vector { + auto is_the_same_right_of_way = + [&](const std::int64_t & lanelet_id, const std::int64_t & following_lanelet) { + const auto right_of_way_lanelet_ids = hdmap_utils->getRightOfWayLaneletIds(lanelet_id); + const auto the_same_right_of_way_it = std::find( + right_of_way_lanelet_ids.begin(), right_of_way_lanelet_ids.end(), following_lanelet); + return the_same_right_of_way_it != std::end(right_of_way_lanelet_ids); + }; + std::vector ret; const auto lanelet_ids_list = hdmap_utils->getRightOfWayLaneletIds(following_lanelets); for (const auto & status : other_entity_status) { @@ -137,7 +145,8 @@ auto ActionNode::getRightOfWayEntities(const lanelet::Ids & following_lanelets) for (const lanelet::Id & lanelet_id : lanelet_ids_list.at(following_lanelet)) { if ( status.second.laneMatchingSucceed() && - traffic_simulator::isSameLaneletId(status.second, lanelet_id)) { + traffic_simulator::isSameLaneletId(status.second, lanelet_id) && + not is_the_same_right_of_way(lanelet_id, following_lanelet)) { ret.emplace_back(status.second); } } From 5b25abe8849e35a01bbb20698df82755673630e3 Mon Sep 17 00:00:00 2001 From: Lukasz Chojnacki Date: Tue, 10 Oct 2023 11:31:10 +0200 Subject: [PATCH 2/2] fix(action_node): calculate the backward distance in getYieldStopDistance Signed-off-by: Lukasz Chojnacki --- simulation/behavior_tree_plugin/src/action_node.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index a7a52c93ba0..427bb9ba930 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -113,10 +113,14 @@ auto ActionNode::getYieldStopDistance(const lanelet::Ids & following_lanelets) c const auto other_status = getOtherEntityStatus(right_of_way_id); if (!other_status.empty() && entity_status->laneMatchingSucceed()) { const auto lanelet_pose = entity_status->getLaneletPose(); - auto distance = hdmap_utils->getLongitudinalDistance( + const auto distance_forward = hdmap_utils->getLongitudinalDistance( lanelet_pose, traffic_simulator::helper::constructLaneletPose(lanelet, 0)); - if (distance) { - distances.insert(distance.value()); + const auto distance_backward = hdmap_utils->getLongitudinalDistance( + traffic_simulator::helper::constructLaneletPose(lanelet, 0), lanelet_pose); + if (distance_forward) { + distances.insert(distance_forward.value()); + } else if (distance_backward) { + distances.insert(-distance_backward.value()); } } }