Implement integer lane offset

This can for example be used to build left turning lanes.

addon/export.py

@@ -777,6 +777,11 @@ def create_lanes(self, obj):
             lane.add_roadmark(road_mark)
             lanesection.add_right_lane(lane)
         lanes.add_lanesection(lanesection)
+        lanes.add_laneoffset(xodr.LaneOffset(0,
+                                             obj['lane_offset_coefficients']['a'],
+                                             obj['lane_offset_coefficients']['b'] / obj['geometry_total_length'],
+                                             obj['lane_offset_coefficients']['c'] / obj['geometry_total_length']**2,
+                                             obj['lane_offset_coefficients']['d'] / obj['geometry_total_length']**3))
 
         return lanes
 

addon/geometry.py

@@ -15,13 +15,16 @@
 from math import pi, copysign
 from numpy import linspace
 
+from . import helpers
+
 class DSC_geometry():
 
     def __init__(self):
         # TODO: Maybe at some point implement mixed geometries
         self.sections = []
         self.section_curves = []
         self.total_length = 0
+        self.lane_offset_coefficients = {'a': 0, 'b': 0, 'c': 0, 'd': 0}
 
     def update_total_length(self):
         '''
@@ -125,7 +128,7 @@ def remove_last_section(self):
         self.update_total_length()
         self.matrix_world = None
 
-    def update(self, params_input, geometry_solver):
+    def update(self, params_input, lane_offset_start, lane_offset_end, geometry_solver):
         '''
             Update parameters of the geometry and local to global tranformation
             matrix.
@@ -135,6 +138,7 @@ def update(self, params_input, geometry_solver):
         self.update_local_to_global(params_input)
         self.update_local_and_section_params(params_input)
         self.update_plan_view(params_input, geometry_solver)
+        self.update_lane_offset_coefficients(lane_offset_start, lane_offset_end)
         self.update_elevation(params_input)
         self.update_total_length()
 
@@ -178,6 +182,14 @@ def update_plan_view(self, params, geometry_solver):
         '''
         raise NotImplementedError()
 
+    def update_lane_offset_coefficients(self, lane_offset_start, lane_offset_end):
+        '''
+            Update lane offset coefficients of road geometry.
+        '''
+        c = 3.0 * (lane_offset_end - lane_offset_start)
+        d = -2.0 * (lane_offset_end - lane_offset_start)
+        self.lane_offset_coefficients = {'a': lane_offset_start, 'b': 0, 'c': c, 'd': d}
+
     def update_elevation(self, params_input):
         '''
             Update elevation of road geometry based on predecessor, successor,
@@ -406,7 +418,7 @@ def get_section_idx_and_s(self, s):
                 break
         return idx_section, s_section
 
-    def get_elevation(self, s):
+    def calculate_elevation(self, s):
         '''
             Return the elevation level for the given value of s in the
             geometry section with the given index and its curvature.
@@ -440,13 +452,14 @@ def sample_cross_section(self, s, t_vec):
             reference line.
         '''
         x_s, y_s, hdg, curvature_plan_view = self.sample_plan_view(s)
-        z, curvature_elevation = self.get_elevation(s)
+        z, curvature_elevation = self.calculate_elevation(s)
         curvature_abs = max(abs(curvature_plan_view), abs(curvature_elevation))
         vector_hdg_t = Vector((1.0, 0.0))
         vector_hdg_t.rotate(Matrix.Rotation(hdg + pi/2, 2))
         xyz = []
         for t in t_vec:
-            xy_vec = Vector((x_s, y_s)) + t * vector_hdg_t
+            lane_offset = helpers.calculate_lane_offset(s, self.lane_offset_coefficients, self.total_length)
+            xy_vec = Vector((x_s, y_s)) + t * vector_hdg_t + lane_offset * vector_hdg_t
             xyz += [(xy_vec.x, xy_vec.y, z)]
         return xyz, hdg, curvature_abs
 

addon/helpers.py

