Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-route' into…

… ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-traffic-lights
tier4 · Feb 19, 2025 · 1eebf06 · 1eebf06
2 parents 48240f2 + 5891dd8
commit 1eebf06
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 19 deletions.
diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
@@ -107,6 +107,7 @@ auto ActionNode::getOtherEntitiesCanonicalizedEntityStatuses() const
   -> std::vector<traffic_simulator::CanonicalizedEntityStatus>
 {
   std::vector<traffic_simulator::CanonicalizedEntityStatus> other_status;
+  other_status.reserve(other_entity_status.size());
   for (const auto & [entity_name, entity_status] : other_entity_status) {
     other_status.push_back(entity_status);
   }
@@ -180,7 +181,14 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf
   constexpr double front_entity_angle_threshold{boost::math::constants::half_pi<double>()};
   constexpr double front_entity_horizon{40.0};
 
-  std::vector<std::pair<std::string, double>> entities_with_distances;
+  std::optional<std::string> front_entity_name;
+  auto try_front_entity = [&front_entity_name, min_distance = std::numeric_limits<double>::max()](
+                            const std::string & name, const double distance) mutable {
+    if (distance < min_distance) {
+      min_distance = distance;
+      front_entity_name = name;
+    }
+  };
   for (const auto & [other_name, other_status] : other_entity_status) {
     if (
       const auto & other_canonicalized_lanelet_pose = other_status.getCanonicalizedLaneletPose()) {
@@ -194,20 +202,13 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf
           other_status.getMapPose().orientation);
         const auto yaw = math::geometry::convertQuaternionToEulerAngle(quaternion).z;
         if (std::fabs(yaw) <= front_entity_angle_threshold) {
-          entities_with_distances.emplace_back(other_name, distance.value());
+          try_front_entity(other_name, distance.value());
         }
       }
     }
   }
 
-  if (!entities_with_distances.empty()) {
-    auto min_entity_with_distance_it = std::min_element(
-      entities_with_distances.begin(), entities_with_distances.end(),
-      [](const auto & lhs, const auto & rhs) { return lhs.second < rhs.second; });
-    return min_entity_with_distance_it->first;
-  } else {
-    return std::nullopt;
-  }
+  return front_entity_name;
 }
 
 auto ActionNode::getEntityStatus(const std::string & target_name) const

diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp
@@ -97,8 +97,9 @@ auto filterLaneletIds(const lanelet::Ids & lanelet_ids, const char subtype[]) ->
 {
   const auto convertToLanelets = [](const lanelet::Ids & lanelet_ids) -> lanelet::Lanelets {
     lanelet::Lanelets lanelets;
+    lanelets.reserve(lanelet_ids.size());
     for (const auto & id : lanelet_ids) {
-      lanelets.emplace_back(LaneletWrapper::map()->laneletLayer.get(id));
+      lanelets.push_back(LaneletWrapper::map()->laneletLayer.get(id));
     }
     return lanelets;
   };
@@ -373,7 +374,7 @@ auto conflictingCrosswalkIds(const lanelet::Ids & lanelet_ids) -> lanelet::Ids
     const auto & conflicting_crosswalks = graphs_container.conflictingInGraph(
       LaneletWrapper::map()->laneletLayer.get(lanelet_id), routing_graph_id, height_clearance);
     for (const auto & conflicting_crosswalk : conflicting_crosswalks) {
-      conflicting_crosswalk_ids.emplace_back(conflicting_crosswalk.id());
+      conflicting_crosswalk_ids.push_back(conflicting_crosswalk.id());
     }
   }
   return conflicting_crosswalk_ids;

diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_lights_base.cpp
@@ -150,7 +150,7 @@ auto TrafficLightsBase::getDistanceToActiveTrafficLightStopLine(
   if (traffic_light_ids.empty()) {
     return std::nullopt;
   }
-  std::set<double> collision_points = {};
+  std::optional<double> min_distance{std::nullopt};
   for (const auto id : traffic_light_ids) {
     using Color = traffic_simulator::TrafficLight::Color;
     using Status = traffic_simulator::TrafficLight::Status;
@@ -160,14 +160,13 @@ auto TrafficLightsBase::getDistanceToActiveTrafficLightStopLine(
         traffic_light.contains(Color::yellow, Status::solid_on, Shape::circle)) {
       const auto collision_point =
         traffic_simulator::distance::distanceToTrafficLightStopLine(spline, id);
-      if (collision_point) {
-        collision_points.insert(collision_point.value());
+      if (
+        collision_point.has_value() and
+        (not min_distance.has_value() or collision_point.value() < min_distance.value())) {
+        min_distance = collision_point;
       }
     }
   }
-  if (collision_points.empty()) {
-    return std::nullopt;
-  }
-  return *collision_points.begin();
+  return min_distance;
 }
 }  // namespace traffic_simulator