From d302390c55396ce840ede2ca26b87d1bcd598ed8 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Tue, 10 Dec 2024 21:47:09 +0100 Subject: [PATCH 01/18] RJD-1489 Changed calculating distance to front entity to boundingBoxRelativeLaneletPose longitudinal distance --- .../behavior_tree_plugin/action_node.hpp | 16 ++---- .../behavior_tree_plugin/src/action_node.cpp | 57 +++++++++++-------- .../follow_front_entity_action.cpp | 5 +- .../follow_lane_action.cpp | 2 +- .../stop_at_crossing_entity_action.cpp | 2 +- .../stop_at_stop_line_action.cpp | 2 +- 6 files changed, 41 insertions(+), 43 deletions(-) diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 4bcba6c41cc..36575674752 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -48,12 +48,10 @@ class ActionNode : public BT::ActionNodeBase auto getDistanceToConflictingEntity( const lanelet::Ids & route_lanelets, const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional; - auto getFrontEntityName(const math::geometry::CatmullRomSplineInterface & spline) const - -> std::optional; + auto getFrontEntityName() const -> std::optional; auto calculateStopDistance(const traffic_simulator_msgs::msg::DynamicConstraints &) const -> double; - auto getDistanceToFrontEntity(const math::geometry::CatmullRomSplineInterface & spline) const - -> std::optional; + auto getDistanceToFrontEntity() const -> std::optional; auto getDistanceToStopLine( const lanelet::Ids & route_lanelets, const std::vector & waypoints) const -> std::optional; @@ -98,10 +96,7 @@ class ActionNode : public BT::ActionNodeBase virtual auto getBlackBoardValues() -> void; auto getEntityStatus(const std::string & target_name) const -> const traffic_simulator::CanonicalizedEntityStatus &; - auto getDistanceToTargetEntityPolygon( - const math::geometry::CatmullRomSplineInterface & spline, const std::string target_name, - double width_extension_right = 0.0, double width_extension_left = 0.0, - double length_extension_front = 0.0, double length_extension_rear = 0.0) const + auto getDistanceToTargetEntityPolygon(const std::string target_name) const -> std::optional; auto setCanonicalizedEntityStatus(const traffic_simulator::EntityStatus & entity_status) -> void; @@ -129,10 +124,7 @@ class ActionNode : public BT::ActionNodeBase const math::geometry::CatmullRomSplineInterface & spline, const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional; auto getDistanceToTargetEntityPolygon( - const math::geometry::CatmullRomSplineInterface & spline, - const traffic_simulator::CanonicalizedEntityStatus & status, double width_extension_right = 0.0, - double width_extension_left = 0.0, double length_extension_front = 0.0, - double length_extension_rear = 0.0) const -> std::optional; + const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional; auto getConflictingEntityStatus(const lanelet::Ids & following_lanelets) const -> std::optional; auto getConflictingEntityStatusOnCrossWalk(const lanelet::Ids & route_lanelets) const diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index cbe86b1c649..e0880cab6f4 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -27,9 +27,11 @@ #include #include #include +#include #include #include #include +// #include namespace entity_behavior { @@ -232,23 +234,21 @@ auto ActionNode::getDistanceToStopLine( return hdmap_utils->getDistanceToStopLine(route_lanelets, waypoints); } -auto ActionNode::getDistanceToFrontEntity( - const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional +auto ActionNode::getDistanceToFrontEntity() const -> std::optional { - auto name = getFrontEntityName(spline); + auto name = getFrontEntityName(); if (!name) { return std::nullopt; } - return getDistanceToTargetEntityPolygon(spline, name.value()); + return getDistanceToTargetEntityPolygon(name.value()); } -auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterface & spline) const - -> std::optional +auto ActionNode::getFrontEntityName() const -> std::optional { std::vector distances; std::vector entities; for (const auto & each : other_entity_status) { - const auto distance = getDistanceToTargetEntityPolygon(spline, each.first); + const auto distance = getDistanceToTargetEntityPolygon(each.first); const auto quat = math::geometry::getRotation( canonicalized_entity_status->getMapPose().orientation, other_entity_status.at(each.first).getMapPose().orientation); @@ -296,32 +296,39 @@ auto ActionNode::getEntityStatus(const std::string & target_name) const } } -auto ActionNode::getDistanceToTargetEntityPolygon( - const math::geometry::CatmullRomSplineInterface & spline, const std::string target_name, - double width_extension_right, double width_extension_left, double length_extension_front, - double length_extension_rear) const -> std::optional +auto ActionNode::getDistanceToTargetEntityPolygon(const std::string target_name) const + -> std::optional { const auto & status = getEntityStatus(target_name); if (status.laneMatchingSucceed()) { - return getDistanceToTargetEntityPolygon( - spline, status, width_extension_right, width_extension_left, length_extension_front, - length_extension_rear); + return getDistanceToTargetEntityPolygon(status); } return std::nullopt; } auto ActionNode::getDistanceToTargetEntityPolygon( - const math::geometry::CatmullRomSplineInterface & spline, - const traffic_simulator::CanonicalizedEntityStatus & status, double width_extension_right, - double width_extension_left, double length_extension_front, double length_extension_rear) const - -> std::optional + const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional { - if (status.laneMatchingSucceed()) { - const auto polygon = math::geometry::transformPoints( - status.getMapPose(), math::geometry::getPointsFromBbox( - status.getBoundingBox(), width_extension_right, width_extension_left, - length_extension_front, length_extension_rear)); - return spline.getCollisionPointIn2D(polygon, false); + if ( + status.laneMatchingSucceed() and canonicalized_entity_status->getCanonicalizedLaneletPose() and + status.getCanonicalizedLaneletPose()) { + traffic_simulator::RoutingConfiguration routing_configuration; + routing_configuration.allow_lane_change = true; + + const auto relative_pose = traffic_simulator::pose::boundingBoxRelativeLaneletPose( + canonicalized_entity_status->getCanonicalizedLaneletPose().value(), + canonicalized_entity_status->getBoundingBox(), status.getCanonicalizedLaneletPose().value(), + status.getBoundingBox(), routing_configuration, hdmap_utils); + + auto distance_from_center = + relative_pose.s + (canonicalized_entity_status->getBoundingBox().dimensions.x / 2); + + if ( + (std::abs(relative_pose.offset) <= + (canonicalized_entity_status->getBoundingBox().dimensions.y / 2)) and + (relative_pose.s >= 0)) { + return distance_from_center; + } } return std::nullopt; } @@ -340,7 +347,7 @@ auto ActionNode::getDistanceToConflictingEntity( } } for (const auto & status : lane_entity_status) { - const auto s = getDistanceToTargetEntityPolygon(spline, status, 0.0, 0.0, 0.0, 1.0); + const auto s = getDistanceToTargetEntityPolygon(status); if (s) { distances.insert(s.value()); } diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp index 8c30fce024a..0383945542c 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp @@ -92,12 +92,11 @@ BT::NodeStatus FollowFrontEntityAction::tick() } auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); auto distance_to_conflicting_entity = getDistanceToConflictingEntity(route_lanelets, *trajectory); - const auto front_entity_name = getFrontEntityName(*trajectory); + const auto front_entity_name = getFrontEntityName(); if (!front_entity_name) { return BT::NodeStatus::FAILURE; } - distance_to_front_entity_ = - getDistanceToTargetEntityPolygon(*trajectory, front_entity_name.value()); + distance_to_front_entity_ = getDistanceToTargetEntityPolygon(front_entity_name.value()); if (!distance_to_front_entity_) { return BT::NodeStatus::FAILURE; } diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp index 1b70dd26599..42938d047c1 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp @@ -89,7 +89,7 @@ BT::NodeStatus FollowLaneAction::tick() if (trajectory == nullptr) { return BT::NodeStatus::FAILURE; } - auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory); + auto distance_to_front_entity = getDistanceToFrontEntity(); if (distance_to_front_entity) { if ( distance_to_front_entity.value() <= diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp index c85e6cdf622..158331e2ed5 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp @@ -113,7 +113,7 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() } distance_to_stop_target_ = getDistanceToConflictingEntity(route_lanelets, *trajectory); auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); - const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory); + const auto distance_to_front_entity = getDistanceToFrontEntity(); if (!distance_to_stop_target_) { in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp index 23011a89e47..5858fa0c7ba 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp @@ -110,7 +110,7 @@ BT::NodeStatus StopAtStopLineAction::tick() } distance_to_stopline_ = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); const auto distance_to_stop_target = getDistanceToConflictingEntity(route_lanelets, *trajectory); - const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory); + const auto distance_to_front_entity = getDistanceToFrontEntity(); if (!distance_to_stopline_) { stopped_ = false; return BT::NodeStatus::FAILURE; From 37e75df0a5ffbadecca51f9b135cca8fdbfeb638 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Wed, 11 Dec 2024 14:37:25 +0100 Subject: [PATCH 02/18] Removed unnecessary includes --- simulation/behavior_tree_plugin/src/action_node.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index e0880cab6f4..ddc562b060d 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -31,7 +31,6 @@ #include #include #include -// #include namespace entity_behavior { From a156c33edd0c0127e92e46a6e9d2064266f97a16 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Thu, 12 Dec 2024 09:38:38 +0100 Subject: [PATCH 03/18] Review stylistic changes --- .../behavior_tree_plugin/action_node.hpp | 7 ++- .../behavior_tree_plugin/src/action_node.cpp | 52 +++++++------------ .../follow_front_entity_action.cpp | 4 +- .../follow_lane_action.cpp | 2 +- 4 files changed, 26 insertions(+), 39 deletions(-) diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 36575674752..31d4784b343 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -96,8 +96,6 @@ class ActionNode : public BT::ActionNodeBase virtual auto getBlackBoardValues() -> void; auto getEntityStatus(const std::string & target_name) const -> const traffic_simulator::CanonicalizedEntityStatus &; - auto getDistanceToTargetEntityPolygon(const std::string target_name) const - -> std::optional; auto setCanonicalizedEntityStatus(const traffic_simulator::EntityStatus & entity_status) -> void; auto calculateUpdatedEntityStatus( @@ -119,12 +117,13 @@ class ActionNode : public BT::ActionNodeBase EntityStatusDict other_entity_status; lanelet::Ids route_lanelets; + auto getDistanceToTargetEntityPolygon( + const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional; + private: auto getDistanceToTargetEntityOnCrosswalk( const math::geometry::CatmullRomSplineInterface & spline, const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional; - auto getDistanceToTargetEntityPolygon( - const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional; auto getConflictingEntityStatus(const lanelet::Ids & following_lanelets) const -> std::optional; auto getConflictingEntityStatusOnCrossWalk(const lanelet::Ids & route_lanelets) const diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index ddc562b060d..5194aa61a6b 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -235,11 +235,11 @@ auto ActionNode::getDistanceToStopLine( auto ActionNode::getDistanceToFrontEntity() const -> std::optional { - auto name = getFrontEntityName(); - if (!name) { + if (const auto entity_name_opt = getFrontEntityName()) { + return getDistanceToTargetEntityPolygon(getEntityStatus(entity_name_opt.value())); + } else { return std::nullopt; } - return getDistanceToTargetEntityPolygon(name.value()); } auto ActionNode::getFrontEntityName() const -> std::optional @@ -247,7 +247,7 @@ auto ActionNode::getFrontEntityName() const -> std::optional std::vector distances; std::vector entities; for (const auto & each : other_entity_status) { - const auto distance = getDistanceToTargetEntityPolygon(each.first); + const auto distance = getDistanceToTargetEntityPolygon(getEntityStatus(each.first)); const auto quat = math::geometry::getRotation( canonicalized_entity_status->getMapPose().orientation, other_entity_status.at(each.first).getMapPose().orientation); @@ -295,38 +295,27 @@ auto ActionNode::getEntityStatus(const std::string & target_name) const } } -auto ActionNode::getDistanceToTargetEntityPolygon(const std::string target_name) const - -> std::optional -{ - const auto & status = getEntityStatus(target_name); - if (status.laneMatchingSucceed()) { - return getDistanceToTargetEntityPolygon(status); - } - return std::nullopt; -} - auto ActionNode::getDistanceToTargetEntityPolygon( const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional { - if ( - status.laneMatchingSucceed() and canonicalized_entity_status->getCanonicalizedLaneletPose() and - status.getCanonicalizedLaneletPose()) { + if (status.laneMatchingSucceed() and canonicalized_entity_status->laneMatchingSucceed()) { + /* boundingBoxRelativeLaneletPose requires routing_configuration, + * allow_lane_change = true gives shortes route possible + */ + traffic_simulator::RoutingConfiguration routing_configuration; routing_configuration.allow_lane_change = true; - const auto relative_pose = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - canonicalized_entity_status->getCanonicalizedLaneletPose().value(), - canonicalized_entity_status->getBoundingBox(), status.getCanonicalizedLaneletPose().value(), - status.getBoundingBox(), routing_configuration, hdmap_utils); - - auto distance_from_center = - relative_pose.s + (canonicalized_entity_status->getBoundingBox().dimensions.x / 2); + const auto & bounding_box = canonicalized_entity_status->getBoundingBox(); + const auto relative_lanelet_pose = traffic_simulator::pose::boundingBoxRelativeLaneletPose( + canonicalized_entity_status->getCanonicalizedLaneletPose().value(), bounding_box, + status.getCanonicalizedLaneletPose().value(), status.getBoundingBox(), routing_configuration, + hdmap_utils); - if ( - (std::abs(relative_pose.offset) <= - (canonicalized_entity_status->getBoundingBox().dimensions.y / 2)) and - (relative_pose.s >= 0)) { - return distance_from_center; + const auto half_width = bounding_box.dimensions.y / 2; + // is in front and is within considered width (lateral distance) + if (relative_lanelet_pose.s >= 0 and std::abs(relative_lanelet_pose.offset) <= half_width) { + return relative_lanelet_pose.s + bounding_box.dimensions.x / 2; } } return std::nullopt; @@ -346,9 +335,8 @@ auto ActionNode::getDistanceToConflictingEntity( } } for (const auto & status : lane_entity_status) { - const auto s = getDistanceToTargetEntityPolygon(status); - if (s) { - distances.insert(s.value()); + if (const auto distance_to_entity = getDistanceToTargetEntityPolygon(status)) { + distances.insert(distance_to_entity.value()); } } if (distances.empty()) { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp index 0383945542c..3e8be9df85f 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp @@ -96,7 +96,8 @@ BT::NodeStatus FollowFrontEntityAction::tick() if (!front_entity_name) { return BT::NodeStatus::FAILURE; } - distance_to_front_entity_ = getDistanceToTargetEntityPolygon(front_entity_name.value()); + const auto & front_entity_status = getEntityStatus(front_entity_name.value()); + distance_to_front_entity_ = getDistanceToTargetEntityPolygon(front_entity_status); if (!distance_to_front_entity_) { return BT::NodeStatus::FAILURE; } @@ -110,7 +111,6 @@ BT::NodeStatus FollowFrontEntityAction::tick() return BT::NodeStatus::FAILURE; } } - const auto & front_entity_status = getEntityStatus(front_entity_name.value()); if (!target_speed) { target_speed = hdmap_utils->getSpeedLimit(route_lanelets); } diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp index 42938d047c1..50b3a266bd4 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp @@ -89,7 +89,7 @@ BT::NodeStatus FollowLaneAction::tick() if (trajectory == nullptr) { return BT::NodeStatus::FAILURE; } - auto distance_to_front_entity = getDistanceToFrontEntity(); + const auto distance_to_front_entity = getDistanceToFrontEntity(); if (distance_to_front_entity) { if ( distance_to_front_entity.value() <= From c159a3860a009e0b2e147c5ed9976d27ce180908 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Thu, 12 Dec 2024 09:50:41 +0100 Subject: [PATCH 04/18] Comment description change --- simulation/behavior_tree_plugin/src/action_node.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 5194aa61a6b..d7ff2de88c1 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -247,7 +247,7 @@ auto ActionNode::getFrontEntityName() const -> std::optional std::vector distances; std::vector entities; for (const auto & each : other_entity_status) { - const auto distance = getDistanceToTargetEntityPolygon(getEntityStatus(each.first)); + const auto distance = getDistanceToTargetEntityPolygon(each.second); const auto quat = math::geometry::getRotation( canonicalized_entity_status->getMapPose().orientation, other_entity_status.at(each.first).getMapPose().orientation); @@ -299,8 +299,9 @@ auto ActionNode::getDistanceToTargetEntityPolygon( const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional { if (status.laneMatchingSucceed() and canonicalized_entity_status->laneMatchingSucceed()) { - /* boundingBoxRelativeLaneletPose requires routing_configuration, - * allow_lane_change = true gives shortes route possible + /* + * boundingBoxRelativeLaneletPose requires routing_configuration, + * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets */ traffic_simulator::RoutingConfiguration routing_configuration; From d4c4a43402b0480cc0852592fbf4b983e91d93c2 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Thu, 12 Dec 2024 11:54:04 +0100 Subject: [PATCH 05/18] Quality gate issues resolves --- simulation/behavior_tree_plugin/src/action_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index d7ff2de88c1..99aed142757 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -298,7 +298,7 @@ auto ActionNode::getEntityStatus(const std::string & target_name) const auto ActionNode::getDistanceToTargetEntityPolygon( const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional { - if (status.laneMatchingSucceed() and canonicalized_entity_status->laneMatchingSucceed()) { + if (status.laneMatchingSucceed() && canonicalized_entity_status->laneMatchingSucceed()) { /* * boundingBoxRelativeLaneletPose requires routing_configuration, * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets @@ -315,7 +315,7 @@ auto ActionNode::getDistanceToTargetEntityPolygon( const auto half_width = bounding_box.dimensions.y / 2; // is in front and is within considered width (lateral distance) - if (relative_lanelet_pose.s >= 0 and std::abs(relative_lanelet_pose.offset) <= half_width) { + if (relative_lanelet_pose.s >= 0 && std::abs(relative_lanelet_pose.offset) <= half_width) { return relative_lanelet_pose.s + bounding_box.dimensions.x / 2; } } From a992f2198e22ed4a31f04d5f90fee19f9332079a Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Tue, 17 Dec 2024 20:15:45 +0100 Subject: [PATCH 06/18] Changed comparing offset to distance between bounding boxes --- .../behavior_tree_plugin/src/action_node.cpp | 45 ++++++++++++++----- 1 file changed, 34 insertions(+), 11 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 99aed142757..bfeb7573f7c 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -306,17 +306,40 @@ auto ActionNode::getDistanceToTargetEntityPolygon( traffic_simulator::RoutingConfiguration routing_configuration; routing_configuration.allow_lane_change = true; - - const auto & bounding_box = canonicalized_entity_status->getBoundingBox(); - const auto relative_lanelet_pose = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - canonicalized_entity_status->getCanonicalizedLaneletPose().value(), bounding_box, - status.getCanonicalizedLaneletPose().value(), status.getBoundingBox(), routing_configuration, - hdmap_utils); - - const auto half_width = bounding_box.dimensions.y / 2; - // is in front and is within considered width (lateral distance) - if (relative_lanelet_pose.s >= 0 && std::abs(relative_lanelet_pose.offset) <= half_width) { - return relative_lanelet_pose.s + bounding_box.dimensions.x / 2; + constexpr bool include_adjacent_lanelet{false}; + constexpr bool include_opposite_direction{true}; + + const auto & from = canonicalized_entity_status->getCanonicalizedLaneletPose().value(); + const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox(); + + const auto & to = status.getCanonicalizedLaneletPose().value(); + const auto & to_bounding_box = status.getBoundingBox(); + + const auto longitudinalDistance = + traffic_simulator::distance::boundingBoxLaneLongitudinalDistance( + from, from_bounding_box, to, to_bounding_box, include_adjacent_lanelet, + include_opposite_direction, routing_configuration, hdmap_utils); + + if (const auto lateral_distance = lateralDistance(from, to, routing_configuration, hdmap_utils); + lateral_distance && longitudinalDistance) { + const auto from_bounding_box_distances = + math::geometry::getDistancesFromCenterToEdge(from_bounding_box); + const auto to_bounding_box_distances = + math::geometry::getDistancesFromCenterToEdge(to_bounding_box); + auto bounding_box_distance = 0.0; + if (lateral_distance.value() > 0.0) { + bounding_box_distance = + std::abs(from_bounding_box_distances.right) + std::abs(to_bounding_box_distances.left); + } else if (lateral_distance.value() < 0.0) { + bounding_box_distance = + std::abs(from_bounding_box_distances.left) + std::abs(to_bounding_box_distances.right); + } + // is in front and is within considered width (lateral distance) + if ( + longitudinalDistance.value() >= 0 && + std::abs(lateral_distance.value()) <= bounding_box_distance) { + return longitudinalDistance.value() + from_bounding_box.dimensions.x / 2; + } } } return std::nullopt; From dac4100ff7f73e6743b15ff68e9b9dacbd95c7a6 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Mon, 20 Jan 2025 14:52:59 +0100 Subject: [PATCH 07/18] Add new algorithm --- .../geometry/spline/catmull_rom_spline.hpp | 2 +- .../spline/catmull_rom_spline_interface.hpp | 2 + .../geometry/spline/catmull_rom_subspline.hpp | 3 + .../src/spline/catmull_rom_subspline.cpp | 7 +- .../behavior_tree_plugin/action_node.hpp | 14 +- .../behavior_tree_plugin/src/action_node.cpp | 142 ++++++++++++++---- .../follow_front_entity_action.cpp | 4 +- .../follow_lane_action.cpp | 2 +- .../stop_at_crossing_entity_action.cpp | 2 +- .../stop_at_stop_line_action.cpp | 2 +- .../hdmap_utils/hdmap_utils.hpp | 8 + .../src/hdmap_utils/hdmap_utils.cpp | 26 +++- 12 files changed, 168 insertions(+), 46 deletions(-) diff --git a/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp b/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp index 9018f938855..80172c65b1c 100644 --- a/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp +++ b/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp @@ -46,7 +46,7 @@ class CatmullRomSpline : public CatmullRomSplineInterface auto getSValue(const geometry_msgs::msg::Pose & pose, double threshold_distance = 3.0) const -> std::optional; auto getSquaredDistanceIn2D(const geometry_msgs::msg::Point & point, const double s) const - -> double; + -> double override; auto getSquaredDistanceVector(const geometry_msgs::msg::Point & point, const double s) const -> geometry_msgs::msg::Vector3; auto getCollisionPointsIn2D( diff --git a/common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp b/common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp index 8969a818308..0d08992f75c 100644 --- a/common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp +++ b/common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp @@ -34,6 +34,8 @@ class CatmullRomSplineInterface virtual std::optional getCollisionPointIn2D( const std::vector & polygon, const bool search_backward = false) const = 0; + virtual double getSquaredDistanceIn2D( + const geometry_msgs::msg::Point & point, const double s) const = 0; }; } // namespace geometry } // namespace math diff --git a/common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp b/common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp index 22827657f2f..10a085932a2 100644 --- a/common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp +++ b/common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp @@ -46,6 +46,9 @@ class CatmullRomSubspline : public CatmullRomSplineInterface const std::vector & polygon, const bool search_backward = false) const override; + auto getSquaredDistanceIn2D(const geometry_msgs::msg::Point & point, const double s) const + -> double override; + private: std::shared_ptr spline_; double start_s_; diff --git a/common/math/geometry/src/spline/catmull_rom_subspline.cpp b/common/math/geometry/src/spline/catmull_rom_subspline.cpp index 8cfadb4ad24..c6835d08b5e 100644 --- a/common/math/geometry/src/spline/catmull_rom_subspline.cpp +++ b/common/math/geometry/src/spline/catmull_rom_subspline.cpp @@ -16,7 +16,6 @@ #include #include #include - namespace math { namespace geometry @@ -61,5 +60,11 @@ std::optional CatmullRomSubspline::getCollisionPointIn2D( } return *begin - start_s_; } + +auto CatmullRomSubspline::getSquaredDistanceIn2D( + const geometry_msgs::msg::Point & point, const double s) const -> double +{ + return spline_->getSquaredDistanceIn2D(point, start_s_ + s); +} } // namespace geometry } // namespace math diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 7f128c916a9..572055994ab 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -48,10 +48,12 @@ class ActionNode : public BT::ActionNodeBase auto getDistanceToConflictingEntity( const lanelet::Ids & route_lanelets, const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional; - auto getFrontEntityName() const -> std::optional; + auto getFrontEntityName(const math::geometry::CatmullRomSplineInterface & spline) const + -> std::optional; auto calculateStopDistance(const traffic_simulator_msgs::msg::DynamicConstraints &) const -> double; - auto getDistanceToFrontEntity() const -> std::optional; + auto getDistanceToFrontEntity(const math::geometry::CatmullRomSplineInterface & spline) const + -> std::optional; auto getDistanceToStopLine( const lanelet::Ids & route_lanelets, const std::vector & waypoints) const -> std::optional; @@ -118,6 +120,7 @@ class ActionNode : public BT::ActionNodeBase lanelet::Ids route_lanelets; auto getDistanceToTargetEntityPolygon( + const math::geometry::CatmullRomSplineInterface & spline, const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional; private: @@ -132,6 +135,13 @@ class ActionNode : public BT::ActionNodeBase -> std::vector; auto isOtherEntityAtConsideredAltitude( const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool; + auto getRoutableCanonicalizedLaneletPose( + const traffic_simulator::CanonicalizedEntityStatus & status) const + -> std::optional; + auto getLateralDistance( + const math::geometry::CatmullRomSplineInterface & spline, + const traffic_simulator::CanonicalizedEntityStatus & status, double longitudinalDistance) const + -> double; }; } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index de0a567456d..a9c512fa425 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -233,21 +234,23 @@ auto ActionNode::getDistanceToStopLine( return hdmap_utils->getDistanceToStopLine(route_lanelets, waypoints); } -auto ActionNode::getDistanceToFrontEntity() const -> std::optional +auto ActionNode::getDistanceToFrontEntity( + const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional { - if (const auto entity_name_opt = getFrontEntityName()) { - return getDistanceToTargetEntityPolygon(getEntityStatus(entity_name_opt.value())); + if (const auto entity_name_opt = getFrontEntityName(spline)) { + return getDistanceToTargetEntityPolygon(spline, getEntityStatus(entity_name_opt.value())); } else { return std::nullopt; } } -auto ActionNode::getFrontEntityName() const -> std::optional +auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterface & spline) const + -> std::optional { std::vector distances; std::vector entities; for (const auto & each : other_entity_status) { - const auto distance = getDistanceToTargetEntityPolygon(each.second); + const auto distance = getDistanceToTargetEntityPolygon(spline, each.second); const auto quat = math::geometry::getRotation( canonicalized_entity_status->getMapPose().orientation, other_entity_status.at(each.first).getMapPose().orientation); @@ -295,7 +298,83 @@ auto ActionNode::getEntityStatus(const std::string & target_name) const } } +auto ActionNode::getRoutableCanonicalizedLaneletPose( + const traffic_simulator::CanonicalizedEntityStatus & status) const + -> std::optional +{ + traffic_simulator::RoutingConfiguration routing_configuration; + routing_configuration.allow_lane_change = true; + + const auto & from = canonicalized_entity_status->getCanonicalizedLaneletPose().value(); + const auto from_id = static_cast(from).lanelet_id; + + auto to = status.getCanonicalizedLaneletPose(); + const auto & to_bounding_box = status.getBoundingBox(); + const auto current_id = + static_cast(to.value()).lanelet_id; + + if (hdmap_utils->getRoute(from_id, current_id, routing_configuration).empty()) { + // no route between entities find + const auto lanelet_ids = hdmap_utils->getMatchingLanes( + static_cast(to.value()), to_bounding_box, false, 5.0, 0.8, + routing_configuration.routing_graph_type); + + if (lanelet_ids) { + for (const auto & lanelet_id : lanelet_ids.value()) { + if ( + (lanelet_id.first != current_id) and + (!hdmap_utils->getRoute(from_id, lanelet_id.first, routing_configuration).empty())) { + if (const auto lanelet = hdmap_utils->toLaneletPose( + static_cast(to.value()), lanelet_id.first, 5.0); + lanelet) { + if (const auto canonicalized = + traffic_simulator::pose::canonicalize(lanelet.value(), hdmap_utils); + canonicalized) { + to = canonicalized; + break; + } + } + } + } + } else { + to = std::nullopt; + } + } + + return to; +} + +auto ActionNode::getLateralDistance( + const math::geometry::CatmullRomSplineInterface & spline, + const traffic_simulator::CanonicalizedEntityStatus & status, double longitudinalDistance) const + -> double +{ + const auto & points = math::geometry::transformPoints( + status.getMapPose(), math::geometry::getPointsFromBbox(status.getBoundingBox())); + std::vector distances; + const auto & diagonal = math::geometry::getDistance(points[0], points[2]); + for (const auto & point : points) { + double s_start = longitudinalDistance - diagonal / 2; + double s_end = longitudinalDistance + diagonal / 2; + double start = spline.getSquaredDistanceIn2D(point, s_start); + double stop = spline.getSquaredDistanceIn2D(point, s_end); + + while ((s_end - s_start) > 0.1) { + if (start > stop) { + s_start += (s_end - s_start) / 2; + start = spline.getSquaredDistanceIn2D(point, s_start); + } else { + s_end -= (s_end - s_start) / 2; + stop = spline.getSquaredDistanceIn2D(point, s_end); + } + } + distances.push_back(start > stop ? stop : start); + } + return *std::min_element(distances.begin(), distances.end()); +} + auto ActionNode::getDistanceToTargetEntityPolygon( + const math::geometry::CatmullRomSplineInterface & spline, const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional { if (status.laneMatchingSucceed() && canonicalized_entity_status->laneMatchingSucceed()) { @@ -311,33 +390,34 @@ auto ActionNode::getDistanceToTargetEntityPolygon( const auto & from = canonicalized_entity_status->getCanonicalizedLaneletPose().value(); const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox(); - const auto & to = status.getCanonicalizedLaneletPose().value(); + const auto & to = getRoutableCanonicalizedLaneletPose(status); const auto & to_bounding_box = status.getBoundingBox(); - const auto longitudinalDistance = - traffic_simulator::distance::boundingBoxLaneLongitudinalDistance( - from, from_bounding_box, to, to_bounding_box, include_adjacent_lanelet, - include_opposite_direction, routing_configuration, hdmap_utils); - - if (const auto lateral_distance = lateralDistance(from, to, routing_configuration, hdmap_utils); - lateral_distance && longitudinalDistance) { - const auto from_bounding_box_distances = - math::geometry::getDistancesFromCenterToEdge(from_bounding_box); - const auto to_bounding_box_distances = - math::geometry::getDistancesFromCenterToEdge(to_bounding_box); - auto bounding_box_distance = 0.0; - if (lateral_distance.value() > 0.0) { - bounding_box_distance = - std::abs(from_bounding_box_distances.right) + std::abs(to_bounding_box_distances.left); - } else if (lateral_distance.value() < 0.0) { - bounding_box_distance = - std::abs(from_bounding_box_distances.left) + std::abs(to_bounding_box_distances.right); - } - // is in front and is within considered width (lateral distance) - if ( - longitudinalDistance.value() >= 0 && - std::abs(lateral_distance.value()) <= bounding_box_distance) { - return longitudinalDistance.value() + from_bounding_box.dimensions.x / 2; + if (to.has_value()) { + const auto longitudinalDistance = + traffic_simulator::distance::boundingBoxLaneLongitudinalDistance( + from, from_bounding_box, *to, to_bounding_box, include_adjacent_lanelet, + include_opposite_direction, routing_configuration, hdmap_utils); + + if (longitudinalDistance) { + const auto lateral_distance = + std::sqrt(getLateralDistance(spline, status, longitudinalDistance.value())); + const auto from_bounding_box_distances = + math::geometry::getDistancesFromCenterToEdge(from_bounding_box); + const auto to_bounding_box_distances = + math::geometry::getDistancesFromCenterToEdge(to_bounding_box); + auto bounding_box_distance = 0.0; + if (lateral_distance > 0.0) { + bounding_box_distance = + std::abs(from_bounding_box_distances.right) + std::abs(to_bounding_box_distances.left); + } else if (lateral_distance < 0.0) { + bounding_box_distance = + std::abs(from_bounding_box_distances.left) + std::abs(to_bounding_box_distances.right); + } + // is in front and is within considered width (lateral distance) + if (longitudinalDistance.value() >= 0 && (lateral_distance <= bounding_box_distance)) { + return longitudinalDistance.value() + from_bounding_box.dimensions.x / 2; + } } } } @@ -365,7 +445,7 @@ auto ActionNode::getDistanceToConflictingEntity( } } for (const auto & status : lane_entity_status) { - if (const auto distance_to_entity = getDistanceToTargetEntityPolygon(status)) { + if (const auto distance_to_entity = getDistanceToTargetEntityPolygon(spline, status)) { distances.insert(distance_to_entity.value()); } } diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp index 3e8be9df85f..668c606a3a2 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp @@ -92,12 +92,12 @@ BT::NodeStatus FollowFrontEntityAction::tick() } auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); auto distance_to_conflicting_entity = getDistanceToConflictingEntity(route_lanelets, *trajectory); - const auto front_entity_name = getFrontEntityName(); + const auto front_entity_name = getFrontEntityName(*trajectory); if (!front_entity_name) { return BT::NodeStatus::FAILURE; } const auto & front_entity_status = getEntityStatus(front_entity_name.value()); - distance_to_front_entity_ = getDistanceToTargetEntityPolygon(front_entity_status); + distance_to_front_entity_ = getDistanceToTargetEntityPolygon(*trajectory, front_entity_status); if (!distance_to_front_entity_) { return BT::NodeStatus::FAILURE; } diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp index 66b7104d586..4d5006a2e89 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp @@ -92,7 +92,7 @@ BT::NodeStatus FollowLaneAction::tick() if (trajectory == nullptr) { return BT::NodeStatus::FAILURE; } - const auto distance_to_front_entity = getDistanceToFrontEntity(); + const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory); if (distance_to_front_entity) { if ( distance_to_front_entity.value() <= diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp index 158331e2ed5..c85e6cdf622 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp @@ -113,7 +113,7 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() } distance_to_stop_target_ = getDistanceToConflictingEntity(route_lanelets, *trajectory); auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); - const auto distance_to_front_entity = getDistanceToFrontEntity(); + const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory); if (!distance_to_stop_target_) { in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp index 5858fa0c7ba..23011a89e47 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp @@ -110,7 +110,7 @@ BT::NodeStatus StopAtStopLineAction::tick() } distance_to_stopline_ = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory); const auto distance_to_stop_target = getDistanceToConflictingEntity(route_lanelets, *trajectory); - const auto distance_to_front_entity = getDistanceToFrontEntity(); + const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory); if (!distance_to_stopline_) { stopped_ = false; return BT::NodeStatus::FAILURE; diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 02e6a893ad2..f489b3cc477 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -331,6 +331,14 @@ class HdMapUtils constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; public: + auto getMatchingLanes( + const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, + const bool include_crosswalk, const double matching_distance = 1.0, + const double reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingConfiguration().routing_graph_type) const + -> std::optional>>; + auto matchToLane( const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, const bool include_crosswalk, const double matching_distance = 1.0, diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index c6cb0554b23..8b1699c61e6 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -525,10 +525,11 @@ auto HdMapUtils::toPoint2d(const geometry_msgs::msg::Point & point) const -> lan return lanelet::BasicPoint2d{point.x, point.y}; } -auto HdMapUtils::matchToLane( +auto HdMapUtils::getMatchingLanes( const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, const bool include_crosswalk, const double matching_distance, const double reduction_ratio, - const traffic_simulator::RoutingGraphType type) const -> std::optional + const traffic_simulator::RoutingGraphType type) const + -> std::optional>> { lanelet::matching::Object2d obj; obj.pose.translation() = toPoint2d(pose.position); @@ -562,10 +563,23 @@ auto HdMapUtils::matchToLane( if (id_and_distance.empty()) { return std::nullopt; } - const auto min_id_and_distance = std::min_element( - id_and_distance.begin(), id_and_distance.end(), - [](auto const & lhs, auto const & rhs) { return lhs.second < rhs.second; }); - return min_id_and_distance->first; + std::sort(id_and_distance.begin(), id_and_distance.end(), [](auto const & lhs, auto const & rhs) { + return lhs.second < rhs.second; + }); + return id_and_distance; +} + +auto HdMapUtils::matchToLane( + const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, + const bool include_crosswalk, const double matching_distance, const double reduction_ratio, + const traffic_simulator::RoutingGraphType type) const -> std::optional +{ + const auto & id_and_distance = + getMatchingLanes(pose, bbox, include_crosswalk, matching_distance, reduction_ratio, type); + if (id_and_distance) { + return id_and_distance->begin()->first; + } + return std::nullopt; } auto HdMapUtils::toLaneletPose( From 84eb38b6fe9d131ab77a7bcc6567600d31d7db87 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Wed, 22 Jan 2025 17:28:57 +0100 Subject: [PATCH 08/18] Review changes --- .../src/spline/catmull_rom_subspline.cpp | 1 + .../behavior_tree_plugin/action_node.hpp | 2 +- .../behavior_tree_plugin/src/action_node.cpp | 43 ++++++---- .../hdmap_utils/hdmap_utils.hpp | 2 +- .../src/hdmap_utils/hdmap_utils.cpp | 78 ++++++++++--------- 5 files changed, 74 insertions(+), 52 deletions(-) diff --git a/common/math/geometry/src/spline/catmull_rom_subspline.cpp b/common/math/geometry/src/spline/catmull_rom_subspline.cpp index c6835d08b5e..0c80d3ac7c3 100644 --- a/common/math/geometry/src/spline/catmull_rom_subspline.cpp +++ b/common/math/geometry/src/spline/catmull_rom_subspline.cpp @@ -16,6 +16,7 @@ #include #include #include + namespace math { namespace geometry diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 572055994ab..4a9ebea7f06 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -141,7 +141,7 @@ class ActionNode : public BT::ActionNodeBase auto getLateralDistance( const math::geometry::CatmullRomSplineInterface & spline, const traffic_simulator::CanonicalizedEntityStatus & status, double longitudinalDistance) const - -> double; + -> const double; }; } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index a9c512fa425..62e10f4e8cc 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -302,52 +302,64 @@ auto ActionNode::getRoutableCanonicalizedLaneletPose( const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional { + // This default hdmap_utils value + constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; + // This value has to allow finding neighbor lanelets, it would be perfect if this is parametized to lanelet width + constexpr static double ALTERNATIVE_LANELETS_DISTANCE = 3.0; + traffic_simulator::RoutingConfiguration routing_configuration; routing_configuration.allow_lane_change = true; - const auto & from = canonicalized_entity_status->getCanonicalizedLaneletPose().value(); - const auto from_id = static_cast(from).lanelet_id; + const auto & from_lanelet_pose = + canonicalized_entity_status->getCanonicalizedLaneletPose().value(); + const auto from_id = + static_cast(from_lanelet_pose).lanelet_id; - auto to = status.getCanonicalizedLaneletPose(); + auto to_lanelet_pose = status.getCanonicalizedLaneletPose(); const auto & to_bounding_box = status.getBoundingBox(); const auto current_id = static_cast(to.value()).lanelet_id; if (hdmap_utils->getRoute(from_id, current_id, routing_configuration).empty()) { - // no route between entities find - const auto lanelet_ids = hdmap_utils->getMatchingLanes( - static_cast(to.value()), to_bounding_box, false, 5.0, 0.8, + // no route between entities found + const auto lanelet_ids = hdmap_utils->findMatchingLanes( + static_cast(to.value()), to_bounding_box, false, + ALTERNATIVE_LANELETS_DISTANCE, DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, routing_configuration.routing_graph_type); if (lanelet_ids) { + std::sort(lanelet_ids->begin(), lanelet_ids->end(), [](auto const & lhs, auto const & rhs) { + return lhs.second < rhs.second; + }); for (const auto & lanelet_id : lanelet_ids.value()) { if ( (lanelet_id.first != current_id) and (!hdmap_utils->getRoute(from_id, lanelet_id.first, routing_configuration).empty())) { if (const auto lanelet = hdmap_utils->toLaneletPose( - static_cast(to.value()), lanelet_id.first, 5.0); + static_cast(to_lanelet_pose.value()), lanelet_id.first, + ALTERNATIVE_LANELETS_DISTANCE); lanelet) { if (const auto canonicalized = traffic_simulator::pose::canonicalize(lanelet.value(), hdmap_utils); canonicalized) { - to = canonicalized; + to_lanelet_pose = canonicalized; break; } } } } } else { - to = std::nullopt; + to_lanelet_pose = std::nullopt; } } - return to; + return to_lanelet_pose; } auto ActionNode::getLateralDistance( const math::geometry::CatmullRomSplineInterface & spline, const traffic_simulator::CanonicalizedEntityStatus & status, double longitudinalDistance) const - -> double + -> const double { const auto & points = math::geometry::transformPoints( status.getMapPose(), math::geometry::getPointsFromBbox(status.getBoundingBox())); @@ -387,17 +399,18 @@ auto ActionNode::getDistanceToTargetEntityPolygon( constexpr bool include_adjacent_lanelet{false}; constexpr bool include_opposite_direction{true}; - const auto & from = canonicalized_entity_status->getCanonicalizedLaneletPose().value(); + const auto & from_lanelet_pose = + canonicalized_entity_status->getCanonicalizedLaneletPose().value(); const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox(); - const auto & to = getRoutableCanonicalizedLaneletPose(status); + const auto & to_lanelet_pose = getRoutableCanonicalizedLaneletPose(status); const auto & to_bounding_box = status.getBoundingBox(); if (to.has_value()) { const auto longitudinalDistance = traffic_simulator::distance::boundingBoxLaneLongitudinalDistance( - from, from_bounding_box, *to, to_bounding_box, include_adjacent_lanelet, - include_opposite_direction, routing_configuration, hdmap_utils); + from_lanelet_pose, from_bounding_box, *to_lanelet_pose, to_bounding_box, + include_adjacent_lanelet, include_opposite_direction, routing_configuration, hdmap_utils); if (longitudinalDistance) { const auto lateral_distance = diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index f489b3cc477..6c508f633bd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -331,7 +331,7 @@ class HdMapUtils constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; public: - auto getMatchingLanes( + auto findMatchingLanes( const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, const bool include_crosswalk, const double matching_distance = 1.0, const double reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 8b1699c61e6..3268b318d30 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -525,48 +525,46 @@ auto HdMapUtils::toPoint2d(const geometry_msgs::msg::Point & point) const -> lan return lanelet::BasicPoint2d{point.x, point.y}; } -auto HdMapUtils::getMatchingLanes( - const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, +auto HdMapUtils::findMatchingLanes( + const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, const bool include_crosswalk, const double matching_distance, const double reduction_ratio, const traffic_simulator::RoutingGraphType type) const -> std::optional>> { - lanelet::matching::Object2d obj; - obj.pose.translation() = toPoint2d(pose.position); - obj.pose.linear() = - Eigen::Rotation2D(math::geometry::convertQuaternionToEulerAngle(pose.orientation).z) - .matrix(); - obj.absoluteHull = absoluteHull( - lanelet::matching::Hull2d{ + const auto absoluteHullPolygon = [&reduction_ratio, + &bounding_box](const auto & pose) -> lanelet::BasicPolygon2d { + auto relative_hull = lanelet::matching::Hull2d{ lanelet::BasicPoint2d{ - bbox.center.x + bbox.dimensions.x * 0.5 * reduction_ratio, - bbox.center.y + bbox.dimensions.y * 0.5 * reduction_ratio}, + bounding_box.center.x + bounding_box.dimensions.x * 0.5 * reduction_ratio, + bounding_box.center.y + bounding_box.dimensions.y * 0.5 * reduction_ratio}, lanelet::BasicPoint2d{ - bbox.center.x - bbox.dimensions.x * 0.5 * reduction_ratio, - bbox.center.y - bbox.dimensions.y * 0.5 * reduction_ratio}}, - obj.pose); - auto matches = - lanelet::matching::getDeterministicMatches(*lanelet_map_ptr_, obj, matching_distance); + bounding_box.center.x - bounding_box.dimensions.x * 0.5 * reduction_ratio, + bounding_box.center.y - bounding_box.dimensions.y * 0.5 * reduction_ratio}}; + lanelet::BasicPolygon2d absolute_hull_polygon; + absolute_hull_polygon.reserve(relative_hull.size()); + for (const auto & relative_hull_point : relative_hull) { + absolute_hull_polygon.push_back(pose * relative_hull_point); + } + return absolute_hull_polygon; + }; + // prepare object for matching + const auto yaw = math::geometry::convertQuaternionToEulerAngle(map_pose.orientation).z; + lanelet::matching::Object2d bounding_box_object; + bounding_box_object.pose.translation() = + lanelet::BasicPoint2d(map_pose.position.x, map_pose.position.y); + bounding_box_object.pose.linear() = Eigen::Rotation2D(yaw).matrix(); + bounding_box_object.absoluteHull = absoluteHullPolygon(bounding_box_object.pose); + // find matches and optionally filter + auto matches = lanelet::matching::getDeterministicMatches( + *LaneletWrapper::map(), bounding_box_object, matching_distance); if (!include_crosswalk) { - matches = lanelet::matching::removeNonRuleCompliantMatches( - matches, routing_graphs_->traffic_rule(type)); + matches = + lanelet::matching::removeNonRuleCompliantMatches(matches, LaneletWrapper::trafficRules(type)); } if (matches.empty()) { return std::nullopt; } - std::vector> id_and_distance; - for (const auto & match : matches) { - if (const auto lanelet_pose = toLaneletPose(pose, match.lanelet.id(), matching_distance)) { - id_and_distance.emplace_back(lanelet_pose->lanelet_id, lanelet_pose->offset); - } - } - if (id_and_distance.empty()) { - return std::nullopt; - } - std::sort(id_and_distance.begin(), id_and_distance.end(), [](auto const & lhs, auto const & rhs) { - return lhs.second < rhs.second; - }); - return id_and_distance; + return matches; } auto HdMapUtils::matchToLane( @@ -574,10 +572,20 @@ auto HdMapUtils::matchToLane( const bool include_crosswalk, const double matching_distance, const double reduction_ratio, const traffic_simulator::RoutingGraphType type) const -> std::optional { - const auto & id_and_distance = - getMatchingLanes(pose, bbox, include_crosswalk, matching_distance, reduction_ratio, type); - if (id_and_distance) { - return id_and_distance->begin()->first; + // find best match (minimize offset) + const auto & matches = + findMatchingLanes(pose, bbox, include_crosswalk, matching_distance, reduction_ratio, type); + if (matches) { + std::optional> min_pair_id_offset; + for (const auto & match : matches) { + if (const auto lanelet_pose = + pose::toLaneletPose(map_pose, match.lanelet.id(), matching_distance); + lanelet_pose && + (!min_pair_id_offset || lanelet_pose->offset < min_pair_id_offset->second)) { + min_pair_id_offset = std::make_pair(lanelet_pose->lanelet_id, lanelet_pose->offset); + } + } + return min_pair_id_offset ? std::optional(min_pair_id_offset->first) : std::nullopt; } return std::nullopt; } From 4c311b89baf36c8d216cd0581651c7ae198e76e6 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Thu, 23 Jan 2025 11:50:46 +0100 Subject: [PATCH 09/18] Move distance calculation method to utils --- .../behavior_tree_plugin/action_node.hpp | 4 -- .../behavior_tree_plugin/src/action_node.cpp | 55 ++++--------------- .../hdmap_utils/hdmap_utils.hpp | 2 +- .../traffic_simulator/utils/distance.hpp | 6 ++ .../src/hdmap_utils/hdmap_utils.cpp | 41 +++++++------- .../traffic_simulator/src/utils/distance.cpp | 34 ++++++++++++ 6 files changed, 73 insertions(+), 69 deletions(-) diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 4a9ebea7f06..5f765731436 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -138,10 +138,6 @@ class ActionNode : public BT::ActionNodeBase auto getRoutableCanonicalizedLaneletPose( const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional; - auto getLateralDistance( - const math::geometry::CatmullRomSplineInterface & spline, - const traffic_simulator::CanonicalizedEntityStatus & status, double longitudinalDistance) const - -> const double; }; } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 62e10f4e8cc..48fd04bb76b 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -303,9 +303,9 @@ auto ActionNode::getRoutableCanonicalizedLaneletPose( -> std::optional { // This default hdmap_utils value - constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; + constexpr double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; // This value has to allow finding neighbor lanelets, it would be perfect if this is parametized to lanelet width - constexpr static double ALTERNATIVE_LANELETS_DISTANCE = 3.0; + constexpr double ALTERNATIVE_LANELETS_DISTANCE = 3.0; traffic_simulator::RoutingConfiguration routing_configuration; routing_configuration.allow_lane_change = true; @@ -318,25 +318,22 @@ auto ActionNode::getRoutableCanonicalizedLaneletPose( auto to_lanelet_pose = status.getCanonicalizedLaneletPose(); const auto & to_bounding_box = status.getBoundingBox(); const auto current_id = - static_cast(to.value()).lanelet_id; + static_cast(to_lanelet_pose.value()).lanelet_id; if (hdmap_utils->getRoute(from_id, current_id, routing_configuration).empty()) { // no route between entities found - const auto lanelet_ids = hdmap_utils->findMatchingLanes( - static_cast(to.value()), to_bounding_box, false, + const auto & lanelet_ids = hdmap_utils->findMatchingLanes( + static_cast(to_lanelet_pose.value()), to_bounding_box, false, ALTERNATIVE_LANELETS_DISTANCE, DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, routing_configuration.routing_graph_type); if (lanelet_ids) { - std::sort(lanelet_ids->begin(), lanelet_ids->end(), [](auto const & lhs, auto const & rhs) { - return lhs.second < rhs.second; - }); for (const auto & lanelet_id : lanelet_ids.value()) { if ( - (lanelet_id.first != current_id) and - (!hdmap_utils->getRoute(from_id, lanelet_id.first, routing_configuration).empty())) { + (lanelet_id.second != current_id) and + (!hdmap_utils->getRoute(from_id, lanelet_id.second, routing_configuration).empty())) { if (const auto lanelet = hdmap_utils->toLaneletPose( - static_cast(to_lanelet_pose.value()), lanelet_id.first, + static_cast(to_lanelet_pose.value()), lanelet_id.second, ALTERNATIVE_LANELETS_DISTANCE); lanelet) { if (const auto canonicalized = @@ -352,39 +349,9 @@ auto ActionNode::getRoutableCanonicalizedLaneletPose( to_lanelet_pose = std::nullopt; } } - return to_lanelet_pose; } -auto ActionNode::getLateralDistance( - const math::geometry::CatmullRomSplineInterface & spline, - const traffic_simulator::CanonicalizedEntityStatus & status, double longitudinalDistance) const - -> const double -{ - const auto & points = math::geometry::transformPoints( - status.getMapPose(), math::geometry::getPointsFromBbox(status.getBoundingBox())); - std::vector distances; - const auto & diagonal = math::geometry::getDistance(points[0], points[2]); - for (const auto & point : points) { - double s_start = longitudinalDistance - diagonal / 2; - double s_end = longitudinalDistance + diagonal / 2; - double start = spline.getSquaredDistanceIn2D(point, s_start); - double stop = spline.getSquaredDistanceIn2D(point, s_end); - - while ((s_end - s_start) > 0.1) { - if (start > stop) { - s_start += (s_end - s_start) / 2; - start = spline.getSquaredDistanceIn2D(point, s_start); - } else { - s_end -= (s_end - s_start) / 2; - stop = spline.getSquaredDistanceIn2D(point, s_end); - } - } - distances.push_back(start > stop ? stop : start); - } - return *std::min_element(distances.begin(), distances.end()); -} - auto ActionNode::getDistanceToTargetEntityPolygon( const math::geometry::CatmullRomSplineInterface & spline, const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional @@ -406,15 +373,15 @@ auto ActionNode::getDistanceToTargetEntityPolygon( const auto & to_lanelet_pose = getRoutableCanonicalizedLaneletPose(status); const auto & to_bounding_box = status.getBoundingBox(); - if (to.has_value()) { + if (to_lanelet_pose.has_value()) { const auto longitudinalDistance = traffic_simulator::distance::boundingBoxLaneLongitudinalDistance( from_lanelet_pose, from_bounding_box, *to_lanelet_pose, to_bounding_box, include_adjacent_lanelet, include_opposite_direction, routing_configuration, hdmap_utils); if (longitudinalDistance) { - const auto lateral_distance = - std::sqrt(getLateralDistance(spline, status, longitudinalDistance.value())); + const auto lateral_distance = traffic_simulator::distance::distanceToSpline( + status.getMapPose(), to_bounding_box, spline, longitudinalDistance.value()); const auto from_bounding_box_distances = math::geometry::getDistancesFromCenterToEdge(from_bounding_box); const auto to_bounding_box_distances = diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 6c508f633bd..fd9d16897e4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -337,7 +337,7 @@ class HdMapUtils const double reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> std::optional>>; + -> std::optional>>; auto matchToLane( const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index 7919a140491..5a43dd030dc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -112,6 +112,12 @@ auto distanceToStopLine( const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array, const lanelet::Id target_stop_line_id, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; + +// spline +auto distanceToSpline( + const geometry_msgs::msg::Pose & map_pose, + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, + const math::geometry::CatmullRomSplineInterface & spline, const double s_reference) -> double; } // namespace distance } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_ diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 3268b318d30..4b805353b75 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -526,10 +526,11 @@ auto HdMapUtils::toPoint2d(const geometry_msgs::msg::Point & point) const -> lan } auto HdMapUtils::findMatchingLanes( - const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, - const bool include_crosswalk, const double matching_distance, const double reduction_ratio, + const geometry_msgs::msg::Pose & map_pose, + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, + const double matching_distance, const double reduction_ratio, const traffic_simulator::RoutingGraphType type) const - -> std::optional>> + -> std::optional>> { const auto absoluteHullPolygon = [&reduction_ratio, &bounding_box](const auto & pose) -> lanelet::BasicPolygon2d { @@ -556,15 +557,24 @@ auto HdMapUtils::findMatchingLanes( bounding_box_object.absoluteHull = absoluteHullPolygon(bounding_box_object.pose); // find matches and optionally filter auto matches = lanelet::matching::getDeterministicMatches( - *LaneletWrapper::map(), bounding_box_object, matching_distance); + *lanelet_map_ptr_, bounding_box_object, matching_distance); if (!include_crosswalk) { - matches = - lanelet::matching::removeNonRuleCompliantMatches(matches, LaneletWrapper::trafficRules(type)); + matches = lanelet::matching::removeNonRuleCompliantMatches( + matches, routing_graphs_->traffic_rule(type)); } if (matches.empty()) { return std::nullopt; } - return matches; + std::set> id_and_distance; + for (const auto & match : matches) { + if (const auto lanelet_pose = toLaneletPose(map_pose, match.lanelet.id(), matching_distance)) { + id_and_distance.emplace(lanelet_pose->offset, lanelet_pose->lanelet_id); + } + } + if (id_and_distance.empty()) { + return std::nullopt; + } + return id_and_distance; } auto HdMapUtils::matchToLane( @@ -572,20 +582,11 @@ auto HdMapUtils::matchToLane( const bool include_crosswalk, const double matching_distance, const double reduction_ratio, const traffic_simulator::RoutingGraphType type) const -> std::optional { - // find best match (minimize offset) - const auto & matches = + const auto & matching_lanes = findMatchingLanes(pose, bbox, include_crosswalk, matching_distance, reduction_ratio, type); - if (matches) { - std::optional> min_pair_id_offset; - for (const auto & match : matches) { - if (const auto lanelet_pose = - pose::toLaneletPose(map_pose, match.lanelet.id(), matching_distance); - lanelet_pose && - (!min_pair_id_offset || lanelet_pose->offset < min_pair_id_offset->second)) { - min_pair_id_offset = std::make_pair(lanelet_pose->lanelet_id, lanelet_pose->offset); - } - } - return min_pair_id_offset ? std::optional(min_pair_id_offset->first) : std::nullopt; + // find best match (minimize offset) + if (matching_lanes) { + return matching_lanes->begin()->second; } return std::nullopt; } diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 98bac102b88..5d15e7d202a 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -311,5 +311,39 @@ auto distanceToStopLine( return spline.getCollisionPointIn2D(polygon); } } + +auto distanceToSpline( + const geometry_msgs::msg::Pose & map_pose, + const traffic_simulator_msgs::msg::BoundingBox & bounding_box, + const math::geometry::CatmullRomSplineInterface & spline, const double s_reference) -> double +{ + constexpr double distance_accuracy{0.05}; + + const auto bounding_box_map_points = + math::geometry::transformPoints(map_pose, math::geometry::getPointsFromBbox(bounding_box)); + const auto bounding_box_diagonal_length = + math::geometry::getDistance(bounding_box_map_points[0], bounding_box_map_points[2]); + + /// @todo it may be a good idea to develop spline.getSquaredDistanceIn2D(point, s_start, s_end); + std::vector distances; + for (const auto & point : bounding_box_map_points) { + auto s_start = s_reference - bounding_box_diagonal_length / 2; + auto s_end = s_reference + bounding_box_diagonal_length / 2; + auto s_start_distance = spline.getSquaredDistanceIn2D(point, s_start); + auto s_end_distance = spline.getSquaredDistanceIn2D(point, s_end); + + while (std::abs(s_start_distance - s_end_distance) > distance_accuracy) { + if (s_start_distance > s_end_distance) { + s_start += (s_end - s_start) / 2; + s_start_distance = spline.getSquaredDistanceIn2D(point, s_start); + } else { + s_end -= (s_end - s_start) / 2; + s_end_distance = spline.getSquaredDistanceIn2D(point, s_end); + } + } + distances.push_back(s_start_distance > s_end_distance ? s_end_distance : s_start_distance); + } + return std::sqrt(*std::min_element(distances.begin(), distances.end())); +} } // namespace distance } // namespace traffic_simulator From 4adf15ff7fa02df01cc3d5dc3925699acbadb114 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Thu, 23 Jan 2025 13:33:57 +0100 Subject: [PATCH 10/18] Move tranformToRoutableCanonicalizedLaneletPose to pose namespace --- .../behavior_tree_plugin/action_node.hpp | 3 - .../behavior_tree_plugin/src/action_node.cpp | 112 ++++++------------ .../include/traffic_simulator/utils/pose.hpp | 6 + .../traffic_simulator/src/utils/pose.cpp | 46 +++++++ 4 files changed, 85 insertions(+), 82 deletions(-) diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 5f765731436..058114bf65c 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -135,9 +135,6 @@ class ActionNode : public BT::ActionNodeBase -> std::vector; auto isOtherEntityAtConsideredAltitude( const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool; - auto getRoutableCanonicalizedLaneletPose( - const traffic_simulator::CanonicalizedEntityStatus & status) const - -> std::optional; }; } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 48fd04bb76b..50bd37befe7 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -298,65 +298,13 @@ auto ActionNode::getEntityStatus(const std::string & target_name) const } } -auto ActionNode::getRoutableCanonicalizedLaneletPose( - const traffic_simulator::CanonicalizedEntityStatus & status) const - -> std::optional -{ - // This default hdmap_utils value - constexpr double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; - // This value has to allow finding neighbor lanelets, it would be perfect if this is parametized to lanelet width - constexpr double ALTERNATIVE_LANELETS_DISTANCE = 3.0; - - traffic_simulator::RoutingConfiguration routing_configuration; - routing_configuration.allow_lane_change = true; - - const auto & from_lanelet_pose = - canonicalized_entity_status->getCanonicalizedLaneletPose().value(); - const auto from_id = - static_cast(from_lanelet_pose).lanelet_id; - - auto to_lanelet_pose = status.getCanonicalizedLaneletPose(); - const auto & to_bounding_box = status.getBoundingBox(); - const auto current_id = - static_cast(to_lanelet_pose.value()).lanelet_id; - - if (hdmap_utils->getRoute(from_id, current_id, routing_configuration).empty()) { - // no route between entities found - const auto & lanelet_ids = hdmap_utils->findMatchingLanes( - static_cast(to_lanelet_pose.value()), to_bounding_box, false, - ALTERNATIVE_LANELETS_DISTANCE, DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, - routing_configuration.routing_graph_type); - - if (lanelet_ids) { - for (const auto & lanelet_id : lanelet_ids.value()) { - if ( - (lanelet_id.second != current_id) and - (!hdmap_utils->getRoute(from_id, lanelet_id.second, routing_configuration).empty())) { - if (const auto lanelet = hdmap_utils->toLaneletPose( - static_cast(to_lanelet_pose.value()), lanelet_id.second, - ALTERNATIVE_LANELETS_DISTANCE); - lanelet) { - if (const auto canonicalized = - traffic_simulator::pose::canonicalize(lanelet.value(), hdmap_utils); - canonicalized) { - to_lanelet_pose = canonicalized; - break; - } - } - } - } - } else { - to_lanelet_pose = std::nullopt; - } - } - return to_lanelet_pose; -} - auto ActionNode::getDistanceToTargetEntityPolygon( const math::geometry::CatmullRomSplineInterface & spline, const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional { - if (status.laneMatchingSucceed() && canonicalized_entity_status->laneMatchingSucceed()) { + if ( + status.laneMatchingSucceed() && canonicalized_entity_status->laneMatchingSucceed() && + isOtherEntityAtConsideredAltitude(status)) { /* * boundingBoxRelativeLaneletPose requires routing_configuration, * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets @@ -370,33 +318,39 @@ auto ActionNode::getDistanceToTargetEntityPolygon( canonicalized_entity_status->getCanonicalizedLaneletPose().value(); const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox(); - const auto & to_lanelet_pose = getRoutableCanonicalizedLaneletPose(status); const auto & to_bounding_box = status.getBoundingBox(); + const auto & to_lanelet_pose = + traffic_simulator::pose::tranformToRoutableCanonicalizedLaneletPose( + canonicalized_entity_status->getLaneletId(), status.getCanonicalizedLaneletPose().value(), + to_bounding_box, hdmap_utils); if (to_lanelet_pose.has_value()) { - const auto longitudinalDistance = - traffic_simulator::distance::boundingBoxLaneLongitudinalDistance( - from_lanelet_pose, from_bounding_box, *to_lanelet_pose, to_bounding_box, - include_adjacent_lanelet, include_opposite_direction, routing_configuration, hdmap_utils); - - if (longitudinalDistance) { - const auto lateral_distance = traffic_simulator::distance::distanceToSpline( - status.getMapPose(), to_bounding_box, spline, longitudinalDistance.value()); - const auto from_bounding_box_distances = - math::geometry::getDistancesFromCenterToEdge(from_bounding_box); - const auto to_bounding_box_distances = - math::geometry::getDistancesFromCenterToEdge(to_bounding_box); - auto bounding_box_distance = 0.0; - if (lateral_distance > 0.0) { - bounding_box_distance = - std::abs(from_bounding_box_distances.right) + std::abs(to_bounding_box_distances.left); - } else if (lateral_distance < 0.0) { - bounding_box_distance = - std::abs(from_bounding_box_distances.left) + std::abs(to_bounding_box_distances.right); - } - // is in front and is within considered width (lateral distance) - if (longitudinalDistance.value() >= 0 && (lateral_distance <= bounding_box_distance)) { - return longitudinalDistance.value() + from_bounding_box.dimensions.x / 2; + const auto longitudinal_target_distance = traffic_simulator::distance::longitudinalDistance( + from_lanelet_pose, *to_lanelet_pose, include_adjacent_lanelet, include_opposite_direction, + routing_configuration, hdmap_utils); + + if (longitudinal_target_distance) { + const auto distance_to_spline = traffic_simulator::distance::distanceToSpline( + static_cast(*to_lanelet_pose), to_bounding_box, spline, + longitudinal_target_distance.value()); + + const auto longitudinal_bounding_box_target_distance = + traffic_simulator::distance::boundingBoxLaneLongitudinalDistance( + from_lanelet_pose, from_bounding_box, *to_lanelet_pose, to_bounding_box, + include_adjacent_lanelet, include_opposite_direction, routing_configuration, + hdmap_utils); + + const auto polygon = math::geometry::transformPoints( + status.getMapPose(), math::geometry::getPointsFromBbox(to_bounding_box)); + + // is in front and is within considered width (from_bounding_box) + if ( + longitudinal_bounding_box_target_distance && + longitudinal_bounding_box_target_distance.value() >= 0 && + ((distance_to_spline <= (from_bounding_box.dimensions.y / 2)) || + spline.getCollisionPointIn2D(polygon, false))) { + return longitudinal_bounding_box_target_distance.value() + + from_bounding_box.dimensions.x / 2; } } } diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 9dc3b8053f8..8a376b8846f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -105,6 +105,12 @@ auto laneletLength( const lanelet::Id lanelet_id, const std::shared_ptr & hdmap_utils_ptr) -> double; +auto tranformToRoutableCanonicalizedLaneletPose( + const lanelet::Id from_lanelet_id, const CanonicalizedLaneletPose & canonicalized_lanelet_pose, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, + const std::shared_ptr & hdmap_utils_ptr) + -> std::optional; + namespace pedestrian { auto transformToCanonicalizedLaneletPose( diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 6dd54fcf700..65cbd278515 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -282,6 +282,52 @@ auto laneletLength( return hdmap_utils_ptr->getLaneletLength(lanelet_id); } +auto tranformToRoutableCanonicalizedLaneletPose( + const lanelet::Id from_lanelet_id, const CanonicalizedLaneletPose & canonicalized_lanelet_pose, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, + const std::shared_ptr & hdmap_utils_ptr) + -> std::optional +{ + constexpr auto search_distance{3.0}; + constexpr auto include_crosswalk{false}; + RoutingConfiguration routing_configuration; + routing_configuration.allow_lane_change = true; + + /// @note if there is already a route from_lanelet_id->to_lanelet_id, return it + /// if not, transform the 'to_lanelet_id' position into the nearby lanelets and search for a route in relation to them + const auto to_lanelet_id = canonicalized_lanelet_pose.getLaneletPose().lanelet_id; + if (!hdmap_utils_ptr->getRoute(from_lanelet_id, to_lanelet_id, routing_configuration).empty()) { + return canonicalized_lanelet_pose; + } else if (const auto nearby_lanelet_ids = hdmap_utils_ptr->findMatchingLanes( + static_cast(canonicalized_lanelet_pose), to_bounding_box, + include_crosswalk, search_distance, 0.8, routing_configuration.routing_graph_type); + nearby_lanelet_ids.has_value()) { + std::vector> routes; + for (const auto & [distance, lanelet_id] : nearby_lanelet_ids.value()) { + if (const auto route = + hdmap_utils_ptr->getRoute(from_lanelet_id, lanelet_id, routing_configuration); + lanelet_id == to_lanelet_id || route.empty()) { + continue; + } else if (const auto lanelet_pose = hdmap_utils_ptr->toLaneletPose( + static_cast(canonicalized_lanelet_pose), lanelet_id, + search_distance); + !lanelet_pose) { + continue; + } else if (const auto canonicalized = canonicalize(lanelet_pose.value(), hdmap_utils_ptr); + canonicalized) { + routes.emplace_back(std::make_pair(canonicalized.value(), route)); + } + } + if (!routes.empty()) { + return std::min_element( + routes.cbegin(), routes.cend(), + [](const auto & a, const auto & b) { return a.second.size() < b.second.size(); }) + ->first; + } + } + return std::nullopt; +} + namespace pedestrian { /* From e4401e738541772a53b086e0c6d8d19ef6795862 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Fri, 24 Jan 2025 14:37:59 +0100 Subject: [PATCH 11/18] Review changes --- .../behavior_tree_plugin/action_node.hpp | 2 +- .../behavior_tree_plugin/src/action_node.cpp | 107 ++++++++++-------- .../follow_front_entity_action.cpp | 2 +- .../include/traffic_simulator/utils/pose.hpp | 2 +- .../src/hdmap_utils/hdmap_utils.cpp | 36 +++--- .../traffic_simulator/src/utils/distance.cpp | 2 +- .../traffic_simulator/src/utils/pose.cpp | 27 +++-- 7 files changed, 104 insertions(+), 74 deletions(-) diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 058114bf65c..51b83c2910b 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -119,7 +119,7 @@ class ActionNode : public BT::ActionNodeBase EntityStatusDict other_entity_status; lanelet::Ids route_lanelets; - auto getDistanceToTargetEntityPolygon( + auto getDistanceToTargetEntity( const math::geometry::CatmullRomSplineInterface & spline, const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional; diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 50bd37befe7..6c95f6ca3d3 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -237,8 +237,8 @@ auto ActionNode::getDistanceToStopLine( auto ActionNode::getDistanceToFrontEntity( const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional { - if (const auto entity_name_opt = getFrontEntityName(spline)) { - return getDistanceToTargetEntityPolygon(spline, getEntityStatus(entity_name_opt.value())); + if (const auto entity_name = getFrontEntityName(spline)) { + return getDistanceToTargetEntity(spline, getEntityStatus(entity_name.value())); } else { return std::nullopt; } @@ -250,7 +250,7 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf std::vector distances; std::vector entities; for (const auto & each : other_entity_status) { - const auto distance = getDistanceToTargetEntityPolygon(spline, each.second); + const auto distance = getDistanceToTargetEntity(spline, each.second); const auto quat = math::geometry::getRotation( canonicalized_entity_status->getMapPose().orientation, other_entity_status.at(each.first).getMapPose().orientation); @@ -298,59 +298,76 @@ auto ActionNode::getEntityStatus(const std::string & target_name) const } } -auto ActionNode::getDistanceToTargetEntityPolygon( +/** + * @note getDistanceToTargetEntity working schematics + * + * 1. Check if route to target entity from refrence entity exists, if not try to tranform pose to other + * routable lanelet, within matching distance (transformToRoutableCanonicalizedLaneletPose). + * 2. Calculate longitudinal distance between entities bounding boxes -> bounding_box_distance. + * 3. Calculate longitudinal distance between entities poses -> position_distance. + * 4. Calculate target entity bounding box distance to refrence entity spline (minimal distance from all corners) + * -> target_to_spline_distance. + * 5. If target_to_spline_distance is less than half width of refrence entity target entity is confilicting. + * 6. Check corner case where target entity width is bigger than width of entity and target entity + * is excatly on the spline -> spline.getCollisionPointIn2D + * 7. If target entity is conflicting return bounding_box_distance enlarged by half of the enity + * length. + */ +auto ActionNode::getDistanceToTargetEntity( const math::geometry::CatmullRomSplineInterface & spline, const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional { if ( status.laneMatchingSucceed() && canonicalized_entity_status->laneMatchingSucceed() && isOtherEntityAtConsideredAltitude(status)) { - /* - * boundingBoxRelativeLaneletPose requires routing_configuration, - * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets - */ + /** + * boundingBoxLaneLongitudinalDistance requires routing_configuration, + * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets + * + */ traffic_simulator::RoutingConfiguration routing_configuration; routing_configuration.allow_lane_change = true; constexpr bool include_adjacent_lanelet{false}; constexpr bool include_opposite_direction{true}; + constexpr bool search_backward{false}; - const auto & from_lanelet_pose = - canonicalized_entity_status->getCanonicalizedLaneletPose().value(); - const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox(); - - const auto & to_bounding_box = status.getBoundingBox(); - const auto & to_lanelet_pose = - traffic_simulator::pose::tranformToRoutableCanonicalizedLaneletPose( + const auto & target_bounding_box = status.getBoundingBox(); + const auto & target_lanelet_pose = + traffic_simulator::pose::transformToRoutableCanonicalizedLaneletPose( canonicalized_entity_status->getLaneletId(), status.getCanonicalizedLaneletPose().value(), - to_bounding_box, hdmap_utils); - - if (to_lanelet_pose.has_value()) { - const auto longitudinal_target_distance = traffic_simulator::distance::longitudinalDistance( - from_lanelet_pose, *to_lanelet_pose, include_adjacent_lanelet, include_opposite_direction, - routing_configuration, hdmap_utils); - - if (longitudinal_target_distance) { - const auto distance_to_spline = traffic_simulator::distance::distanceToSpline( - static_cast(*to_lanelet_pose), to_bounding_box, spline, - longitudinal_target_distance.value()); - - const auto longitudinal_bounding_box_target_distance = - traffic_simulator::distance::boundingBoxLaneLongitudinalDistance( - from_lanelet_pose, from_bounding_box, *to_lanelet_pose, to_bounding_box, - include_adjacent_lanelet, include_opposite_direction, routing_configuration, - hdmap_utils); - - const auto polygon = math::geometry::transformPoints( - status.getMapPose(), math::geometry::getPointsFromBbox(to_bounding_box)); - - // is in front and is within considered width (from_bounding_box) - if ( - longitudinal_bounding_box_target_distance && - longitudinal_bounding_box_target_distance.value() >= 0 && - ((distance_to_spline <= (from_bounding_box.dimensions.y / 2)) || - spline.getCollisionPointIn2D(polygon, false))) { - return longitudinal_bounding_box_target_distance.value() + - from_bounding_box.dimensions.x / 2; + target_bounding_box, hdmap_utils); + + if (target_lanelet_pose) { + const auto & from_lanelet_pose = canonicalized_entity_status->getCanonicalizedLaneletPose(); + const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox(); + if (const auto bounding_box_distance = + traffic_simulator::distance::boundingBoxLaneLongitudinalDistance( + *from_lanelet_pose, from_bounding_box, *target_lanelet_pose, target_bounding_box, + include_adjacent_lanelet, include_opposite_direction, routing_configuration, + hdmap_utils); + not bounding_box_distance || bounding_box_distance.value() < 0.0) { + return std::nullopt; + } else if (const auto position_distance = traffic_simulator::distance::longitudinalDistance( + *from_lanelet_pose, *target_lanelet_pose, include_adjacent_lanelet, + include_opposite_direction, routing_configuration, hdmap_utils); + not position_distance) { + return std::nullopt; + } else { + const auto target_bounding_box_distance = + bounding_box_distance.value() + from_bounding_box.dimensions.x / 2.0; + + /// @note if the distance of the target entity to the spline is smaller than the width of the reference entity + if (const auto target_to_spline_distance = traffic_simulator::distance::distanceToSpline( + static_cast(*target_lanelet_pose), target_bounding_box, + spline, position_distance.value()); + target_to_spline_distance <= from_bounding_box.dimensions.y / 2.0) { + return target_bounding_box_distance; + } + /// @note if the distance of the target entity to the spline cannot be calculated because a collision occurs + else if (const auto target_polygon = math::geometry::transformPoints( + status.getMapPose(), math::geometry::getPointsFromBbox(target_bounding_box)); + spline.getCollisionPointIn2D(target_polygon, search_backward)) { + return target_bounding_box_distance; } } } @@ -379,7 +396,7 @@ auto ActionNode::getDistanceToConflictingEntity( } } for (const auto & status : lane_entity_status) { - if (const auto distance_to_entity = getDistanceToTargetEntityPolygon(spline, status)) { + if (const auto distance_to_entity = getDistanceToTargetEntity(spline, status)) { distances.insert(distance_to_entity.value()); } } diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp index 668c606a3a2..87b19b0b70d 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp @@ -97,7 +97,7 @@ BT::NodeStatus FollowFrontEntityAction::tick() return BT::NodeStatus::FAILURE; } const auto & front_entity_status = getEntityStatus(front_entity_name.value()); - distance_to_front_entity_ = getDistanceToTargetEntityPolygon(*trajectory, front_entity_status); + distance_to_front_entity_ = getDistanceToTargetEntity(*trajectory, front_entity_status); if (!distance_to_front_entity_) { return BT::NodeStatus::FAILURE; } diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 8a376b8846f..a69fe555cfb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -105,7 +105,7 @@ auto laneletLength( const lanelet::Id lanelet_id, const std::shared_ptr & hdmap_utils_ptr) -> double; -auto tranformToRoutableCanonicalizedLaneletPose( +auto transformToRoutableCanonicalizedLaneletPose( const lanelet::Id from_lanelet_id, const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const std::shared_ptr & hdmap_utils_ptr) diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 4b805353b75..b4f8bdf3552 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -562,19 +562,19 @@ auto HdMapUtils::findMatchingLanes( matches = lanelet::matching::removeNonRuleCompliantMatches( matches, routing_graphs_->traffic_rule(type)); } - if (matches.empty()) { - return std::nullopt; - } - std::set> id_and_distance; - for (const auto & match : matches) { - if (const auto lanelet_pose = toLaneletPose(map_pose, match.lanelet.id(), matching_distance)) { - id_and_distance.emplace(lanelet_pose->offset, lanelet_pose->lanelet_id); + if (not matches.empty()) { + std::set> distances_with_ids; + for (const auto & match : matches) { + if ( + const auto lanelet_pose = toLaneletPose(map_pose, match.lanelet.id(), matching_distance)) { + distances_with_ids.emplace(lanelet_pose->offset, lanelet_pose->lanelet_id); + } + } + if (not distances_with_ids.empty()) { + return distances_with_ids; } } - if (id_and_distance.empty()) { - return std::nullopt; - } - return id_and_distance; + return std::nullopt; } auto HdMapUtils::matchToLane( @@ -582,13 +582,14 @@ auto HdMapUtils::matchToLane( const bool include_crosswalk, const double matching_distance, const double reduction_ratio, const traffic_simulator::RoutingGraphType type) const -> std::optional { - const auto & matching_lanes = - findMatchingLanes(pose, bbox, include_crosswalk, matching_distance, reduction_ratio, type); - // find best match (minimize offset) - if (matching_lanes) { + /// @note findMatchingLanes returns a container sorted by distance - increasing, return the nearest id + if ( + const auto matching_lanes = + findMatchingLanes(pose, bbox, include_crosswalk, matching_distance, reduction_ratio, type)) { return matching_lanes->begin()->second; + } else { + return std::nullopt; } - return std::nullopt; } auto HdMapUtils::toLaneletPose( @@ -1233,7 +1234,8 @@ auto HdMapUtils::getLeftLaneletIds( { if (include_opposite_direction) { throw common::Error( - "HdMapUtils::getLeftLaneletIds with include_opposite_direction=true is not implemented yet."); + "HdMapUtils::getLeftLaneletIds with include_opposite_direction=true is not implemented " + "yet."); } else { return getLaneletIds( routing_graphs_->routing_graph(type)->lefts(lanelet_map_ptr_->laneletLayer.get(lanelet_id))); diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 5d15e7d202a..ea488099c86 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -332,7 +332,7 @@ auto distanceToSpline( auto s_start_distance = spline.getSquaredDistanceIn2D(point, s_start); auto s_end_distance = spline.getSquaredDistanceIn2D(point, s_end); - while (std::abs(s_start_distance - s_end_distance) > distance_accuracy) { + while (std::abs(s_start - s_end) > distance_accuracy) { if (s_start_distance > s_end_distance) { s_start += (s_end - s_start) / 2; s_start_distance = spline.getSquaredDistanceIn2D(point, s_start); diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 65cbd278515..703f7c51ca1 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -282,25 +282,33 @@ auto laneletLength( return hdmap_utils_ptr->getLaneletLength(lanelet_id); } -auto tranformToRoutableCanonicalizedLaneletPose( - const lanelet::Id from_lanelet_id, const CanonicalizedLaneletPose & canonicalized_lanelet_pose, +auto transformToRoutableCanonicalizedLaneletPose( + const lanelet::Id from_lanelet_id, const CanonicalizedLaneletPose & to_canonicalized_lanelet_pose, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const std::shared_ptr & hdmap_utils_ptr) -> std::optional { + /// @note search_distance should be minimal, just to check nearest neighbour lanelets constexpr auto search_distance{3.0}; + /// @note default_match_to_lane_reduction_ratio is constant described in hdmap_utils.hpp + constexpr auto default_match_to_lane_reduction_ratio{0.8}; constexpr auto include_crosswalk{false}; + /** + * @note hdmap_utils_ptr->getRoute requires routing_configuration, + * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets + */ RoutingConfiguration routing_configuration; routing_configuration.allow_lane_change = true; /// @note if there is already a route from_lanelet_id->to_lanelet_id, return it /// if not, transform the 'to_lanelet_id' position into the nearby lanelets and search for a route in relation to them - const auto to_lanelet_id = canonicalized_lanelet_pose.getLaneletPose().lanelet_id; + const auto to_lanelet_id = to_canonicalized_lanelet_pose.getLaneletPose().lanelet_id; if (!hdmap_utils_ptr->getRoute(from_lanelet_id, to_lanelet_id, routing_configuration).empty()) { - return canonicalized_lanelet_pose; + return to_canonicalized_lanelet_pose; } else if (const auto nearby_lanelet_ids = hdmap_utils_ptr->findMatchingLanes( - static_cast(canonicalized_lanelet_pose), to_bounding_box, - include_crosswalk, search_distance, 0.8, routing_configuration.routing_graph_type); + static_cast(to_canonicalized_lanelet_pose), + to_bounding_box, include_crosswalk, search_distance, + default_match_to_lane_reduction_ratio, routing_configuration.routing_graph_type); nearby_lanelet_ids.has_value()) { std::vector> routes; for (const auto & [distance, lanelet_id] : nearby_lanelet_ids.value()) { @@ -309,20 +317,23 @@ auto tranformToRoutableCanonicalizedLaneletPose( lanelet_id == to_lanelet_id || route.empty()) { continue; } else if (const auto lanelet_pose = hdmap_utils_ptr->toLaneletPose( - static_cast(canonicalized_lanelet_pose), lanelet_id, + static_cast(to_canonicalized_lanelet_pose), lanelet_id, search_distance); !lanelet_pose) { continue; } else if (const auto canonicalized = canonicalize(lanelet_pose.value(), hdmap_utils_ptr); canonicalized) { - routes.emplace_back(std::make_pair(canonicalized.value(), route)); + routes.emplace_back(std::move(canonicalized.value()), std::move(route)); } } if (!routes.empty()) { + /// @note we want to have alternative lanelet pose with shortest route so we search for minimum return std::min_element( routes.cbegin(), routes.cend(), [](const auto & a, const auto & b) { return a.second.size() < b.second.size(); }) ->first; + } else { + return std::nullopt; } } return std::nullopt; From e3d440d1e6a03e034a260050ff57cbfdb00af177 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Fri, 24 Jan 2025 16:38:39 +0100 Subject: [PATCH 12/18] Formatting changes --- simulation/behavior_tree_plugin/src/action_node.cpp | 3 +-- simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 6c95f6ca3d3..d583b03f5b0 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -323,8 +323,7 @@ auto ActionNode::getDistanceToTargetEntity( /** * boundingBoxLaneLongitudinalDistance requires routing_configuration, * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets - * - */ + */ traffic_simulator::RoutingConfiguration routing_configuration; routing_configuration.allow_lane_change = true; constexpr bool include_adjacent_lanelet{false}; diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index b4f8bdf3552..ced06299c10 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1234,8 +1234,7 @@ auto HdMapUtils::getLeftLaneletIds( { if (include_opposite_direction) { throw common::Error( - "HdMapUtils::getLeftLaneletIds with include_opposite_direction=true is not implemented " - "yet."); + "HdMapUtils::getLeftLaneletIds with include_opposite_direction=true is not implemented yet."); } else { return getLaneletIds( routing_graphs_->routing_graph(type)->lefts(lanelet_map_ptr_->laneletLayer.get(lanelet_id))); From 6da66781f082bd92b972c9e06d08c1488f17700f Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Fri, 24 Jan 2025 17:36:11 +0100 Subject: [PATCH 13/18] Fix spell check errors --- simulation/behavior_tree_plugin/src/action_node.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 75f4f352030..2ed6ec9853f 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -302,16 +302,16 @@ auto ActionNode::getEntityStatus(const std::string & target_name) const /** * @note getDistanceToTargetEntity working schematics * - * 1. Check if route to target entity from refrence entity exists, if not try to tranform pose to other + * 1. Check if route to target entity from reference entity exists, if not try to transform pose to other * routable lanelet, within matching distance (transformToRoutableCanonicalizedLaneletPose). * 2. Calculate longitudinal distance between entities bounding boxes -> bounding_box_distance. * 3. Calculate longitudinal distance between entities poses -> position_distance. - * 4. Calculate target entity bounding box distance to refrence entity spline (minimal distance from all corners) + * 4. Calculate target entity bounding box distance to reference entity spline (minimal distance from all corners) * -> target_to_spline_distance. - * 5. If target_to_spline_distance is less than half width of refrence entity target entity is confilicting. + * 5. If target_to_spline_distance is less than half width of reference entity target entity is conflicting. * 6. Check corner case where target entity width is bigger than width of entity and target entity - * is excatly on the spline -> spline.getCollisionPointIn2D - * 7. If target entity is conflicting return bounding_box_distance enlarged by half of the enity + * is exactly on the spline -> spline.getCollisionPointIn2D + * 7. If target entity is conflicting return bounding_box_distance enlarged by half of the entity * length. */ auto ActionNode::getDistanceToTargetEntity( From 11ed40c42c388809d95364875907977973b57893 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Wed, 29 Jan 2025 09:18:20 +0100 Subject: [PATCH 14/18] SonarCloud issues changes --- .../behavior_tree_plugin/src/action_node.cpp | 95 ++++++++++--------- .../src/lanelet_wrapper/pose.cpp | 3 +- .../traffic_simulator/src/utils/distance.cpp | 2 +- .../traffic_simulator/src/utils/pose.cpp | 6 +- 4 files changed, 53 insertions(+), 53 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 2ed6ec9853f..709188a20ab 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -319,56 +319,57 @@ auto ActionNode::getDistanceToTargetEntity( const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional { if ( - status.laneMatchingSucceed() && canonicalized_entity_status->laneMatchingSucceed() && - isOtherEntityAtConsideredAltitude(status)) { - /** + !status.laneMatchingSucceed() || !canonicalized_entity_status->laneMatchingSucceed() || + !isOtherEntityAtConsideredAltitude(status)) { + return std::nullopt; + } + /** * boundingBoxLaneLongitudinalDistance requires routing_configuration, * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets */ - traffic_simulator::RoutingConfiguration routing_configuration; - routing_configuration.allow_lane_change = true; - constexpr bool include_adjacent_lanelet{false}; - constexpr bool include_opposite_direction{true}; - constexpr bool search_backward{false}; - - const auto & target_bounding_box = status.getBoundingBox(); - const auto & target_lanelet_pose = - traffic_simulator::pose::transformToRoutableCanonicalizedLaneletPose( - canonicalized_entity_status->getLaneletId(), status.getCanonicalizedLaneletPose().value(), - target_bounding_box, hdmap_utils); - - if (target_lanelet_pose) { - const auto & from_lanelet_pose = canonicalized_entity_status->getCanonicalizedLaneletPose(); - const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox(); - if (const auto bounding_box_distance = - traffic_simulator::distance::boundingBoxLaneLongitudinalDistance( - *from_lanelet_pose, from_bounding_box, *target_lanelet_pose, target_bounding_box, - include_adjacent_lanelet, include_opposite_direction, routing_configuration, - hdmap_utils); - not bounding_box_distance || bounding_box_distance.value() < 0.0) { - return std::nullopt; - } else if (const auto position_distance = traffic_simulator::distance::longitudinalDistance( - *from_lanelet_pose, *target_lanelet_pose, include_adjacent_lanelet, - include_opposite_direction, routing_configuration, hdmap_utils); - not position_distance) { - return std::nullopt; - } else { - const auto target_bounding_box_distance = - bounding_box_distance.value() + from_bounding_box.dimensions.x / 2.0; - - /// @note if the distance of the target entity to the spline is smaller than the width of the reference entity - if (const auto target_to_spline_distance = traffic_simulator::distance::distanceToSpline( - static_cast(*target_lanelet_pose), target_bounding_box, - spline, position_distance.value()); - target_to_spline_distance <= from_bounding_box.dimensions.y / 2.0) { - return target_bounding_box_distance; - } - /// @note if the distance of the target entity to the spline cannot be calculated because a collision occurs - else if (const auto target_polygon = math::geometry::transformPoints( - status.getMapPose(), math::geometry::getPointsFromBbox(target_bounding_box)); - spline.getCollisionPointIn2D(target_polygon, search_backward)) { - return target_bounding_box_distance; - } + traffic_simulator::RoutingConfiguration routing_configuration; + routing_configuration.allow_lane_change = true; + constexpr bool include_adjacent_lanelet{false}; + constexpr bool include_opposite_direction{true}; + constexpr bool search_backward{false}; + + const auto & target_bounding_box = status.getBoundingBox(); + const auto & target_lanelet_pose = + traffic_simulator::pose::transformToRoutableCanonicalizedLaneletPose( + canonicalized_entity_status->getLaneletId(), status.getCanonicalizedLaneletPose().value(), + target_bounding_box, hdmap_utils); + + if (target_lanelet_pose) { + const auto & from_lanelet_pose = canonicalized_entity_status->getCanonicalizedLaneletPose(); + const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox(); + if (const auto bounding_box_distance = + traffic_simulator::distance::boundingBoxLaneLongitudinalDistance( + *from_lanelet_pose, from_bounding_box, *target_lanelet_pose, target_bounding_box, + include_adjacent_lanelet, include_opposite_direction, routing_configuration, + hdmap_utils); + !bounding_box_distance || bounding_box_distance.value() < 0.0) { + return std::nullopt; + } else if (const auto position_distance = traffic_simulator::distance::longitudinalDistance( + *from_lanelet_pose, *target_lanelet_pose, include_adjacent_lanelet, + include_opposite_direction, routing_configuration, hdmap_utils); + !position_distance) { + return std::nullopt; + } else { + const auto target_bounding_box_distance = + bounding_box_distance.value() + from_bounding_box.dimensions.x / 2.0; + + /// @note if the distance of the target entity to the spline is smaller than the width of the reference entity + if (const auto target_to_spline_distance = traffic_simulator::distance::distanceToSpline( + static_cast(*target_lanelet_pose), target_bounding_box, + spline, position_distance.value()); + target_to_spline_distance <= from_bounding_box.dimensions.y / 2.0) { + return target_bounding_box_distance; + } + /// @note if the distance of the target entity to the spline cannot be calculated because a collision occurs + else if (const auto target_polygon = math::geometry::transformPoints( + status.getMapPose(), math::geometry::getPointsFromBbox(target_bounding_box)); + spline.getCollisionPointIn2D(target_polygon, search_backward)) { + return target_bounding_box_distance; } } } diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 4af7b3c7e6c..b27ec2a6736 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -414,8 +414,7 @@ auto findMatchingLanes( const traffic_simulator::RoutingGraphType type) -> std::optional>> { - const auto absoluteHullPolygon = [&reduction_ratio, - &bounding_box](const auto & pose) -> lanelet::BasicPolygon2d { + const auto absoluteHullPolygon = [&reduction_ratio, &bounding_box](const auto & pose) { auto relative_hull = lanelet::matching::Hull2d{ lanelet::BasicPoint2d{ bounding_box.center.x + bounding_box.dimensions.x * 0.5 * reduction_ratio, diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index a80e5358bd8..9c728b6d24e 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -324,7 +324,7 @@ auto distanceToSpline( const auto bounding_box_diagonal_length = math::geometry::getDistance(bounding_box_map_points[0], bounding_box_map_points[2]); - /// @todo it may be a good idea to develop spline.getSquaredDistanceIn2D(point, s_start, s_end); + /// @note it may be a good idea to develop spline.getSquaredDistanceIn2D(point, s_start, s_end); std::vector distances; for (const auto & point : bounding_box_map_points) { auto s_start = s_reference - bounding_box_diagonal_length / 2; diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index a8e65744b77..0a9a3674666 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -348,8 +348,8 @@ auto transformToRoutableCanonicalizedLaneletPose( /// @note if there is already a route from_lanelet_id->to_lanelet_id, return it /// if not, transform the 'to_lanelet_id' position into the nearby lanelets and search for a route in relation to them - const auto to_lanelet_id = to_canonicalized_lanelet_pose.getLaneletPose().lanelet_id; - if (!hdmap_utils_ptr->getRoute(from_lanelet_id, to_lanelet_id, routing_configuration).empty()) { + if (const auto to_lanelet_id = to_canonicalized_lanelet_pose.getLaneletPose().lanelet_id; + !hdmap_utils_ptr->getRoute(from_lanelet_id, to_lanelet_id, routing_configuration).empty()) { return to_canonicalized_lanelet_pose; } else if (const auto nearby_lanelet_ids = lanelet_wrapper::pose::findMatchingLanes( static_cast(to_canonicalized_lanelet_pose), @@ -369,7 +369,7 @@ auto transformToRoutableCanonicalizedLaneletPose( continue; } else if (const auto canonicalized = toCanonicalizedLaneletPose(lanelet_pose.value()); canonicalized) { - routes.emplace_back(std::move(canonicalized.value()), std::move(route)); + routes.emplace_back(canonicalized.value(), route); } } if (!routes.empty()) { From 452398822e0d41557e0c08c15943beab4b4035d5 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Wed, 29 Jan 2025 11:34:30 +0100 Subject: [PATCH 15/18] SonarCloud issues changes part 2 --- simulation/behavior_tree_plugin/src/action_node.cpp | 10 +++++----- .../traffic_simulator/src/lanelet_wrapper/pose.cpp | 4 ++-- simulation/traffic_simulator/src/utils/pose.cpp | 6 +++--- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 709188a20ab..329792bac5a 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -334,12 +334,12 @@ auto ActionNode::getDistanceToTargetEntity( constexpr bool search_backward{false}; const auto & target_bounding_box = status.getBoundingBox(); - const auto & target_lanelet_pose = - traffic_simulator::pose::transformToRoutableCanonicalizedLaneletPose( - canonicalized_entity_status->getLaneletId(), status.getCanonicalizedLaneletPose().value(), - target_bounding_box, hdmap_utils); - if (target_lanelet_pose) { + if (const auto & target_lanelet_pose = + traffic_simulator::pose::transformToRoutableCanonicalizedLaneletPose( + canonicalized_entity_status->getLaneletId(), status.getCanonicalizedLaneletPose().value(), + target_bounding_box, hdmap_utils); + target_lanelet_pose) { const auto & from_lanelet_pose = canonicalized_entity_status->getCanonicalizedLaneletPose(); const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox(); if (const auto bounding_box_distance = diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index b27ec2a6736..aa98e68373a 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -443,7 +443,7 @@ auto findMatchingLanes( matches = lanelet::matching::removeNonRuleCompliantMatches(matches, LaneletWrapper::trafficRules(type)); } - if (not matches.empty()) { + if (!matches.empty()) { std::set> distances_with_ids; for (const auto & match : matches) { if ( @@ -451,7 +451,7 @@ auto findMatchingLanes( distances_with_ids.emplace(lanelet_pose->offset, lanelet_pose->lanelet_id); } } - if (not distances_with_ids.empty()) { + if (!distances_with_ids.empty()) { return distances_with_ids; } } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 0a9a3674666..06fdf94b493 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -358,7 +358,7 @@ auto transformToRoutableCanonicalizedLaneletPose( nearby_lanelet_ids.has_value()) { std::vector> routes; for (const auto & [distance, lanelet_id] : nearby_lanelet_ids.value()) { - if (const auto route = + if (auto route = hdmap_utils_ptr->getRoute(from_lanelet_id, lanelet_id, routing_configuration); lanelet_id == to_lanelet_id || route.empty()) { continue; @@ -367,9 +367,9 @@ auto transformToRoutableCanonicalizedLaneletPose( search_distance); !lanelet_pose) { continue; - } else if (const auto canonicalized = toCanonicalizedLaneletPose(lanelet_pose.value()); + } else if (auto canonicalized = toCanonicalizedLaneletPose(lanelet_pose.value()); canonicalized) { - routes.emplace_back(canonicalized.value(), route); + routes.emplace_back(std::move(canonicalized.value()), std::move(route)); } } if (!routes.empty()) { From c689f2b90922da1554fe30baccec05c4ba3844a1 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Thu, 30 Jan 2025 09:01:46 +0100 Subject: [PATCH 16/18] Resolve building problem after merge --- simulation/behavior_tree_plugin/src/action_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index a64d79399ae..d92659bd439 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -246,7 +246,7 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf std::vector distances; std::vector entities; for (const auto & [name, status] : other_entity_status) { - const auto distance = getDistanceToTargetEntityPolygon(spline, name); + const auto distance = getDistanceToTargetEntity(spline, status); const auto quat = math::geometry::getRotation( canonicalized_entity_status->getMapPose().orientation, other_entity_status.at(name).getMapPose().orientation); From 2bfa5fdfb1c93a0db48ce294d038c4353b8a66f8 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Mon, 3 Feb 2025 09:26:34 +0100 Subject: [PATCH 17/18] Review changes --- .../behavior_tree_plugin/src/action_node.cpp | 4 ++-- .../include/traffic_simulator/utils/pose.hpp | 2 +- .../traffic_simulator/src/utils/distance.cpp | 18 +++++++++++++----- .../traffic_simulator/src/utils/pose.cpp | 2 +- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index d92659bd439..5cfcd2266f2 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -298,7 +298,7 @@ auto ActionNode::getEntityStatus(const std::string & target_name) const * @note getDistanceToTargetEntity working schematics * * 1. Check if route to target entity from reference entity exists, if not try to transform pose to other - * routable lanelet, within matching distance (transformToRoutableCanonicalizedLaneletPose). + * routable lanelet, within matching distance (findRoutableAlternativeLaneletPoseFrom). * 2. Calculate longitudinal distance between entities bounding boxes -> bounding_box_distance. * 3. Calculate longitudinal distance between entities poses -> position_distance. * 4. Calculate target entity bounding box distance to reference entity spline (minimal distance from all corners) @@ -331,7 +331,7 @@ auto ActionNode::getDistanceToTargetEntity( const auto & target_bounding_box = status.getBoundingBox(); if (const auto & target_lanelet_pose = - traffic_simulator::pose::transformToRoutableCanonicalizedLaneletPose( + traffic_simulator::pose::findRoutableAlternativeLaneletPoseFrom( canonicalized_entity_status->getLaneletId(), status.getCanonicalizedLaneletPose().value(), target_bounding_box, hdmap_utils); target_lanelet_pose) { diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 90f388111ca..ac269269542 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -106,7 +106,7 @@ auto isAtEndOfLanelets( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const std::shared_ptr & hdmap_utils_ptr) -> bool; -auto transformToRoutableCanonicalizedLaneletPose( +auto findRoutableAlternativeLaneletPoseFrom( const lanelet::Id from_lanelet_id, const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const std::shared_ptr & hdmap_utils_ptr) diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 9c728b6d24e..4ce54c7999f 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -317,6 +317,12 @@ auto distanceToSpline( const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const math::geometry::CatmullRomSplineInterface & spline, const double s_reference) -> double { + /* + * Convergence threshold for binary search. + * The search stops when the interval between `s_start` and `s_end` is below this value. + * The value 0.05 was chosen empirically to balance accuracy and performance. + * A smaller value improves precision but increases computation time. + */ constexpr double distance_accuracy{0.05}; const auto bounding_box_map_points = @@ -333,15 +339,17 @@ auto distanceToSpline( auto s_end_distance = spline.getSquaredDistanceIn2D(point, s_end); while (std::abs(s_start - s_end) > distance_accuracy) { + double s_mid = std::midpoint(s_start, s_end); + double s_mid_distance = spline.getSquaredDistanceIn2D(point, s_mid); if (s_start_distance > s_end_distance) { - s_start += (s_end - s_start) / 2; - s_start_distance = spline.getSquaredDistanceIn2D(point, s_start); + s_start = s_mid; + s_start_distance = s_mid_distance; } else { - s_end -= (s_end - s_start) / 2; - s_end_distance = spline.getSquaredDistanceIn2D(point, s_end); + s_end = s_mid; + s_end_distance = s_mid_distance; } } - distances.push_back(s_start_distance > s_end_distance ? s_end_distance : s_start_distance); + distances.push_back(std::min(s_start_distance, s_end_distance)); } return std::sqrt(*std::min_element(distances.begin(), distances.end())); } diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 06fdf94b493..b04df57b577 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -328,7 +328,7 @@ auto isAtEndOfLanelets( lanelet_wrapper::lanelet_map::laneletLength(lanelet_pose.lanelet_id) <= lanelet_pose.s; } -auto transformToRoutableCanonicalizedLaneletPose( +auto findRoutableAlternativeLaneletPoseFrom( const lanelet::Id from_lanelet_id, const CanonicalizedLaneletPose & to_canonicalized_lanelet_pose, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const std::shared_ptr & hdmap_utils_ptr) From 2661901ff256643570d7107a999fb172fbeeb350 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Mon, 3 Feb 2025 10:27:16 +0100 Subject: [PATCH 18/18] Remove std::midpoint --- simulation/traffic_simulator/src/utils/distance.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 4ce54c7999f..880a9cbfa5e 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -339,7 +339,7 @@ auto distanceToSpline( auto s_end_distance = spline.getSquaredDistanceIn2D(point, s_end); while (std::abs(s_start - s_end) > distance_accuracy) { - double s_mid = std::midpoint(s_start, s_end); + double s_mid = s_start + (s_end - s_start) / 2; double s_mid_distance = spline.getSquaredDistanceIn2D(point, s_mid); if (s_start_distance > s_end_distance) { s_start = s_mid;