fix(traffic_simulator): fix lanelet_wrapper to pass tests

tier4 · Nov 27, 2024 · f0a30db · f0a30db
1 parent 2b42dfb
commit f0a30db
Show file tree

Hide file tree

Showing 5 changed files with 51 additions and 11 deletions.
diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp
@@ -243,7 +243,7 @@ class LaneletLengthCache
 struct TrafficRulesWithRoutingGraph
 {
   lanelet::traffic_rules::TrafficRulesPtr rules;
-  lanelet::routing::RoutingGraphPtr graph;
+  lanelet::routing::RoutingGraphConstPtr graph;
   mutable RouteCache route_cache;
 };
 
@@ -252,18 +252,18 @@ class LaneletWrapper
 public:
   static auto activate(const std::string & lanelet_map_path) -> void;
 
-  [[nodiscard]] static auto map() -> const lanelet::LaneletMapPtr &;
+  [[nodiscard]] static auto map() -> const lanelet::LaneletMapPtr;
   [[nodiscard]] static auto routingGraph(const RoutingGraphType type)
-    -> const lanelet::routing::RoutingGraphConstPtr &;
+    -> const lanelet::routing::RoutingGraphConstPtr;
   [[nodiscard]] static auto trafficRules(const RoutingGraphType type)
-    -> const lanelet::traffic_rules::TrafficRulesPtr &;
+    -> const lanelet::traffic_rules::TrafficRulesPtr;
 
   [[nodiscard]] static auto routeCache(const RoutingGraphType type) -> RouteCache &;
   [[nodiscard]] static auto centerPointsCache() -> CenterPointsCache &;
   [[nodiscard]] static auto laneletLengthCache() -> LaneletLengthCache &;
 
 private:
-  LaneletWrapper(const std::filesystem::path & lanelet2_map_path);
+  LaneletWrapper(const std::filesystem::path & lanelet_map_path);
   static LaneletWrapper & getInstance();
 
   auto calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string)

diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp
@@ -46,13 +46,13 @@ LaneletWrapper & LaneletWrapper::getInstance()
   return *instance;
 }
 
-auto LaneletWrapper::map() -> const lanelet::LaneletMapPtr &
+auto LaneletWrapper::map() -> const lanelet::LaneletMapPtr
 {
   return getInstance().lanelet_map_ptr_;
 }
 
 auto LaneletWrapper::routingGraph(const RoutingGraphType type)
-  -> const lanelet::routing::RoutingGraphConstPtr &
+  -> const lanelet::routing::RoutingGraphConstPtr
 {
   switch (type) {
     case RoutingGraphType::VEHICLE:
@@ -69,7 +69,7 @@ auto LaneletWrapper::routingGraph(const RoutingGraphType type)
 }
 
 auto LaneletWrapper::trafficRules(const RoutingGraphType type)
-  -> const lanelet::traffic_rules::TrafficRulesPtr &
+  -> const lanelet::traffic_rules::TrafficRulesPtr
 {
   switch (type) {
     case RoutingGraphType::VEHICLE:

diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp
@@ -170,8 +170,6 @@ auto toLaneletPoses(
   auto insertIds = [&](const auto & new_ids) {
     lanelet_ids_set.insert(new_ids.begin(), new_ids.end());
   };
-  /// @todo here entity_type is hardcoded as VEHICLE
-  const auto entity_type = traffic_simulator_msgs::build<EntityType>().type(EntityType::VEHICLE);
   insertIds(leftLaneletIds(lanelet_id, type, include_opposite_direction));
   insertIds(rightLaneletIds(lanelet_id, type, include_opposite_direction));
   insertIds(lanelet_map::previousLaneletIds({lanelet_id}, type));

diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp
@@ -64,6 +64,49 @@ auto route(
       LaneletWrapper::routingGraph(routing_configuration.routing_graph_type));
 }
 
+// auto route(
+//   const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id,
+//   const RoutingConfiguration & routing_configuration) -> lanelet::Ids
+// {
+//   auto & cache = LaneletWrapper::routeCache(routing_configuration.routing_graph_type);
+//   std::cout << " ########## A ##########" << std::endl;
+//   if (cache.exists(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change)) {
+//     return cache.getRoute(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change);
+//   }
+//   std::cout << " ########## B ##########" << std::endl;
+
+//   lanelet::Ids ids;
+//   const auto lanelet = LaneletWrapper::map()->laneletLayer.get(from_lanelet_id);
+//   const auto to_lanelet = LaneletWrapper::map()->laneletLayer.get(to_lanelet_id);
+//   std::cout << " ########## B + ##########" << std::endl;
+//   const auto graph = LaneletWrapper::routingGraph(routing_configuration.routing_graph_type);
+//   std::cout << " ########## B ++ ##########" << std::endl;
+//   lanelet::Optional<lanelet::routing::Route> route =
+//     graph->getRoute(lanelet, to_lanelet, 0, routing_configuration.allow_lane_change);
+//   std::cout << " ########## C ##########" << std::endl;
+
+//   if (!route) {
+//     cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids);
+//     return ids;
+//   }
+//   lanelet::routing::LaneletPath shortest_path = route->shortestPath();
+//   std::cout << " ########## D ##########" << std::endl;
+
+//   if (shortest_path.empty()) {
+//     cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids);
+//     return ids;
+//   }
+//   std::cout << " ########## E ##########" << std::endl;
+
+//   for (auto lane_itr = shortest_path.begin(); lane_itr != shortest_path.end(); lane_itr++) {
+//     ids.push_back(lane_itr->id());
+//   }
+//   cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids);
+//   std::cout << " ########## G ##########" << std::endl;
+
+//   return ids;
+// }
+
 auto followingLanelets(
   const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon,
   const bool include_current_lanelet_id, const RoutingGraphType type) -> lanelet::Ids

diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp
@@ -275,7 +275,6 @@ auto distanceToYieldStop(
   const CanonicalizedLaneletPose & reference_pose, const lanelet::Ids & following_lanelets,
   const std::vector<CanonicalizedLaneletPose> & other_poses) -> std::optional<double>
 {
-  constexpr bool allow_lane_change{false};
   auto getPosesOnLanelet = [&other_poses](const auto & lanelet_id) {
     std::vector<CanonicalizedLaneletPose> ret;
     for (const auto & pose : other_poses) {