Merge pull request #1531 from tier4/refactor/lanelet_wrapper_route

HdMapUtils refactor lanelet_wrapper::route

Author: hakuturu583

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -334,7 +334,7 @@ auto ActionNode::getDistanceToTargetEntity(
   if (const auto & target_lanelet_pose =
         traffic_simulator::pose::findRoutableAlternativeLaneletPoseFrom(
           canonicalized_entity_status->getLaneletId(), status.getCanonicalizedLaneletPose().value(),
-          target_bounding_box, hdmap_utils);
+          target_bounding_box);
       target_lanelet_pose) {
     const auto & from_lanelet_pose = canonicalized_entity_status->getCanonicalizedLaneletPose();
     const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox();

simulation/traffic_simulator/CMakeLists.txt

@@ -52,6 +52,7 @@ ament_auto_add_library(traffic_simulator SHARED
   src/lanelet_wrapper/lanelet_map.cpp
   src/lanelet_wrapper/lanelet_wrapper.cpp
   src/lanelet_wrapper/pose.cpp
+  src/lanelet_wrapper/route.cpp
   src/lanelet_wrapper/traffic_rules.cpp
   src/simulation_clock/simulation_clock.cpp
   src/traffic/traffic_controller.cpp

simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp

@@ -49,7 +49,7 @@ class CanonicalizedLaneletPose
   auto alignOrientationToLanelet() -> void;
   auto hasAlternativeLaneletPose() const -> bool { return lanelet_poses_.size() > 1; }
   auto getAlternativeLaneletPoseBaseOnShortestRouteFrom(
-    LaneletPose from, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils,
+    LaneletPose from,
     const RoutingConfiguration & routing_configuration = RoutingConfiguration()) const
     -> std::optional<LaneletPose>;
 

simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp

@@ -0,0 +1,34 @@
+// Copyright 2015 Tier IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TRAFFIC_SIMULATOR__LANELET_WRAPPER_ROUTE_HPP_
+#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_ROUTE_HPP_
+
+#include <lanelet2_core/geometry/Lanelet.h>
+
+#include <traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp>
+
+namespace traffic_simulator
+{
+namespace lanelet_wrapper
+{
+namespace route
+{
+auto route(
+  const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id,
+  const RoutingConfiguration & routing_configuration = RoutingConfiguration()) -> lanelet::Ids;
+}  // namespace route
+}  // namespace lanelet_wrapper
+}  // namespace traffic_simulator
+#endif  // TRAFFIC_SIMULATOR__LANELET_WRAPPER_ROUTE_HPP_

simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp

@@ -113,8 +113,7 @@ auto isAtEndOfLanelets(
 
 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::HdMapUtils> & hdmap_utils_ptr)
+  const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box)
   -> std::optional<traffic_simulator::CanonicalizedLaneletPose>;
 
 namespace pedestrian

simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp

@@ -0,0 +1,36 @@
+// Copyright 2015 Tier IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TRAFFIC_SIMULATOR__UTILS__ROUTE_HPP_
+#define TRAFFIC_SIMULATOR__UTILS__ROUTE_HPP_
+
+#include <geometry/spline/catmull_rom_spline.hpp>
+#include <geometry/spline/catmull_rom_spline_interface.hpp>
+#include <geometry/spline/hermite_curve.hpp>
+#include <traffic_simulator/data_type/lane_change.hpp>
+#include <traffic_simulator/data_type/lanelet_pose.hpp>
+#include <traffic_simulator/lanelet_wrapper/route.hpp>
+
+namespace traffic_simulator
+{
+inline namespace route
+{
+template <typename... Ts>
+auto route(Ts &&... xs)
+{
+  return lanelet_wrapper::route::route(std::forward<decltype(xs)>(xs)...);
+}
+}  // namespace route
+}  // namespace traffic_simulator
+#endif  // TRAFFIC_SIMULATOR__UTILS__ROUTE_HPP_

simulation/traffic_simulator/src/data_type/lanelet_pose.cpp

@@ -20,6 +20,7 @@
 #include <traffic_simulator/data_type/lanelet_pose.hpp>
 #include <traffic_simulator/lanelet_wrapper/lanelet_map.hpp>
 #include <traffic_simulator/utils/pose.hpp>
+#include <traffic_simulator/utils/route.hpp>
 
 namespace traffic_simulator
 {
@@ -67,18 +68,17 @@ auto CanonicalizedLaneletPose::operator=(const CanonicalizedLaneletPose & other)
 }
 
 auto CanonicalizedLaneletPose::getAlternativeLaneletPoseBaseOnShortestRouteFrom(
-  LaneletPose from, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils,
-  const RoutingConfiguration & routing_configuration) const -> std::optional<LaneletPose>
+  LaneletPose from, const RoutingConfiguration & routing_configuration) const
+  -> std::optional<LaneletPose>
 {
   if (lanelet_poses_.empty()) {
     return std::nullopt;
   }
-  lanelet::Ids shortest_route =
-    hdmap_utils->getRoute(from.lanelet_id, lanelet_poses_[0].lanelet_id, routing_configuration);
+  auto shortest_route =
+    route::route(from.lanelet_id, lanelet_poses_[0].lanelet_id, routing_configuration);
   LaneletPose alternative_lanelet_pose = lanelet_poses_[0];
   for (const auto & laneletPose : lanelet_poses_) {
-    const auto route =
-      hdmap_utils->getRoute(from.lanelet_id, laneletPose.lanelet_id, routing_configuration);
+    const auto route = route::route(from.lanelet_id, laneletPose.lanelet_id, routing_configuration);
     if (shortest_route.size() > route.size()) {
       shortest_route = route;
       alternative_lanelet_pose = laneletPose;

simulation/traffic_simulator/src/lanelet_wrapper/route.cpp

@@ -0,0 +1,45 @@
+
+// Copyright 2015 Tier IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
+#include <lanelet2_core/utility/Units.h>
+#include <lanelet2_routing/RoutingGraphContainer.h>
+
+#include <autoware_lanelet2_extension/utility/utilities.hpp>
+#include <traffic_simulator/helper/helper.hpp>
+#include <traffic_simulator/lanelet_wrapper/lanelet_map.hpp>
+#include <traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp>
+#include <traffic_simulator/lanelet_wrapper/route.hpp>
+
+namespace traffic_simulator
+{
+namespace lanelet_wrapper
+{
+namespace route
+{
+auto route(
+  const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id,
+  const RoutingConfiguration & routing_configuration) -> lanelet::Ids
+{
+  /// @todo improve architecture in terms of access to cache of the graph
+  return LaneletWrapper::routeCache(routing_configuration.routing_graph_type)
+    .getRoute(
+      from_lanelet_id, to_lanelet_id, LaneletWrapper::map(), routing_configuration,
+      LaneletWrapper::routingGraph(routing_configuration.routing_graph_type));
+}
+}  // namespace route
+}  // namespace lanelet_wrapper
+}  // namespace traffic_simulator

simulation/traffic_simulator/src/utils/distance.cpp

@@ -68,7 +68,7 @@ auto longitudinalDistance(
     if (to.hasAlternativeLaneletPose()) {
       if (
         const auto to_canonicalized_opt = to.getAlternativeLaneletPoseBaseOnShortestRouteFrom(
-          static_cast<LaneletPose>(from), hdmap_utils_ptr, routing_configuration)) {
+          static_cast<LaneletPose>(from), routing_configuration)) {
         to_canonicalized = to_canonicalized_opt.value();
       }
     }

simulation/traffic_simulator/src/utils/pose.cpp

@@ -24,6 +24,7 @@
 #include <traffic_simulator/lanelet_wrapper/pose.hpp>
 #include <traffic_simulator/utils/distance.hpp>
 #include <traffic_simulator/utils/pose.hpp>
+#include <traffic_simulator/utils/route.hpp>
 #include <traffic_simulator_msgs/msg/lanelet_pose.hpp>
 
 namespace traffic_simulator
@@ -386,26 +387,25 @@ auto isAtEndOfLanelets(
 
 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::HdMapUtils> & hdmap_utils_ptr)
+  const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box)
   -> std::optional<traffic_simulator::CanonicalizedLaneletPose>
 {
   /// @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 
-    */
+  /**
+   * @note route::route 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
   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()) {
+      !route::route(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<geometry_msgs::msg::Pose>(to_canonicalized_lanelet_pose),
@@ -414,8 +414,7 @@ auto findRoutableAlternativeLaneletPoseFrom(
              nearby_lanelet_ids.has_value()) {
     std::vector<std::pair<CanonicalizedLaneletPose, lanelet::Ids>> routes;
     for (const auto & [distance, lanelet_id] : nearby_lanelet_ids.value()) {
-      if (auto route =
-            hdmap_utils_ptr->getRoute(from_lanelet_id, lanelet_id, routing_configuration);
+      if (auto route = route::route(from_lanelet_id, lanelet_id, routing_configuration);
           lanelet_id == to_lanelet_id || route.empty()) {
         continue;
       } else if (const auto lanelet_pose = lanelet_wrapper::pose::toLaneletPose(

simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp

@@ -191,8 +191,8 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout
   const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0);
   const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0);
 
-  const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils);
-  const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils);
+  const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1);
+  const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2);
 
   ASSERT_TRUE(result1.has_value());
   ASSERT_TRUE(result2.has_value());
@@ -210,8 +210,8 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout
   const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0);
   const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0);
 
-  const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils);
-  const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils);
+  const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1);
+  const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2);
 
   ASSERT_TRUE(result1.has_value());
   ASSERT_TRUE(result2.has_value());
@@ -230,8 +230,8 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout
   const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0);
   const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0);
 
-  const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils);
-  const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils);
+  const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1);
+  const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2);
 
   ASSERT_TRUE(result1.has_value());
   ASSERT_TRUE(result2.has_value());