Merge pull request #1533 from tier4/refactor/lanelet_wrapper_bound

HdMapUtils refactor lanelet_wrapper::lanelet_map::leftBound rightBound

mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp

@@ -46,8 +46,7 @@ class GetDistanceToLaneBoundScenario : public cpp_mock_scenarios::CppScenarioNod
     }
     auto & ego_entity = api_.getEntity("ego");
     const auto distance = traffic_simulator::distance::distanceToLaneBound(
-      ego_entity.getMapPose(), ego_entity.getBoundingBox(), ego_entity.getRouteLanelets(),
-      api_.getHdmapUtils());
+      ego_entity.getMapPose(), ego_entity.getBoundingBox(), ego_entity.getRouteLanelets());
     // LCOV_EXCL_START
     if (distance <= 0.4 && distance >= 0.52) {
       stop(cpp_mock_scenarios::Result::FAILURE);

simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp

@@ -179,8 +179,6 @@ class HdMapUtils
     const traffic_simulator::RoutingConfiguration & routing_configuration =
       traffic_simulator::RoutingConfiguration()) const -> std::optional<double>;
 
-  auto getLeftBound(const lanelet::Id) const -> std::vector<geometry_msgs::msg::Point>;
-
   auto getLongitudinalDistance(
     const traffic_simulator_msgs::msg::LaneletPose & from_pose,
     const traffic_simulator_msgs::msg::LaneletPose & to_pose,
@@ -200,8 +198,6 @@ class HdMapUtils
     const traffic_simulator::RoutingGraphType type =
       traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids;
 
-  auto getRightBound(const lanelet::Id) const -> std::vector<geometry_msgs::msg::Point>;
-
   auto getRightOfWayLaneletIds(const lanelet::Ids &) const
     -> std::unordered_map<lanelet::Id, lanelet::Ids>;
 

simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp

@@ -89,6 +89,14 @@ auto previousLaneletIds(
 auto previousLaneletIds(
   const lanelet::Ids & lanelet_ids, std::string_view turn_direction,
   const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids;
+
+// Bounds
+auto leftBound(const lanelet::Id lanelet_id) -> std::vector<Point>;
+
+auto rightBound(const lanelet::Id lanelet_id) -> std::vector<Point>;
+
+// Polygons
+auto toPolygon(const lanelet::ConstLineString3d & line_string) -> std::vector<Point>;
 }  // namespace lanelet_map
 }  // namespace lanelet_wrapper
 }  // namespace traffic_simulator

simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp

@@ -74,33 +74,33 @@ auto boundingBoxLaneLongitudinalDistance(
 // Bounds
 auto distanceToLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double;
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id)
+  -> double;
 
 auto distanceToLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double;
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids)
+  -> double;
 
 auto distanceToLeftLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double;
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id)
+  -> double;
 
 auto distanceToLeftLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double;
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids)
+  -> double;
 
 auto distanceToRightLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double;
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id)
+  -> double;
 
 auto distanceToRightLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double;
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids)
+  -> double;
 
 // Other objects
 auto distanceToCrosswalk(

simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

@@ -740,18 +740,6 @@ auto HdMapUtils::getTrafficLightBulbPosition(
   return std::nullopt;
 }
 
-auto HdMapUtils::getLeftBound(const lanelet::Id lanelet_id) const
-  -> std::vector<geometry_msgs::msg::Point>
-{
-  return toPolygon(lanelet_map_ptr_->laneletLayer.get(lanelet_id).leftBound());
-}
-
-auto HdMapUtils::getRightBound(const lanelet::Id lanelet_id) const
-  -> std::vector<geometry_msgs::msg::Point>
-{
-  return toPolygon(lanelet_map_ptr_->laneletLayer.get(lanelet_id).rightBound());
-}
-
 auto HdMapUtils::getLaneChangeTrajectory(
   const traffic_simulator_msgs::msg::LaneletPose & from_pose,
   const traffic_simulator::lane_change::Parameter & lane_change_parameter) const

simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp

@@ -235,6 +235,28 @@ auto previousLaneletIds(
   }
   return lanelet::Ids(previous_lanelet_ids_set.begin(), previous_lanelet_ids_set.end());
 }
+
+// Bounds
+auto leftBound(const lanelet::Id lanelet_id) -> std::vector<Point>
+{
+  return toPolygon(LaneletWrapper::map()->laneletLayer.get(lanelet_id).leftBound());
+}
+
+auto rightBound(const lanelet::Id lanelet_id) -> std::vector<Point>
+{
+  return toPolygon(LaneletWrapper::map()->laneletLayer.get(lanelet_id).rightBound());
+}
+
+// Polygons
+auto toPolygon(const lanelet::ConstLineString3d & line_string) -> std::vector<Point>
+{
+  std::vector<Point> points;
+  points.reserve(line_string.size());
+  for (const auto & point : line_string) {
+    points.push_back(geometry_msgs::build<Point>().x(point.x()).y(point.y()).z(point.z()));
+  }
+  return points;
+}
 }  // namespace lanelet_map
 }  // namespace lanelet_wrapper
 }  // namespace traffic_simulator