@@ -355,6 +355,17 @@ def set_connecting_road_properties(context, joint_side_start, road_contact_point
     context.scene.dsc_properties.connecting_road_properties.lanes[0].road_mark_type = 'none'
     context.scene.dsc_properties.connecting_road_properties.lanes[1].road_mark_type = 'none'
 
+def calculate_lane_offset(s, lane_offset_coefficients, total_length):
+    '''
+        Return lane offset in [m] for the given s value based on the
+        pre-calculated polynomial coefficients. Note that 'b' is not
+        required for the implemented solution.
+    '''
+    s_norm = s / total_length
+    lane_offset = lane_offset_coefficients['a'] + \
+        (lane_offset_coefficients['c'] * s_norm**2 + lane_offset_coefficients['d'] * s_norm**3)
+    return lane_offset
+
 def get_lane_widths_road_joint(obj, contact_point):
     '''
         Return the widths of the left and right road side of the road end
@@ -463,24 +474,30 @@ def point_to_road_connector(obj, point):
     dist_end_r = (Vector(obj['cp_end_r']) - point).length
     distances = [dist_start_l, dist_start_r, dist_end_l, dist_end_r]
     arg_min_dist = distances.index(min(distances))
-    width_left_start, width_right_start = get_lane_widths_road_joint(obj, contact_point='start')
-    width_left_end, width_right_end = get_lane_widths_road_joint(obj, contact_point='end')
+    widths_left_start, widths_right_start = get_lane_widths_road_joint(obj, contact_point='start')
+    widths_left_end, widths_right_end = get_lane_widths_road_joint(obj, contact_point='end')
     if arg_min_dist == 0:
+        lane_offset = calculate_lane_offset(0, obj['lane_offset_coefficients'], obj['geometry_total_length'])
         return 'cp_start_l', Vector(obj['cp_start_l']), obj['geometry'][0]['heading_start'] - pi, \
-            -obj['geometry'][0]['curvature_start'], -obj['geometry'][0]['slope_start'], \
-            width_left_start, width_right_start, obj['lanes_left_types'], obj['lanes_right_types']
+            -obj['geometry'][0]['curvature_start'], -obj['geometry'][0]['slope_start'], lane_offset, \
+            widths_left_start, widths_right_start, obj['lanes_left_types'], obj['lanes_right_types']
     if arg_min_dist == 1:
+        lane_offset = calculate_lane_offset(0, obj['lane_offset_coefficients'], obj['geometry_total_length'])
         return 'cp_start_r', Vector(obj['cp_start_r']), obj['geometry'][0]['heading_start'] - pi, \
-            -obj['geometry'][0]['curvature_start'], -obj['geometry'][0]['slope_start'], \
-            width_left_start, width_right_start, obj['lanes_left_types'], obj['lanes_right_types']
+            -obj['geometry'][0]['curvature_start'], -obj['geometry'][0]['slope_start'], lane_offset, \
+            widths_left_start, widths_right_start, obj['lanes_left_types'], obj['lanes_right_types']
     elif arg_min_dist == 2:
+        lane_offset = calculate_lane_offset(
+            obj['geometry_total_length'], obj['lane_offset_coefficients'], obj['geometry_total_length'])
         return 'cp_end_l', Vector(obj['cp_end_l']), obj['geometry'][-1]['heading_end'], \
-            obj['geometry'][-1]['curvature_end'], obj['geometry'][-1]['slope_end'], \
-            width_left_end, width_right_end, obj['lanes_left_types'], obj['lanes_right_types']
+            obj['geometry'][-1]['curvature_end'], obj['geometry'][-1]['slope_end'], lane_offset, \
+            widths_left_end, widths_right_end, obj['lanes_left_types'], obj['lanes_right_types']
     else:
+        lane_offset = calculate_lane_offset(
+            obj['geometry_total_length'], obj['lane_offset_coefficients'], obj['geometry_total_length'])
         return 'cp_end_r', Vector(obj['cp_end_r']), obj['geometry'][-1]['heading_end'], \
-            obj['geometry'][-1]['curvature_end'], obj['geometry'][-1]['slope_end'], \
-            width_left_end, width_right_end, obj['lanes_left_types'], obj['lanes_right_types']
+            obj['geometry'][-1]['curvature_end'], obj['geometry'][-1]['slope_end'], lane_offset, \
+            widths_left_end, widths_right_end, obj['lanes_left_types'], obj['lanes_right_types']
 
 def point_to_junction_joint_exterior(obj, point):
     '''
@@ -516,8 +533,8 @@ def get_closest_joint_lane_contact_point(joint, point, joint_side):
         lane_id = 0
         for width_left in list(joint['lane_widths_left']):
             lane_id += 1
-            t_contact_point = t
-            t_lane_center = t + width_left/2
+            t_contact_point = t + joint['lane_offset']
+            t_lane_center = t + width_left/2 + joint['lane_offset']
             t += width_left
             lane_ids_left.append(lane_id)
             vec_hdg = Vector((1.0, 0.0, 0.0))
@@ -531,8 +548,8 @@ def get_closest_joint_lane_contact_point(joint, point, joint_side):
         lane_id = 0
         for width_right in joint['lane_widths_right']:
             lane_id -= 1
-            t_contact_point = t
-            t_lane_center = t - width_right/2
+            t_contact_point = t + joint['lane_offset']
+            t_lane_center = t - width_right/2 + joint['lane_offset']
             t -= width_right
             lane_ids_right.append(lane_id)
             vec_hdg = Vector((1.0, 0.0, 0.0))
@@ -651,6 +668,7 @@ def mouse_to_road_joint_params(context, event, road_type, joint_side='both'):
     heading = 0
     curvature = 0
     slope = 0
+    lane_offset_coefficients = {'a': 0, 'b': 0, 'c': 0, 'd': 0}
     lane_widths_left = []
     lane_widths_right = []
     lane_types_left = []
@@ -663,7 +681,7 @@ def mouse_to_road_joint_params(context, event, road_type, joint_side='both'):
             if obj['dsc_category'] == 'OpenDRIVE':
                 if obj['dsc_type'].startswith('road_') and obj['dsc_type'] != 'road_object':
                     hit_type = 'road'
-                    point_type, snapped_point, heading, curvature, slope, \
+                    point_type, snapped_point, heading, curvature, slope, lane_offset_coefficients, \
                     lane_widths_left, lane_widths_right, lane_types_left, lane_types_right \
                         = point_to_road_connector(obj, raycast_point)
                     id_obj = obj['id_odr']
@@ -720,6 +738,7 @@ def mouse_to_road_joint_params(context, event, road_type, joint_side='both'):
             'heading': heading,
             'curvature': curvature,
             'slope': slope,
+            'lane_offset_coefficients': lane_offset_coefficients,
             'lane_widths_left': lane_widths_left,
             'lane_widths_right': lane_widths_right,
             'lane_types_left': lane_types_left,

addon/junction.py

@@ -24,14 +24,15 @@
 
 class junction_joint:
     def __init__(self, id_joint, id_incoming, contact_point_type, contact_point_vec,
-                 heading, curvature, slope, lane_widths_left, lane_widths_right, lane_types_left, lane_types_right):
+                 heading, curvature, slope, lane_offset, lane_widths_left, lane_widths_right, lane_types_left, lane_types_right):
         self.id_joint = id_joint
         self.id_incoming = id_incoming
         self.contact_point_type = contact_point_type
         self.contact_point_vec = contact_point_vec
         self.heading = heading
         self.curvature = curvature
         self.slope = slope
