Add new routing graph for road shoulder

simulation/traffic_simulator/CMakeLists.txt

@@ -45,6 +45,7 @@ ament_auto_add_library(traffic_simulator SHARED
   src/entity/pedestrian_entity.cpp
   src/entity/vehicle_entity.cpp
   src/hdmap_utils/hdmap_utils.cpp
+  src/hdmap_utils/traffic_rules.cpp
   src/helper/helper.cpp
   src/job/job_list.cpp
   src/job/job.cpp

simulation/traffic_simulator/include/traffic_simulator/data_type/routing_graph_type.hpp

@@ -21,7 +21,7 @@ namespace traffic_simulator
 {
 inline namespace routing_graph_type
 {
-enum class RoutingGraphType : std::uint8_t { VEHICLE, PEDESTRIAN };
+enum class RoutingGraphType : std::uint8_t { VEHICLE, PEDESTRIAN, VEHICLE_WITH_ROAD_SHOULDER };
 
 char const * to_string(const RoutingGraphType &);
 }  // namespace routing_graph_type

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

@@ -47,6 +47,7 @@
 #include <traffic_simulator/data_type/lane_change.hpp>
 #include <traffic_simulator/data_type/routing_graph_type.hpp>
 #include <traffic_simulator/hdmap_utils/cache.hpp>
+#include <traffic_simulator/hdmap_utils/traffic_rules.hpp>
 #include <traffic_simulator_msgs/msg/bounding_box.hpp>
 #include <traffic_simulator_msgs/msg/entity_status.hpp>
 #include <tuple>
@@ -200,28 +201,40 @@ class HdMapUtils
     const geometry_msgs::msg::Point &, const double distance_threshold,
     const std::size_t search_count = 5) const -> lanelet::Ids;
 
-  auto getNextLaneletIds(const lanelet::Ids &) const -> lanelet::Ids;
+  auto getNextLaneletIds(
+    const lanelet::Ids &, const traffic_simulator::RoutingGraphType type =
+                            traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER) const
+    -> lanelet::Ids;
 
   auto getNextLaneletIds(
     const lanelet::Ids &, const std::string & turn_direction,
     const traffic_simulator::RoutingGraphType type =
       traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids;
 
-  auto getNextLaneletIds(const lanelet::Id) const -> lanelet::Ids;
+  auto getNextLaneletIds(
+    const lanelet::Id, const traffic_simulator::RoutingGraphType type =
+                         traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER) const
+    -> lanelet::Ids;
 
   auto getNextLaneletIds(
     const lanelet::Id, const std::string & turn_direction,
     const traffic_simulator::RoutingGraphType type =
       traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids;
 
-  auto getPreviousLaneletIds(const lanelet::Ids &) const -> lanelet::Ids;
+  auto getPreviousLaneletIds(
+    const lanelet::Ids &, const traffic_simulator::RoutingGraphType type =
+                            traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER) const
+    -> lanelet::Ids;
 
   auto getPreviousLaneletIds(
     const lanelet::Ids &, const std::string & turn_direction,
     const traffic_simulator::RoutingGraphType type =
       traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids;
 
-  auto getPreviousLaneletIds(const lanelet::Id) const -> lanelet::Ids;
+  auto getPreviousLaneletIds(
+    const lanelet::Id, const traffic_simulator::RoutingGraphType type =
+                         traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER) const
+    -> lanelet::Ids;
 
   auto getPreviousLaneletIds(
     const lanelet::Id, const std::string & turn_direction,
@@ -242,8 +255,10 @@ class HdMapUtils
 
   auto getRightOfWayLaneletIds(const lanelet::Id) const -> lanelet::Ids;
 
-  auto getRoute(const lanelet::Id from, const lanelet::Id to, bool allow_lane_change = false) const
-    -> lanelet::Ids;
+  auto getRoute(
+    const lanelet::Id from, const lanelet::Id to, bool allow_lane_change = false,
+    const traffic_simulator::RoutingGraphType type =
+      traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids;
 
   auto getSpeedLimit(const lanelet::Ids &) const -> double;
 
@@ -337,8 +352,6 @@ class HdMapUtils
 
   lanelet::LaneletMapPtr lanelet_map_ptr_;
 
-  lanelet::ConstLanelets shoulder_lanelets_;
-
   class RoutingGraphs
   {
   public:
@@ -367,6 +380,8 @@ class HdMapUtils
 
     RuleWithGraph vehicle;
 
+    RuleWithGraph vehicle_with_road_shoulder;
+
     RuleWithGraph pedestrian;
   };
 
@@ -412,10 +427,6 @@ class HdMapUtils
     const traffic_simulator::lane_change::TrajectoryShape,
     const double tangent_vector_size = 100) const -> math::geometry::HermiteCurve;
 
-  auto getNextRoadShoulderLanelet(const lanelet::Id) const -> lanelet::Ids;
-
-  auto getPreviousRoadShoulderLanelet(const lanelet::Id) const -> lanelet::Ids;
-
   auto getStopLines() const -> lanelet::ConstLineStrings3d;
 
   auto getStopLinesOnPath(const lanelet::Ids &) const -> lanelet::ConstLineStrings3d;

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

@@ -0,0 +1,71 @@
+// 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__HDMAP_UTILS__TRAFFIC_RULES_HPP_
+#define TRAFFIC_SIMULATOR__HDMAP_UTILS__TRAFFIC_RULES_HPP_
+
+#include <lanelet2_traffic_rules/GermanTrafficRules.h>
+
+namespace hdmap_utils
+{
+struct Locations
+{
+  /// @note DIRTY HACK!! Originally, a location code should be an ISO country code.
+  /// @sa https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/507033b82d9915f086f2539d56c3b62e71802438/lanelet2_traffic_rules/include/lanelet2_traffic_rules/TrafficRules.h#L97
+  static constexpr char RoadShoulderPassableGermany[] = "de_road_shoulder_passable";
+};
+
+class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanVehicle
+{
+public:
+  using lanelet::traffic_rules::GermanVehicle::GermanVehicle;
+
+protected:
+  /// @note this function overrides and adds road shoulder handling to GenericTrafficRules::canPass
+  lanelet::Optional<bool> canPass(
+    const std::string & type, const std::string & /*location*/) const override
+  {
+    using lanelet::AttributeValueString;
+    using lanelet::Participants;
+    const static std::map<std::string, std::vector<std::string>> participants_map{
+      // clang-format off
+      {"",                                  {Participants::Vehicle}},
+      {AttributeValueString::Road,          {Participants::Vehicle, Participants::Bicycle}},
+      {"road_shoulder",                     {Participants::Vehicle, Participants::Bicycle}},  // add road_shoulder
+      {AttributeValueString::Highway,       {Participants::Vehicle}},
+      {AttributeValueString::BicycleLane,   {Participants::Bicycle}},
+      {AttributeValueString::PlayStreet,    {Participants::Pedestrian, Participants::Bicycle, Participants::Vehicle}},
+      {AttributeValueString::EmergencyLane, {Participants::VehicleEmergency}},
+      {AttributeValueString::Exit,          {Participants::Pedestrian, Participants::Bicycle, Participants::Vehicle}},
+      {AttributeValueString::Walkway,       {Participants::Pedestrian}},
+      {AttributeValueString::Crosswalk,     {Participants::Pedestrian}},
+      {AttributeValueString::Stairs,        {Participants::Pedestrian}},
+      {AttributeValueString::SharedWalkway, {Participants::Pedestrian, Participants::Bicycle}}
+      // clang-format on
+    };
+    auto participants = participants_map.find(type);
+    if (participants == participants_map.end()) {
+      return {};
+    }
+
+    auto startsWith = [](const std::string & str, const std::string & substr) {
+      return str.compare(0, substr.size(), substr) == 0;
+    };
+    return lanelet::utils::anyOf(participants->second, [this, startsWith](auto & participant) {
+      return startsWith(this->participant(), participant);
+    });
+  }
+};
+}  // namespace hdmap_utils
+#endif  // TRAFFIC_SIMULATOR__HDMAP_UTILS__TRAFFIC_RULES_HPP_

simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

@@ -77,13 +77,6 @@ HdMapUtils::HdMapUtils(
     THROW_SIMULATION_ERROR("Failed to load lanelet map (", ss.str(), ")");
   }
   overwriteLaneletsCenterline();
-  std::vector<lanelet::routing::RoutingGraphConstPtr> all_graphs;
-  all_graphs.push_back(
-    routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE));
-  all_graphs.push_back(
-    routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::PEDESTRIAN));
-  shoulder_lanelets_ =
-    lanelet::utils::query::shoulderLanelets(lanelet::utils::query::laneletLayer(lanelet_map_ptr_));
 }
 
 auto HdMapUtils::getAllCanonicalizedLaneletPoses(
@@ -903,12 +896,11 @@ auto HdMapUtils::getFollowingLanelets(
 }
 
 auto HdMapUtils::getRoute(
-  const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, bool allow_lane_change) const
-  -> lanelet::Ids
+  const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, bool allow_lane_change,
+  const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids
 {
   return routing_graphs_->getRoute(
-    from_lanelet_id, to_lanelet_id, lanelet_map_ptr_, allow_lane_change,
-    traffic_simulator::RoutingGraphType::VEHICLE);
+    from_lanelet_id, to_lanelet_id, lanelet_map_ptr_, allow_lane_change, type);
 }
 
 auto HdMapUtils::getCenterPointsSpline(const lanelet::Id lanelet_id) const
@@ -981,40 +973,25 @@ auto HdMapUtils::getLaneletLength(const lanelet::Id lanelet_id) const -> double
   return ret;
 }
 
-auto HdMapUtils::getPreviousRoadShoulderLanelet(const lanelet::Id lanelet_id) const -> lanelet::Ids
-{
-  lanelet::Ids ids;
-  const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id);
-  for (const auto & shoulder_lanelet : shoulder_lanelets_) {
-    if (lanelet::geometry::follows(shoulder_lanelet, lanelet)) {
-      ids.push_back(shoulder_lanelet.id());
-    }
-  }
-  return ids;
-}
-
-/// @todo add RoutingGraphType argument after supporting a routing graph with road shoulder lanelets
-auto HdMapUtils::getPreviousLaneletIds(const lanelet::Id lanelet_id) const -> lanelet::Ids
+auto HdMapUtils::getPreviousLaneletIds(
+  const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type) const
+  -> lanelet::Ids
 {
   lanelet::Ids ids;
   const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id);
