planning updated plot planning.

modules/tools/mapshow/planning.py

@@ -32,6 +32,14 @@ class Planning:
         self.speed_data_time = {}
         self.speed_data_val = {}
 
+        self.traj_data_lock = threading.Lock()
+        self.traj_speed_history_len = 30
+        self.traj_speed_t_history = []
+        self.traj_speed_v_history = []
+        self.traj_acc_history_len = 30
+        self.traj_acc_t_history = []
+        self.traj_acc_a_history = []
+
         self.st_data_lock = threading.Lock()
         self.st_curve_s = {}
         self.st_curve_t = {}
@@ -233,6 +241,44 @@ class Planning:
 
         self.st_data_lock.release()
 
+    def compute_traj_data(self):
+        traj_speed_t = []
+        traj_speed_v = []
+        traj_acc_t = []
+        traj_acc_a = []
+        base_time = self.planning_pb.header.timestamp_sec
+        for trajectory_point in self.planning_pb.trajectory_point:
+            traj_acc_t.append(base_time + trajectory_point.relative_time)
+            traj_acc_a.append(trajectory_point.a)
+
+        for trajectory_point in self.planning_pb.trajectory_point:
+            traj_speed_t.append(base_time + trajectory_point.relative_time)
+            traj_speed_v.append(trajectory_point.v)
+
+        self.traj_data_lock.acquire()
+        
+        self.traj_speed_t_history.append(traj_speed_t)
+        self.traj_speed_v_history.append(traj_speed_v)
+        if len(self.traj_speed_t_history) > self.traj_speed_history_len:
+            self.traj_speed_t_history = \
+                self.traj_speed_t_history[len(self.traj_speed_t_history)
+                                          - self.traj_speed_history_len:]
+            self.traj_speed_v_history = \
+                self.traj_speed_v_history[len(self.traj_speed_v_history)
+                                          - self.traj_speed_history_len:]
+            
+        self.traj_acc_t_history.append(traj_acc_t)
+        self.traj_acc_a_history.append(traj_acc_a)
+        if len(self.traj_acc_t_history) > self.traj_acc_history_len:
+            self.traj_acc_t_history = \
+                self.traj_acc_t_history[len(self.traj_acc_t_history)
+                                          - self.traj_acc_history_len:]
+            self.traj_acc_a_history = \
+                self.traj_acc_a_history[len(self.traj_acc_a_history)
+                                          - self.traj_acc_history_len:]
+
+        self.traj_data_lock.release()
+
     def replot_sl_data(self,
                        sl_static_obstacle_lower_boundary,
                        sl_static_obstacle_upper_boundary,

modules/tools/mapshow/plot_smoothness.py

+import rospy
+from modules.planning.proto import planning_pb2
+import matplotlib.pyplot as plt
+from planning import Planning
+import matplotlib.animation as animation
+from subplot_traj_speed import TrajSpeedSubplot
+from subplot_traj_acc import TrajAccSubplot
+
+planning = Planning()
+
+def update(frame_number):
+    traj_speed_subplot.show(planning)
+    traj_acc_subplot.show(planning)
+
+def planning_callback(planning_pb):
+    planning.update_planning_pb(planning_pb)
+    planning.compute_traj_data()
+
+def add_listener():
+    rospy.init_node('st_plot', anonymous=True)
+    rospy.Subscriber('/apollo/planning', planning_pb2.ADCTrajectory,
+                     planning_callback)
+
+def press_key():
+    pass
+
+if __name__ == '__main__':
+    add_listener()
+    fig = plt.figure(figsize=(14, 6))
+    fig.canvas.mpl_connect('key_press_event', press_key)
+
+    ax = plt.subplot2grid((1, 2), (0, 0))
+    traj_speed_subplot = TrajSpeedSubplot(ax)
+
+    ax2 = plt.subplot2grid((1, 2), (0, 1))
+    traj_acc_subplot = TrajAccSubplot(ax2)
+
+    ani = animation.FuncAnimation(fig, update, interval=100)
+
+    plt.show()

modules/tools/mapshow/subplot_traj_acc.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 matplotlib.pyplot as plt
+from matplotlib import cm as cmx
+from matplotlib import colors as mcolors
+
+class TrajAccSubplot:
+    def __init__(self, ax):
+        self.ax = ax
+        self.acc_lines = []
+        self.acc_lines_size = 30
+        self.colors = []
+        self.init_colors()
+        #self.colors = ['b','r', 'y', 'k']
+        for i in range(self.acc_lines_size):
+            line, = ax.plot(
+                [0], [0],
+                c=self.colors[i % len(self.colors)],
+                ls="-",
+                marker='',
+                lw=3,
+                alpha=0.8)
+            self.acc_lines.append(line)
+
+        ax.set_xlabel("t (second)")
+        #ax.set_xlim([-2, 10])
+        ax.set_ylim([-6, 6])
+        self.ax.autoscale_view()
+        #self.ax.relim()
+        ax.set_ylabel("acc (m/s^2)")
+        ax.set_title("PLANNING ACC")
+        self.set_visible(False)
+
+    def init_colors(self):
+        self.colors = []
+        values = range(self.acc_lines_size)
+        jet = plt.get_cmap('brg')
+        color_norm = mcolors.Normalize(vmin=0, vmax=values[-1])
+        scalar_map = cmx.ScalarMappable(norm=color_norm, cmap=jet)
+        for val in values:
+            color_val = scalar_map.to_rgba(val)
+            self.colors.append(color_val)
+
+    def set_visible(self, visible):
+        for line in self.acc_lines:
+            line.set_visible(visible)
+
+    def show(self, planning):
+        planning.traj_data_lock.acquire()
+        for i in range(len(planning.traj_speed_t_history)):
+            if i >= self.acc_lines_size:
+                print "WARNING: number of path lines is more than " \
+                      + str(self.acc_lines_size)
+                continue
+            speed_line = self.acc_lines[self.acc_lines_size-i-1]
+
+            speed_line.set_xdata(planning.traj_acc_t_history[i])
+            speed_line.set_ydata(planning.traj_acc_a_history[i])
+            #speed_line.set_xdata([1,2,3,4])
+            #speed_line.set_ydata([1,2,3,4])
+            #speed_line.set_label(name[0:5])
+            speed_line.set_visible(True)
+
+        #self.ax.legend(loc="upper left", borderaxespad=0., ncol=5)
+        #self.ax.axis('equal')
+        planning.traj_data_lock.release()
+        self.ax.autoscale_view()
+        self.ax.relim()
\ No newline at end of file

modules/tools/mapshow/subplot_traj_speed.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 matplotlib.pyplot as plt
+from matplotlib import cm as cmx
+from matplotlib import colors as mcolors
+
+class TrajSpeedSubplot:
+    def __init__(self, ax):
+        self.ax = ax
+        self.speed_lines = []
+        self.speed_lines_size = 30
+        self.colors = []
+        self.init_colors()
+        #self.colors = ['b','r', 'y', 'k']
+        for i in range(self.speed_lines_size):
+            line, = ax.plot(
+                [0], [0],
+                c=self.colors[i % len(self.colors)],
+                ls="-",
+                marker='',
+                lw=3,
+                alpha=0.8)
+            self.speed_lines.append(line)
+
+        ax.set_xlabel("t (second)")
+        #ax.set_xlim([-2, 10])
+        ax.set_ylim([-1, 13])
+        self.ax.autoscale_view()
+        #self.ax.relim()
+        ax.set_ylabel("speed (m/s)")
+        ax.set_title("PLANNING SPEED")
+        self.set_visible(False)
+
+    def init_colors(self):
+        self.colors = []
+        values = range(self.speed_lines_size)
+        jet = plt.get_cmap('brg')
+        color_norm = mcolors.Normalize(vmin=0, vmax=values[-1])
+        scalar_map = cmx.ScalarMappable(norm=color_norm, cmap=jet)
+        for val in values:
+            color_val = scalar_map.to_rgba(val)
+            self.colors.append(color_val)
+
+    def set_visible(self, visible):
+        for line in self.speed_lines:
+            line.set_visible(visible)
+
+    def show(self, planning):
+        planning.traj_data_lock.acquire()
+        for i in range(len(planning.traj_speed_t_history)):
+            if i >= self.speed_lines_size:
+                print "WARNING: number of path lines is more than " \
+                      + str(self.speed_lines_size)
+                continue
+            speed_line = self.speed_lines[self.speed_lines_size-i-1]
+
+            speed_line.set_xdata(planning.traj_speed_t_history[i])
+            speed_line.set_ydata(planning.traj_speed_v_history[i])
+            #speed_line.set_xdata([1,2,3,4])
+            #speed_line.set_ydata([1,2,3,4])
+            #speed_line.set_label(name[0:5])
+            speed_line.set_visible(True)
+
+        #self.ax.legend(loc="upper left", borderaxespad=0., ncol=5)
+        #self.ax.axis('equal')
+        planning.traj_data_lock.release()
+        self.ax.autoscale_view()
+        self.ax.relim()
\ No newline at end of file