From 420a8eff1ee4e77ea439c773683e59c7ff72dd16 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Fri, 24 Dec 2021 16:30:03 +0100 Subject: [PATCH 1/4] [WIP] Check how many perfect points has the expermient reached --- behavior_metrics/pilot.py | 2 - behavior_metrics/utils/controller.py | 11 +++--- behavior_metrics/utils/metrics.py | 55 +++++++++++++++++++++++++++- 3 files changed, 59 insertions(+), 9 deletions(-) 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..19449a82 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) diff --git a/behavior_metrics/utils/metrics.py b/behavior_metrics/utils/metrics.py index c90ba26f..92794352 100644 --- a/behavior_metrics/utils/metrics.py +++ b/behavior_metrics/utils/metrics.py @@ -156,6 +156,59 @@ def lap_percentage_completed(stats_filename, perfect_lap_checkpoints, circuit_di min_distance_last = dist last_perfect_checkpoint_position = i + previous_perfect_correspondence = -1 + min_distance = 100 + points_completed = 0 + for i, experiment_point in enumerate(checkpoints): + current_point = np.array([experiment_point['pose.pose.position.x'], experiment_point['pose.pose.position.y']]) + if previous_perfect_correspondence == -1: + for w, perfect_point in enumerate(perfect_lap_checkpoints): + experiment_perfect_point = np.array( + [perfect_point['pose.pose.position.x'], perfect_point['pose.pose.position.y']]) + dist = (current_point - experiment_perfect_point) ** 2 + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + if dist < min_distance_last: + min_distance = dist + previous_perfect_correspondence = w + print(previous_perfect_correspondence) + print(previous_perfect_correspondence) + print(previous_perfect_correspondence) + points_completed += 1 + else: + new_perfect_correspondance = previous_perfect_correspondence + 1 + previous_perfect_correspondence += 1 + found_perfect_correspondance = False + # Find following point + if new_perfect_correspondance >= len(perfect_lap_checkpoints): + new_perfect_correspondance = 0 + previous_perfect_correspondence = 0 + perfect_correspondance_to_find = np.array( + [perfect_lap_checkpoints[new_perfect_correspondance]['pose.pose.position.x'], + perfect_lap_checkpoints[new_perfect_correspondance]['pose.pose.position.y']]) + for i, experiment_point in enumerate(checkpoints): + current_point = np.array( + [experiment_point['pose.pose.position.x'], experiment_point['pose.pose.position.y']]) + dist = (current_point - perfect_correspondance_to_find) ** 2 + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + # print(dist) + if dist < 1: + found_perfect_correspondance = True + break + if not found_perfect_correspondance: + break + else: + points_completed += 1 + print('POINTS COMPLETED:') + print(points_completed) + print(points_completed) + print(points_completed) + print(points_completed) + print(len(perfect_lap_checkpoints)) + print(points_completed / len(perfect_lap_checkpoints)) + print('POINTS COMPLETED**************') + 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: @@ -258,4 +311,4 @@ 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 From 318bb5118dc04aaf7621baf8dc20ce4e35b781c7 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Sun, 26 Dec 2021 22:15:53 +0100 Subject: [PATCH 2/4] New condition for bags with edge cases for lap percentage --- behavior_metrics/utils/metrics.py | 65 ++++--------------------------- 1 file changed, 8 insertions(+), 57 deletions(-) diff --git a/behavior_metrics/utils/metrics.py b/behavior_metrics/utils/metrics.py index 92794352..c9d90a8c 100644 --- a/behavior_metrics/utils/metrics.py +++ b/behavior_metrics/utils/metrics.py @@ -156,59 +156,6 @@ def lap_percentage_completed(stats_filename, perfect_lap_checkpoints, circuit_di min_distance_last = dist last_perfect_checkpoint_position = i - previous_perfect_correspondence = -1 - min_distance = 100 - points_completed = 0 - for i, experiment_point in enumerate(checkpoints): - current_point = np.array([experiment_point['pose.pose.position.x'], experiment_point['pose.pose.position.y']]) - if previous_perfect_correspondence == -1: - for w, perfect_point in enumerate(perfect_lap_checkpoints): - experiment_perfect_point = np.array( - [perfect_point['pose.pose.position.x'], perfect_point['pose.pose.position.y']]) - dist = (current_point - experiment_perfect_point) ** 2 - dist = np.sum(dist, axis=0) - dist = np.sqrt(dist) - if dist < min_distance_last: - min_distance = dist - previous_perfect_correspondence = w - print(previous_perfect_correspondence) - print(previous_perfect_correspondence) - print(previous_perfect_correspondence) - points_completed += 1 - else: - new_perfect_correspondance = previous_perfect_correspondence + 1 - previous_perfect_correspondence += 1 - found_perfect_correspondance = False - # Find following point - if new_perfect_correspondance >= len(perfect_lap_checkpoints): - new_perfect_correspondance = 0 - previous_perfect_correspondence = 0 - perfect_correspondance_to_find = np.array( - [perfect_lap_checkpoints[new_perfect_correspondance]['pose.pose.position.x'], - perfect_lap_checkpoints[new_perfect_correspondance]['pose.pose.position.y']]) - for i, experiment_point in enumerate(checkpoints): - current_point = np.array( - [experiment_point['pose.pose.position.x'], experiment_point['pose.pose.position.y']]) - dist = (current_point - perfect_correspondance_to_find) ** 2 - dist = np.sum(dist, axis=0) - dist = np.sqrt(dist) - # print(dist) - if dist < 1: - found_perfect_correspondance = True - break - if not found_perfect_correspondance: - break - else: - points_completed += 1 - print('POINTS COMPLETED:') - print(points_completed) - print(points_completed) - print(points_completed) - print(points_completed) - print(len(perfect_lap_checkpoints)) - print(points_completed / len(perfect_lap_checkpoints)) - print('POINTS COMPLETED**************') - 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: @@ -220,10 +167,14 @@ def lap_percentage_completed(stats_filename, perfect_lap_checkpoints, circuit_di # 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 + 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) * (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') From 75d2d8232c072028cb6ea7cfccc491b27432c616 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 27 Dec 2021 16:46:06 +0100 Subject: [PATCH 3/4] Metrics reformated --- behavior_metrics/utils/controller.py | 3 +- behavior_metrics/utils/metrics.py | 61 +++++++++++++++++++--------- 2 files changed, 42 insertions(+), 22 deletions(-) diff --git a/behavior_metrics/utils/controller.py b/behavior_metrics/utils/controller.py index 19449a82..d437f33d 100644 --- a/behavior_metrics/utils/controller.py +++ b/behavior_metrics/utils/controller.py @@ -231,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 c9d90a8c..918bed21 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,36 @@ def lap_percentage_completed(stats_filename, perfect_lap_checkpoints, circuit_di checkpoints = [] for index, row in dataframe_pose.iterrows(): checkpoints.append(row) + + # average_speed + lap_statistics = get_distance_completed(lap_statistics, checkpoints) + lap_statistics, previous_lap_point = get_average_speed(lap_statistics, stats_filename, checkpoints) + # percentage_completed + lap_statistics = get_percentage_completed(lap_statistics, checkpoints, perfect_lap_checkpoints) + # additional stats + lap_statistics = get_lap_completed_stats(lap_statistics, circuit_diameter, previous_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, stats_filename, checkpoints): 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): @@ -129,7 +145,10 @@ def lap_percentage_completed(stats_filename, perfect_lap_checkpoints, circuit_di seconds_start = start_clock['clock.secs'] seconds_end = clock_points[len(clock_points) - 1]['clock.secs'] lap_statistics['average_speed'] = lap_statistics['completed_distance'] / (seconds_end - seconds_start) + return lap_statistics, previous_lap_point + +def get_percentage_completed(lap_statistics, checkpoints, perfect_lap_checkpoints): # 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']]) @@ -164,21 +183,6 @@ def lap_percentage_completed(stats_filename, perfect_lap_checkpoints, circuit_di 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: - 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) * (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 @@ -263,3 +267,20 @@ def get_robot_position_deviation_score(perfect_lap_checkpoints, checkpoints, lap lap_statistics['position_deviation_total_err'] = sum(min_dists) return lap_statistics + + +def get_lap_completed_stats(lap_statistics, circuit_diameter, previous_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 \ No newline at end of file From 5b077084a96f698d5d39c3a011aa375834dd1612 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Mon, 27 Dec 2021 17:32:12 +0100 Subject: [PATCH 4/4] Reformat metrics file --- behavior_metrics/utils/metrics.py | 48 +++++++++++++++---------------- 1 file changed, 23 insertions(+), 25 deletions(-) diff --git a/behavior_metrics/utils/metrics.py b/behavior_metrics/utils/metrics.py index 918bed21..f867b1ad 100644 --- a/behavior_metrics/utils/metrics.py +++ b/behavior_metrics/utils/metrics.py @@ -105,25 +105,6 @@ def get_statistics(stats_filename, perfect_lap_checkpoints, circuit_diameter): for index, row in dataframe_pose.iterrows(): checkpoints.append(row) - # average_speed - lap_statistics = get_distance_completed(lap_statistics, checkpoints) - lap_statistics, previous_lap_point = get_average_speed(lap_statistics, stats_filename, checkpoints) - # percentage_completed - lap_statistics = get_percentage_completed(lap_statistics, checkpoints, perfect_lap_checkpoints) - # additional stats - lap_statistics = get_lap_completed_stats(lap_statistics, circuit_diameter, previous_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, stats_filename, checkpoints): data_file = stats_filename.split('.bag')[0] + '/clock.csv' dataframe_pose = pd.read_csv(data_file) clock_points = [] @@ -144,11 +125,28 @@ def get_average_speed(lap_statistics, stats_filename, checkpoints): 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, previous_lap_point + return lap_statistics -def get_percentage_completed(lap_statistics, checkpoints, perfect_lap_checkpoints): +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']]) @@ -174,12 +172,12 @@ def get_percentage_completed(lap_statistics, checkpoints, perfect_lap_checkpoint 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) @@ -269,7 +267,7 @@ def get_robot_position_deviation_score(perfect_lap_checkpoints, checkpoints, lap return lap_statistics -def get_lap_completed_stats(lap_statistics, circuit_diameter, previous_lap_point): +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: @@ -283,4 +281,4 @@ def get_lap_completed_stats(lap_statistics, circuit_diameter, previous_lap_point else: logger.info('Lap not completed') - return lap_statistics \ No newline at end of file + return lap_statistics