-  for (const auto & llt :
-       routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE)
-         ->previous(lanelet)) {
+  for (const auto & llt : routing_graphs_->routing_graph(type)->previous(lanelet)) {
     ids.push_back(llt.id());
   }
-  for (const auto & id : getPreviousRoadShoulderLanelet(lanelet_id)) {
-    ids.push_back(id);
-  }
   return ids;
 }
 
-/// @todo add RoutingGraphType argument after fixing the bug to get next lanelet instead of previous one
-auto HdMapUtils::getPreviousLaneletIds(const lanelet::Ids & lanelet_ids) const -> lanelet::Ids
+auto HdMapUtils::getPreviousLaneletIds(
+  const lanelet::Ids & lanelet_ids, const traffic_simulator::RoutingGraphType type) const
+  -> lanelet::Ids
 {
   lanelet::Ids ids;
   for (const auto & id : lanelet_ids) {
-    ids += getNextLaneletIds(id);
+    ids += getNextLaneletIds(id, type);
   }
   return sortAndUnique(ids);
 }
@@ -1044,40 +1021,25 @@ auto HdMapUtils::getPreviousLaneletIds(
   return sortAndUnique(ids);
 }
 
-auto HdMapUtils::getNextRoadShoulderLanelet(const lanelet::Id lanelet_id) const -> lanelet::Ids
-{
-  lanelet::Ids ids;
-  const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id);
-  for (const auto & shoulder_lanelet : shoulder_lanelets_) {
-    if (lanelet::geometry::follows(lanelet, shoulder_lanelet)) {
-      ids.push_back(shoulder_lanelet.id());
-    }
-  }
-  return ids;
-}
-
-/// @todo add RoutingGraphType argument after supporting a routing graph with road shoulder lanelets
-auto HdMapUtils::getNextLaneletIds(const lanelet::Id lanelet_id) const -> lanelet::Ids
+auto HdMapUtils::getNextLaneletIds(
+  const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type) const
+  -> lanelet::Ids
 {
   lanelet::Ids ids;
   const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id);
