navi: improved planning algorithm and added reference line.

modules/tools/navigation/planning/navigation_planning.py

@@ -79,29 +79,12 @@ def mobileye_callback(mobileye_pb):
     mobileye_provider.process_obstacles()
 
     if ENABLE_ROUTING_AID:
-        vx = localization_provider.localization_pb.pose.position.x
-        vy = localization_provider.localization_pb.pose.position.y
-        heading = localization_provider.localization_pb.pose.heading
-        local_smooth_seg_x, local_smooth_seg_y = \
-            routing_provider.get_local_segment_spline(vx, vy, heading)
-
-        if len(local_smooth_seg_x) <= 0:
-            path_x, path_y, path_length = path_decider.get_path(
-                mobileye_provider.left_lane_marker_coef,
-                mobileye_provider.right_lane_marker_coef,
-                chassis_provider.get_speed_mps())
-        else:
-            #print local_smooth_seg_y[0]
-            path_x, path_y, path_length = path_decider.get_routing_aid_path(
-                mobileye_provider.left_lane_marker_coef,
-                mobileye_provider.right_lane_marker_coef,
-                chassis_provider.get_speed_mps(),
-                mobileye_pb, local_smooth_seg_x, local_smooth_seg_y)
+        path_x, path_y, path_length = path_decider.get_path_by_lmr(
+            mobileye_provider, routing_provider,
+            localization_provider, chassis_provider)
     else:
-        path_x, path_y, path_length = path_decider.get_path(
-            mobileye_provider.left_lane_marker_coef,
-            mobileye_provider.right_lane_marker_coef,
-            chassis_provider.get_speed_mps())
+        path_x, path_y, path_length = path_decider.get_path_by_lm(
+            mobileye_provider, chassis_provider)
 
     final_path_length = path_length
     speed = CRUISE_SPEED

modules/tools/navigation/planning/navigation_routing_debug.py

@@ -95,7 +95,7 @@ def mobileye_callback(mobileye_pb):
     local_smooth_seg_x, local_smooth_seg_y = routing_provider.get_local_segment_spline(
         vx, vy, heading)
 
-    left_marker_coef = mobileye_provider.left_lane_marker_coef
+    left_marker_coef = mobileye_provider.left_lm_coef
     left_marker_x = []
     left_marker_y = []
     for x in range(int(mobileye_provider.left_lane_marker_range)):
@@ -103,7 +103,7 @@ def mobileye_callback(mobileye_pb):
         left_marker_x.append(x)
         left_marker_y.append(-y)
 
-    right_marker_coef = mobileye_provider.right_lane_marker_coef
+    right_marker_coef = mobileye_provider.right_lm_coef
     right_marker_x = []
     right_marker_y = []
     for x in range(int(mobileye_provider.right_lane_marker_range)):

modules/tools/navigation/planning/path_decider.py

@@ -15,8 +15,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 ###############################################################################
-import math
-from numpy.polynomial.polynomial import polyval
+
+from reference_path import ReferencePath
 
 
 class PathDecider:
@@ -24,80 +24,13 @@ class PathDecider:
         self.MINIMUM_PATH_LENGTH = 5
         self.MAX_LAT_CHANGE = 0.1
         self.last_init_lat = None
+        self.ref = ReferencePath()
 
