Enable subclassing of RoutingCost from python

fzi-forschungszentrum-informatik · Jan 20, 2025 · a8e8954 · a8e8954
1 parent 1d88ad2
commit a8e8954
Show file tree

Hide file tree

Showing 2 changed files with 106 additions and 20 deletions.
diff --git a/lanelet2_examples/scripts/tutorial.py b/lanelet2_examples/scripts/tutorial.py
@@ -6,7 +6,7 @@
 from lanelet2.core import (AllWayStop, AttributeMap, BasicPoint2d,
                            BoundingBox2d, Lanelet, LaneletMap,
                            LaneletWithStopLine, LineString3d, Point2d, Point3d,
-                           RightOfWay, TrafficLight, getId)
+                           RightOfWay, TrafficLight, getId, createMapFromLanelets)
 from lanelet2.projection import (UtmProjector, MercatorProjector,
                                  LocalCartesianProjector, GeocentricProjector)
 
@@ -150,7 +150,7 @@ def part3lanelet_map():
     assert len(map.pointLayer.search(searchBox)) > 1
 
     # you can also create a map from a list of primitives (replace Lanelets by the other types)
-    mapBulk = lanelet2.core.createMapFromLanelets([get_a_lanelet()])
+    mapBulk = createMapFromLanelets([get_a_lanelet()])
     assert len(mapBulk.laneletLayer) == 1
 
 
@@ -162,38 +162,39 @@ def part4reading_and_writing():
     map.add(lanelet)
     path = os.path.join(tempfile.mkdtemp(), 'mapfile.osm')
     # Select a suitable projector depending on the data source
-    ## UtmProjector: (0,0,0) is at the provided lat/lon on the WGS84 ellipsoid
+    # UtmProjector: (0,0,0) is at the provided lat/lon on the WGS84 ellipsoid
     projector = UtmProjector(lanelet2.io.Origin(49, 8.4))
-    ## MarcatorProjector: (0,0,0) is at the provided lat/lon on the mercator cylinder
+    # MarcatorProjector: (0,0,0) is at the provided lat/lon on the mercator cylinder
     projector = MercatorProjector(lanelet2.io.Origin(49, 8.4))
-    ## LocalCartesianProjector: (0,0,0) is at the provided origin (including elevation)
+    # LocalCartesianProjector: (0,0,0) is at the provided origin (including elevation)
     projector = LocalCartesianProjector(lanelet2.io.Origin(49, 8.4, 123))
 
     # Writing the map to a file
-    ## 1. Write with the given projector and use default parameters
+    # 1. Write with the given projector and use default parameters
     lanelet2.io.write(path, map, projector)
 
-    ## 2. Write and get the possible errors
+    # 2. Write and get the possible errors
     write_errors = lanelet2.io.writeRobust(path, map, projector)
     assert not write_errors
 
-    ## 3. Write using the default spherical mercator projector at the giver origin
-    ## This was the default projection in Lanelet1. Use is not recommended.
+    # 3. Write using the default spherical mercator projector at the giver origin
+    # This was the default projection in Lanelet1. Use is not recommended.
     lanelet2.io.write(path, map, lanelet2.io.Origin(49, 8.4))
 
-    ## 4. Write using the given projector and override the default values of the optional parameters for JOSM
+    # 4. Write using the given projector and override the default values of the optional parameters for JOSM
     params = {
-               "josm_upload": "true",          # value for the attribute "upload", default is "false"
-               "josm_format_elevation": "true"  # whether to limit up to 2 decimals, default is the same as for lat/lon
-             };
+        "josm_upload": "true",          # value for the attribute "upload", default is "false"
+        # whether to limit up to 2 decimals, default is the same as for lat/lon
+        "josm_format_elevation": "true"
+    }
     lanelet2.io.write(path, map, projector, params)
 
     # Loading the map from a file
     loadedMap, load_errors = lanelet2.io.loadRobust(path, projector)
     assert not load_errors
     assert loadedMap.laneletLayer.exists(lanelet.id)
 