-  for (const auto & llt :
-       routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE)
-         ->following(lanelet)) {
+  for (const auto & llt : routing_graphs_->routing_graph(type)->following(lanelet)) {
     ids.push_back(llt.id());
   }
-  for (const auto & id : getNextRoadShoulderLanelet(lanelet_id)) {
-    ids.push_back(id);
-  }
   return ids;
 }
 
-/// @todo add RoutingGraphType argument after supporting a routing graph with road shoulder lanelets
-auto HdMapUtils::getNextLaneletIds(const lanelet::Ids & lanelet_ids) const -> lanelet::Ids
+auto HdMapUtils::getNextLaneletIds(
+  const lanelet::Ids & lanelet_ids, const traffic_simulator::RoutingGraphType type) const
+  -> lanelet::Ids
 {
   lanelet::Ids ids;
   for (const auto & id : lanelet_ids) {
-    ids += getNextLaneletIds(id);
+    ids += getNextLaneletIds(id, type);
   }
   return sortAndUnique(ids);
 }
@@ -2199,6 +2161,10 @@ HdMapUtils::RoutingGraphs::RoutingGraphs(const lanelet::LaneletMapPtr & lanelet_
   vehicle.rules = lanelet::traffic_rules::TrafficRulesFactory::create(
     lanelet::Locations::Germany, lanelet::Participants::Vehicle);
   vehicle.graph = lanelet::routing::RoutingGraph::build(*lanelet_map, *vehicle.rules);
+  vehicle_with_road_shoulder.rules = lanelet::traffic_rules::TrafficRulesFactory::create(
+    Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle);
+  vehicle_with_road_shoulder.graph =
+    lanelet::routing::RoutingGraph::build(*lanelet_map, *vehicle_with_road_shoulder.rules);
   pedestrian.rules = lanelet::traffic_rules::TrafficRulesFactory::create(
     lanelet::Locations::Germany, lanelet::Participants::Pedestrian);
   pedestrian.graph = lanelet::routing::RoutingGraph::build(*lanelet_map, *pedestrian.rules);
@@ -2210,6 +2176,8 @@ lanelet::routing::RoutingGraphPtr HdMapUtils::RoutingGraphs::routing_graph(
   switch (type) {
     case traffic_simulator::RoutingGraphType::VEHICLE:
       return vehicle.graph;
+    case traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER:
+      return vehicle_with_road_shoulder.graph;
     case traffic_simulator::RoutingGraphType::PEDESTRIAN:
       return pedestrian.graph;
     default:
@@ -2225,6 +2193,8 @@ lanelet::traffic_rules::TrafficRulesPtr HdMapUtils::RoutingGraphs::traffic_rule(
   switch (type) {
     case traffic_simulator::RoutingGraphType::VEHICLE:
       return vehicle.rules;
+    case traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER:
+      return vehicle_with_road_shoulder.rules;
     case traffic_simulator::RoutingGraphType::PEDESTRIAN:
       return pedestrian.rules;
     default:
@@ -2239,6 +2209,8 @@ RouteCache & HdMapUtils::RoutingGraphs::route_cache(const traffic_simulator::Rou
   switch (type) {
     case traffic_simulator::RoutingGraphType::VEHICLE:
       return vehicle.route_cache;
+    case traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER:
+      return vehicle_with_road_shoulder.route_cache;
     case traffic_simulator::RoutingGraphType::PEDESTRIAN:
       return pedestrian.route_cache;
     default:

simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp

@@ -0,0 +1,21 @@
+// 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_traffic_rules/TrafficRulesFactory.h>
+
+#include <traffic_simulator/hdmap_utils/traffic_rules.hpp>
+
+lanelet::traffic_rules::RegisterTrafficRules<hdmap_utils::GermanRoadShoulderPassableVehicle>
+  germanRoadShoulderPassableVehicleRules(
+    hdmap_utils::Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle);

simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp

@@ -2735,3 +2735,18 @@ TEST_F(HdMapUtilsTest_StandardMap, getPreviousLanelets)
 
   EXPECT_EQ(result_previous, actual_previous);
 }
+
+TEST_F(HdMapUtilsTest_WithRoadShoulderMap, routingWithRoadShoulder)
+{
+  // default: traffic_simulator::RoutingGraphType::VEHICLE
+  const auto default_route = hdmap_utils.getRoute(34693, 34615, false);
+  EXPECT_EQ(default_route.size(), 0);
+
+  const auto route_with_road_shoulder = hdmap_utils.getRoute(
+    34693, 34615, false, traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER);
+  EXPECT_EQ(route_with_road_shoulder.size(), 4);
+  EXPECT_EQ(route_with_road_shoulder[0], 34693);
+  EXPECT_EQ(route_with_road_shoulder[1], 34696);  // road shoulder
+  EXPECT_EQ(route_with_road_shoulder[2], 34768);  // road shoulder
+  EXPECT_EQ(route_with_road_shoulder[3], 34615);
+}