Merge pull request #273 from JdeRobot/issue-272

Edge case for lap percentage

Author: sergiopaniego

behavior_metrics/pilot.py

@@ -109,7 +109,6 @@ def initialize_robot(self):
 
     def stop_interfaces(self):
         """Function that kill the current interfaces of the robot. For reloading purposes."""
-
         if self.sensors:
             self.sensors.kill()
         if self.actuators:
@@ -228,7 +227,6 @@ def finish_line(self):
         dist = (self.start_pose - current_point) ** 2
         dist = np.sum(dist, axis=0)
         dist = np.sqrt(dist)
-        # print(dist)
         if dist < self.max_distance:
             return True
         return False

behavior_metrics/utils/controller.py

@@ -91,7 +91,6 @@ def get_data(self, frame_id):
         try:
             with self.__data_loc:
                 data = self.data.get(frame_id, None)
-                # self.data[frame_id] = None
         except Exception:
             pass
 
@@ -184,10 +183,11 @@ def record_stats(self, perfect_lap_filename, stats_record_dir_path, world_counte
             current_brain_head, current_brain_tail = os.path.split(self.pilot.configuration.brain_path[brain_counter])
         else:
             current_brain_head, current_brain_tail = os.path.split(self.pilot.configuration.brain_path)
-        self.metrics = {}
-        self.metrics['world'] = current_world_tail
-        self.metrics['brain_path'] = current_brain_tail
-        self.metrics['robot_type'] = self.pilot.configuration.robot_type
+        self.metrics = {
+            'world': current_world_tail,
+            'brain_path': current_brain_tail,
+            'robot_type': self.pilot.configuration.robot_type
+        }
         if hasattr(self.pilot.configuration, 'experiment_model'):
             if brain_counter is not None:
                 self.metrics['experiment_model'] = self.pilot.configuration.experiment_model[brain_counter]
@@ -224,16 +224,14 @@ def stop_record_stats(self, pitch_error=False):
         while os.path.isfile(self.stats_filename + '.active'):
             pass
 
-        checkpoints = []
         metrics_str = json.dumps(self.metrics)
         with rosbag.Bag(self.stats_filename, 'a') as bag:
             metadata_msg = String(data=metrics_str)
             bag.write('/metadata', metadata_msg, rospy.Time(bag.get_end_time()))
         bag.close()
         perfect_lap_checkpoints, circuit_diameter = metrics.read_perfect_lap_rosbag(self.perfect_lap_filename)
         if not pitch_error:
-            self.lap_statistics = metrics.lap_percentage_completed(self.stats_filename, perfect_lap_checkpoints,
-                                                                   circuit_diameter)
+            self.lap_statistics = metrics.get_statistics(self.stats_filename, perfect_lap_checkpoints, circuit_diameter)
         else:
             self.lap_statistics = {'percentage_completed': 0, 'average_speed': 0, 'lap_seconds': 0,
                                    'circuit_diameter': 0, 'position_deviation_mae': 0,

behavior_metrics/utils/metrics.py

@@ -91,7 +91,7 @@ def read_perfect_lap_rosbag(ground_truth_lap_file):
     return perfect_lap_checkpoints, circuit_diameter
 
 
-def lap_percentage_completed(stats_filename, perfect_lap_checkpoints, circuit_diameter):
+def get_statistics(stats_filename, perfect_lap_checkpoints, circuit_diameter):
     lap_statistics = {}
     bag_reader = bagreader(stats_filename)
     csv_files = []
@@ -104,20 +104,17 @@ def lap_percentage_completed(stats_filename, perfect_lap_checkpoints, circuit_di
     checkpoints = []
     for index, row in dataframe_pose.iterrows():
         checkpoints.append(row)
+
     data_file = stats_filename.split('.bag')[0] + '/clock.csv'
     dataframe_pose = pd.read_csv(data_file)
     clock_points = []
     for index, row in dataframe_pose.iterrows():
         clock_points.append(row)
-
-    end_point = checkpoints[len(checkpoints) - 1]
     start_clock = clock_points[0]
-    lap_statistics['completed_distance'] = circuit_distance_completed(checkpoints, end_point)
     lap_point = 0
     start_point = checkpoints[0]
     previous_lap_point = 0
     laps = 0
-    ckp_iter = 0
     for ckp_iter, point in enumerate(checkpoints):
         if ckp_iter != 0 and point['header.stamp.secs'] - 10 > start_point['header.stamp.secs'] \
                 and is_finish_line(point, start_point):
@@ -128,8 +125,28 @@ def lap_percentage_completed(stats_filename, perfect_lap_checkpoints, circuit_di
             previous_lap_point = ckp_iter
     seconds_start = start_clock['clock.secs']
     seconds_end = clock_points[len(clock_points) - 1]['clock.secs']
+
+    lap_statistics = get_distance_completed(lap_statistics, checkpoints)
+    lap_statistics = get_average_speed(lap_statistics, seconds_start, seconds_end)
+    lap_statistics = get_percentage_completed(lap_statistics, checkpoints, perfect_lap_checkpoints, seconds_start, seconds_end, laps)
+    lap_statistics = get_lap_completed_stats(lap_statistics, circuit_diameter, previous_lap_point, lap_point)
+
+    shutil.rmtree(stats_filename.split('.bag')[0])
+    return lap_statistics
+
+
+def get_distance_completed(lap_statistics, checkpoints):
+    end_point = checkpoints[len(checkpoints) - 1]
+    lap_statistics['completed_distance'] = circuit_distance_completed(checkpoints, end_point)
+    return lap_statistics
+
+
+def get_average_speed(lap_statistics, seconds_start, seconds_end):
     lap_statistics['average_speed'] = lap_statistics['completed_distance'] / (seconds_end - seconds_start)
+    return lap_statistics
 
+
+def get_percentage_completed(lap_statistics, checkpoints, perfect_lap_checkpoints, seconds_start, seconds_end, laps):
     # Find last and first checkpoints for retrieving percentage completed
     first_checkpoint = checkpoints[0]
     first_checkpoint = np.array([first_checkpoint['pose.pose.position.x'], first_checkpoint['pose.pose.position.y']])
@@ -155,26 +172,15 @@ def lap_percentage_completed(stats_filename, perfect_lap_checkpoints, circuit_di
             if dist < min_distance_last:
                 min_distance_last = dist
                 last_perfect_checkpoint_position = i
-
     if first_perfect_checkpoint_position > last_perfect_checkpoint_position and lap_statistics['completed_distance'] > MIN_COMPLETED_DISTANCE_EXPERIMENT and seconds_end - seconds_start > MIN_EXPERIMENT_TIME:
         lap_statistics['percentage_completed'] = (((len(perfect_lap_checkpoints) - first_perfect_checkpoint_position + last_perfect_checkpoint_position) / len(perfect_lap_checkpoints)) * 100) + laps * 100
     else:
         if seconds_end - seconds_start > MIN_EXPERIMENT_TIME:
-            lap_statistics['percentage_completed'] = (((last_perfect_checkpoint_position - first_perfect_checkpoint_position) / len(perfect_lap_checkpoints)) * 100) + laps * 100
+            lap_statistics['percentage_completed'] = (((last_perfect_checkpoint_position - first_perfect_checkpoint_position) / len(
+                perfect_lap_checkpoints)) * 100) + laps * 100
         else:
             lap_statistics['percentage_completed'] = (((last_perfect_checkpoint_position - first_perfect_checkpoint_position) / len(perfect_lap_checkpoints)) * 100)
     lap_statistics = get_robot_position_deviation_score(perfect_lap_checkpoints, checkpoints, lap_statistics)
-
-    # If lap is completed, add more statistic information
-    if type(lap_point) is not int and lap_statistics['percentage_completed'] > LAP_COMPLETED_PERCENTAGE:
-        seconds_start = start_clock['clock.secs']
-        seconds_end = clock_points[int(len(clock_points) * (ckp_iter / len(checkpoints)))]['clock.secs']
-        lap_statistics['lap_seconds'] = seconds_end - seconds_start
-        lap_statistics['circuit_diameter'] = circuit_diameter
-    else:
-        logger.info('Lap not completed')
-
-    shutil.rmtree(stats_filename.split('.bag')[0])
     return lap_statistics
 
 
@@ -258,4 +264,21 @@ def get_robot_position_deviation_score(perfect_lap_checkpoints, checkpoints, lap
     lap_statistics['position_deviation_mae'] = sum(min_dists) / len(min_dists)
     lap_statistics['position_deviation_total_err'] = sum(min_dists)
 
-    return lap_statistics
+    return lap_statistics
+
+
+def get_lap_completed_stats(lap_statistics, circuit_diameter, previous_lap_point, lap_point):
+    # If lap is completed, add more statistic information
+    if type(lap_point) is not int and lap_statistics['percentage_completed'] > LAP_COMPLETED_PERCENTAGE:
+        if abs(((lap_statistics['completed_distance'] / circuit_diameter) * 100) - lap_statistics['percentage_completed']) > 5:
+            logger.info('Error in experiment! The actual lap percentage and the approximated one are different.')
+            lap_statistics['percentage_completed'] = (lap_statistics['completed_distance'] / circuit_diameter) * 100
+        else:
+            seconds_start = start_clock['clock.secs']
+            seconds_end = clock_points[int(len(clock_points) * (previous_lap_point / len(checkpoints)))]['clock.secs']
+            lap_statistics['lap_seconds'] = seconds_end - seconds_start
+            lap_statistics['circuit_diameter'] = circuit_diameter
+    else:
+        logger.info('Lap not completed')
+
+    return lap_statistics