+        self.lane_offset = lane_offset
         self.lane_widths_left = lane_widths_left
         self.lane_widths_right = lane_widths_right
         self.lane_types_left = lane_types_left
@@ -65,7 +66,8 @@ def get_new_id_joint(self):
         return id_next
 
     def add_joint_incoming(self, id_incoming, contact_point_type, contact_point_vec,
-                 heading, curvature, slope, lane_widths_left, lane_widths_right, lane_types_left, lane_types_right):
+                 heading, curvature, slope, lane_offset,
+                 lane_widths_left, lane_widths_right, lane_types_left, lane_types_right):
         '''
             Add a new joint, i.e. an incoming road to the junction if it does
             not exist yet.
@@ -75,18 +77,20 @@ def add_joint_incoming(self, id_incoming, contact_point_type, contact_point_vec,
         else:
             id_joint = self.get_new_id_joint()
             joint = junction_joint(id_joint, id_incoming, contact_point_type, contact_point_vec,
-                                   heading, curvature, slope, lane_widths_left, lane_widths_right, lane_types_left, lane_types_right)
+                                   heading, curvature, slope, lane_offset,
+                                   lane_widths_left, lane_widths_right, lane_types_left, lane_types_right)
             self.joints.append(joint)
             return True
 
-    def add_joint_open(self, contact_point_vec, heading, slope,
+    def add_joint_open(self, contact_point_vec, heading, slope, lane_offset,
                        lane_widths_left, lane_widths_right, lane_types_left, lane_types_right):
         '''
             Add a new joint without connecting to an incoming road.
         '''
         id_joint = self.get_new_id_joint()
         joint = junction_joint(id_joint, None, 'junction_joint_open', contact_point_vec,
-            heading, 0.0, slope, lane_widths_left, lane_widths_right, lane_types_left, lane_types_right)
+            heading, 0.0, slope, lane_offset,
+            lane_widths_left, lane_widths_right, lane_types_left, lane_types_right)
         self.joints.append(joint)
         return True
 
@@ -231,10 +235,10 @@ def get_mesh(self, wireframe=False):
                 # Find corner points of incoming road joint
                 vector_s = Vector((1.0, 0.0, 0.0))
                 vector_s.rotate(Matrix.Rotation(joint.heading + pi/2, 3, 'Z'))
-                width_left = sum(joint.lane_widths_left)
+                width_left = sum(joint.lane_widths_left) + joint.lane_offset
                 point_local_left = matrix_world.inverted() \
                     @ (joint.contact_point_vec + vector_s * width_left)
-                width_right = sum(joint.lane_widths_right)
+                width_right = sum(joint.lane_widths_right) - joint.lane_offset
                 point_local_right = matrix_world.inverted() \
                     @ (joint.contact_point_vec - vector_s * width_right)
                 joints_heading.append(joint.heading)

addon/modal_junction_generic.py

@@ -68,20 +68,23 @@ def modal(self, context, event):
                         contact_point_vec = self.params_snap['point'].copy()
                         # Calculate width of junction joints based on road direction
                         if self.params_snap['point_type'].startswith('cp_end'):
+                            joint_lane_offset = self.params_snap['lane_offset_coefficients']
                             joint_widths_left = self.params_snap['lane_widths_left']
                             joint_widths_right = self.params_snap['lane_widths_right']
                             joint_lane_types_left = self.params_snap['lane_types_left']
                             joint_lane_types_right = self.params_snap['lane_types_right']
                         else:
                             # Switch direction if road points away from junction joint
+                            joint_lane_offset = self.params_snap['lane_offset_coefficients']
                             joint_widths_left = self.params_snap['lane_widths_right']
                             joint_widths_right = self.params_snap['lane_widths_left']
                             joint_lane_types_left = self.params_snap['lane_types_right']
                             joint_lane_types_right = self.params_snap['lane_types_left']
                         joint_added = self.junction.add_joint_incoming(self.params_snap['id_obj'],
                             self.params_snap['point_type'], contact_point_vec,
                             self.params_snap['heading'], self.params_snap['curvature'], self.params_snap['slope'],
-                            joint_widths_left, joint_widths_right, joint_lane_types_left, joint_lane_types_right)
+                            joint_lane_offset, joint_widths_left, joint_widths_right,
+                            joint_lane_types_left, joint_lane_types_right)
                         if joint_added:
                             self.junction.update_stencil()
                         else:

addon/modal_road_base.py

@@ -391,6 +391,7 @@ def modal(self, context, event):
                     self.selected_slope = 0
                     self.selected_normal_start = Vector((0.0,0.0,1.0))
                     if (event.alt or self.always_adjust_heading) \
+                        and self.state != 'SELECT_START' \
                         and self.params_input['connected_start'] == False \
                         and len(self.params_input['points']) == 2:
                         # Calculate angular change to update start heading

addon/modal_road_object_base.py

@@ -26,9 +26,9 @@
 from . geometry_parampoly3 import DSC_geometry_parampoly3
 
 
-def load_geometry(road_type, section_data):
+def load_geometry(road_type, section_data, lane_offset):
     '''
-        Load geometry from stored sections data.
+        Load geometry from stored geometry data.
     '''
     geometry = None
     if road_type == 'road_straight':
@@ -44,6 +44,7 @@ def load_geometry(road_type, section_data):
 
     if geometry != None:
         geometry.load_sections(section_data)
+        geometry.lane_offset_coefficients = lane_offset
 
     return geometry