Fix centerline creation from OpenDRIVE maps with very short lane segments (#12)

* handle invalid centerlines in odr maps

* add warning when fixing centerline in map_odr

Author: MichaelSchuldes

omega_prime/locator.py

@@ -38,7 +38,9 @@ def extend_linestring(cls, l: shapely.LineString, simplify: bool = True, l_appen
         if isinstance(l, shapely.Point):
             cl = np.asarray(l.coords)[0]
             return shapely.LineString(np.stack([cl - np.array([0, 1]), cl, cl + np.array([0, 1])]))
-
+        if l.is_empty:
+            cl = np.array([0, 0])
+            return shapely.LineString(np.stack([cl - np.array([0, 1]), cl, cl + np.array([0, 1])]))
         startvec = np.asarray(l.interpolate(0, normalized=True).coords) - np.asarray(
             l.interpolate(0 + cls.epsi, normalized=True).coords
         )

omega_prime/map_odr.py

@@ -1,7 +1,7 @@
 import logging
 from dataclasses import dataclass
 from omega_prime.map import Map, Lane, LaneBoundary
-from shapely import LineString, Polygon, simplify
+from shapely import LineString, Polygon, simplify, make_valid
 import numpy as np
 from betterosi import LaneClassificationType, LaneClassificationSubtype, LaneBoundaryClassificationType
 from pyxodr_omega_prime.road_objects.network import RoadNetwork as PyxodrRoadNetwork
@@ -17,7 +17,6 @@
 from collections import namedtuple
 import warnings
 
-
 logger = logging.getLogger(__name__)
 
 
@@ -352,19 +351,17 @@ def create(
         lane_idx: int = None,
     ):
         if side == "left":
-            if len(boundary.boundary_line) == 1:
-                polyline = LineString([boundary.boundary_line[0]] * 2)
-            else:
-                polyline = LineString(boundary.boundary_line)
+            bl = boundary.boundary_line
         elif side == "right":
-            if len(boundary.lane_reference_line) == 1:
-                polyline = LineString([boundary.lane_reference_line[0]] * 2)
-            else:
-                polyline = LineString(boundary.lane_reference_line)
-
+            bl = boundary.lane_reference_line
         else:
             raise ValueError(f"Invalid side '{side}'. Expected 'left' or 'right'.")
 
+        if len(bl) == 1:
+            polyline = LineString([bl[0]] * 2)
+        else:
+            polyline = LineString(bl)
+
         if type is None and hasattr(boundary, "lane_xml"):
             type = cls._extract_lane_boundary_type_from_xml(boundary, side)
 
@@ -402,17 +399,23 @@ class LaneXodr(Lane):
 
     @classmethod
     def create(cls, lane: PyxodrLane, road: PyxodrRoad, lane_section_id: int, lane_idx: int = None):
+        idx = XodrLaneId(road.id, lane.id, lane_section_id)
         centre_line = getattr(lane, "centre_line", None)
         if centre_line is None or not len(centre_line):
             raise ValueError(f"Lane {lane.id} has no centre_line")
 
-        centerline_2d = lane.centre_line[:, :2]
+        centerline = LineString(centre_line[:, :2])
+        if not centerline.is_valid:
+            centerline = make_valid(centerline)
+            warnings.warn(
+                f"Needed to make centerline of lane {idx} valid. Most likely, because the OpenDRIVE geometry is translated to a zero length polyline. Try to decrease `step_size`."
+            )
+
         lane_type, lane_subtype = cls._determine_lane_type_and_subtype(lane, road)
-        idx = XodrLaneId(road.id, lane.id, lane_section_id)
         return cls(
             _xodr=lane,
             idx=idx,
-            centerline=LineString(centerline_2d),
+            centerline=centerline,
             type=lane_type,
             subtype=lane_subtype,
             successor_ids=[