-    def get_path_length(self, speed_mps):
-        path_length = self.MINIMUM_PATH_LENGTH
-        current_speed = speed_mps
-        if current_speed is not None:
-            if path_length < current_speed * 2:
-                path_length = math.ceil(current_speed * 2)
-        return path_length
-
-    def get_reference_line_offset(self, current_init_lat):
-        if self.last_init_lat is None:
-            return 0
-        if abs(current_init_lat - self.last_init_lat) < self.MAX_LAT_CHANGE:
-            return 0
-        else:
-            if current_init_lat > self.last_init_lat:
-                return - (abs(current_init_lat - self.last_init_lat) -
-                          self.MAX_LAT_CHANGE)
-            else:
-                return abs(
-                    current_init_lat - self.last_init_lat) - self.MAX_LAT_CHANGE
-
-    def get_path(self, left_marker_coef, right_marker_coef, speed_mps):
-        path_length = self.get_path_length(speed_mps)
-        offset = (right_marker_coef[0] + left_marker_coef[0]) / 2.0
-        if offset < -1 * self.MAX_LAT_CHANGE:
-            offset = -1 * self.MAX_LAT_CHANGE
-        if offset > self.MAX_LAT_CHANGE:
-            offset = self.MAX_LAT_CHANGE
-
-        path_coef = [0, 0, 0, 0]
-
-        path_coef[0] = offset
-        for i in range(1, 4):
-            path_coef[i] = (right_marker_coef[i] +
-                            left_marker_coef[i]) / 2.0
-
-        path_x = []
-        path_y = []
-        for x in range(int(path_length)):
-            y = -1 * polyval(x, path_coef)
-            path_x.append(x)
-            path_y.append(y)
-
-        self.last_init_lat = path_coef[0]
-        return path_x, path_y, path_length
-
-    def get_routing_aid_path(self, left_marker_coef, right_marker_coef,
-                             speed_mps, mobileye_pb,
-                             local_smooth_seg_x, local_smooth_seg_y):
-        path_length = self.get_path_length(speed_mps)
-
-        offset = self.get_reference_line_offset(
-            (right_marker_coef[0] +
-             left_marker_coef[0]) / 2.0)
-
-        routing_shift = ((right_marker_coef[0] + left_marker_coef[0]) / 2.0) + \
-                        offset - local_smooth_seg_y[0]
-
-        left_marker_quality = mobileye_pb.lka_766.quality / 3.0
-        right_marker_quality = mobileye_pb.lka_768.quality / 3.0
-
-        path_x = []
-        path_y = []
-        for i in range(int(path_length)):
-            routing_y = local_smooth_seg_y[i] + routing_shift
-            left_marker_y = (-1 * polyval(i,
-                                          left_marker_coef) - offset) * left_marker_quality
-            right_marker_y = (-1 * polyval(i,
-                                           right_marker_coef) - offset) * right_marker_quality
-            y = (routing_y + left_marker_y + right_marker_y) / (
-                1 + left_marker_quality + right_marker_quality)
-            path_x.append(i)
-            path_y.append(y)
+    def get_path_by_lm(self, mobileye, chassis):
+        return self.ref.get_ref_path_by_lm(mobileye, chassis)
 
-        self.last_init_lat = path_y[0]
-        return path_x, path_y, path_length
+    def get_path_by_lmr(self, perception, routing, localization, chassis):
+        return self.ref.get_ref_path_by_lmr(perception,
+                                            routing,
+                                            localization,
+                                            chassis)

modules/tools/navigation/planning/provider_localization.py

@@ -20,6 +20,12 @@
 class LocalizationProvider:
     def __init__(self):
         self.localization_pb = None
+        self.x = 0
+        self.y = 0
+        self.heading = 0
 
     def update(self, localization_pb):
         self.localization_pb = localization_pb
+        self.x = self.localization_pb.pose.position.x
+        self.y = self.localization_pb.pose.position.y
+        self.heading = self.localization_pb.pose.heading

modules/tools/navigation/planning/provider_mobileye.py

@@ -30,11 +30,14 @@ class Obstacle:
 class MobileyeProvider:
     def __init__(self):
         self.mobileye_pb = None
-        self.right_lane_marker_coef = [1, 0, 0, 0]
-        self.left_lane_marker_coef = [1, 0, 0, 0]
+        self.right_lm_coef = [1, 0, 0, 0]
+        self.left_lm_coef = [1, 0, 0, 0]
         self.right_lane_marker_range = 0
         self.left_lane_marker_range = 0
         self.obstacles = []
+        self.right_lm_quality = 0.0
+        self.left_lm_quality = 0.0
+        
 
     def update(self, mobileye_pb):
         self.mobileye_pb = mobileye_pb
@@ -46,14 +49,17 @@ class MobileyeProvider:
         rc2 = self.mobileye_pb.lka_768.curvature
         rc3 = self.mobileye_pb.lka_768.curvature_derivative
         self.right_lane_marker_range = self.mobileye_pb.lka_769.view_range
-        self.right_lane_marker_coef = [rc0, rc1, rc2, rc3]
+        self.right_lm_coef = [rc0, rc1, rc2, rc3]
 
         lc0 = self.mobileye_pb.lka_766.position
         lc1 = self.mobileye_pb.lka_767.heading_angle
         lc2 = self.mobileye_pb.lka_766.curvature
         lc3 = self.mobileye_pb.lka_766.curvature_derivative
         self.left_lane_marker_range = self.mobileye_pb.lka_767.view_range
-        self.left_lane_marker_coef = [lc0, lc1, lc2, lc3]
+        self.left_lm_coef = [lc0, lc1, lc2, lc3]
+
+        self.left_lm_quality = self.mobileye_pb.lka_766.quality / 3.0
+        self.right_lm_quality = self.mobileye_pb.lka_768.quality / 3.0
 
     def process_obstacles(self):
         if self.mobileye_pb is None:
@@ -79,7 +85,7 @@ class MobileyeProvider:
         vy = localization_provider.localization_pb.pose.position.y
         heading = localization_provider.localization_pb.pose.heading
         position = (vx, vy)
