draw rectangle object

src/components/detection/l_shape_fitting/l_shape_fitting_detector.py

@@ -132,3 +132,7 @@ def update(self, point_cloud):
         mearged_clusters_list = self._mearge_clusters(clusters_list)
 
         self._search_rectangles(mearged_clusters_list)
+
+    def draw(self, axes, elems, x_m, y_m, yaw_rad):
+        for rectangle in self.latest_rectangles_list:
+            rectangle.draw(axes, elems, x_m, y_m, yaw_rad)

src/components/detection/l_shape_fitting/rectangle.py

@@ -50,3 +50,10 @@ def _calculate_contour(self):
         self.contour_x[2], self.contour_y[2] = self._calculate_cross_point(self.a[2:4], self.b[2:4], self.c[2:4])
         self.contour_x[3], self.contour_y[3] = self._calculate_cross_point([self.a[3], self.a[0]], [self.b[3], self.b[0]], [self.c[3], self.c[0]])
         self.contour_x[4], self.contour_y[4] = self.contour_x[0], self.contour_y[0]
+
+    def draw(self, axes, elems, x_m, y_m, angle_rad):
+        transformed_contour = self.contour.homogeneous_transformation(x_m, y_m, angle_rad)
+        rectangle_plot, = axes.plot(transformed_contour.get_x_data(),
+                                    transformed_contour.get_y_data(),
+                                    color='g', ls='-')
+        elems.append(rectangle_plot)

src/components/vehicle/four_wheels_vehicle.py

@@ -77,8 +77,27 @@ def _draw_sensors_data(self, axes, elems, state):
         if self.sensors: self.sensors.draw_data(axes, elems, state)
 
     def _update_detection_data(self):
+        """
+        Private function to update each detected object data
+        """
+
         if self.detector: self.detector.update(self.sensors.get_point_cloud_from_lidar())
 
+    def _draw_detection_data(self, axes, elems, state):
+        """
+        Private function to draw each detected object data
+        axes: Axes object of figure
+        elems: List of plot object
+        state: Vehicle's state object
+        """
+
+        if self.detector:
+            self.detector.draw(axes, 
+                               elems,
+                               self.sensors.get_lidar_global_x_m(),
+                               self.sensors.get_lidar_global_y_m(),
+                               state.get_yaw_rad())
+
     def _update_control_data(self):
         """
         Private function to update controller's data
@@ -130,6 +149,8 @@ def draw(self, axes, elems):
 
         self._draw_sensors_data(axes, elems, self.state)
 
+        self._draw_detection_data(axes, elems, self.state)
+
         steer_rad = self._draw_control_data(axes, elems)
 
         self.state.draw(axes, elems)