From fedbf3a7086b223bb031b52afc4717236e95893d Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 29 Feb 2024 12:01:29 +0100 Subject: [PATCH 1/4] Use entity specific lanelet matching distance for setting entity status Signed-off-by: Mateusz Palczuk --- .../include/traffic_simulator/api/api.hpp | 1 + .../include/traffic_simulator/entity/entity_manager.hpp | 1 + .../traffic_simulator/hdmap_utils/hdmap_utils.hpp | 4 ++-- simulation/traffic_simulator/src/api/api.cpp | 8 +++++--- .../traffic_simulator/src/hdmap_utils/hdmap_utils.cpp | 9 +++++---- 5 files changed, 14 insertions(+), 9 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 89e141727af..331b8c22790 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -331,6 +331,7 @@ class API FORWARD_TO_ENTITY_MANAGER(getRelativePose); FORWARD_TO_ENTITY_MANAGER(getStandStillDuration); FORWARD_TO_ENTITY_MANAGER(getTraveledDistance); + FORWARD_TO_ENTITY_MANAGER(getDefaultMatchingDistanceForLaneletPoseCalculation); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLight); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLights); FORWARD_TO_ENTITY_MANAGER(isEgoSpawned); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 9a7d3029d1b..2815bea854e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -304,6 +304,7 @@ class EntityManager FORWARD_TO_ENTITY(getRouteLanelets, const); FORWARD_TO_ENTITY(getStandStillDuration, const); FORWARD_TO_ENTITY(getTraveledDistance, const); + FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); FORWARD_TO_ENTITY(laneMatchingSucceed, const); FORWARD_TO_ENTITY(activateOutOfRangeJob, ); FORWARD_TO_ENTITY(cancelRequest, ); 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 24375e4b211..4c206b0e224 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 @@ -266,8 +266,8 @@ class HdMapUtils auto matchToLane( const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, - const bool include_crosswalk, const double reduction_ratio = 0.8) const - -> std::optional; + const bool include_crosswalk, const double matching_distance = 1.0, + const double reduction_ratio = 0.8) const -> std::optional; auto toLaneletPose( const geometry_msgs::msg::Pose &, const bool include_crosswalk, diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 31e87cd8354..5380b4ee676 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -78,7 +78,8 @@ auto API::setEntityStatus( status.action_status = action_status; if ( const auto lanelet_pose = entity_manager_ptr_->toLaneletPose( - status.pose, getBoundingBox(reference_entity_name), false)) { + status.pose, getBoundingBox(reference_entity_name), false, + getDefaultMatchingDistanceForLaneletPoseCalculation(reference_entity_name))) { status.lanelet_pose_valid = true; status.lanelet_pose = lanelet_pose.value(); } else { @@ -127,8 +128,9 @@ auto API::setEntityStatus( status.name = name; status.action_status = action_status; if ( - const auto lanelet_pose = - entity_manager_ptr_->toLaneletPose(map_pose, getBoundingBox(name), false)) { + const auto lanelet_pose = entity_manager_ptr_->toLaneletPose( + map_pose, getBoundingBox(name), false, + getDefaultMatchingDistanceForLaneletPoseCalculation(name))) { status.lanelet_pose = lanelet_pose.value(); } else { status.lanelet_pose_valid = false; diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index c17cebc71c6..413c0e1482d 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -492,7 +492,8 @@ auto HdMapUtils::toPoint2d(const geometry_msgs::msg::Point & point) const -> lan auto HdMapUtils::matchToLane( const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, - const bool include_crosswalk, const double reduction_ratio) const -> std::optional + const bool include_crosswalk, const double matching_distance, const double reduction_ratio) const + -> std::optional { std::optional id; lanelet::matching::Object2d obj; @@ -521,8 +522,8 @@ auto HdMapUtils::matchToLane( /** * @note Hard coded parameter. Matching threshold for lanelet. */ - if (match.distance <= 1.0) { - auto lanelet_pose = toLaneletPose(pose, match.lanelet.id()); + if (match.distance <= matching_distance) { + auto lanelet_pose = toLaneletPose(pose, match.lanelet.id(), matching_distance); if (lanelet_pose) { id_and_distance.emplace_back(lanelet_pose->lanelet_id, lanelet_pose->offset); } @@ -604,7 +605,7 @@ auto HdMapUtils::toLaneletPose( const bool include_crosswalk, const double matching_distance) const -> std::optional { - const auto lanelet_id = matchToLane(pose, bbox, include_crosswalk); + const auto lanelet_id = matchToLane(pose, bbox, include_crosswalk, matching_distance); if (!lanelet_id) { return toLaneletPose(pose, include_crosswalk, matching_distance); } From c43382122b218927e8e59026c94edf90d4e23a8d Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 29 Feb 2024 13:48:07 +0100 Subject: [PATCH 2/4] Simplify code Signed-off-by: Mateusz Palczuk --- .../src/hdmap_utils/hdmap_utils.cpp | 21 +++++++------------ 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 413c0e1482d..61e95b49966 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -510,7 +510,8 @@ auto HdMapUtils::matchToLane( 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, 3.0); + auto matches = + lanelet::matching::getDeterministicMatches(*lanelet_map_ptr_, obj, matching_distance); if (!include_crosswalk) { matches = lanelet::matching::removeNonRuleCompliantMatches(matches, traffic_rules_vehicle_ptr_); } @@ -519,23 +520,17 @@ auto HdMapUtils::matchToLane( } std::vector> id_and_distance; for (const auto & match : matches) { - /** - * @note Hard coded parameter. Matching threshold for lanelet. - */ - if (match.distance <= matching_distance) { - auto lanelet_pose = toLaneletPose(pose, match.lanelet.id(), matching_distance); - if (lanelet_pose) { - id_and_distance.emplace_back(lanelet_pose->lanelet_id, lanelet_pose->offset); - } + 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[0].first; + 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; } auto HdMapUtils::toLaneletPose( From d5fe4e33692e436423e09bbaeadf981a0ac6f65c Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 29 Feb 2024 16:42:08 +0100 Subject: [PATCH 3/4] Use correct entity parameters for lanelet matching Signed-off-by: Mateusz Palczuk --- simulation/traffic_simulator/src/api/api.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 5380b4ee676..30e8ebcad62 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -78,8 +78,8 @@ auto API::setEntityStatus( status.action_status = action_status; if ( const auto lanelet_pose = entity_manager_ptr_->toLaneletPose( - status.pose, getBoundingBox(reference_entity_name), false, - getDefaultMatchingDistanceForLaneletPoseCalculation(reference_entity_name))) { + status.pose, getBoundingBox(name), false, + getDefaultMatchingDistanceForLaneletPoseCalculation(name))) { status.lanelet_pose_valid = true; status.lanelet_pose = lanelet_pose.value(); } else { From 76bc24631c68acf93285a8d820229539e2d0a679 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 13 Mar 2024 23:00:45 +0900 Subject: [PATCH 4/4] make API::getDefaultMatchingDistanceForLaneletPoseCalculation to the private functions Signed-off-by: Masaya Kataoka --- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 331b8c22790..a98227d3c94 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -331,7 +331,6 @@ class API FORWARD_TO_ENTITY_MANAGER(getRelativePose); FORWARD_TO_ENTITY_MANAGER(getStandStillDuration); FORWARD_TO_ENTITY_MANAGER(getTraveledDistance); - FORWARD_TO_ENTITY_MANAGER(getDefaultMatchingDistanceForLaneletPoseCalculation); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLight); FORWARD_TO_ENTITY_MANAGER(getV2ITrafficLights); FORWARD_TO_ENTITY_MANAGER(isEgoSpawned); @@ -359,6 +358,10 @@ class API FORWARD_TO_ENTITY_MANAGER(setVelocityLimit); FORWARD_TO_ENTITY_MANAGER(toMapPose); +private: + FORWARD_TO_ENTITY_MANAGER(getDefaultMatchingDistanceForLaneletPoseCalculation); + +public: #undef FORWARD_TO_ENTITY_MANAGER auto canonicalize(const LaneletPose & maybe_non_canonicalized_lanelet_pose) const