Ensure vehicle is always within planning trajectory

modules/tools/record_play/rtk_player.py

@@ -44,12 +44,12 @@ import hmi_status_helper
 
 SEARCH_INTERVAL = 1000
 
-
 class RtkPlayer(object):
     """
     rtk player class
     """
 
+
     def __init__(self, record_file, speedmultiplier, completepath, replan):
         """Init player."""
         self.firstvalid = False
@@ -80,7 +80,7 @@ class RtkPlayer(object):
 
         b, a = signal.butter(6, 0.05, 'low')
         self.data['acceleration'] = signal.filtfilt(b, a,
-                                                    self.data['acceleration'])
+            self.data['acceleration'])
 
         self.start = 0
         self.end = 0
@@ -129,22 +129,44 @@ class RtkPlayer(object):
         self.padmsg.CopyFrom(data)
 
     def restart(self):
-        shortest_dist_sqr = float('inf')
         self.logger.info("before replan self.start=%s, self.closestpoint=%s" %
                          (self.start, self.closestpoint))
+
+        self.closestpoint = closest_dist()
+        self.start = max(self.closestpoint - 100, 0)
+        self.starttime = rospy.get_time()
+        self.end = min(self.start + 1000, len(self.data) - 1)
+        self.logger.info("finish replan at time %s, self.closestpoint=%s" %
+                         (self.starttime, self.closestpoint))
+
+    def closest_dist(self):
+        shortest_dist_sqr = float('inf')
+        self.logger.info("before closest self.start=%s" %
+                         (self.start))
         search_start = max(self.start - SEARCH_INTERVAL / 2, 0)
         search_end = min(self.start + SEARCH_INTERVAL / 2, len(self.data))
+        start = self.start
         for i in range(search_start, search_end):
             dist_sqr = (self.carx - self.data['x'][i]) ** 2 + \
                    (self.cary - self.data['y'][i]) ** 2
             if dist_sqr <= shortest_dist_sqr:
-                self.start = i
+                start = i
                 shortest_dist_sqr = dist_sqr
+        return start
+
+    def closest_time(self):
+        time_elapsed = rospy.get_time() - self.starttime
+        closest_time = self.start
+        time_diff = self.data['time'][closest_time] - \
+           self.data['time'][self.closestpoint]
+
+        while time_diff < time_elapsed and closest_time < (
+                len(self.data) - 1):
+            closest_time = closest_time + 1
+            time_diff = self.data['time'][closest_time] - \
+                self.data['time'][self.closestpoint]
 
-        self.closestpoint = self.start
-        self.starttime = rospy.get_time()
-        self.logger.info("finish replan at time %s, self.closestpoint=%s" %
-                         (self.starttime, self.closestpoint))
+        return closest_time
 
     def publish_planningmsg(self):
         """
@@ -171,31 +193,25 @@ class RtkPlayer(object):
                 % (self.replan, self.sequence_num, self.automode))
             self.restart()
         else:
-            time_elapsed = now - self.starttime
-            time_diff = self.data['time'][self.start] - \
-                self.data['time'][self.closestpoint]
-            while time_diff < time_elapsed and self.start < (
-                    len(self.data) - 1):
-                self.start = self.start + 1
-                time_diff = self.data['time'][self.start] - \
-                    self.data['time'][self.closestpoint]
-
-        xdiff_sqr = (self.data['x'][self.start] - self.carx)**2
-        ydiff_sqr = (self.data['y'][self.start] - self.cary)**2
-        if xdiff_sqr + ydiff_sqr > 4.0:
-            self.logger.info("trigger replan: distance larger than 2.0")
-            self.restart()
+            timepoint = closest_time()
+            distpoint = closest_dist()
+            self.start = max(min(timepoint, distpoint) - 100, 0)
+            self.end = min(max(timepoint, distpoint) + 900, len(self.data) - 1)
+
+            xdiff_sqr = (self.data['x'][timepoint] - self.carx)**2
+            ydiff_sqr = (self.data['y'][timepoint] - self.cary)**2
+            if xdiff_sqr + ydiff_sqr > 4.0:
+                self.logger.info("trigger replan: distance larger than 2.0")
+                self.restart()
 
         if self.completepath:
             self.start = 0
             self.end = len(self.data) - 1
-        else:
-            self.start = max(self.start - 100, 0)
-            self.end = min(self.start + 1000, len(self.data) - 1)
 
         self.logger.debug(
             "publish_planningmsg: after adjust start: self.start = %s, self.end=%s"
             % (self.start, self.end))
+
         for i in range(self.start, self.end):
             adc_point = planning_pb2.ADCTrajectoryPoint()
             adc_point.x = self.data['x'][i]