Enable subclassing of RoutingCost from python (#384)

Author: poggenhans

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
 
 
@@ -237,6 +237,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: lanelet2.routing.RoutingGraph, start: lanelet2.core.Lanelet, target: lanelet2.core.Lanelet):
     class TargetFound(BaseException):

lanelet2_python/python_api/routing.cpp

@@ -52,6 +52,54 @@ 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 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 {
+    return BaseT::getCostSucceeding(trafficRules, from, to);
+  }
+
+  double getCostLaneChange(const traffic_rules::TrafficRules& trafficRules, const ConstLanelets& from,
+                           const ConstLanelets& to) const noexcept 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 {
+    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 +125,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);