From d4c519199eb5dd779f71439b2416cb88624379b8 Mon Sep 17 00:00:00 2001 From: jiangyifei Date: Tue, 12 Sep 2017 16:24:21 -0700 Subject: [PATCH] planning updated plot planning. --- modules/tools/mapshow/planning.py | 46 +++++++++++ modules/tools/mapshow/plot_smoothness.py | 40 ++++++++++ modules/tools/mapshow/subplot_traj_acc.py | 84 +++++++++++++++++++++ modules/tools/mapshow/subplot_traj_speed.py | 84 +++++++++++++++++++++ 4 files changed, 254 insertions(+) create mode 100644 modules/tools/mapshow/plot_smoothness.py create mode 100644 modules/tools/mapshow/subplot_traj_acc.py create mode 100644 modules/tools/mapshow/subplot_traj_speed.py diff --git a/modules/tools/mapshow/planning.py b/modules/tools/mapshow/planning.py index 0a66890ed9..9410373c7f 100644 --- a/modules/tools/mapshow/planning.py +++ b/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, diff --git a/modules/tools/mapshow/plot_smoothness.py b/modules/tools/mapshow/plot_smoothness.py new file mode 100644 index 0000000000..8c388b6b68 --- /dev/null +++ b/modules/tools/mapshow/plot_smoothness.py @@ -0,0 +1,40 @@ +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() diff --git a/modules/tools/mapshow/subplot_traj_acc.py b/modules/tools/mapshow/subplot_traj_acc.py new file mode 100644 index 0000000000..7f439ebc4f --- /dev/null +++ b/modules/tools/mapshow/subplot_traj_acc.py @@ -0,0 +1,84 @@ +#!/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 diff --git a/modules/tools/mapshow/subplot_traj_speed.py b/modules/tools/mapshow/subplot_traj_speed.py new file mode 100644 index 0000000000..81d160b33d --- /dev/null +++ b/modules/tools/mapshow/subplot_traj_speed.py @@ -0,0 +1,84 @@ +#!/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 -- GitLab