[planning] added init point in plot planning path

modules/tools/mapshow/planning.py

@@ -23,7 +23,7 @@ from modules.planning.proto import planning_internal_pb2
 class Planning:
     def __init__(self, planning_pb=None):
         self.data_lock = threading.Lock()
-
+        self.init_point_lock = threading.Lock()
         self.planning_pb = planning_pb
         self.path_data_lock = threading.Lock()
         self.path_data_x = {}
@@ -60,10 +60,19 @@ class Planning:
         self.sl_aggregated_boundary_high_l = []
         self.sl_aggregated_boundary_s = []
 
+        self.init_point_x = []
+        self.init_point_y = []
 
     def update_planning_pb(self, planning_pb):
         self.planning_pb = planning_pb
 
+    def compute_init_point(self):
+        self.init_point_lock.acquire()
+        init_point = self.planning_pb.debug.planning_data.init_point
+        self.init_point_x = [init_point.path_point.x]
+        self.init_point_y = [init_point.path_point.y]
+        self.init_point_lock.release()
+
     def compute_sl_data(self):
         sl_sampled_s = []
         sl_map_lower_boundary = []

modules/tools/mapshow/plot_planning.py

@@ -52,6 +52,7 @@ def planning_callback(planning_pb):
     planning.compute_sl_data()
     planning.compute_path_data()
     planning.compute_speed_data()
+    planning.compute_init_point()
 
 def add_listener():
     rospy.init_node('st_plot', anonymous=True)

modules/tools/mapshow/subplot_path.py

@@ -39,7 +39,7 @@ class PathSubplot:
 
         self.vehicle_position_line, = ax.plot([0], [0], 'go', alpha=0.3)
         self.vehicle_polygon_line, = ax.plot([0], [0], 'g-')
-
+        self.init_point_line, = ax.plot([0], [0], 'ro', alpha=0.3)
         self.set_visible(False)
         ax.set_title("PLANNING PATH")
 
@@ -48,6 +48,7 @@ class PathSubplot:
             line.set_visible(visible)
         self.vehicle_position_line.set_visible(False)
         self.vehicle_polygon_line.set_visible(False)
+        self.init_point_line.set_visible(False)
 
     def show(self, planning, localization):
         cnt = 0
@@ -67,6 +68,13 @@ class PathSubplot:
             cnt += 1
         planning.path_data_lock.release()
 
+        planning.init_point_lock.acquire()
+        print planning.init_point_x
+        self.init_point_line.set_xdata(planning.init_point_x)
+        self.init_point_line.set_ydata(planning.init_point_y)
+        self.init_point_line.set_visible(True)
+        planning.init_point_lock.release()
+
         localization.localization_data_lock.acquire()
         self.draw_vehicle(localization)
         try: