diff --git a/lanelet2_examples/scripts/tutorial.py b/lanelet2_examples/scripts/tutorial.py index 68aea514..d9ee2f38 100755 --- a/lanelet2_examples/scripts/tutorial.py +++ b/lanelet2_examples/scripts/tutorial.py @@ -178,7 +178,7 @@ def part4reading_and_writing(): assert not write_errors ## 3. Write using the default spherical mercator projector at the giver origin - ## This was the default projection in Lanelet1 + ## 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 @@ -193,6 +193,14 @@ def part4reading_and_writing(): assert not load_errors assert loadedMap.laneletLayer.exists(lanelet.id) + ## 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 + loadedMap, load_errors = lanelet2.io.loadRobust(path, gc_projector) + assert not load_errors + assert loadedMap.laneletLayer.exists(lanelet.id) + def part5traffic_rules(): # this is just as you would expect diff --git a/lanelet2_projection/include/lanelet2_projection/Geocentric.h b/lanelet2_projection/include/lanelet2_projection/Geocentric.h index 33808e00..93737efb 100644 --- a/lanelet2_projection/include/lanelet2_projection/Geocentric.h +++ b/lanelet2_projection/include/lanelet2_projection/Geocentric.h @@ -6,6 +6,9 @@ namespace projection { class GeocentricProjector : public Projector { public: + // initialize the origin so that it's not the default one which causes + // IOHandler::handleDefaultProjector to throw an exception + GeocentricProjector() : Projector{Origin({90.0, 0.0, -6356752.3})} {} BasicPoint3d forward(const GPSPoint& gps) const override; GPSPoint reverse(const BasicPoint3d& enu) const override; };