diff --git a/lanelet2_examples/scripts/tutorial.py b/lanelet2_examples/scripts/tutorial.py index d9ee2f38..f04af87d 100755 --- 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,30 +162,31 @@ 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 @@ -193,7 +194,7 @@ def part4reading_and_writing(): 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 index f159a242..b49b5b71 100644 --- a/lanelet2_python/python_api/routing.cpp +++ b/lanelet2_python/python_api/routing.cpp @@ -52,6 +52,54 @@ Optional objectToOptional(const object& o) { return o == object() ? Optional{} : Optional{extract(o)()}; } +class RoutingCostBaseWrapper : public lanelet::routing::RoutingCost, public wrapper { + 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 +class RoutingCostWrapper : public BaseT, public wrapper { + 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(); // Routing costs - class_>( // NOLINT - "RoutingCost", "Object for calculating routing costs between lanelets", no_init); + class_( // 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>(); - class_, std::shared_ptr>( // NOLINT + class_, bases>( // NOLINT "RoutingCostDistance", "Distance based routing cost calculation object", - init((arg("laneChangeCost"), arg("minLaneChangeDistance") = 0))); + init((arg("laneChangeCost"), arg("minLaneChangeDistance") = 0))) + .def("getCostSucceeding", &RoutingCostDistance::getCostSucceeding, + &RoutingCostWrapper::defaultGetCostSucceeding, + "Get the cost of the transition from one to another lanelet", (arg("trafficRules"), arg("from"), arg("to"))) + .def("getCostLaneChange", &RoutingCostDistance::getCostLaneChange, + &RoutingCostWrapper::defaultGetCostLaneChange, + "Get the cost of the lane change between two adjacent lanelets", + (arg("trafficRules"), arg("from"), arg("to"))); + register_ptr_to_python>(); - class_, std::shared_ptr>( // NOLINT + class_, bases>( // NOLINT "RoutingCostTravelTime", "Travel time based routing cost calculation object", - init((arg("laneChangeCost"), arg("minLaneChangeTime") = 0))); + init((arg("laneChangeCost"), arg("minLaneChangeTime") = 0))) + .def("getCostSucceeding", &RoutingCostTravelTime::getCostSucceeding, + &RoutingCostWrapper::defaultGetCostSucceeding, + "Get the cost of the transition from one to another lanelet", (arg("trafficRules"), arg("from"), arg("to"))) + .def("getCostLaneChange", &RoutingCostTravelTime::getCostLaneChange, + &RoutingCostWrapper::defaultGetCostLaneChange, + "Get the cost of the lane change between two adjacent lanelets", + (arg("trafficRules"), arg("from"), arg("to"))); + register_ptr_to_python>(); auto possPCost = static_cast( &RoutingGraph::possiblePaths);