Merge pull request #376 from JdeRobot/issue-375

Show graphically direction where the robot is heading on displayed image

Author: sergiopaniego

behavior_metrics/brains/f1/brain_f1_keras_opencv_dataset.py

@@ -67,13 +67,23 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
             print("- Models path: " + PRETRAINED_MODELS)
             print("- Model: " + str(model))
 
-    def update_frame(self, frame_id, data):
+    def update_frame(self, frame_id, data, angular_speed=None):
         """Update the information to be shown in one of the GUI's frames.
 
         Arguments:
             frame_id {str} -- Id of the frame that will represent the data
             data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc)
         """
+        if angular_speed:
+            import math
+            x1, y1 = int(data.shape[:2][1] / 2), data.shape[:2][0]  # ancho, alto
+            length = 200
+            angle = (90 + int(math.degrees(-angular_speed))) * 3.14 / 180.0
+            x2 = int(x1 - length * math.cos(angle))
+            y2 = int(y1 - length * math.sin(angle))
+
+            line_thickness = 2
+            cv2.line(data, (x1, y1), (x2, y2), (0, 0, 0), thickness=line_thickness)
         self.handler.update_frame(frame_id, data)
 
     def execute(self):
@@ -83,12 +93,13 @@ def execute(self):
 
         image = self.camera.getImage().data
         # image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
+        base_image = image
 
         if self.cont == 1:
             self.first_image = image
 
         image = self.handler.transform_image(image, self.config['ImageTranform'])
-        self.update_frame('frame_0', image)
+        #self.update_frame('frame_0', image)
 
         try:
             if self.config['ImageCropped']:
@@ -129,6 +140,8 @@ def execute(self):
             self.previous_v = prediction[0][0]
             self.previous_w = prediction[0][1]
 
+            self.update_frame('frame_0', base_image, prediction_w)
+
             # GradCAM from image
             i = np.argmax(prediction[0])
             cam = GradCAM(self.net, i)
@@ -138,4 +151,4 @@ def execute(self):
             self.update_frame('frame_1', output)
 
         except Exception as err:
-            print(err)
+            print(err)

behavior_metrics/brains/f1/brain_f1_keras_seq_3_opencv_dataset.py

@@ -57,7 +57,7 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
         self.third_image = []
 
         if self.config['GPU'] is False:
-            os.environ["CUDA_VISIBLE_DEVICES"]="-1"
+            os.environ["CUDA_VISIBLE_DEVICES"] = "-1"
 
         self.gpu_inference = True if tf.test.gpu_device_name() else False
 
@@ -72,13 +72,23 @@ def __init__(self, sensors, actuators, model=None, handler=None, config=None):
             print("- Models path: " + PRETRAINED_MODELS)
             print("- Model: " + str(model))
 
-    def update_frame(self, frame_id, data):
+    def update_frame(self, frame_id, data, angular_speed=None):
         """Update the information to be shown in one of the GUI's frames.
 
         Arguments:
             frame_id {str} -- Id of the frame that will represent the data
             data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc)
         """
+        if angular_speed:
+            import math
+            x1, y1 = int(data.shape[:2][1] / 2), data.shape[:2][0]  # ancho, alto
+            length = 200
+            angle = (90 + int(math.degrees(-angular_speed))) * 3.14 / 180.0
+            x2 = int(x1 - length * math.cos(angle))
+            y2 = int(y1 - length * math.sin(angle))
+
+            line_thickness = 2
+            cv2.line(data, (x1, y1), (x2, y2), (0, 0, 0), thickness=line_thickness)
         self.handler.update_frame(frame_id, data)
 
     def check_center(self, position_x):
@@ -115,17 +125,18 @@ def execute(self):
         '''
 
         image = self.camera.getImage().data
+        base_image = image
         if self.cont == 1:
             self.first_image = image
-        image = self.handler.transform_image(image,self.config['ImageTranform'])
+        image = self.handler.transform_image(image, self.config['ImageTranform'])
         try:
             if self.config['ImageCropped']:
                 image = image[240:480, 0:640]
             if 'ImageSize' in self.config:
                 img = cv2.resize(image, (self.config['ImageSize'][0], self.config['ImageSize'][1]))
             else:
                 img = image
-            self.update_frame('frame_0', img)
+            # sself.update_frame('frame_0', img)
             if self.config['ImageNormalized']:
                 AUGMENTATIONS_TEST = Compose([
                     Normalize()
@@ -170,21 +181,22 @@ def execute(self):
                 start_time = time.time()
                 prediction = self.net.predict(img)
                 self.inference_times.append(time.time() - start_time)
-                #prediction = prediction[0]
                 if self.config['PredictionsNormalized']:
-                    prediction_v = prediction[0][0]*(24 - (6.5)) + (6.5)
-                    prediction_w = prediction[0][1]*(7.1 - (-7.1)) + (-7.1)
+                    prediction_v = prediction[0][0] * (24 - (6.5)) + (6.5)
+                    prediction_w = prediction[0][1] * (7.1 - (-7.1)) + (-7.1)
                 else:
                     prediction_v = prediction[0][0]
                     prediction_w = prediction[0][1]
                 if prediction_w != '' and prediction_w != '':
                     self.motors.sendV(prediction_v)
                     self.motors.sendW(prediction_w)
 
+                self.update_frame('frame_0', base_image, prediction_w)
+
                 if self.previous_v != None:
                     a = np.array((prediction[0][0], prediction[0][1]))
                     b = np.array((self.previous_v, self.previous_w))
-                    distance = np.linalg.norm(a-b)
+                    distance = np.linalg.norm(a - b)
                     self.suddenness_distance.append(distance)
                 self.previous_v = prediction[0][0]
                 self.previous_w = prediction[0][1]

behavior_metrics/brains/f1/brain_f1_opencv.py

@@ -50,8 +50,8 @@ def __init__(self, sensors, actuators, handler, config=None):
         self.cont = 0
         self.iteration = 0
 
-        #self.previous_timestamp = 0
-        #self.previous_image = 0
+        # self.previous_timestamp = 0
+        # self.previous_image = 0
 
         self.previous_v = None
         self.previous_w = None
@@ -66,7 +66,23 @@ def __init__(self, sensors, actuators, handler, config=None):
         '''
         time.sleep(2)
 
-    def update_frame(self, frame_id, data):
+    def update_frame(self, frame_id, data, angular_speed=None):
+        """Update the information to be shown in one of the GUI's frames.
+
+        Arguments:
+            frame_id {str} -- Id of the frame that will represent the data
+            data {*} -- Data to be shown in the frame. Depending on the type of frame (rgbimage, laser, pose3d, etc)
+        """
+        if angular_speed:
+            import math
+            x1, y1 = int(data.shape[:2][1] / 2), data.shape[:2][0]  # ancho, alto
+            length = 200
+            angle = (90 + int(math.degrees(-angular_speed))) * 3.14 / 180.0
+            x2 = int(x1 - length * math.cos(angle))
+            y2 = int(y1 - length * math.sin(angle))
+
+            line_thickness = 2
+            cv2.line(data, (x1, y1), (x2, y2), (0, 0, 0), thickness=line_thickness)
         self.handler.update_frame(frame_id, data)
 
     def collinear3(self, x1, y1, x2, y2, x3, y3):
@@ -115,8 +131,8 @@ def execute(self):
             self.previous_timestamp = timestamp
         if (timestamp - self.previous_timestamp >= 0.085):
             self.previous_image = self.camera.getImage().data
+        image = self.previous_image
         '''
-        #image = self.previous_image
 
         image = self.camera.getImage().data
         if image.shape == (3, 3, 3):
@@ -144,9 +160,6 @@ def execute(self):
         image_mask = cv2.inRange(image_hsv, red_lower, red_upper)
         # image_eroded = cv2.erode(image_mask, kernel, iterations=3)
 
-        # show image in gui -> frame_0
-        self.update_frame('frame_0', image)
-
         rows, cols = image_mask.shape
         rows = rows - 1  # para evitar desbordamiento
 
@@ -202,6 +215,8 @@ def execute(self):
         self.motors.sendW(w)
         self.motors.sendV(v)
 
+        self.update_frame('frame_0', image, w)
+
         v = np.interp(np.array([v]), (6.5, 24), (0, 1))[0]
         w = np.interp(np.array([w]), (-7.1, 7.1), (0, 1))[0]
         if self.previous_v != None: