fix: fix Spiral error

imap/lib/opendrive/lanes.py

@@ -17,6 +17,8 @@
 
 import math
 
+import imap.global_var as global_var
+
 from imap.lib.common import shift_t
 from imap.lib.draw import draw_line
 
@@ -225,13 +227,14 @@ def generate_boundary(self, left_boundary):
       cpoint3d = shift_t(point3d, width * self.direction / 2)
       self.center_line.append(cpoint3d)
 
-    # TODO(zero): debug use
-    if self.lane_type == "driving":
-      draw_line(self.left_boundary, 'g')
-      draw_line(self.right_boundary, 'g')
-    else:
-      draw_line(self.left_boundary)
-      draw_line(self.right_boundary)
+    debug_mode = global_var.get_element_value("debug_mode")
+    if not debug_mode:
+      if self.lane_type == "driving":
+        draw_line(self.left_boundary, 'g')
+        draw_line(self.right_boundary, 'g')
+      else:
+        draw_line(self.left_boundary)
+        draw_line(self.right_boundary)
     return self.right_boundary
 
   def generate_boundary_type(self, left_boundary_type) -> str:

imap/lib/opendrive/plan_view.py

@@ -67,22 +67,25 @@ def parse_from(self, raw_geometry):
     self.curv_start = float(raw_spiral.attrib.get('curvStart'))
     self.curv_end = float(raw_spiral.attrib.get('curvEnd'))
 
+    self.cdot = (self.curv_end - self.curv_start) / self.length
+    self.s0_spiral = self.curv_start / self.cdot
+    self.s0, self.t0, self.theta0 = odr_spiral(self.s0_spiral, self.cdot)
+
   def sampling(self, delta_s):
     sample_count = math.ceil(self.length / delta_s) + 1
-    cdot = (self.curv_end - self.curv_start) / self.length
 
-    tf = Transform(self.x, self.y, 0, self.hdg, 0, 0)
+    tf = Transform(self.x, self.y, 0, self.hdg - self.theta0, 0, 0)
 
     points = []
     for i in range(sample_count):
       local_s = min(i * delta_s, self.length)
-      s, t, theta = odr_spiral(local_s, cdot)
-      x, y, z = tf.transform(s, t, 0.0)
+      s, t, theta = odr_spiral(local_s + self.s0_spiral, self.cdot)
+      x, y, z = tf.transform(s - self.s0, t - self.t0, 0.0)
 
       absolute_s = self.s + local_s
 
       point3d = Point3d(x, y, z, absolute_s)
-      point3d.set_rotate(self.hdg + theta)
+      point3d.set_rotate(self.hdg + theta - self.theta0)
       points.append(point3d)
     return points
 

imap/lib/opendrive/road.py

@@ -17,14 +17,15 @@
 
 import math
 
+import imap.global_var as global_var
+
 from imap.lib.opendrive.common import convert_speed
 from imap.lib.opendrive.plan_view import PlanView
 from imap.lib.opendrive.profile import ElevationProfile, LateralProfile
 from imap.lib.opendrive.lanes import Lanes
 
 
 GEOMETRY_SKIP_LENGTH = 0.01
-SAMPLING_LENGTH = 1.0
 
 
 # Type
@@ -177,7 +178,8 @@ def generate_reference_line(self):
       if geometry.length < GEOMETRY_SKIP_LENGTH:
         continue
 
-      points = geometry.sampling(SAMPLING_LENGTH)
+      sampling_length = global_var.get_element_value("sampling_length")
+      points = geometry.sampling(sampling_length)
       self.reference_line.extend(points)
 
     assert len(self.reference_line) != 0, \

imap/main.py

@@ -59,12 +59,19 @@ def main(args=sys.argv):
     parser.add_argument(
         "-o", "--output", action="store", type=str, required=False,
         help="map output path")
-
+    parser.add_argument(
+        "-s", "--sampling", action="store", type=int, required=False,
+        default=1, help="sampling length")
+    parser.add_argument(
+        "-d", "--debug", action="store", type=bool, required=False,
+        nargs='?', const=True, default=False, help="debug mode")
 
     args = parser.parse_args(args[1:])
 
     # 1. Init global var
     global_var._init()
+    global_var.set_element_vaule("sampling_length", args.sampling)
+    global_var.set_element_vaule("debug_mode", args.debug)
 
     # 2. show map
     if args.map is not None:

setup.py

@@ -5,7 +5,7 @@
 
 setuptools.setup(
     name="imap_box",
-    version="0.1.0",
+    version="0.1.1",
     author="daohu527",
     author_email="[email protected]",
     description="High-resolution map visualization and conversion tool",