-    ## GeocentricProjector: the origin is the centre of the Earth
+    # GeocentricProjector: the origin is the centre of the Earth
     gc_projector = GeocentricProjector()
     write_errors = lanelet2.io.writeRobust(path, map, gc_projector)
     assert not write_errors
@@ -235,6 +236,17 @@ def part6routing():
     # for more complex queries, you can use the forEachSuccessor function and pass it a function object
     assert hasPathFromTo(graph, lanelet, toLanelet)
 
+    # it is also possible to create custom routing costs to influence the routing.
+    # Note that this will be much slower than the costs implemented in C++, but it
+    # is useful for prototyping and testing.
+    class ConstantCost(lanelet2.routing.RoutingCost):
+        def getCostSucceeding(self, rules, from_lanelet, to_lanelet):
+            return 1
+
+        def getCostLaneChange(self, rules, from_lanelet, to_lanelet):
+            return 1
+    graph = lanelet2.routing.RoutingGraph(map, traffic_rules, [ConstantCost()])
+
 
 def hasPathFromTo(graph, start, target):
     class TargetFound(BaseException):

diff --git a/lanelet2_python/python_api/routing.cpp b/lanelet2_python/python_api/routing.cpp
@@ -52,6 +52,58 @@ Optional<T> objectToOptional(const object& o) {
   return o == object() ? Optional<T>{} : Optional<T>{extract<T>(o)()};
 }
 