simulation/traffic_simulator/src/utils/distance.cpp

@@ -15,6 +15,7 @@
 #include <geometry/bounding_box.hpp>
 #include <geometry/distance.hpp>
 #include <geometry/transform.hpp>
+#include <traffic_simulator/lanelet_wrapper/lanelet_map.hpp>
 #include <traffic_simulator/lanelet_wrapper/pose.hpp>
 #include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator_msgs/msg/waypoints_array.hpp>
@@ -197,12 +198,13 @@ auto boundingBoxLaneLongitudinalDistance(
   return std::nullopt;
 }
 
+// Bounds
 auto distanceToLeftLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id)
+  -> double
 {
-  if (const auto bound = hdmap_utils_ptr->getLeftBound(lanelet_id); bound.empty()) {
+  if (const auto bound = lanelet_wrapper::lanelet_map::leftBound(lanelet_id); bound.empty()) {
     THROW_SEMANTIC_ERROR(
       "Failed to calculate left bounds of lanelet_id : ", lanelet_id, " please check lanelet map.");
   } else if (const auto polygon =
@@ -216,26 +218,28 @@ auto distanceToLeftLaneBound(
 
 auto distanceToLeftLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids)
+  -> double
 {
   if (lanelet_ids.empty()) {
     THROW_SEMANTIC_ERROR("Failing to calculate distanceToLeftLaneBound given an empty vector.");
   }
-  std::vector<double> distances;
-  std::transform(
-    lanelet_ids.begin(), lanelet_ids.end(), std::back_inserter(distances), [&](auto lanelet_id) {
-      return distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr);
-    });
-  return *std::min_element(distances.begin(), distances.end());
+  double min_distance = std::numeric_limits<double>::max();
+  for (const auto & lanelet_id : lanelet_ids) {
+    const auto distance = distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id);
+    if (distance < min_distance) {
+      min_distance = distance;
+    }
+  }
+  return min_distance;
 }
 
 auto distanceToRightLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id)
+  -> double
 {
-  if (const auto bound = hdmap_utils_ptr->getRightBound(lanelet_id); bound.empty()) {
+  if (const auto & bound = lanelet_wrapper::lanelet_map::rightBound(lanelet_id); bound.empty()) {
     THROW_SEMANTIC_ERROR(
       "Failed to calculate right bounds of lanelet_id : ", lanelet_id,
       " please check lanelet map.");
@@ -250,38 +254,40 @@ auto distanceToRightLaneBound(
 
 auto distanceToRightLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids)
+  -> double
 {
   if (lanelet_ids.empty()) {
     THROW_SEMANTIC_ERROR("Failing to calculate distanceToRightLaneBound for given empty vector.");
   }
-  std::vector<double> distances;
-  std::transform(
-    lanelet_ids.begin(), lanelet_ids.end(), std::back_inserter(distances), [&](auto lanelet_id) {
-      return distanceToRightLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr);
-    });
-  return *std::min_element(distances.begin(), distances.end());
+  double min_distance = std::numeric_limits<double>::max();
+  for (const auto & lanelet_id : lanelet_ids) {
+    const double distance = distanceToRightLaneBound(map_pose, bounding_box, lanelet_id);
+    if (distance < min_distance) {
+      min_distance = distance;
+    }
+  }
+  return min_distance;
 }
 
 auto distanceToLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, lanelet::Id lanelet_id,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Id lanelet_id)
+  -> double
 {
   return std::min(
-    distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr),
-    distanceToRightLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr));
+    distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id),
+    distanceToRightLaneBound(map_pose, bounding_box, lanelet_id));
 }
 
 auto distanceToLaneBound(
   const geometry_msgs::msg::Pose & map_pose,
-  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> double
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids)
+  -> double
 {
   return std::min(
-    distanceToLeftLaneBound(map_pose, bounding_box, lanelet_ids, hdmap_utils_ptr),
-    distanceToRightLaneBound(map_pose, bounding_box, lanelet_ids, hdmap_utils_ptr));
+    distanceToLeftLaneBound(map_pose, bounding_box, lanelet_ids),
+    distanceToRightLaneBound(map_pose, bounding_box, lanelet_ids));
 }
 
 auto distanceToCrosswalk(