diff --git a/behavior_metrics/pilot.py b/behavior_metrics/pilot.py index 8c128882..7f8e591b 100644 --- a/behavior_metrics/pilot.py +++ b/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 diff --git a/behavior_metrics/utils/controller.py b/behavior_metrics/utils/controller.py index 8844db72..d437f33d 100644 --- a/behavior_metrics/utils/controller.py +++ b/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,7 +224,6 @@ 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) @@ -232,8 +231,7 @@ def stop_record_stats(self, pitch_error=False): 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, diff --git a/behavior_metrics/utils/metrics.py b/behavior_metrics/utils/metrics.py index c90ba26f..f867b1ad 100644 --- a/behavior_metrics/utils/metrics.py +++ b/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 \ No newline at end of file + 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