+class RoutingCostBaseWrapper : public lanelet::routing::RoutingCost, public wrapper<lanelet::routing::RoutingCost> {
+ public:
+  double getCostSucceeding(const traffic_rules::TrafficRules& trafficRules, const ConstLaneletOrArea& from,
+                           const ConstLaneletOrArea& to) const noexcept override {
+    return this->get_override("getCostSucceeding")(boost::ref(trafficRules), from, to);
+  }
+
+  double getCostLaneChange(const traffic_rules::TrafficRules& trafficRules, const ConstLanelets& from,
+                           const ConstLanelets& to) const noexcept override {
+    return this->get_override("getCostLaneChange")(boost::ref(trafficRules), from, to);
+  }
+};
+
+template <typename BaseT>
+class RoutingCostWrapper : public BaseT, public wrapper<BaseT> {
+ public:
+  RoutingCostWrapper(const BaseT& base) : BaseT(base) {}
+  RoutingCostWrapper(double laneChangeCost, double minLaneChange) : BaseT(laneChangeCost, minLaneChange) {}
+
+  double getCostSucceeding(const traffic_rules::TrafficRules& trafficRules, const ConstLaneletOrArea& from,
+                           const ConstLaneletOrArea& to) const
+      noexcept(noexcept(BaseT::getCostSucceeding(trafficRules, from, to))) override {
+    const auto o = this->get_override("getCostSucceeding");
+    if (o) {
+      return o(boost::ref(trafficRules), from, to);
+    }
+    return BaseT::getCostSucceeding(trafficRules, from, to);
+  }
+
+  double defaultGetCostSucceeding(const traffic_rules::TrafficRules& trafficRules, const ConstLaneletOrArea& from,
+                                  const ConstLaneletOrArea& to) const
+      noexcept(noexcept(BaseT::getCostSucceeding(trafficRules, from, to))) {
+    return BaseT::getCostSucceeding(trafficRules, from, to);
+  }
+
+  double getCostLaneChange(const traffic_rules::TrafficRules& trafficRules, const ConstLanelets& from,
+                           const ConstLanelets& to) const
+      noexcept(noexcept(BaseT::getCostLaneChange(trafficRules, from, to))) override {
+    const auto o = this->get_override("getCostLaneChange");
+    if (o) {
+      return o(boost::ref(trafficRules), from, to);
+    }
+    return BaseT::getCostLaneChange(trafficRules, from, to);
+  }
+
+  double defaultGetCostLaneChange(const traffic_rules::TrafficRules& trafficRules, const ConstLanelets& from,
+                                  const ConstLanelets& to) const
+      noexcept(noexcept(BaseT::getCostLaneChange(trafficRules, from, to))) {
+    return BaseT::getCostLaneChange(trafficRules, from, to);
+  }
+};
+
 BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) {  // NOLINT
   auto trafficRules = import("lanelet2.traffic_rules");
   using namespace lanelet::routing;
@@ -77,16 +129,38 @@ BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) {  // NOLINT
   implicitly_convertible<LaneletMapPtr, LaneletMapConstPtr>();
 
   // Routing costs
-  class_<RoutingCost, boost::noncopyable, std::shared_ptr<RoutingCost>>(  // NOLINT
-      "RoutingCost", "Object for calculating routing costs between lanelets", no_init);
+  class_<RoutingCostBaseWrapper, boost::noncopyable>(  // NOLINT
+      "RoutingCost", "Object for calculating routing costs between lanelets")
+      .def("getCostSucceeding", pure_virtual(&RoutingCost::getCostSucceeding),
+           "Get the cost of the transition from one to another lanelet", (arg("trafficRules"), arg("from"), arg("to")))
+      .def("getCostLaneChange", pure_virtual(&RoutingCost::getCostLaneChange),
+           "Get the cost of the lane change between two adjacent lanelets",
+           (arg("trafficRules"), arg("from"), arg("to")));
+  register_ptr_to_python<std::shared_ptr<RoutingCost>>();
 
-  class_<RoutingCostDistance, bases<RoutingCost>, std::shared_ptr<RoutingCostDistance>>(  // NOLINT
+  class_<RoutingCostWrapper<RoutingCostDistance>, bases<RoutingCost>>(  // NOLINT
       "RoutingCostDistance", "Distance based routing cost calculation object",
-      init<double, double>((arg("laneChangeCost"), arg("minLaneChangeDistance") = 0)));
+      init<double, double>((arg("laneChangeCost"), arg("minLaneChangeDistance") = 0)))
+      .def("getCostSucceeding", &RoutingCostDistance::getCostSucceeding,
+           &RoutingCostWrapper<RoutingCostDistance>::defaultGetCostSucceeding,
+           "Get the cost of the transition from one to another lanelet", (arg("trafficRules"), arg("from"), arg("to")))
+      .def("getCostLaneChange", &RoutingCostDistance::getCostLaneChange,
+           &RoutingCostWrapper<RoutingCostDistance>::defaultGetCostLaneChange,
+           "Get the cost of the lane change between two adjacent lanelets",
+           (arg("trafficRules"), arg("from"), arg("to")));
+  register_ptr_to_python<std::shared_ptr<RoutingCostDistance>>();
 
-  class_<RoutingCostTravelTime, bases<RoutingCost>, std::shared_ptr<RoutingCostTravelTime>>(  // NOLINT
+  class_<RoutingCostWrapper<RoutingCostTravelTime>, bases<RoutingCost>>(  // NOLINT
       "RoutingCostTravelTime", "Travel time based routing cost calculation object",
-      init<double, double>((arg("laneChangeCost"), arg("minLaneChangeTime") = 0)));
+      init<double, double>((arg("laneChangeCost"), arg("minLaneChangeTime") = 0)))
+      .def("getCostSucceeding", &RoutingCostTravelTime::getCostSucceeding,
+           &RoutingCostWrapper<RoutingCostTravelTime>::defaultGetCostSucceeding,
+           "Get the cost of the transition from one to another lanelet", (arg("trafficRules"), arg("from"), arg("to")))
+      .def("getCostLaneChange", &RoutingCostTravelTime::getCostLaneChange,
+           &RoutingCostWrapper<RoutingCostTravelTime>::defaultGetCostLaneChange,
+           "Get the cost of the lane change between two adjacent lanelets",
+           (arg("trafficRules"), arg("from"), arg("to")));
+  register_ptr_to_python<std::shared_ptr<RoutingCostTravelTime>>();
 
   auto possPCost = static_cast<LaneletPaths (RoutingGraph::*)(const ConstLanelet&, double, RoutingCostId, bool) const>(
       &RoutingGraph::possiblePaths);