-        corrector = LaneMarkerCorrector(self.left_lane_marker_coef,
-                                        self.right_lane_marker_coef)
-        self.left_lane_marker_coef, self.right_lane_marker_coef = \
+        corrector = LaneMarkerCorrector(self.left_lm_coef,
+                                        self.right_lm_coef)
+        self.left_lm_coef, self.right_lm_coef = \
             corrector.correct(position, heading, routing_segment)

modules/tools/navigation/planning/provider_routing.py

@@ -37,7 +37,11 @@ def error_function(c, x, y, t, k, w=None):
         diff = np.einsum('...i,...i', diff, diff)
     else:
         diff = np.dot(diff * diff, w)
-    return np.abs(diff)
+    ddf = splev(x, (t, c, k), der=2)
+    smoothness = 0
+    for i in ddf:
+        smoothness += i * i
+    return np.abs(diff) + 1000 * smoothness
 
 
 def optimized_spline(x, y, k=3, s=0, w=None):
@@ -81,6 +85,8 @@ class RoutingProvider:
         routing = LineString(self.routing_points)
         if routing.distance(point) > 10:
             return []
+        if routing.length < 10:
+            return []
         distance = routing.project(point)
         points = []
         total_length = routing.length

modules/tools/navigation/planning/reference_path.py

+#!/usr/bin/env python
+
+###############################################################################
+# Copyright 2017 The Apollo Authors. All Rights Reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+###############################################################################
+import math
+from numpy.polynomial.polynomial import polyval
+
+
+class ReferencePath:
+    def __init__(self):
+        self.MINIMUM_PATH_LENGTH = 5
+        self.MAX_LAT_CHANGE = 0.1
+        self.init_y_last = None
+
+    def get_path_length(self, speed_mps):
+        path_length = self.MINIMUM_PATH_LENGTH
+        current_speed = speed_mps
+        if current_speed is not None:
+            if path_length < current_speed * 2:
+                path_length = math.ceil(current_speed * 2)
+        return path_length
+
+    def get_ref_path_init_y(self, init_y_perception):
+        if self.init_y_last is None:
+            return 0
+        if abs(init_y_perception - self.init_y_last) < self.MAX_LAT_CHANGE:
+            return init_y_perception
+        else:
+            if init_y_perception > self.init_y_last:
+                return self.init_y_last + self.MAX_LAT_CHANGE
+            else:
+                return self.init_y_last - self.MAX_LAT_CHANGE
+
+    def get_ref_path_by_lm(self, perception, chassis):
+        path_length = self.get_path_length(chassis.get_speed_mps())
+        init_y_perception = (perception.right_lm_coef[0] +
+                             perception.left_lm_coef[0]) / -2.0
+        init_y = self.get_ref_path_init_y(init_y_perception)
+        self.init_y_last = init_y
+        path_x, path_y = self._get_perception_ref_path(
+            perception, path_length, init_y)
+        return path_x, path_y, path_length
+
+    def _get_perception_ref_path(self, perception, path_length, init_y):
+        path_coef = [0, 0, 0, 0]
+
+        path_coef[0] = -1 * init_y
+        quality = perception.right_lm_quality + perception.left_lm_quality
+        if quality > 0:
+            for i in range(1, 4):
+                path_coef[i] = (perception.right_lm_coef[i] *
+                                perception.right_lm_quality +
+                                perception.left_lm_coef[i] *
+                                perception.left_lm_quality) / quality
+        path_x = []
+        path_y = []
+        for x in range(int(path_length)):
+            y = -1 * polyval(x, path_coef)
+            path_x.append(x)
+            path_y.append(y)
+        return path_x, path_y
+
+    def get_ref_path_by_lmr(self, perception, routing, localization, chassis):
+
+        path_length = self.get_path_length(chassis.get_speed_mps())
+
+        init_y_perception = (perception.right_lm_coef[0] +
+                             perception.left_lm_coef[0]) / -2.0
+        init_y = self.get_ref_path_init_y(init_y_perception)
+        self.init_y_last = init_y
+
+        lmpath_x, lmpath_y = self._get_perception_ref_path(
+            perception, path_length, init_y)
+
+        quality = perception.right_lm_quality + perception.left_lm_quality
+        quality = quality / 2.0
+
+        rpath_x, rpath_y = routing.get_local_segment_spline(localization.x,
+                                                            localization.y,
+                                                            localization.heading)
+
+        if (len(rpath_x) < path_length):
+            return lmpath_x, lmpath_y, path_length
+
+        routing_shift = rpath_y[0] - init_y
+        path_x = []
+        path_y = []
+        for i in range(int(path_length)):
+            y = (lmpath_y[i] * quality + rpath_y[i] - routing_shift) / (1 + quality)
+            path_x.append(i)
+            path_y.append(y)
+
+        return path_x, path_y, path_length