[planning] fixed a bug in qp frenet frame and updated plot planning tool with aggregated boundary.

modules/planning/tasks/qp_spline_path/qp_frenet_frame.cc

@@ -505,6 +505,8 @@ bool QpFrenetFrame::GetBound(
   }
 
   if (std::isinf(low_second)) {
+    bound->second = low_second;
+  }else {
     bound->second = low_second * (1 - weight) + high_second * weight;
   }
   return true;

modules/tools/mapshow/planning.py

@@ -46,6 +46,10 @@ class Planning:
         self.sl_map_upper_boundary = []
         self.sl_path_s = []
         self.sl_path_l = []
+        self.sl_aggregated_boundary_low_l = []
+        self.sl_aggregated_boundary_high_l = []
+        self.sl_aggregated_boundary_s = []
+
 
     def update_planning_pb(self, planning_pb):
         self.planning_pb = planning_pb
@@ -60,6 +64,9 @@ class Planning:
         sl_dynamic_obstacle_upper_boundary = []
         sl_path_s = []
         sl_path_l = []
+        sl_aggregated_boundary_low_l = []
+        sl_aggregated_boundary_high_l = []
+        sl_aggregated_boundary_s = []
 
         for sl_frame in self.planning_pb.debug.planning_data.sl_frame:
             for s in sl_frame.sampled_s:
@@ -85,6 +92,12 @@ class Planning:
             for slpoint in sl_frame.sl_path:
                 sl_path_s.append(slpoint.s)
                 sl_path_l.append(slpoint.l)
+            for l in sl_frame.aggregated_boundary_low:
+                sl_aggregated_boundary_low_l.append(l)
+            for l in sl_frame.aggregated_boundary_high:
+                sl_aggregated_boundary_high_l.append(l)
+            for s in sl_frame.aggregated_boundary_s:
+                sl_aggregated_boundary_s.append(s)
 
         self.sl_data_lock.acquire()
         self.sl_sampled_s = sl_sampled_s
@@ -96,6 +109,9 @@ class Planning:
         self.sl_dynamic_obstacle_upper_boundary = sl_dynamic_obstacle_upper_boundary
         self.sl_path_s = sl_path_s
         self.sl_path_l = sl_path_l
+        self.sl_aggregated_boundary_low_l = sl_aggregated_boundary_low_l
+        self.sl_aggregated_boundary_high_l = sl_aggregated_boundary_high_l
+        self.sl_aggregated_boundary_s = sl_aggregated_boundary_s
         self.sl_data_lock.release()
 
     def compute_st_data(self):
@@ -142,7 +158,9 @@ class Planning:
                        sl_dynamic_obstacle_lower_boundary,
                        sl_dynamic_obstacle_upper_boundary,
                        sl_map_lower_boundary,
-                       sl_map_upper_boundary, sl_path):
+                       sl_map_upper_boundary, sl_path,
+                       sl_aggregated_boundary_low_line,
+                       sl_aggregated_boundary_high_line):
         self.sl_data_lock.acquire()
         sl_static_obstacle_lower_boundary.set_visible(True)
         sl_static_obstacle_upper_boundary.set_visible(True)
@@ -151,6 +169,8 @@ class Planning:
         sl_map_lower_boundary.set_visible(True)
         sl_map_upper_boundary.set_visible(True)
         sl_path.set_visible(True)
+        sl_aggregated_boundary_low_line.set_visible(True)
+        sl_aggregated_boundary_high_line.set_visible(True)
 
         new_sampled_s = []
         for s in self.sl_sampled_s:
@@ -188,6 +208,10 @@ class Planning:
         sl_static_obstacle_upper_boundary.set_ydata(new_static_upper)
         sl_path.set_xdata(self.sl_path_s)
         sl_path.set_ydata(self.sl_path_l)
+        sl_aggregated_boundary_low_line.set_xdata(self.sl_aggregated_boundary_s)
+        sl_aggregated_boundary_low_line.set_ydata(self.sl_aggregated_boundary_low_l)
+        sl_aggregated_boundary_high_line.set_xdata(self.sl_aggregated_boundary_s)
+        sl_aggregated_boundary_high_line.set_ydata(self.sl_aggregated_boundary_high_l)
         self.sl_data_lock.release()
 
     def replot_st_data(self, boundaries_pool, st_line,

modules/tools/mapshow/plot_planning.py

@@ -60,6 +60,9 @@ sl_dynamic_obstacle_upper_boundary = None
 sl_map_lower_boundary = None
 sl_map_upper_boundary = None
 sl_path = None
+sl_aggregated_boundary_low_line = None
+sl_aggregated_boundary_high_line = None
+
 
 
 def localization_callback(localization_pb):
@@ -110,6 +113,8 @@ def update(frame_number):
     sl_map_lower_boundary.set_visible(False)
     sl_map_upper_boundary.set_visible(False)
     sl_path.set_visible(False)
+    sl_aggregated_boundary_low_line.set_visible(False)
+    sl_aggregated_boundary_high_line.set_visible(False)
 
     vehicle_position_line.set_visible(False)
     vehicle_polygon_line.set_visible(False)
@@ -121,7 +126,9 @@ def update(frame_number):
                             sl_dynamic_obstacle_lower_boundary,
                             sl_dynamic_obstacle_upper_boundary,
                             sl_map_lower_boundary,
-                            sl_map_upper_boundary, sl_path)
+                            sl_map_upper_boundary, sl_path,
+                            sl_aggregated_boundary_low_line,
+                            sl_aggregated_boundary_high_line)
 
     if len(planning.st_data_s.keys()) >= 1:
         planning.replot_st_data(
@@ -157,6 +164,7 @@ def init_line_pool(central_x, central_y):
     global sl_dynamic_obstacle_upper_boundary
     global sl_map_lower_boundary
     global sl_map_upper_boundary, sl_path
+    global sl_aggregated_boundary_low_line, sl_aggregated_boundary_high_line
 
     colors = ['b', 'g', 'r', 'k']
 
@@ -187,8 +195,11 @@ def init_line_pool(central_x, central_y):
         ax4.plot([0], [0], "b-", lw=0.3, ms=2, alpha=0.8)
     sl_map_upper_boundary, = \
         ax4.plot([0], [0], "b-", lw=0.3, ms=4, alpha=0.8)
-
-    sl_path, = ax4.plot([0], [0], "k-")
+    sl_path, = ax4.plot([0], [0], "k--")
+    sl_aggregated_boundary_low_line, = \
+        ax4.plot([0], [0], "k-", lw=1, ms=2)
+    sl_aggregated_boundary_high_line, = \
+        ax4.plot([0], [0], "k-", lw=1, ms=2)
 
     for i in range(obstacle_line_pool_size):
         line, = ax2.plot([0], [0],