From 1cbf2aabcc16605572f2e3a44004701d577277cc Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 9 Feb 2022 11:15:49 +0100 Subject: [PATCH 1/6] New lap percentage metric calculation --- behavior_metrics/utils/metrics.py | 81 ++++++++++++++----------------- 1 file changed, 37 insertions(+), 44 deletions(-) diff --git a/behavior_metrics/utils/metrics.py b/behavior_metrics/utils/metrics.py index 6973eca7..ad0192c1 100644 --- a/behavior_metrics/utils/metrics.py +++ b/behavior_metrics/utils/metrics.py @@ -42,7 +42,7 @@ def is_finish_line(point, start_point): start_point = np.array([start_point['pose.pose.position.x'], start_point['pose.pose.position.y']]) except IndexError: start_point = start_point - #start_point = np.array([start_point['pose.pose.position.x'], start_point['pose.pose.position.y']]) + # start_point = np.array([start_point['pose.pose.position.x'], start_point['pose.pose.position.y']]) dist = (start_point - current_point) ** 2 dist = np.sum(dist, axis=0) dist = np.sqrt(dist) @@ -136,7 +136,8 @@ def get_metrics(stats_filename, perfect_lap_checkpoints, circuit_diameter): experiment_metrics = get_average_speed(experiment_metrics, seconds_start, seconds_end) experiment_metrics = get_percentage_completed(experiment_metrics, checkpoints, perfect_lap_checkpoints, seconds_start, seconds_end, laps) - experiment_metrics = get_lap_completed_stats(experiment_metrics, circuit_diameter, first_previous_lap_point, lap_point, + experiment_metrics = get_lap_completed_stats(experiment_metrics, circuit_diameter, first_previous_lap_point, + lap_point, start_clock, clock_points, checkpoints) experiment_metrics['experiment_total_simulated_time'] = seconds_end - seconds_start logger.info('* Experiment total simulated time ---> ' + str(experiment_metrics['experiment_total_simulated_time'])) @@ -158,48 +159,39 @@ def get_average_speed(experiment_metrics, seconds_start, seconds_end): def get_percentage_completed(experiment_metrics, 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']]) - last_checkpoint = checkpoints[len(checkpoints) - 1] - last_checkpoint = np.array([last_checkpoint['pose.pose.position.x'], last_checkpoint['pose.pose.position.y']]) - min_distance_first = 100 - min_distance_last = 100 - first_perfect_checkpoint_position = 0 - last_perfect_checkpoint_position = 0 - for i, point in enumerate(perfect_lap_checkpoints): - current_point = np.array([point['pose.pose.position.x'], point['pose.pose.position.y']]) - if i != 0: - dist = (first_checkpoint - current_point) ** 2 - dist = np.sum(dist, axis=0) - dist = np.sqrt(dist) - if dist < min_distance_first: - min_distance_first = dist - first_perfect_checkpoint_position = i - - dist = (last_checkpoint - current_point) ** 2 - dist = np.sum(dist, axis=0) - dist = np.sqrt(dist) - if dist < min_distance_last: - min_distance_last = dist - last_perfect_checkpoint_position = i - if first_perfect_checkpoint_position > last_perfect_checkpoint_position and experiment_metrics['completed_distance'] \ - > MIN_COMPLETED_DISTANCE_EXPERIMENT and seconds_end - seconds_start > MIN_EXPERIMENT_TIME: - experiment_metrics['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: - # if the percectage is very close to 100%, remove lap. - if abs((((last_perfect_checkpoint_position - first_perfect_checkpoint_position) / len(perfect_lap_checkpoints)) * 100) - 100) < 1 and laps >= 1: - laps -= 1 - experiment_metrics['percentage_completed'] = \ - (((last_perfect_checkpoint_position - first_perfect_checkpoint_position) / len(perfect_lap_checkpoints)) - * 100) + laps * 100 + first_checkpoint = np.array([checkpoints[0]['pose.pose.position.x'], checkpoints[0]['pose.pose.position.y']]) + perfect_point_iter = 0 + min_dist = 100 + for position, perfect_checkpoint in enumerate(perfect_lap_checkpoints): + perfect_checkpoint = np.array( + [perfect_checkpoint['pose.pose.position.x'], perfect_checkpoint['pose.pose.position.y']]) + dist = (perfect_checkpoint - first_checkpoint) ** 2 + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + if dist < min_dist: + min_dist = dist + perfect_point_iter = position + + checkpoints_reached = 1 + chck_iter = 1 + perfect_point_iter += 1 + while chck_iter < len(checkpoints): + current_checkpoint = np.array( + [checkpoints[chck_iter]['pose.pose.position.x'], checkpoints[chck_iter]['pose.pose.position.y']]) + perfect_checkpoint = np.array([perfect_lap_checkpoints[perfect_point_iter]['pose.pose.position.x'], + perfect_lap_checkpoints[perfect_point_iter]['pose.pose.position.y']]) + dist = (perfect_checkpoint - current_checkpoint) ** 2 + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + if dist < 2: + checkpoints_reached += 1 + perfect_point_iter += 1 + if perfect_point_iter >= len(perfect_lap_checkpoints): + perfect_point_iter = 0 else: - experiment_metrics['percentage_completed'] = \ - (((last_perfect_checkpoint_position - first_perfect_checkpoint_position) / len(perfect_lap_checkpoints)) - * 100) + chck_iter += 1 + + experiment_metrics['percentage_completed'] = (checkpoints_reached / len(perfect_lap_checkpoints)) * 100 logger.info('* Percentage completed ---> ' + str(experiment_metrics['percentage_completed'])) experiment_metrics = get_robot_position_deviation_score(perfect_lap_checkpoints, checkpoints, experiment_metrics) return experiment_metrics @@ -289,7 +281,8 @@ def get_robot_position_deviation_score(perfect_lap_checkpoints, checkpoints, exp return experiment_metrics -def get_lap_completed_stats(experiment_metrics, circuit_diameter, first_previous_lap_point, lap_point, start_clock, clock_points, checkpoints): +def get_lap_completed_stats(experiment_metrics, circuit_diameter, first_previous_lap_point, lap_point, start_clock, + clock_points, checkpoints): # If lap is completed, add more statistic information if type(lap_point) is not int and experiment_metrics['percentage_completed'] > LAP_COMPLETED_PERCENTAGE: seconds_start = start_clock['clock.secs'] From fbbf081d77289ebbe6954fa059b54d2e96fde334 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 9 Feb 2022 12:59:18 +0100 Subject: [PATCH 2/6] Updated lap percentage metric calculation --- behavior_metrics/utils/metrics.py | 39 +++++++++++-------------------- 1 file changed, 13 insertions(+), 26 deletions(-) diff --git a/behavior_metrics/utils/metrics.py b/behavior_metrics/utils/metrics.py index ad0192c1..bc568d88 100644 --- a/behavior_metrics/utils/metrics.py +++ b/behavior_metrics/utils/metrics.py @@ -114,30 +114,14 @@ def get_metrics(stats_filename, perfect_lap_checkpoints, circuit_diameter): for index, row in dataframe_pose.iterrows(): clock_points.append(row) start_clock = clock_points[0] - lap_point = 0 - start_point = checkpoints[0] - previous_lap_point = 0 - first_previous_lap_point = 0 - laps = 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): - if type(lap_point) == int: - lap_point = point - if abs(ckp_iter - previous_lap_point) > 20: - laps += 1 - previous_lap_point = ckp_iter - if first_previous_lap_point == 0: - first_previous_lap_point = ckp_iter seconds_start = start_clock['clock.secs'] seconds_end = clock_points[len(clock_points) - 1]['clock.secs'] experiment_metrics = get_distance_completed(experiment_metrics, checkpoints) experiment_metrics = get_average_speed(experiment_metrics, seconds_start, seconds_end) - experiment_metrics = get_percentage_completed(experiment_metrics, checkpoints, perfect_lap_checkpoints, - seconds_start, seconds_end, laps) - experiment_metrics = get_lap_completed_stats(experiment_metrics, circuit_diameter, first_previous_lap_point, - lap_point, + experiment_metrics, lap_checkpoint = get_percentage_completed(experiment_metrics, checkpoints, + perfect_lap_checkpoints) + experiment_metrics = get_lap_completed_stats(experiment_metrics, circuit_diameter, lap_checkpoint, start_clock, clock_points, checkpoints) experiment_metrics['experiment_total_simulated_time'] = seconds_end - seconds_start logger.info('* Experiment total simulated time ---> ' + str(experiment_metrics['experiment_total_simulated_time'])) @@ -158,7 +142,7 @@ def get_average_speed(experiment_metrics, seconds_start, seconds_end): return experiment_metrics -def get_percentage_completed(experiment_metrics, checkpoints, perfect_lap_checkpoints, seconds_start, seconds_end, laps): +def get_percentage_completed(experiment_metrics, checkpoints, perfect_lap_checkpoints): first_checkpoint = np.array([checkpoints[0]['pose.pose.position.x'], checkpoints[0]['pose.pose.position.y']]) perfect_point_iter = 0 min_dist = 100 @@ -175,6 +159,7 @@ def get_percentage_completed(experiment_metrics, checkpoints, perfect_lap_checkp checkpoints_reached = 1 chck_iter = 1 perfect_point_iter += 1 + lap_checkpoint = 0 while chck_iter < len(checkpoints): current_checkpoint = np.array( [checkpoints[chck_iter]['pose.pose.position.x'], checkpoints[chck_iter]['pose.pose.position.y']]) @@ -183,18 +168,20 @@ def get_percentage_completed(experiment_metrics, checkpoints, perfect_lap_checkp dist = (perfect_checkpoint - current_checkpoint) ** 2 dist = np.sum(dist, axis=0) dist = np.sqrt(dist) - if dist < 2: + if dist < 5: checkpoints_reached += 1 perfect_point_iter += 1 + if checkpoints_reached / len(perfect_lap_checkpoints) == 1: + lap_checkpoint = chck_iter if perfect_point_iter >= len(perfect_lap_checkpoints): perfect_point_iter = 0 else: chck_iter += 1 - experiment_metrics['percentage_completed'] = (checkpoints_reached / len(perfect_lap_checkpoints)) * 100 + experiment_metrics['percentage_completed'] = checkpoints_reached / len(perfect_lap_checkpoints) * 100 logger.info('* Percentage completed ---> ' + str(experiment_metrics['percentage_completed'])) experiment_metrics = get_robot_position_deviation_score(perfect_lap_checkpoints, checkpoints, experiment_metrics) - return experiment_metrics + return experiment_metrics, lap_checkpoint def get_robot_position_deviation_score(perfect_lap_checkpoints, checkpoints, experiment_metrics): @@ -281,12 +268,12 @@ def get_robot_position_deviation_score(perfect_lap_checkpoints, checkpoints, exp return experiment_metrics -def get_lap_completed_stats(experiment_metrics, circuit_diameter, first_previous_lap_point, lap_point, start_clock, +def get_lap_completed_stats(experiment_metrics, circuit_diameter, first_lap_point, start_clock, clock_points, checkpoints): # If lap is completed, add more statistic information - if type(lap_point) is not int and experiment_metrics['percentage_completed'] > LAP_COMPLETED_PERCENTAGE: + if experiment_metrics['percentage_completed'] > LAP_COMPLETED_PERCENTAGE: seconds_start = start_clock['clock.secs'] - seconds_end = clock_points[int(len(clock_points) * (first_previous_lap_point / len(checkpoints)))]['clock.secs'] + seconds_end = clock_points[int(len(clock_points) * (first_lap_point / len(checkpoints)))]['clock.secs'] experiment_metrics['lap_seconds'] = seconds_end - seconds_start experiment_metrics['circuit_diameter'] = circuit_diameter logger.info('* Lap seconds ---> ' + str(experiment_metrics['lap_seconds'])) From 778bbed480955facc487d25833cf28cbea106f14 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 9 Feb 2022 13:06:28 +0100 Subject: [PATCH 3/6] Refactor code --- behavior_metrics/utils/metrics.py | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/behavior_metrics/utils/metrics.py b/behavior_metrics/utils/metrics.py index bc568d88..3e4f50da 100644 --- a/behavior_metrics/utils/metrics.py +++ b/behavior_metrics/utils/metrics.py @@ -42,7 +42,6 @@ def is_finish_line(point, start_point): start_point = np.array([start_point['pose.pose.position.x'], start_point['pose.pose.position.y']]) except IndexError: start_point = start_point - # start_point = np.array([start_point['pose.pose.position.x'], start_point['pose.pose.position.y']]) dist = (start_point - current_point) ** 2 dist = np.sum(dist, axis=0) dist = np.sqrt(dist) @@ -143,8 +142,9 @@ def get_average_speed(experiment_metrics, seconds_start, seconds_end): def get_percentage_completed(experiment_metrics, checkpoints, perfect_lap_checkpoints): + # Find starting position to calculate percentage first_checkpoint = np.array([checkpoints[0]['pose.pose.position.x'], checkpoints[0]['pose.pose.position.y']]) - perfect_point_iter = 0 + perfect_point_iterator = 0 min_dist = 100 for position, perfect_checkpoint in enumerate(perfect_lap_checkpoints): perfect_checkpoint = np.array( @@ -154,29 +154,30 @@ def get_percentage_completed(experiment_metrics, checkpoints, perfect_lap_checkp dist = np.sqrt(dist) if dist < min_dist: min_dist = dist - perfect_point_iter = position + perfect_point_iterator = position + # Find checkpoints reached checkpoints_reached = 1 - chck_iter = 1 - perfect_point_iter += 1 + checkpoint_iterator = 1 + perfect_point_iterator += 1 lap_checkpoint = 0 - while chck_iter < len(checkpoints): + while checkpoint_iterator < len(checkpoints): current_checkpoint = np.array( - [checkpoints[chck_iter]['pose.pose.position.x'], checkpoints[chck_iter]['pose.pose.position.y']]) - perfect_checkpoint = np.array([perfect_lap_checkpoints[perfect_point_iter]['pose.pose.position.x'], - perfect_lap_checkpoints[perfect_point_iter]['pose.pose.position.y']]) + [checkpoints[checkpoint_iterator]['pose.pose.position.x'], checkpoints[checkpoint_iterator]['pose.pose.position.y']]) + perfect_checkpoint = np.array([perfect_lap_checkpoints[perfect_point_iterator]['pose.pose.position.x'], + perfect_lap_checkpoints[perfect_point_iterator]['pose.pose.position.y']]) dist = (perfect_checkpoint - current_checkpoint) ** 2 dist = np.sum(dist, axis=0) dist = np.sqrt(dist) if dist < 5: checkpoints_reached += 1 - perfect_point_iter += 1 + perfect_point_iterator += 1 if checkpoints_reached / len(perfect_lap_checkpoints) == 1: - lap_checkpoint = chck_iter - if perfect_point_iter >= len(perfect_lap_checkpoints): - perfect_point_iter = 0 + lap_checkpoint = checkpoint_iterator + if perfect_point_iterator >= len(perfect_lap_checkpoints): + perfect_point_iterator = 0 else: - chck_iter += 1 + checkpoint_iterator += 1 experiment_metrics['percentage_completed'] = checkpoints_reached / len(perfect_lap_checkpoints) * 100 logger.info('* Percentage completed ---> ' + str(experiment_metrics['percentage_completed'])) From fcb79349622e6fc52c00314a8b25bc0f6d8526a7 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 9 Feb 2022 16:44:51 +0100 Subject: [PATCH 4/6] Support for both directions --- behavior_metrics/utils/metrics.py | 51 +++++++++++++++++++++---------- 1 file changed, 35 insertions(+), 16 deletions(-) diff --git a/behavior_metrics/utils/metrics.py b/behavior_metrics/utils/metrics.py index 3e4f50da..9974bca3 100644 --- a/behavior_metrics/utils/metrics.py +++ b/behavior_metrics/utils/metrics.py @@ -1,9 +1,7 @@ #!/usr/bin/env python """This module contains the metrics manager. - This module is in charge of generating metrics for a brain execution. - This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later @@ -42,6 +40,7 @@ def is_finish_line(point, start_point): start_point = np.array([start_point['pose.pose.position.x'], start_point['pose.pose.position.y']]) except IndexError: start_point = start_point + # start_point = np.array([start_point['pose.pose.position.x'], start_point['pose.pose.position.y']]) dist = (start_point - current_point) ** 2 dist = np.sum(dist, axis=0) dist = np.sqrt(dist) @@ -156,31 +155,51 @@ def get_percentage_completed(experiment_metrics, checkpoints, perfect_lap_checkp min_dist = dist perfect_point_iterator = position - # Find checkpoints reached - checkpoints_reached = 1 - checkpoint_iterator = 1 - perfect_point_iterator += 1 lap_checkpoint = 0 + # Direction 1 + checkpoints_reached_dir_1 = 1 + checkpoint_iterator = 1 + perfect_point_iterator_dir_1 = perfect_point_iterator + 1 while checkpoint_iterator < len(checkpoints): - current_checkpoint = np.array( - [checkpoints[checkpoint_iterator]['pose.pose.position.x'], checkpoints[checkpoint_iterator]['pose.pose.position.y']]) - perfect_checkpoint = np.array([perfect_lap_checkpoints[perfect_point_iterator]['pose.pose.position.x'], - perfect_lap_checkpoints[perfect_point_iterator]['pose.pose.position.y']]) + current_checkpoint = np.array([checkpoints[checkpoint_iterator]['pose.pose.position.x'], checkpoints[checkpoint_iterator]['pose.pose.position.y']]) + perfect_checkpoint = np.array([perfect_lap_checkpoints[perfect_point_iterator_dir_1]['pose.pose.position.x'], + perfect_lap_checkpoints[perfect_point_iterator_dir_1]['pose.pose.position.y']]) dist = (perfect_checkpoint - current_checkpoint) ** 2 dist = np.sum(dist, axis=0) dist = np.sqrt(dist) if dist < 5: - checkpoints_reached += 1 - perfect_point_iterator += 1 + checkpoints_reached_dir_1 += 1 + perfect_point_iterator_dir_1 += 1 if checkpoints_reached / len(perfect_lap_checkpoints) == 1: lap_checkpoint = checkpoint_iterator - if perfect_point_iterator >= len(perfect_lap_checkpoints): - perfect_point_iterator = 0 + if perfect_point_iterator_dir_1 >= len(perfect_lap_checkpoints): + perfect_point_iterator_dir_1 = 0 else: checkpoint_iterator += 1 + percentage_completed_dir_1 = (checkpoints_reached_dir_1 / len(perfect_lap_checkpoints)) * 100 - experiment_metrics['percentage_completed'] = checkpoints_reached / len(perfect_lap_checkpoints) * 100 - logger.info('* Percentage completed ---> ' + str(experiment_metrics['percentage_completed'])) + # Direction 2 + checkpoints_reached_dir_2 = 1 + checkpoint_iterator = 1 + perfect_point_iterator_dir_2 = perfect_point_iterator - 1 + while checkpoint_iterator < len(checkpoints): + current_checkpoint = np.array([checkpoints[checkpoint_iterator]['pose.pose.position.x'], checkpoints[checkpoint_iterator]['pose.pose.position.y']]) + perfect_checkpoint = np.array([perfect_lap_checkpoints[perfect_point_iterator_dir_2]['pose.pose.position.x'], + perfect_lap_checkpoints[perfect_point_iterator_dir_2]['pose.pose.position.y']]) + dist = (perfect_checkpoint - current_checkpoint) ** 2 + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + if dist < 5: + checkpoints_reached_dir_2 += 1 + perfect_point_iterator_dir_2 -= 1 + if checkpoints_reached / len(perfect_lap_checkpoints) == 1: + lap_checkpoint = checkpoint_iterator + if perfect_point_iterator_dir_2 <= 0: + perfect_point_iterator_dir_2 = len(perfect_lap_checkpoints) + else: + checkpoint_iterator += 1 + percentage_completed_dir_2 = (checkpoints_reached_dir_2 / len(perfect_lap_checkpoints)) * 100 + experiment_metrics['percentage_completed'] = percentage_completed_dir_1 if percentage_completed_dir_1 > percentage_completed_dir_2 else percentage_completed_dir_2 experiment_metrics = get_robot_position_deviation_score(perfect_lap_checkpoints, checkpoints, experiment_metrics) return experiment_metrics, lap_checkpoint From c2301c207301d57466db7fdae2a35e94c08ce314 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 9 Feb 2022 16:51:06 +0100 Subject: [PATCH 5/6] Solved typo --- behavior_metrics/utils/metrics.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/behavior_metrics/utils/metrics.py b/behavior_metrics/utils/metrics.py index 9974bca3..ee41149c 100644 --- a/behavior_metrics/utils/metrics.py +++ b/behavior_metrics/utils/metrics.py @@ -170,7 +170,7 @@ def get_percentage_completed(experiment_metrics, checkpoints, perfect_lap_checkp if dist < 5: checkpoints_reached_dir_1 += 1 perfect_point_iterator_dir_1 += 1 - if checkpoints_reached / len(perfect_lap_checkpoints) == 1: + if checkpoints_reached_dir_1 / len(perfect_lap_checkpoints) == 1: lap_checkpoint = checkpoint_iterator if perfect_point_iterator_dir_1 >= len(perfect_lap_checkpoints): perfect_point_iterator_dir_1 = 0 @@ -192,7 +192,7 @@ def get_percentage_completed(experiment_metrics, checkpoints, perfect_lap_checkp if dist < 5: checkpoints_reached_dir_2 += 1 perfect_point_iterator_dir_2 -= 1 - if checkpoints_reached / len(perfect_lap_checkpoints) == 1: + if checkpoints_reached_dir_2 / len(perfect_lap_checkpoints) == 1: lap_checkpoint = checkpoint_iterator if perfect_point_iterator_dir_2 <= 0: perfect_point_iterator_dir_2 = len(perfect_lap_checkpoints) From 92c5ea04635325223be1ef986c4531e85415c9c4 Mon Sep 17 00:00:00 2001 From: sergiopaniego Date: Wed, 9 Feb 2022 16:51:52 +0100 Subject: [PATCH 6/6] Comment removed --- behavior_metrics/utils/metrics.py | 1 - 1 file changed, 1 deletion(-) diff --git a/behavior_metrics/utils/metrics.py b/behavior_metrics/utils/metrics.py index ee41149c..a0b5823f 100644 --- a/behavior_metrics/utils/metrics.py +++ b/behavior_metrics/utils/metrics.py @@ -40,7 +40,6 @@ def is_finish_line(point, start_point): start_point = np.array([start_point['pose.pose.position.x'], start_point['pose.pose.position.y']]) except IndexError: start_point = start_point - # start_point = np.array([start_point['pose.pose.position.x'], start_point['pose.pose.position.y']]) dist = (start_point - current_point) ** 2 dist = np.sum(dist, axis=0) dist = np.sqrt(dist)