Merge pull request #1208 from tier4/fix/lanelet-matching-distance

Fix/lanelet matching distance
tier4 · Mar 13, 2024 · 404cdde · 404cdde
2 parents 1027d47 + 76bc246
commit 404cdde
Show file tree

Hide file tree

Showing 5 changed files with 23 additions and 20 deletions.
diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp
@@ -358,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

diff --git 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
@@ -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<lanelet::Id>;
+    const bool include_crosswalk, const double matching_distance = 1.0,
+    const double reduction_ratio = 0.8) const -> std::optional<lanelet::Id>;
 
   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
@@ -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(name), false,
+      getDefaultMatchingDistanceForLaneletPoseCalculation(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
@@ -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<lanelet::Id>
+  const bool include_crosswalk, const double matching_distance, const double reduction_ratio) const
+  -> std::optional<lanelet::Id>
 {
   std::optional<lanelet::Id> id;
   lanelet::matching::Object2d obj;
@@ -509,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_);
   }
@@ -518,23 +520,17 @@ auto HdMapUtils::matchToLane(
   }
   std::vector<std::pair<lanelet::Id, double>> id_and_distance;
   for (const auto & match : matches) {
-    /**
-     * @note Hard coded parameter. Matching threshold for lanelet.
-     */
-    if (match.distance <= 1.0) {
-      auto lanelet_pose = toLaneletPose(pose, match.lanelet.id());
-      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(
@@ -604,7 +600,7 @@ auto HdMapUtils::toLaneletPose(
   const bool include_crosswalk, const double matching_distance) const
   -> std::optional<traffic_simulator_msgs::msg::LaneletPose>
 {
-  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);
   }