def parallel_save_laser_scan_msgs(save_laser_scan_msgs_args):
bag_file_path, scans_file_path, topic_name = save_laser_scan_msgs_args
if shared_terminate.value:
return
print_info("start : save_laser_scan_msgs {}".format(bag_file_path))
# noinspection PyBroadException
try:
save_laser_scan_msgs(bag_file_path, scans_file_path, topic_name)
except KeyboardInterrupt:
print_info(
"parallel_save_laser_scan_msgs: metrics computation interrupted (bag {})"
.format(bag_file_path))
shared_terminate.value = True
except:
print_error(
"parallel_save_laser_scan_msgs: failed metrics computation for bag {}"
.format(bag_file_path))
print_error(traceback.format_exc())
shared_progress.value += 1
print_info("finish: save_laser_scan_msgs {:3d}% {}".format(
int(shared_progress.value * 100 / shared_num_runs.value),
bag_file_path))
def ps_snapshot_timer_callback(self):
ps_snapshot_file_path = path.join(
self.ps_output_folder,
"ps_{i:08d}.pkl".format(i=self.ps_snapshot_count))
processes_dicts_list = list()
for process in self.ps_processes:
try:
process_copy = copy.deepcopy(
process.as_dict()) # get all information about the process
except psutil.NoSuchProcess: # processes may have died, causing this exception to be raised from psutil.Process.as_dict
continue
try:
# delete uninteresting values
del process_copy['connections']
del process_copy['memory_maps']
del process_copy['environ']
processes_dicts_list.append(process_copy)
except KeyError:
pass
try:
with open(ps_snapshot_file_path, 'wb') as ps_snapshot_file:
pickle.dump(processes_dicts_list, ps_snapshot_file)
except TypeError:
print_error(traceback.format_exc())
self.ps_snapshot_count += 1
def get_result_callback(self, future):
status = future.result().status
if status == GoalStatus.STATUS_SUCCEEDED:
goal_position = self.current_goal.pose.pose.position
current_position = self.latest_ground_truth_pose_msg.pose.pose.position
distance_from_goal = np.sqrt(
(goal_position.x - current_position.x)**2 +
(goal_position.y - current_position.y)**2)
if distance_from_goal < self.goal_tolerance:
self.write_event(self.get_clock().now(), 'target_pose_reached')
self.goal_succeeded_count += 1
else:
print_error(
"goal status succeeded but current position farther from goal position than tolerance"
)
self.write_event(self.get_clock().now(),
'target_pose_not_reached')
self.goal_failed_count += 1
else:
print_info(
'navigation action failed with status {}'.format(status))
self.write_event(self.get_clock().now(), 'target_pose_not_reached')
self.goal_failed_count += 1
self.current_goal = None
# if all goals have been sent end the run, otherwise send the next goal
if len(self.traversal_path_poses) == 0:
self.write_event(self.get_clock().now(), 'run_completed')
rclpy.shutdown()
else:
self.send_goal()
def cpu_and_memory_usage_metrics(ps_snapshots_folder_path):
cpu_and_memory_usage_dict = dict()
cpu_and_memory_usage_list_dict = defaultdict(list)
# get list of all ps snapshots
ps_snapshot_paths_list = sorted(
glob.glob(path.join(ps_snapshots_folder_path, "ps_*.pkl")))
for ps_snapshot_path in ps_snapshot_paths_list:
try:
with open(ps_snapshot_path, 'rb') as ps_snapshot_file:
ps_snapshot = pickle.load(ps_snapshot_file)
except EOFError as e:
print_error(
"Could not load pickled ps snapshot. Error: {t} {e}. Pickle file: {f}"
.format(e=e, t=type(e), f=ps_snapshot_path))
continue
for process_info in ps_snapshot:
process_name = process_info['name']
cpu_and_memory_usage_list_dict["{p}_uss".format(
p=process_name)].append(process_info['memory_full_info'].uss)
cpu_and_memory_usage_list_dict["{p}_rss".format(
p=process_name)].append(process_info['memory_full_info'].rss)
cpu_and_memory_usage_list_dict["{p}_accumulated_cpu_time".format(
p=process_name)].append(process_info['cpu_times'].user +
process_info['cpu_times'].system)
for metric_name, metric_values in cpu_and_memory_usage_list_dict.items():
cpu_and_memory_usage_dict[metric_name] = max(
cpu_and_memory_usage_list_dict[metric_name])
return cpu_and_memory_usage_dict
def __init__(self, map_info_path):
with open(map_info_path) as map_info_file:
map_info = yaml.safe_load(map_info_file)
if path.isabs(map_info['image']):
map_image_path = map_info['image']
else:
map_image_path = path.join(path.dirname(map_info_path), map_info['image'])
self.map_image = Image.open(map_image_path)
self.width, self.height = self.map_image.size
self.map_offset_meters = np.array(map_info['origin'][0:2], dtype=float)
map_rotation_offset = np.array(map_info['origin'][2:3], dtype=float)
if map_rotation_offset != 0:
print_error("convert_grid_map_to_gt_map: map rotation not supported")
self.resolution = float(map_info['resolution']) # meter/pixel
self.map_size_pixels = np.array(self.map_image.size)
self.map_size_meters = self.map_size_pixels * self.resolution
self.map_frame_meters = -self.map_offset_meters
if 'initial_pose' in map_info:
self.initial_position = map_info['initial_pose']
else:
self.initial_position = [0.0, 0.0]
self._complete_free_voronoi_graph_file_path = path.join(path.dirname(map_info_path), "complete_free_voronoi_graph_cache.pkl")
self._occupancy_map = None
self._complete_free_voronoi_graph = None
def save_laser_scan_msgs(bag_file_path, scans_file_path, topic_name):
if not path.exists(bag_file_path):
print_error("file not found: {}".format(bag_file_path))
return
if path.exists(scans_file_path):
print_info("scans file already exists: {}".format(scans_file_path))
return
scans_file_dir = path.dirname(scans_file_path)
if not path.exists(scans_file_dir):
os.makedirs(scans_file_dir)
bag = rosbag.Bag(bag_file_path)
with open(scans_file_path, 'w') as scans_file:
for _, laser_scan_msg, _ in bag.read_messages(topics=[topic_name]):
scans_file.write(
"{t}, {angle_min}, {angle_max}, {angle_increment}, {range_min}, {range_max}, {ranges}\n"
.format(t=laser_scan_msg.header.stamp.to_sec(),
angle_min=laser_scan_msg.angle_min,
angle_max=laser_scan_msg.angle_max,
angle_increment=laser_scan_msg.angle_increment,
range_min=laser_scan_msg.range_min,
range_max=laser_scan_msg.range_max,
ranges=', '.join(map(str, laser_scan_msg.ranges))))
bag.close()
def localization_node_transition_event_callback(
self, transition_event_msg: lifecycle_msgs.msg.TransitionEvent):
# send the initial pose as soon as the localization node activates the first time
if transition_event_msg.goal_state.label == 'active' and not self.localization_node_activated:
if self.initial_pose is None:
print_error("initial_pose is still None")
return
self.localization_node_activated = True
self.initial_pose_publisher.publish(self.initial_pose)
def shutdown(self):
if self._launch_task is None:
print_error(
"ComponentsLauncher.shutdown: components launcher has not been launched"
)
return
if not self._launch_task.done():
asyncio.ensure_future(self._launch_service.shutdown(),
loop=self._loop)
self._loop.run_until_complete(self._launch_task)
def absolute_localization_error_metrics(estimated_poses_file_path, ground_truth_poses_file_path):
# check required files exist
if not path.isfile(estimated_poses_file_path):
print_error("absolute_localization_error_metrics: estimated_poses file not found {}".format(estimated_poses_file_path))
return
if not path.isfile(ground_truth_poses_file_path):
print_error("absolute_localization_error_metrics: ground_truth_poses file not found {}".format(ground_truth_poses_file_path))
return
estimated_poses_df = pd.read_csv(estimated_poses_file_path)
if len(estimated_poses_df.index) < 2:
print_error("not enough estimated poses data points")
return
ground_truth_poses_df = pd.read_csv(ground_truth_poses_file_path)
df = compute_ground_truth_interpolated_poses(estimated_poses_df, ground_truth_poses_df)
# if no matching ground truth data points are found, the metrics con not be computed
if len(df.index) < 2:
print_error("no matching ground truth data points were found")
return
absolute_error_of_each_segment = np.sqrt((df['x_est'] - df['x_gt'])**2 + (df['y_est'] - df['y_gt'])**2)
absolute_error_dict = dict()
absolute_error_dict['sum'] = float(absolute_error_of_each_segment.sum())
absolute_error_dict['mean'] = float(absolute_error_of_each_segment.mean())
return absolute_error_dict
def trajectory_length_metric(ground_truth_poses_file_path):
# check required files exist
if not path.isfile(ground_truth_poses_file_path):
print_error("compute_trajectory_length: ground_truth_poses file not found {}".format(ground_truth_poses_file_path))
return None
df = pd.read_csv(ground_truth_poses_file_path)
squared_deltas = (df[['x', 'y']] - df[['x', 'y']].shift(periods=1)) ** 2 # equivalent to (x_2-x_1)**2, (y_2-y_1)**2, for each row
sum_of_squared_deltas = squared_deltas['x'] + squared_deltas['y'] # equivalent to (x_2-x_1)**2 + (y_2-y_1)**2, for each row
euclidean_distance_of_deltas = np.sqrt(sum_of_squared_deltas) # equivalent to sqrt( (x_2-x_1)**2 + (y_2-y_1)**2 ), for each row
trajectory_length = euclidean_distance_of_deltas.sum()
return float(trajectory_length)
def log(self, event): # /home/furkan/ds/performance_modelling/output/test_planning/benchmark_log.csv -> writing benchmark_log.csv related event
if not path.exists(self.benchmark_log_path):
with open(self.benchmark_log_path, 'a') as output_file:
output_file.write("timestamp, run_id, event\n")
t = time.time()
print_info("t: {t}, run: {run_id}, event: {event}".format(t=t, run_id=self.run_id, event=event))
try:
with open(self.benchmark_log_path, 'a') as output_file:
output_file.write("{t}, {run_id}, {event}\n".format(t=t, run_id=self.run_id, event=event))
except IOError as e:
print_error("benchmark_log: could not write event to file: {t}, {run_id}, {event}".format(t=t, run_id=self.run_id, event=event))
print_error(e)
def log(self, event):
if not path.exists(self.benchmark_log_path):
with open(self.benchmark_log_path, 'a') as output_file:
output_file.write("timestamp, run_id, event\n")
t = time.time()
print_info(f"t: {t}, run: {self.run_id}, event: {event}")
try:
with open(self.benchmark_log_path, 'a') as output_file:
output_file.write(f"{t}, {self.run_id}, {event}\n")
except IOError as e:
print_error(
f"benchmark_log: could not write event to file: {t}, {self.run_id}, {event}"
)
print_error(e)
def goal_response_callback(self, future):
goal_handle = future.result()
# if the goal is rejected try with the next goal
if not goal_handle.accepted:
print_error('navigation action goal rejected')
self.write_event(self.get_clock().now(), 'target_pose_rejected')
self.goal_rejected_count += 1
self.current_goal = None
self.send_goal()
return
self.write_event(self.get_clock().now(), 'target_pose_accepted')
self.navigate_to_pose_action_result_future = goal_handle.get_result_async(
)
self.navigate_to_pose_action_result_future.add_done_callback(
self.get_result_callback)
def main(args=None):
rclpy.init(args=args)
node = None
# noinspection PyBroadException
try:
node = LocalizationBenchmarkSupervisor()
node.start_run()
rclpy.spin(node)
except KeyboardInterrupt:
node.ros_shutdown_callback()
except RunFailException as e:
print_error(e)
except Exception:
print_error(traceback.format_exc())
finally:
if node is not None:
node.end_run()
def log_packages_and_repos(source_workspace_path, log_dir_path):
source_workspace_path = path.normpath(
path.expanduser(source_workspace_path))
log_dir_path = path.normpath(path.expanduser(log_dir_path))
if path.isdir(log_dir_path):
backup_file_if_exists(log_dir_path)
os.makedirs(log_dir_path)
else:
if not path.exists(log_dir_path):
os.makedirs(log_dir_path)
else:
print_error(
"log_dir_path already exists but is not a directory [{}]".
format(log_dir_path))
return False
source_repos_version_file_path = path.join(log_dir_path,
"source_repos_version")
source_repos_status_file_path = path.join(log_dir_path,
"source_repos_status")
installed_packages_version_file_path = path.join(
log_dir_path, "installed_packages_version")
ros_packages_file_path = path.join(log_dir_path, "ros_packages")
if not path.isdir(source_workspace_path):
print_error(
"source_workspace_path does not exists or is not a directory [{}]".
format(source_workspace_path))
return False
with open(source_repos_version_file_path,
'w') as source_repos_version_list_file:
source_repo_git_path_list = glob.glob(
path.join(source_workspace_path, "**", ".git"))
for source_repo_git_path in source_repo_git_path_list:
source_repo_path = path.abspath(
path.join(source_repo_git_path, ".."))
head_hash_output = subprocess.check_output(
["git", "rev-parse", "HEAD"], cwd=source_repo_path)
head_hash = head_hash_output.rstrip().replace('\n', ';')
source_repos_version_list_file.write("{}, {}\n".format(
source_repo_path, head_hash))
with open(source_repos_status_file_path,
'w') as source_repos_status_list_file:
source_repo_git_path_list = glob.glob(
path.join(source_workspace_path, "**", ".git"))
for source_repo_git_path in source_repo_git_path_list:
source_repo_path = path.abspath(
path.join(source_repo_git_path, ".."))
status_output = subprocess.check_output(["git", "status"],
cwd=source_repo_path)
diff_output = subprocess.check_output(["git", "diff"],
cwd=source_repo_path)
source_repos_status_list_file.write(
"STATUS_BEGIN {repo_path}\n{status}STATUS_END\nDIFF_BEGIN {repo_path}\n{diff}\nDIFF_END\n"
.format(repo_path=source_repo_path,
status=status_output,
diff=diff_output))
with open(installed_packages_version_file_path,
'w') as installed_packages_version_file:
dpkg_list_output = subprocess.check_output(["dpkg", "-l"])
installed_packages_version_file.write(dpkg_list_output)
with open(ros_packages_file_path, 'w') as ros_packages_file:
rospack_list_output = subprocess.check_output(
["rospack", "list"], cwd=source_workspace_path)
ros_packages_file.write(rospack_list_output)
return True
def update_run_parameters(base_run_folder_path):
base_run_folder = path.expanduser(base_run_folder_path)
if not path.isdir(base_run_folder):
print_error(
"update_run_parameters: base_run_folder does not exists or is not a directory"
.format(base_run_folder))
return None, None
run_folders = sorted(
list(
filter(path.isdir,
glob.glob(path.abspath(base_run_folder) + '/*'))))
# collect results from runs not already cached
print_info("update_run_parameters: reading run data")
no_output = True
for i, run_folder in enumerate(run_folders):
current_run_info_file_path = path.join(run_folder, "run_info.yaml")
original_run_info_file_path = path.join(run_folder,
"run_info_original.yaml")
if not path.exists(current_run_info_file_path) and not path.exists(
original_run_info_file_path):
print_error(
"update_run_parameters: run_info file does not exists [{}]".
format(run_folder))
no_output = False
continue
# backup current run_info if there is not already a backup of the original
# do not overwrite the original, since it will always be used to create the updated run_info
if not path.exists(original_run_info_file_path):
shutil.copyfile(current_run_info_file_path,
original_run_info_file_path)
with open(original_run_info_file_path) as original_run_info_file:
run_info = yaml.load(original_run_info_file)
if 'run_parameters' not in run_info:
print_error(
"update_run_parameters: run_parameters not in run_info [{}]".
format(run_folder))
no_output = False
continue
if 'slam_node' not in run_info['run_parameters']:
print_error(
"slam_node not in run_info['run_parameters'] [{}]".format(
run_folder))
no_output = False
continue
slam_node = run_info['run_parameters']['slam_node']
# linear_update, angular_update -> linear_angular_update
if slam_node == 'slam_toolbox':
slam_config_path = path.join(run_folder,
"components_configuration",
"slam_toolbox",
"slam_toolbox_online_async.yaml")
if not path.exists(slam_config_path):
print_error("not path.exists(slam_config_path) [{}]".format(
slam_config_path))
no_output = False
continue
slam_config = get_yaml(slam_config_path)
linear_update = slam_config['minimum_travel_distance']
angular_update = slam_config['minimum_travel_heading']
run_info['run_parameters']['linear_angular_update'] = [
linear_update, angular_update
]
elif slam_node == 'gmapping':
slam_config_path = path.join(run_folder,
"components_configuration",
"gmapping", "gmapping.yaml")
if not path.exists(slam_config_path):
print_error("not path.exists(slam_config_path) [{}]".format(
slam_config_path))
no_output = False
continue
slam_config = get_yaml(slam_config_path)
linear_update = slam_config['linearUpdate']
angular_update = slam_config['angularUpdate']
run_info['run_parameters']['linear_angular_update'] = [
linear_update, angular_update
]
else:
print_error("slam_node = {}".format(slam_node))
no_output = False
continue
if 'linear_update' in run_info['run_parameters']:
del run_info['run_parameters']['linear_update']
if 'angular_update' in run_info['run_parameters']:
del run_info['run_parameters']['angular_update']
# remove lidar_model
if 'lidar_model' in run_info['run_parameters']:
del run_info['run_parameters']['lidar_model']
# add goal_tolerance
nav_config_path = path.join(run_folder, "components_configuration",
"move_base", "move_base_tb3.yaml")
if not path.exists(nav_config_path):
print_error("not path.exists(nav_config_path) [{}]".format(
nav_config_path))
no_output = False
continue
nav_config = get_yaml(nav_config_path)
xy_goal_tolerance = nav_config['DWAPlannerROS']['xy_goal_tolerance']
yaw_goal_tolerance = nav_config['DWAPlannerROS']['yaw_goal_tolerance']
run_info['run_parameters']['goal_tolerance'] = [
xy_goal_tolerance, yaw_goal_tolerance
]
# add fewer_nav_goals (false if not specified in the configuration)
sup_config_path = path.join(run_folder, "components_configuration",
"slam_benchmark_supervisor",
"slam_benchmark_supervisor.yaml")
if not path.exists(sup_config_path):
print_error("not path.exists(sup_config_path) [{}]".format(
sup_config_path))
no_output = False
continue
sup_config = get_yaml(sup_config_path)
if 'fewer_nav_goals' in sup_config:
fewer_nav_goals = sup_config['fewer_nav_goals']
else:
fewer_nav_goals = False
run_info['run_parameters']['fewer_nav_goals'] = fewer_nav_goals
with open(current_run_info_file_path, 'w') as current_run_info_file:
yaml.dump(run_info,
current_run_info_file,
default_flow_style=False)
print_info("update_run_parameters: {}%".format(
int((i + 1) * 100 / len(run_folders))),
replace_previous_line=no_output)
no_output = True
def execute_grid_benchmark(benchmark_run_object, grid_benchmark_configuration,
environment_folders, base_run_folder, num_runs,
ignore_executed_params_combinations, shuffle,
headless, show_ros_info):
if not path.exists(base_run_folder):
os.makedirs(base_run_folder)
log_file_path = path.join(base_run_folder, "benchmark_log.csv")
# get list of param combinations already executed in
executed_params_combinations = defaultdict(int)
run_folders = sorted(
list(
filter(path.isdir,
glob.glob(path.abspath(base_run_folder) + '/*'))))
if not ignore_executed_params_combinations:
for i, run_folder in enumerate(run_folders):
run_info_file_path = path.join(run_folder, "run_info.yaml")
if path.exists(run_info_file_path):
# noinspection PyBroadException
try:
with open(run_info_file_path) as run_info_file:
run_info = yaml.safe_load(run_info_file)
params_dict = run_info['run_parameters']
params_dict['environment_name'] = path.basename(
path.abspath(run_info['environment_folder']))
for param_name, param_value in params_dict.items():
params_dict[param_name] = tuple(
param_value
) if type(
param_value
) == list else param_value # convert any list into a tuple to allow hashing
params_hashable_dict = hashable_dict(params_dict)
executed_params_combinations[params_hashable_dict] += 1
except:
print_error(traceback.format_exc())
with open(grid_benchmark_configuration, 'r') as f:
grid_benchmark_configuration = yaml.safe_load(f)
if isinstance(grid_benchmark_configuration['combinatorial_parameters'],
list):
combinatorial_parameters_dict_list = grid_benchmark_configuration[
'combinatorial_parameters']
elif isinstance(grid_benchmark_configuration['combinatorial_parameters'],
dict):
combinatorial_parameters_dict_list = [
grid_benchmark_configuration['combinatorial_parameters']
]
else:
print_error(
"grid_benchmark_configuration combinatorial_parameters must be a list or dict"
)
sys.exit(-1)
environment_folders_by_name = dict()
parameters_combinations_dict_list = list()
for combinatorial_parameters_dict in combinatorial_parameters_dict_list:
# add environment_name to the parameters and populate the environment_folders_by_name lookup dict
combinatorial_parameters_dict['environment_name'] = list()
for environment_folder in environment_folders:
environment_name = path.basename(path.abspath(environment_folder))
environment_folders_by_name[environment_name] = environment_folder
combinatorial_parameters_dict['environment_name'].append(
environment_name)
# convert the dict with {parameter_name_1: [parameter_value_1, parameter_value_2], ...}
# to the list [(parameter_name_1, parameter_value_1), (parameter_name_1, parameter_value_2), ...]
parameters_alternatives = list()
for parameter_name, parameter_values_list in combinatorial_parameters_dict.items(
):
parameter_list_of_tuples = list(
map(lambda parameter_value: (parameter_name, parameter_value),
parameter_values_list))
parameters_alternatives.append(parameter_list_of_tuples)
# obtain the list of combinations from the list of alternatives
# ex: itertools.product([(a, 1), (a, 2)], [(b, 0.1), (b, 0.2)]) --> [ [(a, 1), (b, 0.1)], [(a, 1), (b, 0.2)], [(a, 2), (b, 0.1)], [(a, 2), (b, 0.2)] ]
parameters_combinations_lists = list(
itertools.product(*parameters_alternatives))
# convert the list of lists to a list of dicts
parameters_combinations_dict_list += list(
map(dict, parameters_combinations_lists))
remaining_params_combinations = list()
for parameters_combination_dict in parameters_combinations_dict_list:
parameters_combination_dict_copy = parameters_combination_dict.copy()
for param_name, param_value in parameters_combination_dict_copy.items(
):
parameters_combination_dict_copy[param_name] = tuple(
param_value
) if type(
param_value
) == list else param_value # convert any list into a tuple to allow hashing
parameters_combination_hashable_dict = hashable_dict(
parameters_combination_dict_copy)
executed_repetitions = executed_params_combinations[
parameters_combination_hashable_dict]
num_remaining_runs = num_runs - executed_repetitions
if num_remaining_runs > 0:
remaining_params_combinations += [parameters_combination_dict
] * num_remaining_runs
executed_params_combinations[
parameters_combination_hashable_dict] += num_remaining_runs # count this combination in executed_params_combinations in case there are duplicated combinations in parameters_combinations_dict_list
num_combinations = len(parameters_combinations_dict_list)
num_runs_remaining = len(remaining_params_combinations)
num_executed_runs = len(run_folders)
num_executed_combinations = len(
list(filter(lambda x: x > 0, executed_params_combinations.values())))
if ignore_executed_params_combinations:
print_info("ignoring previous runs")
else:
print_info(
"found {num_executed_runs} executed runs with {num_executed_combinations} params combinations"
.format(num_executed_runs=num_executed_runs,
num_executed_combinations=num_executed_combinations))
print_info("number of parameter combinations: {}".format(num_combinations))
print_info("number of repetition runs: {}".format(num_runs))
print_info("total number of runs: {}".format(num_runs *
num_combinations))
print_info(
"remaining number of runs: {}".format(num_runs_remaining))
if shuffle:
# shuffle the remaining params combinations, to avoid executing consecutively the run repetitions with the same combination
print_info("shuffling remaining params combinations")
random.shuffle(remaining_params_combinations)
else:
print_info("not shuffling remaining params combinations")
# generate a session id
session_id = datetime.utcnow().strftime('%Y-%m-%d_%H-%M-%S_%f')
for n_executed_runs, parameters_combination_dict in enumerate(
remaining_params_combinations):
environment_folder = environment_folders_by_name[
parameters_combination_dict['environment_name']]
# find an available run folder path
i = 0
run_folder = path.join(
base_run_folder,
"session_{session_id}_run_{run_number:09d}".format(
session_id=session_id, run_number=i))
while path.exists(run_folder):
i += 1
run_folder = path.join(
base_run_folder,
"session_{session_id}_run_{run_number:09d}".format(
session_id=session_id, run_number=i))
print_info(
"\n\n\nbenchmark: starting run {run_index} ({remaining_runs} remaining)"
.format(run_index=i,
remaining_runs=len(remaining_params_combinations) -
n_executed_runs))
print_info("\tenvironment_folder:", environment_folder)
print_info("\tparameters_combination_dict:")
for k, v in parameters_combination_dict.items():
print_info("\t\t{}: {}".format(k, v))
# instantiate and execute the run
# noinspection PyBroadException
try:
r = benchmark_run_object(
run_id=i,
run_output_folder=run_folder,
benchmark_log_path=log_file_path,
environment_folder=environment_folder,
parameters_combination_dict=parameters_combination_dict,
benchmark_configuration_dict=grid_benchmark_configuration,
show_ros_info=show_ros_info,
headless=headless,
)
r.execute_run()
if r.aborted:
print_info(
"benchmark: run {run_index} aborted".format(run_index=i))
print_info("benchmark: interrupted")
sys.exit(0)
else:
print_info(
"benchmark: run {run_index} completed".format(run_index=i))
except IOError:
print_fatal(traceback.format_exc())
except ValueError:
print_fatal(traceback.format_exc())
except ZeroDivisionError:
print_fatal(traceback.format_exc())
except SystemExit as system_exit_ex:
print_info("System exit code: ", system_exit_ex.code)
sys.exit(0)
except:
print_fatal(traceback.format_exc())
sys.exit(0)
print_info("benchmark: finished")
def black_white_to_ground_truth_map(input_map_path,
map_info_path,
trim_borders=False,
occupied_threshold=150,
blur_filter_radius=0,
do_not_recompute=False,
backup_if_exists=False,
map_files_dump_path=None):
with open(map_info_path) as map_info_file:
map_info = yaml.safe_load(map_info_file)
if path.isabs(map_info['image']):
map_image_path = map_info['image']
else:
map_image_path = path.join(path.dirname(map_info_path),
map_info['image'])
if path.exists(map_image_path) and do_not_recompute:
print_info("do_not_recompute: will not recompute image {}".format(
map_image_path))
return
input_image = Image.open(input_map_path)
if input_image.mode != 'RGB':
# remove alpha channel by pasting on white background
background = Image.new("RGB", input_image.size,
GroundTruthMap.free_rgb)
background.paste(input_image)
input_image = background
# apply a blur filter to reduce artifacts in images that have been saved to lossy formats (may cause the thickness of walls to change)
if blur_filter_radius > 0:
input_image = input_image.filter(
ImageFilter.BoxBlur(blur_filter_radius))
# saturate all colors below the threshold to black and all values above threshold to the unknown color value (they will be colored to free later)
# this is needed because some images have been heavily compressed and contain all sort of colors
threshold_image = input_image.point(
lambda original_value: GroundTruthMap.occupied
if original_value < occupied_threshold else GroundTruthMap.unknown)
threshold_image.save(path.expanduser("~/tmp/threshold_image.pgm"))
# trim borders not containing black pixels (some simulators ignore non-black borders while placing the pixels in the simulated map and computing its resolution, so they need to be ignored in the following calculations)
if trim_borders:
trimmed_image = trim(threshold_image, GroundTruthMap.unknown_rgb)
trimmed_image.save(path.expanduser("~/tmp/trimmed_image.pgm"))
else:
trimmed_image = threshold_image
map_offset_meters = np.array(map_info['origin'][0:2], dtype=float)
map_rotation_offset = np.array(map_info['origin'][2:3], dtype=float)
if map_rotation_offset != 0:
print_error("convert_grid_map_to_gt_map: map rotation not supported")
resolution = float(map_info['resolution']) # meter/pixel
map_frame_meters = -map_offset_meters
if 'initial_pose' in map_info:
initial_position_list = map_info['initial_pose']
else:
initial_position_list = [0.0, 0.0]
initial_position_meters = np.array(initial_position_list, dtype=float)
w, h = trimmed_image.size
i_x, i_y = initial_position_pixels = list(
map(
int,
np.array([0, h]) + np.array([1, -1]) *
(map_frame_meters + initial_position_meters) / resolution))
if i_x < 0 or i_x >= w or i_y < 0 or i_y >= h:
print_fatal("initial_position out of map bounds")
return
pixels = trimmed_image.load()
if pixels[i_x, i_y] != GroundTruthMap.unknown_rgb:
print_fatal("initial position in a wall pixel")
return
# rename variable for clarity
map_image = trimmed_image
# convert to free the pixels accessible from the initial position
floodfill(map_image,
initial_position_pixels,
GroundTruthMap.free_rgb,
thresh=10)
if backup_if_exists:
backup_file_if_exists(map_image_path)
try:
map_image.save(map_image_path)
if map_files_dump_path is not None:
print(map_files_dump_path)
map_files_dump_path = path.abspath(
path.expanduser(map_files_dump_path))
print(map_files_dump_path)
if not path.exists(map_files_dump_path):
os.makedirs(map_files_dump_path)
dataset_name = path.basename(
path.dirname(path.dirname(map_image_path)))
map_image.save(
path.join(map_files_dump_path, dataset_name + '.pgm'))
except IOError:
print_fatal(
"Error while saving image {img}:".format(img=map_image_path))
print_error(traceback.format_exc())
except TypeError:
print_fatal(
"Error while saving image {img}:".format(img=map_image_path))
print_error(traceback.format_exc())
progress * 100 // len(environment_folders), environment_folder))
map_info_file_path = path.join(environment_folder, "data", "map.yaml")
# compute GroundTruthMap data from source image
source_map_file_path = None
source_pgm_map_file_path = path.join(environment_folder, "data",
"source_map.pgm")
source_png_map_file_path = path.join(environment_folder, "data",
"source_map.png")
if path.exists(source_pgm_map_file_path):
source_map_file_path = source_pgm_map_file_path
elif path.exists(source_png_map_file_path):
source_map_file_path = source_png_map_file_path
else:
print_error("source_map file not found")
if source_map_file_path is not None:
black_white_to_ground_truth_map(
source_map_file_path,
map_info_file_path,
do_not_recompute=not recompute_data,
map_files_dump_path=dump_path)
if save_visualization_plots:
# compute voronoi plot
robot_radius_plot = 2.5 * 0.2
voronoi_plot_file_path = path.join(environment_folder, "data",
"visualization", "voronoi.svg")
reduced_voronoi_plot_file_path = path.join(environment_folder,
"data", "visualization",
def collect_data(base_run_folder_path, invalidate_cache=False):
base_run_folder = path.expanduser(base_run_folder_path)
cache_file_path = path.join(base_run_folder, "run_data_per_waypoint_cache.pkl")
if not path.isdir(base_run_folder):
print_error("collect_data: base_run_folder does not exists or is not a directory".format(base_run_folder))
return None, None
run_folders = sorted(list(filter(path.isdir, glob.glob(path.abspath(base_run_folder) + '/*'))))
record_list = list()
if invalidate_cache or not path.exists(cache_file_path):
df = pd.DataFrame()
parameter_names = set()
cached_run_folders = set()
else:
print_info("collect_data: updating cache")
with open(cache_file_path, 'rb') as f:
cache = pickle.load(f)
df = cache['df']
parameter_names = cache['parameter_names']
cached_run_folders = set(df['run_folder'])
# collect results from runs not already cached
print_info("collect_data: reading run data")
no_output = True
for i, run_folder in enumerate(run_folders):
metric_results_folder = path.join(run_folder, "metric_results")
run_info_file_path = path.join(run_folder, "run_info.yaml")
metrics_file_path = path.join(metric_results_folder, "metrics.yaml")
if not path.exists(metric_results_folder):
print_error("collect_data: metric_results_folder does not exists [{}]".format(metric_results_folder))
no_output = False
continue
if not path.exists(run_info_file_path):
print_error("collect_data: run_info file does not exists [{}]".format(run_info_file_path))
no_output = False
continue
if run_folder in cached_run_folders:
continue
run_info = get_yaml(run_info_file_path)
try:
metrics_dict = get_yaml(metrics_file_path)
except IOError:
print_error("metric_results could not be read: {}".format(metrics_file_path))
no_output = False
continue
run_record = dict()
for parameter_name, parameter_value in run_info['run_parameters'].items():
parameter_names.add(parameter_name)
if type(parameter_value) == list:
parameter_value = tuple(parameter_value)
run_record[parameter_name] = parameter_value
# parameter_names.add('environment_name') # environment name is already inside
run_record['environment_name'] = path.basename(path.abspath(run_info['environment_folder']))
run_record['run_folder'] = run_folder
# collect per waypoint metric results
euclidean_length_over_voronoi_distance_per_waypoint_dict = dict()
euclidean_length_over_voronoi_distance_per_waypoint_list = get_yaml_by_path(metrics_dict, ['euclidean_length_over_voronoi_distance'])
if euclidean_length_over_voronoi_distance_per_waypoint_list is not None:
for euclidean_length_over_voronoi_distance_per_waypoint in euclidean_length_over_voronoi_distance_per_waypoint_list:
if euclidean_length_over_voronoi_distance_per_waypoint is not None and 'i_x' and 'i_y' and 'g_x' and 'g_y' in euclidean_length_over_voronoi_distance_per_waypoint:
euclidean_length_over_voronoi_distance_per_waypoint_dict[
euclidean_length_over_voronoi_distance_per_waypoint[
'i_x'], euclidean_length_over_voronoi_distance_per_waypoint[
'i_y'], euclidean_length_over_voronoi_distance_per_waypoint[
'g_x'], euclidean_length_over_voronoi_distance_per_waypoint[
'g_y']] = euclidean_length_over_voronoi_distance_per_waypoint
planning_time_over_voronoi_distance_per_waypoint_dict = dict()
planning_time_over_voronoi_distance_per_waypoint_list = get_yaml_by_path(metrics_dict, ['planning_time_over_voronoi_distance'])
if planning_time_over_voronoi_distance_per_waypoint_list is not None:
for planning_time_over_voronoi_distance_per_waypoint in planning_time_over_voronoi_distance_per_waypoint_list:
if planning_time_over_voronoi_distance_per_waypoint is not None and 'i_x' and 'i_y' and 'g_x' and 'g_y' in planning_time_over_voronoi_distance_per_waypoint:
planning_time_over_voronoi_distance_per_waypoint_dict[
planning_time_over_voronoi_distance_per_waypoint[
'i_x'], planning_time_over_voronoi_distance_per_waypoint[
'i_y'], planning_time_over_voronoi_distance_per_waypoint[
'g_x'], planning_time_over_voronoi_distance_per_waypoint[
'g_y']] = planning_time_over_voronoi_distance_per_waypoint
feasibility_rate_per_waypoint_dict = dict()
feasibility_rate_per_waypoint_list = get_yaml_by_path(metrics_dict, ['feasibility_rate'])
if feasibility_rate_per_waypoint_list is not None:
for feasibility_rate_per_waypoint in feasibility_rate_per_waypoint_list:
if feasibility_rate_per_waypoint is not None and 'i_x' and 'i_y' and 'g_x' and 'g_y' in feasibility_rate_per_waypoint:
feasibility_rate_per_waypoint_dict[
feasibility_rate_per_waypoint[
'i_x'], feasibility_rate_per_waypoint[
'i_y'], feasibility_rate_per_waypoint[
'g_x'], feasibility_rate_per_waypoint[
'g_y']] = feasibility_rate_per_waypoint
mean_passage_width_per_waypoint_dict = dict()
mean_passage_width_per_waypoint_list = get_yaml_by_path(metrics_dict, ['mean_passage_width'])
if mean_passage_width_per_waypoint_list is not None:
for mean_passage_width_per_waypoint in mean_passage_width_per_waypoint_list:
if mean_passage_width_per_waypoint is not None and 'i_x' and 'i_y' and 'g_x' and 'g_y' in mean_passage_width_per_waypoint:
mean_passage_width_per_waypoint_dict[
mean_passage_width_per_waypoint[
'i_x'], mean_passage_width_per_waypoint[
'i_y'], mean_passage_width_per_waypoint[
'g_x'], mean_passage_width_per_waypoint[
'g_y']] = mean_passage_width_per_waypoint
mean_normalized_passage_width_per_waypoint_dict = dict()
mean_normalized_passage_width_per_waypoint_list = get_yaml_by_path(metrics_dict, ['mean_normalized_passage_width'])
if mean_normalized_passage_width_per_waypoint_list is not None:
for mean_normalized_passage_width_per_waypoint in mean_normalized_passage_width_per_waypoint_list:
if mean_normalized_passage_width_per_waypoint is not None and 'i_x' and 'i_y' and 'g_x' and 'g_y' in mean_normalized_passage_width_per_waypoint:
mean_normalized_passage_width_per_waypoint_dict[
mean_normalized_passage_width_per_waypoint[
'i_x'], mean_normalized_passage_width_per_waypoint[
'i_y'], mean_normalized_passage_width_per_waypoint[
'g_x'], mean_normalized_passage_width_per_waypoint[
'g_y']] = mean_normalized_passage_width_per_waypoint
minimum_passage_width_per_waypoint_dict = dict()
minimum_passage_width_per_waypoint_list = get_yaml_by_path(metrics_dict, ['minimum_passage_width'])
if minimum_passage_width_per_waypoint_list is not None:
for minimum_passage_width_per_waypoint in minimum_passage_width_per_waypoint_list:
if minimum_passage_width_per_waypoint is not None and 'i_x' and 'i_y' and 'g_x' and 'g_y' in minimum_passage_width_per_waypoint:
minimum_passage_width_per_waypoint_dict[
minimum_passage_width_per_waypoint[
'i_x'], minimum_passage_width_per_waypoint[
'i_y'], minimum_passage_width_per_waypoint[
'g_x'], minimum_passage_width_per_waypoint[
'g_y']] = minimum_passage_width_per_waypoint
for waypoint_position in minimum_passage_width_per_waypoint_dict.keys():
run_record_per_waypoint = run_record.copy()
run_record_per_waypoint['i_x'] = waypoint_position[0] # position i_x
run_record_per_waypoint['i_y'] = waypoint_position[1] # position i_y
run_record_per_waypoint['g_x'] = waypoint_position[2] # position g_x
run_record_per_waypoint['g_y'] = waypoint_position[3] # position g_y
all_euclidean_length_over_voronoi_distance_metrics = get_yaml_by_path(euclidean_length_over_voronoi_distance_per_waypoint_dict, [waypoint_position])
if all_euclidean_length_over_voronoi_distance_metrics is not None:
for euclidean_length_over_voronoi_distance_metric_name, euclidean_length_over_voronoi_distance_metric_value in all_euclidean_length_over_voronoi_distance_metrics.items():
run_record_per_waypoint['euclidean_length_over_voronoi_distance_' + euclidean_length_over_voronoi_distance_metric_name] = euclidean_length_over_voronoi_distance_metric_value
all_planning_time_over_voronoi_distance_metrics = get_yaml_by_path(planning_time_over_voronoi_distance_per_waypoint_dict, [waypoint_position])
if all_planning_time_over_voronoi_distance_metrics is not None:
for planning_time_over_voronoi_distance_metric_name, planning_time_over_voronoi_distance_metric_value in all_planning_time_over_voronoi_distance_metrics.items():
run_record_per_waypoint['planning_time_over_voronoi_distance_' + planning_time_over_voronoi_distance_metric_name] = planning_time_over_voronoi_distance_metric_value
all_feasibility_rate_metrics = get_yaml_by_path(feasibility_rate_per_waypoint_dict, [waypoint_position])
if all_feasibility_rate_metrics is not None:
for feasibility_rate_metric_name, feasibility_rate_metric_value in all_feasibility_rate_metrics.items():
run_record_per_waypoint['feasibility_rate_' + feasibility_rate_metric_name] = feasibility_rate_metric_value
all_mean_passage_width_metrics = get_yaml_by_path(mean_passage_width_per_waypoint_dict, [waypoint_position])
if all_mean_passage_width_metrics is not None:
for mean_passage_width_metric_name, mean_passage_width_metric_value in all_mean_passage_width_metrics.items():
run_record_per_waypoint['mean_passage_width_' + mean_passage_width_metric_name] = mean_passage_width_metric_value
all_mean_normalized_passage_width_metrics = get_yaml_by_path(mean_normalized_passage_width_per_waypoint_dict, [waypoint_position])
if all_mean_normalized_passage_width_metrics is not None:
for mean_normalized_passage_width_metric_name, mean_normalized_passage_width_metric_value in all_mean_normalized_passage_width_metrics.items():
run_record_per_waypoint[
'mean_normalized_passage_width_' + mean_normalized_passage_width_metric_name] = mean_normalized_passage_width_metric_value
minimum_passage_width_metrics = get_yaml_by_path(minimum_passage_width_per_waypoint_dict, [waypoint_position])
if minimum_passage_width_metrics is not None:
for minimum_passage_width_metrics_name, minimum_passage_width_metric_value in minimum_passage_width_metrics.items():
run_record_per_waypoint['minimum_passage_width_' + minimum_passage_width_metrics_name] = minimum_passage_width_metric_value
record_list.append(run_record_per_waypoint)
print_info("collect_data: reading run data: {}% {}/{} {}".format(int((i + 1)*100/len(run_folders)), i, len(run_folders), path.basename(run_folder)), replace_previous_line=no_output)
no_output = True
df = pd.DataFrame(record_list)
# save cache
if cache_file_path is not None:
cache = {'df': df, 'parameter_names': parameter_names}
with open(cache_file_path, 'wb') as f:
pickle.dump(cache, f, protocol=2)
return df, parameter_names
def relative_localization_error_metrics(log_output_folder, estimated_poses_file_path, ground_truth_poses_file_path, alpha=0.99, max_error=0.02, compute_sequential_relations=False):
"""
Generates the ordered and the random relations files and computes the metrics
"""
if not path.exists(log_output_folder):
os.makedirs(log_output_folder)
# check required files exist
if not path.isfile(estimated_poses_file_path):
print_error("compute_relative_localization_error: estimated_poses file not found {}".format(estimated_poses_file_path))
return
if not path.isfile(ground_truth_poses_file_path):
print_error("compute_relative_localization_error: ground_truth_poses file not found {}".format(ground_truth_poses_file_path))
return
relative_errors_dict = dict()
# find the metricEvaluator executable
metric_evaluator_exec_path = path.join(path.dirname(path.abspath(__file__)), "metricEvaluator", "metricEvaluator")
estimated_poses_df = pd.read_csv(estimated_poses_file_path)
if len(estimated_poses_df.index) < 2:
print_error("not enough estimated poses data points")
return
ground_truth_poses_df = pd.read_csv(ground_truth_poses_file_path)
interpolated_ground_truth_df = compute_ground_truth_interpolated_poses(estimated_poses_df, ground_truth_poses_df)
# if no matching ground truth data points are found, the metrics con not be computed
if len(interpolated_ground_truth_df.index) < 2:
print_error("no matching ground truth data points were found")
return
# convert estimated_poses_file to the CARMEN format
estimated_poses_carmen_file_path = path.join(log_output_folder, "estimated_poses_carmen_format")
with open(estimated_poses_carmen_file_path, "w") as estimated_poses_carmen_file:
for index, row in estimated_poses_df.iterrows():
estimated_poses_carmen_file.write("FLASER 0 0.0 0.0 0.0 {x} {y} {theta} {t}\n".format(x=row['x'], y=row['y'], theta=row['theta'], t=row['t']))
# random relations
relations_re_file_path = path.join(log_output_folder, "re_relations")
with open(relations_re_file_path, "w") as relations_file_re:
n_samples = 500
for _ in range(n_samples):
two_random_poses = interpolated_ground_truth_df[['t', 'x_gt', 'y_gt', 'theta_gt']].sample(n=2)
t_1, x_1, y_1, theta_1 = two_random_poses.iloc[0]
t_2, x_2, y_2, theta_2 = two_random_poses.iloc[1]
# reorder data so that t_1 is before t_2
if t_1 > t_2:
# swap 1 and 2
t_1, x_1, y_1, theta_1, t_2, x_2, y_2, theta_2 = t_2, x_2, y_2, theta_2, t_1, x_1, y_1, theta_1
rel = get_matrix_diff((x_1, y_1, theta_1), (x_2, y_2, theta_2))
x_relative = rel[0, 3]
y_relative = rel[1, 3]
theta_relative = math.atan2(rel[1, 0], rel[0, 0])
relations_file_re.write("{t_1} {t_2} {x} {y} 0.000000 0.000000 0.000000 {theta}\n".format(t_1=t_1, t_2=t_2, x=x_relative, y=y_relative, theta=theta_relative))
# Run the metric evaluator on this relations file, read the sample standard deviation and exploit it to rebuild a better sample
# Compute translational sample size
summary_t_file_path = path.join(log_output_folder, "summary_t_errors")
metric_evaluator(exec_path=metric_evaluator_exec_path,
poses_path=estimated_poses_carmen_file_path,
relations_path=relations_re_file_path,
weights="{1, 1, 1, 0, 0, 0}",
log_path=path.join(log_output_folder, "summary_t.log"),
errors_path=summary_t_file_path)
error_file = open(summary_t_file_path, "r")
content = error_file.readlines()
words = content[1].split(", ")
std = float(words[1])
var = math.pow(std, 2)
z_a_2 = t.ppf(alpha, n_samples - 1)
delta = max_error
n_samples_t = int(math.pow(z_a_2, 2) * var / math.pow(delta, 2))
# Compute rotational sample size
summary_r_file_path = path.join(log_output_folder, "summary_r_errors")
metric_evaluator(exec_path=metric_evaluator_exec_path,
poses_path=estimated_poses_carmen_file_path,
relations_path=relations_re_file_path,
weights="{0, 0, 0, 1, 1, 1}",
log_path=path.join(log_output_folder, "summary_r.log"),
errors_path=summary_r_file_path)
error_file = open(summary_r_file_path, "r")
content = error_file.readlines()
words = content[1].split(", ")
std = float(words[1])
var = math.pow(std, 2)
z_a_2 = t.ppf(alpha, n_samples - 1)
delta = max_error
n_samples_r = int(math.pow(z_a_2, 2) * var / math.pow(delta, 2))
# Select the biggest of the two
n_samples = max(n_samples_t, n_samples_r)
if n_samples < 10:
print_error("n_samples too low", n_samples, n_samples_t, n_samples_r)
return
with open(relations_re_file_path, "w") as relations_file_re:
for _ in range(n_samples):
two_random_poses = interpolated_ground_truth_df[['t', 'x_gt', 'y_gt', 'theta_gt']].sample(n=2)
t_1, x_1, y_1, theta_1 = two_random_poses.iloc[0]
t_2, x_2, y_2, theta_2 = two_random_poses.iloc[1]
# reorder data so that t_1 is before t_2
if t_1 > t_2:
# swap 1 and 2
t_1, x_1, y_1, theta_1, t_2, x_2, y_2, theta_2 = t_2, x_2, y_2, theta_2, t_1, x_1, y_1, theta_1
rel = get_matrix_diff((x_1, y_1, theta_1), (x_2, y_2, theta_2))
x_relative = rel[0, 3]
y_relative = rel[1, 3]
theta_relative = math.atan2(rel[1, 0], rel[0, 0])
relations_file_re.write("{t_1} {t_2} {x} {y} 0.000000 0.000000 0.000000 {theta}\n".format(t_1=t_1, t_2=t_2, x=x_relative, y=y_relative, theta=theta_relative))
relative_errors_dict['random_relations'] = dict()
metric_evaluator_re_t_results_csv_path = path.join(log_output_folder, "re_t.csv")
metric_evaluator(exec_path=metric_evaluator_exec_path,
poses_path=estimated_poses_carmen_file_path,
relations_path=relations_re_file_path,
weights="{1, 1, 1, 0, 0, 0}",
log_path=path.join(log_output_folder, "re_t.log"),
errors_path=metric_evaluator_re_t_results_csv_path,
unsorted_errors_path=path.join(log_output_folder, "re_t_unsorted_errors"))
metric_evaluator_re_t_results_df = pd.read_csv(metric_evaluator_re_t_results_csv_path, sep=', ', engine='python')
relative_errors_dict['random_relations']['translation'] = dict()
relative_errors_dict['random_relations']['translation']['mean'] = float(metric_evaluator_re_t_results_df['Mean'][0])
relative_errors_dict['random_relations']['translation']['std'] = float(metric_evaluator_re_t_results_df['Std'][0])
relative_errors_dict['random_relations']['translation']['min'] = float(metric_evaluator_re_t_results_df['Min'][0])
relative_errors_dict['random_relations']['translation']['max'] = float(metric_evaluator_re_t_results_df['Max'][0])
relative_errors_dict['random_relations']['translation']['n'] = float(metric_evaluator_re_t_results_df['NumMeasures'][0])
metric_evaluator_re_r_results_csv_path = path.join(log_output_folder, "re_r.csv")
metric_evaluator(exec_path=metric_evaluator_exec_path,
poses_path=estimated_poses_carmen_file_path,
relations_path=relations_re_file_path,
weights="{0, 0, 0, 1, 1, 1}",
log_path=path.join(log_output_folder, "re_r.log"),
errors_path=metric_evaluator_re_r_results_csv_path,
unsorted_errors_path=path.join(log_output_folder, "re_r_unsorted_errors"))
metric_evaluator_re_r_results_df = pd.read_csv(metric_evaluator_re_r_results_csv_path, sep=', ', engine='python')
relative_errors_dict['random_relations']['rotation'] = dict()
relative_errors_dict['random_relations']['rotation']['mean'] = float(metric_evaluator_re_r_results_df['Mean'][0])
relative_errors_dict['random_relations']['rotation']['std'] = float(metric_evaluator_re_r_results_df['Std'][0])
relative_errors_dict['random_relations']['rotation']['min'] = float(metric_evaluator_re_r_results_df['Min'][0])
relative_errors_dict['random_relations']['rotation']['max'] = float(metric_evaluator_re_r_results_df['Max'][0])
relative_errors_dict['random_relations']['rotation']['n'] = float(metric_evaluator_re_r_results_df['NumMeasures'][0])
# sequential relations
if compute_sequential_relations:
ordered_relations_file_path = path.join(log_output_folder, "ordered_relations")
with open(ordered_relations_file_path, "w") as relations_file_ordered:
idx_delta = int(len(interpolated_ground_truth_df.index)/n_samples)
if idx_delta == 0:
idx_delta = 1
for idx, first_row in interpolated_ground_truth_df.iloc[::idx_delta].iloc[0: -1].iterrows():
second_row = interpolated_ground_truth_df.iloc[idx + idx_delta]
rel = get_matrix_diff((first_row['x_gt'], first_row['y_gt'], first_row['theta_gt']), (second_row['x_gt'], second_row['y_gt'], second_row['theta_gt']))
x_relative = rel[0, 3]
y_relative = rel[1, 3]
theta_relative = math.atan2(rel[1, 0], rel[0, 0])
relations_file_ordered.write("{t_1} {t_2} {x} {y} 0.000000 0.000000 0.000000 {theta}\n".format(t_1=first_row['t'], t_2=second_row['t'], x=x_relative, y=y_relative, theta=theta_relative))
relative_errors_dict['sequential_relations'] = dict()
metric_evaluator_ordered_t_results_csv_path = path.join(log_output_folder, "ordered_t.csv")
metric_evaluator(exec_path=metric_evaluator_exec_path,
poses_path=estimated_poses_carmen_file_path,
relations_path=ordered_relations_file_path,
weights="{1, 1, 1, 0, 0, 0}",
log_path=path.join(log_output_folder, "ordered_t.log"),
errors_path=metric_evaluator_ordered_t_results_csv_path,
unsorted_errors_path=path.join(log_output_folder, "ordered_t_unsorted_errors"))
metric_evaluator_ordered_t_results_df = pd.read_csv(metric_evaluator_ordered_t_results_csv_path, sep=', ', engine='python')
relative_errors_dict['sequential_relations']['translation'] = dict()
relative_errors_dict['sequential_relations']['translation']['mean'] = float(metric_evaluator_ordered_t_results_df['Mean'][0])
relative_errors_dict['sequential_relations']['translation']['std'] = float(metric_evaluator_ordered_t_results_df['Std'][0])
relative_errors_dict['sequential_relations']['translation']['min'] = float(metric_evaluator_ordered_t_results_df['Min'][0])
relative_errors_dict['sequential_relations']['translation']['max'] = float(metric_evaluator_ordered_t_results_df['Max'][0])
relative_errors_dict['sequential_relations']['translation']['n'] = float(metric_evaluator_ordered_t_results_df['NumMeasures'][0])
metric_evaluator_ordered_r_results_csv_path = path.join(log_output_folder, "ordered_r.csv")
metric_evaluator(exec_path=metric_evaluator_exec_path,
poses_path=estimated_poses_carmen_file_path,
relations_path=ordered_relations_file_path,
weights="{0, 0, 0, 1, 1, 1}",
log_path=path.join(log_output_folder, "ordered_r.log"),
errors_path=metric_evaluator_ordered_r_results_csv_path,
unsorted_errors_path=path.join(log_output_folder, "ordered_r_unsorted_errors"))
metric_evaluator_ordered_r_results_df = pd.read_csv(metric_evaluator_ordered_r_results_csv_path, sep=', ', engine='python')
relative_errors_dict['sequential_relations']['rotation'] = dict()
relative_errors_dict['sequential_relations']['rotation']['mean'] = float(metric_evaluator_ordered_r_results_df['Mean'][0])
relative_errors_dict['sequential_relations']['rotation']['std'] = float(metric_evaluator_ordered_r_results_df['Std'][0])
relative_errors_dict['sequential_relations']['rotation']['min'] = float(metric_evaluator_ordered_r_results_df['Min'][0])
relative_errors_dict['sequential_relations']['rotation']['max'] = float(metric_evaluator_ordered_r_results_df['Max'][0])
relative_errors_dict['sequential_relations']['rotation']['n'] = float(metric_evaluator_ordered_r_results_df['NumMeasures'][0])
return relative_errors_dict
def absolute_error_vs_scan_range(estimated_poses_file_path, ground_truth_poses_file_path, scans_file_path, samples_per_second=1.0):
# check required files exist
if not path.isfile(estimated_poses_file_path):
print_error("absolute_localization_error_metrics: estimated_poses file not found {}".format(estimated_poses_file_path))
return
if not path.isfile(ground_truth_poses_file_path):
print_error("absolute_localization_error_metrics: ground_truth_poses file not found {}".format(ground_truth_poses_file_path))
return
if not path.isfile(scans_file_path):
print_error("absolute_localization_error_metrics: scans_file_path file not found {}".format(scans_file_path))
return
start_time = time.time()
estimated_poses_df = pd.read_csv(estimated_poses_file_path)
if len(estimated_poses_df.index) < 2:
print_error("not enough estimated poses data points")
return
print_info("estimated_poses_df", time.time() - start_time)
with open(scans_file_path) as scans_file:
scan_lines = scans_file.read().split('\n')
start_time = time.time()
scans_df = pd.DataFrame(columns=['t', 'min_range', 'max_range', 'median_range', 'num_valid_ranges'])
for scan_line in scan_lines:
scan_fields = scan_line.split(', ')
if len(scan_fields) > 1:
t, angle_min, angle_max, angle_increment, range_min, range_max = map(float, scan_fields[0:6])
ranges = map(float, scan_fields[6:])
num_valid_ranges = sum(map(lambda r: range_min < r < range_max, ranges))
record = {
't': t,
'min_range': min(ranges),
'max_range': max(ranges),
'median_range': np.median(ranges),
'num_valid_ranges': num_valid_ranges,
'sensor_range_min': range_min,
'sensor_range_max': range_max,
'num_ranges': len(ranges),
'angle_increment': angle_increment,
'fov_rad': angle_max - angle_min
}
scans_df = scans_df.append(record, ignore_index=True)
print_info("scans_df", time.time() - start_time)
# print(scans_df)
start_time = time.time()
ground_truth_poses_df = pd.read_csv(ground_truth_poses_file_path)
print_info("ground_truth_poses_df", time.time() - start_time)
start_time = time.time()
ground_truth_interpolated_poses_file_path = path.splitext(estimated_poses_file_path)[0] + "ground_truth_interpolated.csv"
if path.exists(ground_truth_interpolated_poses_file_path):
complete_trajectory_df = pd.read_csv(ground_truth_interpolated_poses_file_path)
else:
complete_trajectory_df = compute_ground_truth_interpolated_poses(estimated_poses_df, ground_truth_poses_df)
complete_trajectory_df.to_csv(ground_truth_interpolated_poses_file_path)
run_duration = ground_truth_poses_df.iloc[-1]['t'] - ground_truth_poses_df.iloc[0]['t']
resample_n = int(run_duration*samples_per_second)
resample_ratio = max(1, len(complete_trajectory_df) // resample_n)
trajectory_df = complete_trajectory_df.iloc[::resample_ratio].copy(deep=False)
trajectory_df['abs_err'] = np.sqrt((trajectory_df['x_est'] - trajectory_df['x_gt'])**2 + (trajectory_df['y_est'] - trajectory_df['y_gt'])**2)
# print(trajectory_df)
print_info("trajectory_df", time.time() - start_time)
start_time = time.time()
merge_tolerance = 0.25
tolerance = pd.Timedelta('{}s'.format(merge_tolerance))
trajectory_df['t_datetime'] = pd.to_datetime(trajectory_df['t'], unit='s')
scans_df['t_datetime'] = pd.to_datetime(scans_df['t'], unit='s')
near_matches_df = pd.merge_asof(
left=scans_df,
right=trajectory_df,
on='t_datetime',
direction='nearest',
tolerance=tolerance,
suffixes=('_scan', '_gt')
)
trajectory_and_scan_df = near_matches_df[(pd.notnull(near_matches_df['t_scan'])) & (pd.notnull(near_matches_df['t_gt']))].copy(deep=False)
print_info("trajectory_and_scan_df", time.time() - start_time)
# import matplotlib.pyplot as plt
# plt.scatter(trajectory_and_scan_df['min_range'], trajectory_and_scan_df['abs_err']) # , color='red', s=4.0, marker='o')
# plt.xlabel("min_range")
# plt.ylabel("abs_err")
# plt.show()
# plt.cla()
#
# plt.plot(trajectory_and_scan_df['t_gt'], trajectory_and_scan_df['abs_err'])
# plt.plot(trajectory_and_scan_df['t_scan'], trajectory_and_scan_df['min_range']/trajectory_and_scan_df['sensor_range_max'])
# plt.plot(trajectory_and_scan_df['t_scan'], trajectory_and_scan_df['num_valid_ranges']/trajectory_and_scan_df['num_ranges'])
# plt.legend()
# plt.show()
# plt.cla()
return trajectory_and_scan_df
def __init__(self, run_id, run_output_folder, benchmark_log_path, environment_folder, parameters_combination_dict, benchmark_configuration_dict, show_ros_info, headless):
# print("PRINT:\n")
# print(run_id,'\n') #exmaple: 0
# print(run_output_folder,'\n') #exmaple: /home/furkan/ds/performance_modelling/output/test_planning/session_2020-09-30_17-09-01_964405_run_000000000
# print(benchmark_log_path,'\n') #exmaple: /home/furkan/ds/performance_modelling/output/test_planning/benchmark_log.csv
# print(environment_folder,'\n') #exmaple: /home/furkan/ds/performance_modelling/test_datasets/dataset/airlab
# print(parameters_combination_dict,'\n') #exmaple: {'use_dijkstra': True, 'environment_name': 'airlab', 'global_planner_name': 'GlobalPlanner'}
# print(benchmark_configuration_dict,'\n') #exmaple: {'components_configurations_folder': '~/turtlebot3_melodic_ws/src/global_planning_performance_modelling/config/component_configurations',
# # 'supervisor_component': 'global_planning_benchmark_supervisor',
# # 'components_configuration': {'move_base': {'GlobalPlanner': 'move_base/globalPlanner.yaml'},
# # 'supervisor': 'global_planning_benchmark_supervisor/global_planning_benchmark_supervisor.yaml',
# # 'rviz': 'rviz/default_view.rviz'},
# # 'combinatorial_parameters': {'use_dijkstra': [True, False], 'environment_name': ['airlab'], 'global_planner_name': ['GlobalPlanner']}}
# print(show_ros_info,'\n') #exmaple: False
# print(headless,'\n') #exmaple: False
# run configuration
self.run_id = run_id
self.run_output_folder = run_output_folder
self.benchmark_log_path = benchmark_log_path
self.run_parameters = parameters_combination_dict
self.benchmark_configuration = benchmark_configuration_dict
self.components_ros_output = 'screen' if show_ros_info else 'log'
self.headless = headless
# environment parameters
self.environment_folder = environment_folder
self.map_info_file_path = path.join(environment_folder, "data", "map.yaml")
# take run parameters from parameters_combination_dictionary
global_planner_name = self.run_parameters['global_planner_name']
if global_planner_name == 'GlobalPlanner':
robot_kinematic = self.run_parameters['robot_kinematic']
robot_radius = self.run_parameters['robot_radius']
robot_major_radius = self.run_parameters['robot_major_radius']
planner_type = self.run_parameters['planner_type']
if planner_type == 'dijkstra':
use_dijkstra = True
else:
use_dijkstra = False
#use_dijkstra = self.run_parameters['use_dijkstra']
lethal_cost = self.run_parameters['lethal_cost']
elif global_planner_name == 'SBPLLatticePlanner':
sbpl_primitives_directory_path = path.expanduser(self.benchmark_configuration['sbpl_primitives_path'])
planner_type = self.run_parameters['planner_type']
robot_kinematic = self.run_parameters['robot_kinematic']
robot_radius = self.run_parameters['robot_radius']
robot_major_radius = self.run_parameters['robot_major_radius']
resolution = self.run_parameters['resolution']
if resolution == 0.05:
resolution_str = "005"
elif resolution == 0.1:
resolution_str = "01"
elif resolution == 0.25:
resolution_str = "025"
else:
raise ValueError()
if robot_kinematic == 'unicycle':
primitives_per_angle = self.run_parameters['primitives_per_angle']
sbpl_primitives_name ="res" + str(resolution_str) + robot_kinematic + "16prim" + str(primitives_per_angle) + ".mprim"
elif robot_kinematic == 'bicycle':
direction = self.run_parameters['direction']
max_steering_angle = self.run_parameters['max_steering_angle']
sbpl_primitives_name = "res" + str(resolution_str) + robot_kinematic + "16prim" + str(max_steering_angle) + str(direction) + ".mprim"
else:
print_error("Wrong robot kinematic model defined. Only unicycle or bicycle model can be used.")
raise ValueError()
sbpl_primitives_file_path = path.join(sbpl_primitives_directory_path, sbpl_primitives_name)
elif global_planner_name == 'OmplGlobalPlanner':
robot_kinematic = self.run_parameters['robot_kinematic']
robot_radius = self.run_parameters['robot_radius']
robot_major_radius = self.run_parameters['robot_major_radius']
planner_type = self.run_parameters['planner_type']
lethal_cost = self.run_parameters['lethal_cost']
time_out = self.run_parameters['time_out']
# TODO add some parameters for OMPL
else:
raise ValueError()
# run variables
self.aborted = False
# prepare folder structure
run_configuration_path = path.join(self.run_output_folder, "components_configuration")
run_info_file_path = path.join(self.run_output_folder, "run_info.yaml")
source_workspace_path = self.benchmark_configuration['source_workspace_path']
software_versions_log_path = path.join(self.run_output_folder, "software_versions_log")
backup_file_if_exists(self.run_output_folder)
os.mkdir(self.run_output_folder)
os.mkdir(run_configuration_path)
# benchmark_configuration_parameters
self.run_timeout = self.benchmark_configuration['run_timeout']
self.robot_footprint_turtlebot3 = self.benchmark_configuration['robot_model_turtlebot3']
self.robot_footprint_agilex_hunter = self.benchmark_configuration['robot_model_agilex_hunter']
# components original configuration paths (inside your workspace path)
components_configurations_folder = path.expanduser(self.benchmark_configuration['components_configurations_folder'])
original_supervisor_configuration_path = path.join(components_configurations_folder, self.benchmark_configuration['components_configuration']['supervisor'])
original_move_base_configuration_path = path.join(components_configurations_folder, self.benchmark_configuration['components_configuration']['move_base'])
if global_planner_name == 'GlobalPlanner':
original_move_base_global_planner_configuration_path = path.join(components_configurations_folder, self.benchmark_configuration['components_configuration']['move_base_global_planner'])
elif global_planner_name == 'SBPLLatticePlanner':
original_move_base_global_planner_configuration_path = path.join(components_configurations_folder, self.benchmark_configuration[ 'components_configuration']['move_base_sbpl_planner'])
elif global_planner_name == 'OmplGlobalPlanner':
original_move_base_global_planner_configuration_path = path.join(components_configurations_folder, self.benchmark_configuration['components_configuration']['move_base_ompl_planner'])
else:
raise ValueError()
self.original_rviz_configuration_path = path.join(components_configurations_folder, self.benchmark_configuration['components_configuration']['rviz'])
original_robot_urdf_path = path.join(environment_folder, "gazebo", "robot.urdf")
# components configuration relative paths
supervisor_configuration_relative_path = path.join("components_configuration", self.benchmark_configuration['components_configuration']['supervisor'])
move_base_configuration_relative_path = path.join("components_configuration", self.benchmark_configuration['components_configuration']['move_base'])
if global_planner_name == 'GlobalPlanner':
move_base_global_planner_configuration_relative_path = path.join("components_configuration", self.benchmark_configuration['components_configuration']['move_base_global_planner'])
elif global_planner_name == 'SBPLLatticePlanner':
move_base_global_planner_configuration_relative_path = path.join("components_configuration", self.benchmark_configuration[ 'components_configuration']['move_base_sbpl_planner'])
elif global_planner_name == 'OmplGlobalPlanner':
move_base_global_planner_configuration_relative_path = path.join("components_configuration", self.benchmark_configuration['components_configuration']['move_base_ompl_planner'])
else:
raise ValueError()
robot_realistic_urdf_relative_path = path.join("components_configuration", "gazebo", "robot_realistic.urdf")
# components configuration paths in run folder (inside ds output file path)
self.supervisor_configuration_path = path.join(self.run_output_folder, supervisor_configuration_relative_path)
self.move_base_configuration_path = path.join(self.run_output_folder, move_base_configuration_relative_path)
self.move_base_global_planner_configuration_path = path.join(self.run_output_folder, move_base_global_planner_configuration_relative_path)
self.robot_realistic_urdf_path = path.join(self.run_output_folder, robot_realistic_urdf_relative_path)
# copy the configuration of the supervisor to the run folder and update its parameters
with open(original_supervisor_configuration_path) as supervisor_configuration_file:
supervisor_configuration = yaml.load(supervisor_configuration_file)
supervisor_configuration['run_output_folder'] = self.run_output_folder
supervisor_configuration['pid_father'] = os.getpid()
supervisor_configuration['ground_truth_map_info_path'] = self.map_info_file_path
supervisor_configuration['run_timeout'] = self.run_timeout
# copy the configuration of move_base to the run folder
# move_base global planner config
with open(original_move_base_global_planner_configuration_path) as move_base_global_planner_configuration_file:
move_base_global_planner_configuration = yaml.load(move_base_global_planner_configuration_file)
move_base_global_planner_configuration['planner_patience'] = self.run_timeout
move_base_global_planner_configuration['controller_patience'] = self.run_timeout
if robot_kinematic == 'unicycle':
move_base_global_planner_configuration['footprint'] = self.robot_footprint_turtlebot3
elif robot_kinematic == 'bicycle':
move_base_global_planner_configuration['footprint'] = self.robot_footprint_agilex_hunter
else:
raise ValueError()
if global_planner_name == 'GlobalPlanner':
supervisor_configuration['robot_kinematic'] = robot_kinematic
supervisor_configuration['robot_radius'] = robot_radius
supervisor_configuration['robot_major_radius'] = robot_major_radius
move_base_global_planner_configuration['GlobalPlanner']['use_dijkstra'] = use_dijkstra
move_base_global_planner_configuration['GlobalPlanner']['use_grid_path'] = not use_dijkstra
move_base_global_planner_configuration['GlobalPlanner']['lethal_cost'] = lethal_cost
# todo we can add neutral_cost and cost_factor
elif global_planner_name == 'SBPLLatticePlanner':
supervisor_configuration['robot_kinematic'] = robot_kinematic
supervisor_configuration['robot_radius'] = robot_radius
supervisor_configuration['robot_major_radius'] = robot_major_radius
move_base_global_planner_configuration['SBPLLatticePlanner']['planner_type'] = planner_type
move_base_global_planner_configuration['SBPLLatticePlanner']['primitive_filename'] = sbpl_primitives_file_path
# todo
elif global_planner_name == 'OmplGlobalPlanner':
supervisor_configuration['robot_kinematic'] = robot_kinematic
supervisor_configuration['robot_radius'] = robot_radius
supervisor_configuration['robot_major_radius'] = robot_major_radius
move_base_global_planner_configuration['OmplGlobalPlanner']['planner_type'] = planner_type
move_base_global_planner_configuration['OmplGlobalPlanner']['lethal_cost'] = lethal_cost
move_base_global_planner_configuration['OmplGlobalPlanner']['time_out'] = time_out
else:
raise ValueError()
#move_base global config
if not path.exists(path.dirname(self.move_base_global_planner_configuration_path)):
os.makedirs(path.dirname(self.move_base_global_planner_configuration_path))
with open(self.move_base_global_planner_configuration_path, 'w') as move_base_global_planner_configuration_file:
yaml.dump(move_base_global_planner_configuration, move_base_global_planner_configuration_file, default_flow_style=False)
#supervisor global config
if not path.exists(path.dirname(self.supervisor_configuration_path)):
os.makedirs(path.dirname(self.supervisor_configuration_path))
with open(self.supervisor_configuration_path, 'w') as supervisor_configuration_file:
yaml.dump(supervisor_configuration, supervisor_configuration_file, default_flow_style=False)
# move_base general config (costmaps, local_planner, etc)
if not path.exists(path.dirname(self.move_base_configuration_path)):
os.makedirs(path.dirname(self.move_base_configuration_path))
shutil.copyfile(original_move_base_configuration_path, self.move_base_configuration_path)
# copy the configuration of the robot urdf to the run folder and update the link names for realistic data
if not path.exists(path.dirname(self.robot_realistic_urdf_path)):
os.makedirs(path.dirname(self.robot_realistic_urdf_path))
shutil.copyfile(original_robot_urdf_path, self.robot_realistic_urdf_path)
# write run info to file
run_info_dict = dict()
run_info_dict["run_id"] = self.run_id
run_info_dict["run_folder"] = self.run_output_folder
run_info_dict["environment_folder"] = environment_folder
run_info_dict["run_parameters"] = self.run_parameters
run_info_dict["local_components_configuration"] = {
'supervisor': supervisor_configuration_relative_path,
'move_base': move_base_configuration_relative_path,
'move_base_global_planner': move_base_global_planner_configuration_relative_path,
'robot_realistic_urdf': robot_realistic_urdf_relative_path,
}
with open(run_info_file_path, 'w') as run_info_file:
yaml.dump(run_info_dict, run_info_file, default_flow_style=False)
# log packages and software versions and status
log_packages_and_repos(source_workspace_path=source_workspace_path, log_dir_path=software_versions_log_path)
def explored_area_metrics(map_file_path, map_info_file_path,
ground_truth_map_file_path,
ground_truth_map_info_file_path):
# check required files exist
if not path.isfile(map_file_path):
print_error(
"compute_map_metrics: map file not found {}".format(map_file_path))
return
if not path.isfile(map_info_file_path):
print_error("compute_map_metrics: map_info file not found {}".format(
map_info_file_path))
return
if not path.isfile(ground_truth_map_file_path):
print_error(
"compute_map_metrics: ground_truth_map file not found {}".format(
ground_truth_map_file_path))
return
if not path.isfile(ground_truth_map_info_file_path):
print_error(
"compute_map_metrics: ground_truth_map_info file not found {}".
format(ground_truth_map_info_file_path))
return
with open(ground_truth_map_info_file_path) as stage_info_file:
stage_info_yaml = yaml.load(stage_info_file)
ground_truth_map = Image.open(ground_truth_map_file_path)
ground_truth_map_size_meters = np.array([
float(stage_info_yaml['map']['size']['x']),
float(stage_info_yaml['map']['size']['y'])
])
ground_truth_map_size_pixels = np.array(ground_truth_map.size)
ground_truth_resolution = ground_truth_map_size_meters / ground_truth_map_size_pixels # meter/pixel, on both axis, except y axis is inverted in image
ground_truth_cell_area = float(
ground_truth_resolution[0] *
ground_truth_resolution[1]) # width × height of one pixel, meters^2
ground_truth_free_cell_count = 0
ground_truth_occupied_cell_count = 0
ground_truth_unknown_cell_count = 0
ground_truth_total_cell_count = ground_truth_map.size[
0] * ground_truth_map.size[1]
ground_truth_pixels = ground_truth_map.load()
for i in range(ground_truth_map.size[0]):
for j in range(ground_truth_map.size[1]):
ground_truth_free_cell_count += ground_truth_pixels[i, j] == (254,
254,
254)
ground_truth_occupied_cell_count += ground_truth_pixels[i,
j] == (0,
0,
0)
ground_truth_unknown_cell_count += ground_truth_pixels[i,
j] == (205,
205,
205)
explorable_area = ground_truth_free_cell_count * ground_truth_cell_area
result_map = Image.open(map_file_path)
with open(map_info_file_path) as result_map_info_file:
result_map_info_yaml = yaml.load(result_map_info_file)
# ground_truth_map_size_pixels = np.array(result_map.size)
result_map_resolution = float(result_map_info_yaml['info']
['resolution']) # meter/pixel, on both axis
result_cell_area = result_map_resolution**2 # length of one pixel squared, meters^2
result_free_cell_count = 0
result_occupied_cell_count = 0
result_unknown_cell_count = 0
result_total_cell_count = result_map.size[0] * result_map.size[1]
result_map_pixels = result_map.load()
for i in range(result_map.size[0]):
for j in range(result_map.size[1]):
result_free_cell_count += result_map_pixels[i, j] == 254
result_occupied_cell_count += result_map_pixels[i, j] == 0
result_unknown_cell_count += result_map_pixels[i, j] == 205
explored_area = result_free_cell_count * result_cell_area
explored_area_dict = {
'result_map': {
'count': {
'free': result_free_cell_count,
'occupied': result_occupied_cell_count,
'unknown': result_unknown_cell_count,
'total': result_total_cell_count,
},
'area': {
'free': explored_area,
'occupied': result_occupied_cell_count * result_cell_area,
'unknown': result_unknown_cell_count * result_cell_area,
'total': result_total_cell_count * result_cell_area,
},
},
'ground_truth_map': {
'count': {
'free': ground_truth_free_cell_count,
'occupied': ground_truth_occupied_cell_count,
'unknown': ground_truth_unknown_cell_count,
'total': ground_truth_total_cell_count,
},
'area': {
'free': explorable_area,
'occupied':
ground_truth_occupied_cell_count * ground_truth_cell_area,
'unknown':
ground_truth_unknown_cell_count * ground_truth_cell_area,
'total':
ground_truth_total_cell_count * ground_truth_cell_area,
},
},
'normalised_explored_area': float(explored_area / explorable_area)
}
return explored_area_dict
def execute_run(self):
# components parameters
rviz_params = {
'rviz_config_file': self.original_rviz_configuration_path,
}
environment_params = {
'gazebo_model_path_env_var': self.gazebo_model_path_env_var,
'gazebo_plugin_path_env_var': self.gazebo_plugin_path_env_var,
'world_model_file': self.gazebo_world_model_path,
'robot_gt_urdf_file': self.robot_gt_urdf_path,
'robot_realistic_urdf_file': self.robot_realistic_urdf_path,
'headless': self.headless,
}
localization_params = {
'params_file': self.nav2_amcl_configuration_path,
'map': self.map_info_file_path,
'use_sim_time': self.use_sim_time,
}
navigation_params = {
'params_file': self.nav2_navigation_configuration_path,
'use_sim_time': self.use_sim_time,
'map_subscribe_transient_local': True,
}
supervisor_params = {
'configuration': self.supervisor_configuration_path,
'use_sim_time': self.use_sim_time
}
# declare components
rviz = Component('rviz', 'localization_performance_modelling',
'rviz.launch.py', rviz_params)
# recorder = Component('recorder', 'localization_performance_modelling', 'rosbag_recorder.launch.py', recorder_params)
environment = Component('gazebo', 'localization_performance_modelling',
'gazebo.launch.py', environment_params)
localization = Component('nav2_amcl',
'localization_performance_modelling',
'nav2_amcl.launch.py', localization_params)
navigation = Component('nav2_navigation',
'localization_performance_modelling',
'nav2_navigation.launch.py', navigation_params)
supervisor = Component('supervisor',
'localization_performance_modelling',
'localization_benchmark_supervisor.launch.py',
supervisor_params)
# TODO manage launch exceptions in Component.__init__
# add components to launcher
components_launcher = ComponentsLauncher()
if not self.headless:
components_launcher.add_component(rviz)
# recorder.launch()
components_launcher.add_component(environment)
components_launcher.add_component(navigation)
components_launcher.add_component(localization)
components_launcher.add_component(supervisor)
# launch components and wait for the supervisor to finish
self.log(event="waiting_supervisor_finish")
components_launcher.launch()
self.log(event="supervisor_shutdown")
# make sure remaining components have shutdown
components_launcher.shutdown()
print_info("execute_run: components shutdown completed")
# compute all relevant metrics and visualisations
# noinspection PyBroadException
try:
self.log(event="start_compute_metrics")
compute_metrics(self.run_output_folder)
except:
print_error("failed metrics computation")
print_error(traceback.format_exc())
self.log(event="run_end")
print_info(f"run {self.run_id} completed")
def absolute_error_vs_geometric_similarity(estimated_poses_file_path, ground_truth_poses_file_path, ground_truth_map, horizon_length=3.5, samples_per_second=10.0, max_iterations=20):
# import matplotlib.pyplot as plt
from icp import iterative_closest_point
from skimage.draw import line, circle_perimeter
# plt.rcParams["figure.figsize"] = (10, 10)
start_time = time.time()
estimated_poses_df = pd.read_csv(estimated_poses_file_path)
if len(estimated_poses_df.index) < 2:
print_error("not enough estimated poses data points")
return
print_info("estimated_poses_df", time.time() - start_time)
start_time = time.time()
ground_truth_poses_df = pd.read_csv(ground_truth_poses_file_path)
print_info("ground_truth_poses_df", time.time() - start_time)
start_time = time.time()
ground_truth_interpolated_poses_file_path = path.splitext(estimated_poses_file_path)[0] + "ground_truth_interpolated.csv"
if path.exists(ground_truth_interpolated_poses_file_path):
print_info("using cached ground_truth_interpolated_poses [{}]".format(ground_truth_interpolated_poses_file_path))
complete_trajectory_df = pd.read_csv(ground_truth_interpolated_poses_file_path)
else:
complete_trajectory_df = compute_ground_truth_interpolated_poses(estimated_poses_df, ground_truth_poses_df)
complete_trajectory_df.to_csv(ground_truth_interpolated_poses_file_path)
run_duration = ground_truth_poses_df.iloc[-1]['t'] - ground_truth_poses_df.iloc[0]['t']
resample_n = int(run_duration*samples_per_second)
resample_ratio = max(1, len(complete_trajectory_df) // resample_n)
trajectory_df = complete_trajectory_df.iloc[::resample_ratio].copy(deep=False)
trajectory_df['abs_err'] = np.sqrt((trajectory_df['x_est'] - trajectory_df['x_gt'])**2 + (trajectory_df['y_est'] - trajectory_df['y_gt'])**2)
print_info("trajectory_df", time.time() - start_time)
delta_length = 0.2 # displacement of x and x_prime
ic = ground_truth_map.map_frame_to_image_coordinates
mf = ground_truth_map.image_to_map_frame_coordinates
map_image_array = np.array(ground_truth_map.map_image.convert(mode="L")).transpose()
occupancy_grid = map_image_array == 0
translation_score_0_column = list()
translation_score_column = list()
rotation_score_0_column = list()
rotation_score_column = list()
plot_lines = list()
icp_time = 0
ray_tracing_time = 0
rows_count = len(trajectory_df[['x_gt', 'y_gt']])
progress_count = 0
for i, row_df in trajectory_df[['x_gt', 'y_gt']].iterrows():
x_mf = np.array(row_df)
p_x, p_y = ic(x_mf)
ray_tracing_start_time = time.time()
visible_points_x = set()
perimeter_points = np.array(circle_perimeter(p_x, p_y, int((horizon_length + delta_length)/ground_truth_map.resolution), method='andres')).transpose()
for perimeter_x, perimeter_y in perimeter_points:
ray_points = np.array(line(p_x, p_y, perimeter_x, perimeter_y)).transpose()
for ray_x, ray_y in ray_points:
if occupancy_grid[ray_x, ray_y]:
visible_points_x.add((ray_x, ray_y))
break
visible_points_x_mf = np.array(list(map(mf, visible_points_x)))
if len(visible_points_x_mf) == 0:
translation_score_0_column.append(np.nan)
rotation_score_0_column.append(np.nan)
translation_score_column.append(np.nan)
rotation_score_column.append(np.nan)
continue
visible_points_x_mf_o = visible_points_x_mf - x_mf
ray_tracing_time += time.time() - ray_tracing_start_time
delta_trans_list = delta_length * np.array([
(1, 0),
(0, 1),
(-1, 0),
(0, -1),
(.7, .7),
(.7, -.7),
(-.7, .7),
(-.7, -.7),
])
delta_theta_list = [-0.1, -0.05, 0.05, 0.1]
translation_score_0_list = list()
translation_score_list = list()
for delta_trans in delta_trans_list:
x_prime_mf = x_mf + delta_trans
p_x_prime, p_y_prime = ic(x_prime_mf)
ray_tracing_start_time = time.time()
visible_points_x_prime = set()
perimeter_points_prime = np.array(circle_perimeter(p_x_prime, p_y_prime, int(horizon_length / ground_truth_map.resolution), method='andres')).transpose()
for perimeter_x_prime, perimeter_y_prime in perimeter_points_prime:
ray_points_prime = np.array(line(p_x_prime, p_y_prime, perimeter_x_prime, perimeter_y_prime)).transpose()
for ray_x_prime, ray_y_prime in ray_points_prime:
if occupancy_grid[ray_x_prime, ray_y_prime]:
visible_points_x_prime.add((ray_x_prime, ray_y_prime))
break
visible_points_x_prime_mf = np.array(list(map(mf, visible_points_x_prime)))
ray_tracing_time += time.time() - ray_tracing_start_time
if len(visible_points_x_prime_mf) == 0:
continue
visible_points_x_prime_mf_o = visible_points_x_prime_mf - x_prime_mf
start_time = time.time()
transform, scores, transformed_points_prime, num_iterations = iterative_closest_point(visible_points_x_prime_mf_o, visible_points_x_mf_o, max_iterations=max_iterations, tolerance=0.001)
translation_score_0_list.append(scores[0])
translation = transform[:2, 2]
translation_score = float(np.sqrt(np.sum((translation - delta_trans) ** 2)) / np.sqrt(np.sum(delta_trans ** 2)))
# print("translation score: {s:1.4f} \t translation score_0: {s0:1.4f} \t i: {i}".format(s=translation_score, s0=scores[0], i=num_iterations))
translation_score_list.append(translation_score)
icp_time += time.time() - start_time
# if translation_score > 0.5:
# plt.scatter(*visible_points_x_mf_o.transpose(), color='black', marker='x')
# plt.scatter(*visible_points_x_prime_mf_o.transpose(), color='red', marker='x')
# plt.scatter(*transformed_points_prime.transpose(), color='blue', marker='x')
# hl = horizon_length + delta_length
# plt.xlim(-hl, hl)
# plt.ylim(-hl, hl)
# plt.gca().set_aspect('equal', adjustable='box')
# plt.show()
plot_lines.append((x_mf, x_prime_mf, translation_score))
if len(translation_score_0_list) == 0:
translation_score_0_column.append(np.nan)
translation_score_column.append(np.nan)
else:
translation_score_0_column.append(np.mean(translation_score_0_list))
translation_score_column.append(np.mean(translation_score_list))
rotation_score_0_list = list()
rotation_score_list = list()
for delta_theta in delta_theta_list:
rot_mat = np.array([
[np.cos(delta_theta), np.sin(delta_theta)],
[-np.sin(delta_theta), np.cos(delta_theta)]
])
visible_points_x_prime_mf_o = np.dot(visible_points_x_mf_o, rot_mat)
start_time = time.time()
transform, scores, transformed_points_prime, num_iterations = iterative_closest_point(visible_points_x_prime_mf_o, visible_points_x_mf_o, max_iterations=max_iterations, tolerance=0.001)
rotation_score_0_list.append(scores[0])
rotation_mat = transform[:2, :2].T
rotation = math.atan2(rotation_mat[0, 1], rotation_mat[0, 0])
rotation_score = float(np.fabs(rotation + delta_theta)) / np.fabs(delta_theta)
# print("rotation_score: {s:1.4f} \t rotation_score_0: {s0:1.4f} \t i: {i} \t transform angle: {r:1.4f} \t delta theta: {dt:1.4f}".format(s=rotation_score, s0=scores[0], i=num_iterations, r=rotation, dt=delta_theta))
rotation_score_list.append(rotation_score)
icp_time += time.time() - start_time
# if rotation_score > 0.5:
# plt.scatter(*visible_points_x_mf_o.transpose(), color='black', marker='x')
# plt.scatter(*visible_points_x_prime_mf_o.transpose(), color='red', marker='x')
# plt.scatter(*transformed_points_prime.transpose(), color='blue', marker='x')
# hl = horizon_length + delta_length
# plt.xlim(-hl, hl)
# plt.ylim(-hl, hl)
# plt.gca().set_aspect('equal', adjustable='box')
# plt.show()
# plot_lines.append((x_mf, x_prime_mf, translation_score))
if len(rotation_score_0_list) == 0:
rotation_score_0_column.append(np.nan)
rotation_score_column.append(np.nan)
else:
rotation_score_0_column.append(np.mean(rotation_score_0_list))
rotation_score_column.append(np.mean(rotation_score_list))
progress_count += 1
print_info(int(progress_count / float(rows_count) * 100), "%", replace_previous_line=True)
trajectory_df['translation_score_0'] = translation_score_0_column
trajectory_df['translation_score'] = translation_score_column
trajectory_df['rotation_score_0'] = rotation_score_0_column
trajectory_df['rotation_score'] = rotation_score_column
# print(trajectory_df)
print_info("icp_time", icp_time)
print_info("ray_tracing_time", ray_tracing_time)
# for (x_0, y_0), (x_1, y_1), score in plot_lines:
# plt.plot([x_0, x_1], [y_0, y_1], linewidth=3*score, color='black')
# plt.plot([trajectory_df['x_gt'], trajectory_df['x_est']], [trajectory_df['y_gt'], trajectory_df['y_est']], color='red')
# plt.show()
# plt.plot(trajectory_df['t'], trajectory_df['abs_err'], label='abs_err')
# plt.plot(trajectory_df['t'], trajectory_df['rotation_score_0'], label='rotation_score_0')
# plt.legend()
# plt.show()
#
# plt.scatter(trajectory_df['rotation_score_0'], trajectory_df['abs_err'])
# plt.xlabel("rotation_score_0")
# plt.ylabel("abs_err")
# plt.legend()
# plt.show()
#
# plt.plot(trajectory_df['t'], trajectory_df['abs_err'], label='abs_err')
# plt.plot(trajectory_df['t'], trajectory_df['rotation_score'], label='rotation_score')
# plt.legend()
# plt.show()
#
# plt.scatter(trajectory_df['rotation_score'], trajectory_df['abs_err'], label='abs_err vs rotation_score')
# plt.xlabel("rotation_score")
# plt.ylabel("abs_err")
# plt.legend()
# plt.show()
#
# plt.plot(trajectory_df['t'], trajectory_df['abs_err'], label='abs_err')
# plt.plot(trajectory_df['t'], trajectory_df['translation_score_0'], label='translation_score_0')
# plt.plot(trajectory_df['t'], trajectory_df['translation_score'], label='translation_score')
# plt.legend()
# plt.show()
#
# plt.scatter(trajectory_df['translation_score_0'], trajectory_df['abs_err'], label='abs_err vs translation_score_0')
# plt.xlabel("translation_score_0")
# plt.ylabel("abs_err")
# plt.legend()
# plt.show()
#
# plt.scatter(trajectory_df['translation_score'], trajectory_df['abs_err'], label='abs_err vs translation_score')
# plt.xlabel("translation_score")
# plt.ylabel("abs_err")
# plt.legend()
# plt.show()
return trajectory_df
'-t',
dest='gazebo_template_path',
help='Folder containing the gazebo template. Example: {}'.format(
default_gazebo_template_path),
type=str,
required=True)
args = parser.parse_args()
gazebo_template_path = path.expanduser(args.gazebo_template_path)
target_dataset_path = path.expanduser(args.target_dataset_path)
source_environment_paths = filter(path.isdir,
glob.glob(target_dataset_path))
if not path.exists(gazebo_template_path):
print_error("gazebo_template_path does not exists:",
gazebo_template_path)
sys.exit(-1)
for target_environment_path in source_environment_paths:
environment_name = path.basename(target_environment_path)
print("environment_name", environment_name)
print("target_environment_path", target_environment_path)
gazebo_file_relative_paths = map(
lambda ap: path.relpath(ap, gazebo_template_path),
filter(path.isfile,
glob.glob(gazebo_template_path + '/**', recursive=True)))
for gazebo_file_relative_path in gazebo_file_relative_paths:
source_gazebo_file_path = path.join(gazebo_template_path,
gazebo_file_relative_path)
def execute_run(self):
# components parameters
rviz_params = {
'rviz_config_file': self.original_rviz_configuration_path,
'headless': self.headless,
'output': "log"
}
state_publisher_param = {
'robot_realistic_urdf_file': self.robot_realistic_urdf_path,
'output': "log"
}
navigation_params = {
'params_file': self.move_base_configuration_path,
'global_planner_params_file': self.move_base_global_planner_configuration_path,
'map_file': self.map_info_file_path,
'output': "log"
}
supervisor_params = {
'params_file': self.supervisor_configuration_path,
'output': "screen"
}
recorder_params = {
'bag_file_path': path.join(self.run_output_folder, "benchmark_data.bag"),
'not_recorded_topics' : "/move_base/DWAPlannerROS/(.*)",
'output': "log"
}
# recorder_params2 = {
# 'bag_file_path': path.join(self.run_output_folder, "benchmark_data2.bag"),
# 'topics': "/cmd_vel /initialpose /map /map_metadata /map_updates /move_base/DWAPlannerROS/cost_cloud /move_base/DWAPlannerROS/global_plan \
# /move_base/DWAPlannerROS/local_plan /move_base/DWAPlannerROS/parameter_descriptions /move_base/DWAPlannerROS/parameter_updates \
# /move_base/DWAPlannerROS/trajectory_cloud /move_base/(.*)/plan /move_base/SBPLLatticePlanner/sbpl_lattice_planner_stats /move_base/cancel /move_base/current_goal \
# /move_base/feedback /move_base/global_costmap/costmap /move_base/global_costmap/costmap_updates /move_base/global_costmap/footprint \
# /move_base/global_costmap/inflation_layer/parameter_descriptions /move_base/global_costmap/inflation_layer/parameter_updates \
# /move_base/global_costmap/parameter_descriptions /move_base/global_costmap/parameter_updates /move_base/global_costmap/static_layer/parameter_descriptions \
# /move_base/global_costmap/static_layer/parameter_updates /move_base/goal /move_base/local_costmap/costmap /move_base/local_costmap/costmap_updates \
# /move_base/local_costmap/footprint /move_base/local_costmap/inflation_layer/parameter_descriptions /move_base/local_costmap/inflation_layer/parameter_updates \
# /move_base/local_costmap/parameter_descriptions /move_base/local_costmap/parameter_updates /move_base/local_costmap/static_layer/parameter_descriptions \
# /move_base/local_costmap/static_layer/parameter_updates /move_base/parameter_descriptions /move_base/parameter_updates /move_base/result /move_base/status \
# /move_base_simple/goal /odom /rosout /rosout_agg /tf /tf_static",
# 'output': "log"
# }
# declare components
roscore = Component('roscore', 'global_planning_performance_modelling', 'roscore.launch')
state_publisher = Component('state_publisher', 'global_planning_performance_modelling', 'robot_state_launcher.launch', state_publisher_param)
rviz = Component('rviz', 'global_planning_performance_modelling', 'rviz.launch', rviz_params)
navigation = Component('move_base', 'global_planning_performance_modelling', 'move_base.launch', navigation_params)
supervisor = Component('supervisor', 'global_planning_performance_modelling', 'global_planning_benchmark_supervisor.launch', supervisor_params)
recorder = Component('recorder', 'global_planning_performance_modelling', 'rosbag_recorder.launch', recorder_params)
# recorder2 = Component('recorder', 'global_planning_performance_modelling', 'rosbag_recorder2.launch', recorder_params2)
# launch roscore and setup a node to monitor ros
roscore.launch()
rospy.init_node("benchmark_monitor", anonymous=True)
# launch components
rviz.launch()
navigation.launch()
supervisor.launch()
recorder.launch()
# recorder2.launch()
# launch components and wait for the supervisor to finish
self.log(event="waiting_supervisor_finish")
supervisor.wait_to_finish()
self.log(event="supervisor_shutdown")
# check if the rosnode is still ok, otherwise the ros infrastructure has been shutdown and the benchmark is aborted
if rospy.is_shutdown():
print_error("execute_run: supervisor finished by ros_shutdown")
self.aborted = True
# shut down components
navigation.shutdown()
rviz.shutdown()
roscore.shutdown()
recorder.shutdown()
# recorder2.shutdown()
print_info("execute_run: components shutdown completed")
# compute all relevant metrics and visualisations
# try:
# self.log(event="start_compute_metrics")
# compute_metrics(self.run_output_folder) # open here to calculate metric
# except:
# print_error("failed metrics computation")
# print_error(traceback.format_exc())
self.log(event="run_end")
print_info("run {run_id} completed".format(run_id=self.run_id))
def collect_data(base_run_folder_path, invalidate_cache=False):
base_run_folder = path.expanduser(base_run_folder_path)
cache_file_path = path.join(base_run_folder, "run_data_cache.pkl")
if not path.isdir(base_run_folder):
print_error(
"collect_data: base_run_folder does not exists or is not a directory"
.format(base_run_folder))
return None, None
run_folders = sorted(
list(
filter(path.isdir,
glob.glob(path.abspath(base_run_folder) + '/*'))))
if invalidate_cache or not path.exists(cache_file_path):
df = pd.DataFrame()
parameter_names = set()
cached_run_folders = set()
else:
print_info("collect_data: updating cache")
with open(cache_file_path, 'rb') as f:
cache = pickle.load(f)
df = cache['df']
parameter_names = cache['parameter_names']
cached_run_folders = set(df['run_folder'])
# collect results from runs not already cached
print_info("collect_data: reading run data")
no_output = True
for i, run_folder in enumerate(run_folders):
try:
metric_results_folder = path.join(run_folder, "metric_results")
benchmark_data_folder = path.join(run_folder, "benchmark_data")
run_info_file_path = path.join(run_folder, "run_info.yaml")
if not path.exists(metric_results_folder):
print_error(
"collect_data: metric_results_folder does not exists [{}]".
format(metric_results_folder))
no_output = False
continue
if not path.exists(run_info_file_path):
print_error(
"collect_data: run_info file does not exists [{}]".format(
run_info_file_path))
no_output = False
continue
if run_folder in cached_run_folders:
continue
run_info = get_yaml(run_info_file_path)
run_record = dict()
for parameter_name, parameter_value in run_info[
'run_parameters'].items():
parameter_names.add(parameter_name)
if type(parameter_value) == list:
parameter_value = tuple(parameter_value)
run_record[parameter_name] = parameter_value
# parameter_names.add('environment_name') # in my run_info.yaml file inside run_parameters we have environment_name
# run_record['environment_name'] = path.basename(path.abspath(run_info['environment_folder']))
run_record['environment_name'] = run_info['run_parameters'][
'environment_name']
run_record['run_folder'] = run_folder
run_record['failure_rate'] = 0
try:
run_events = pd.read_csv(
path.join(benchmark_data_folder, "run_events.csv"))
metrics_dict = get_yaml(
path.join(metric_results_folder, "metrics.yaml"))
except IOError:
run_record['failure_rate'] = 1
df = df.append(run_record, ignore_index=True)
continue
feasibility_rate = metrics_dict[
'feasibility_rate'] if 'feasibility_rate' in metrics_dict else None
run_record['feasibility_rate'] = feasibility_rate
if feasibility_rate is None:
run_record['failure_rate'] = 1
df = df.append(run_record, ignore_index=True)
continue
if 'average_planning_time' in metrics_dict and metrics_dict[
'average_planning_time'] is not None:
run_record['average_planning_time'] = metrics_dict[
'average_planning_time']
if 'normalised_plan_length' in metrics_dict and metrics_dict[
'normalised_plan_length'] is not None:
run_record['normalised_plan_length'] = metrics_dict[
'normalised_plan_length']
if 'normalised_planning_time' in metrics_dict and metrics_dict[
'normalised_planning_time'] is not None:
run_record['normalised_planning_time'] = metrics_dict[
'normalised_planning_time']
run_record['num_target_pose_set'] = len(
run_events[run_events["event"] == "target_pose_set"])
run_record['num_target_pose_reached'] = len(
run_events[run_events["event"] == "target_pose_reached"])
run_record['num_target_pose_aborted'] = len(
run_events[run_events["event"] == "target_pose_aborted"])
run_record['num_initial_pose_true'] = len(
run_events[run_events["event"] == "initial_pose_true"])
run_record['num_away_from_initial_pose'] = len(
run_events[run_events["event"] == "away_from_initial_pose"])
run_record['num_goal_pose_true'] = len(
run_events[run_events["event"] == "goal_pose_true"])
run_record['num_away_from_goal_pose'] = len(
run_events[run_events["event"] == "away_from_goal_pose"])
run_start_events = run_events["event"] == "run_start"
run_completed_events = run_events["event"] == "run_completed"
if len(run_start_events) == 0 or len(run_completed_events) == 0:
run_record['failure_rate'] = 1
df = df.append(run_record, ignore_index=True)
continue
if len(run_events[run_events["event"] == "run_start"]
["time"]) == 0:
print_error("collect_data: run_start event does not exists")
no_output = False
run_record['failure_rate'] = 1
df = df.append(run_record, ignore_index=True)
continue
if len(run_events[run_events["event"] == "run_completed"]
["time"]) == 0:
print_error(
"collect_data: run_completed event does not exists")
no_output = False
run_record['failure_rate'] = 1
df = df.append(run_record, ignore_index=True)
continue
run_start_time = float(
run_events[run_events["event"] == "run_start"]["time"])
supervisor_finish_time = float(
run_events[run_events["event"] == "run_completed"]["time"])
run_execution_time = supervisor_finish_time - run_start_time
run_record['run_execution_time'] = run_execution_time
# if metrics_dict['cpu_and_memory_usage'] is not None and 'amcl_accumulated_cpu_time' in metrics_dict['cpu_and_memory_usage']:
# run_record['normalised_cpu_time'] = metrics_dict['cpu_and_memory_usage']['amcl_accumulated_cpu_time'] / run_execution_time
#
# if metrics_dict['cpu_and_memory_usage'] is not None and 'amcl_uss' in metrics_dict['cpu_and_memory_usage']:
# run_record['max_memory'] = metrics_dict['cpu_and_memory_usage']['amcl_uss']
#
# if metrics_dict['cpu_and_memory_usage'] is not None and 'localization_slam_toolbox_node_accumulated_cpu_time' in metrics_dict['cpu_and_memory_usage']:
# run_record['normalised_cpu_time'] = metrics_dict['cpu_and_memory_usage']['localization_slam_toolbox_node_accumulated_cpu_time'] / run_execution_time
#
# if metrics_dict['cpu_and_memory_usage'] is not None and 'localization_slam_toolbox_node_uss' in metrics_dict['cpu_and_memory_usage']:
# run_record['max_memory'] = metrics_dict['cpu_and_memory_usage']['localization_slam_toolbox_node_uss']
df = df.append(run_record, ignore_index=True)
print_info("collect_data: reading run data: {}%".format(
int((i + 1) * 100 / len(run_folders))),
replace_previous_line=no_output)
no_output = True
except:
traceback.format_exc()
# save cache
if cache_file_path is not None:
cache = {'df': df, 'parameter_names': parameter_names}
with open(cache_file_path, 'wb') as f:
pickle.dump(cache, f, protocol=2)
return df, parameter_names
def collect_data(base_run_folder_path, invalidate_cache=False):
base_run_folder = path.expanduser(base_run_folder_path)
cache_file_path = path.join(base_run_folder, "run_data_cache.pkl")
if not path.isdir(base_run_folder):
print_error(
"collect_data: base_run_folder does not exists or is not a directory"
.format(base_run_folder))
return None, None
run_folders = sorted(
list(
filter(path.isdir,
glob.glob(path.abspath(base_run_folder) + '/*'))))
if invalidate_cache or not path.exists(cache_file_path):
df = pd.DataFrame()
parameter_names = set()
cached_run_folders = set()
else:
print_info("collect_data: updating cache")
with open(cache_file_path, 'rb') as f:
cache = pickle.load(f)
df = cache['df']
parameter_names = cache['parameter_names']
cached_run_folders = set(df['run_folder'])
# collect results from runs not already cached
print_info("collect_data: reading run data")
no_output = True
for i, run_folder in enumerate(run_folders):
metric_results_folder = path.join(run_folder, "metric_results")
benchmark_data_folder = path.join(run_folder, "benchmark_data")
run_info_file_path = path.join(run_folder, "run_info.yaml")
if not path.exists(metric_results_folder):
print_error(
"collect_data: metric_results_folder does not exists [{}]".
format(metric_results_folder))
no_output = False
continue
if not path.exists(run_info_file_path):
print_error(
"collect_data: run_info file does not exists [{}]".format(
run_info_file_path))
no_output = False
continue
if run_folder in cached_run_folders:
continue
run_info = get_yaml(run_info_file_path)
run_record = dict()
for parameter_name, parameter_value in run_info[
'run_parameters'].items():
parameter_names.add(parameter_name)
if type(parameter_value) == list:
parameter_value = tuple(parameter_value)
run_record[parameter_name] = parameter_value
parameter_names.add('environment_name')
run_record['environment_name'] = path.basename(
path.abspath(run_info['environment_folder']))
run_record['run_folder'] = run_folder
run_record['failure_rate'] = 0
try:
run_events = get_csv(
path.join(benchmark_data_folder, "run_events.csv"))
metrics_dict = get_yaml(
path.join(metric_results_folder, "metrics.yaml"))
except IOError:
run_record['failure_rate'] = 1
df = df.append(run_record, ignore_index=True)
continue
trajectory_length = get_yaml_by_path(metrics_dict,
['trajectory_length'])
run_record['trajectory_length'] = trajectory_length
# if trajectory_length is None or trajectory_length < 3.0:
# run_record['failure_rate'] = 1
# df = df.append(run_record, ignore_index=True)
# continue
run_record['mean_absolute_error'] = get_yaml_by_path(
metrics_dict, ['absolute_localization_error', 'mean'])
run_record['mean_relative_translation_error'] = get_yaml_by_path(
metrics_dict, [
'relative_localization_error', 'random_relations',
'translation', 'mean'
])
run_record['mean_relative_rotation_error'] = get_yaml_by_path(
metrics_dict, [
'relative_localization_error', 'random_relations', 'rotation',
'mean'
])
run_record['num_target_pose_set'] = len(
run_events[run_events["event"] == "target_pose_set"])
run_record['num_target_pose_reached'] = len(
run_events[run_events["event"] == "target_pose_reached"])
run_record['num_target_pose_not_reached'] = len(
run_events[run_events["event"] == "target_pose_not_reached"])
run_start_events = run_events["event"] == "run_start"
run_completed_events = run_events["event"] == "run_completed"
if len(run_start_events) == 0 or len(run_completed_events) == 0:
run_record['failure_rate'] = 1
df = df.append(run_record, ignore_index=True)
continue
if len(run_events[run_events["event"] == "run_start"]
["timestamp"]) == 0:
print_error("collect_data: run_start event does not exists")
no_output = False
run_record['failure_rate'] = 1
df = df.append(run_record, ignore_index=True)
continue
if len(run_events[run_events["event"] == "run_completed"]
["timestamp"]) == 0:
print_error("collect_data: run_completed event does not exists")
no_output = False
run_record['failure_rate'] = 1
df = df.append(run_record, ignore_index=True)
continue
run_start_time = float(
run_events[run_events["event"] == "run_start"]["timestamp"])
supervisor_finish_time = float(
run_events[run_events["event"] == "run_completed"]["timestamp"])
run_execution_time = supervisor_finish_time - run_start_time
run_record['run_execution_time'] = run_execution_time
amcl_accumulated_cpu_time = get_yaml_by_path(
metrics_dict,
['cpu_and_memory_usage', 'amcl_accumulated_cpu_time'])
if amcl_accumulated_cpu_time is not None:
run_record[
'normalised_cpu_time'] = amcl_accumulated_cpu_time / run_execution_time
run_record['max_memory'] = get_yaml_by_path(
metrics_dict, ['cpu_and_memory_usage', 'amcl_uss'])
slam_toolbox_localization_accumulated_cpu_time = get_yaml_by_path(
metrics_dict, [
'cpu_and_memory_usage',
'localization_slam_toolbox_node_accumulated_cpu_time'
])
if slam_toolbox_localization_accumulated_cpu_time is not None:
run_record[
'normalised_cpu_time'] = slam_toolbox_localization_accumulated_cpu_time / run_execution_time
run_record['max_memory'] = get_yaml_by_path(
metrics_dict,
['cpu_and_memory_usage', 'localization_slam_toolbox_node_uss'])
gmapping_accumulated_cpu_time = get_yaml_by_path(
metrics_dict,
['cpu_and_memory_usage', 'slam_gmapping_accumulated_cpu_time'])
if gmapping_accumulated_cpu_time is not None:
run_record[
'normalised_cpu_time'] = gmapping_accumulated_cpu_time / run_execution_time
run_record['max_memory'] = get_yaml_by_path(
metrics_dict, ['cpu_and_memory_usage', 'slam_gmapping_uss'])
slam_toolbox_slam_accumulated_cpu_time = get_yaml_by_path(
metrics_dict, [
'cpu_and_memory_usage',
'async_slam_toolbox_node_accumulated_cpu_time'
])
if slam_toolbox_slam_accumulated_cpu_time is not None:
run_record[
'normalised_cpu_time'] = slam_toolbox_slam_accumulated_cpu_time / run_execution_time
run_record['max_memory'] = get_yaml_by_path(
metrics_dict,
['cpu_and_memory_usage', 'async_slam_toolbox_node_uss'])
df = df.append(run_record, ignore_index=True)
print_info("collect_data: reading run data: {}%".format(
int((i + 1) * 100 / len(run_folders))),
replace_previous_line=no_output)
no_output = True
# save cache
if cache_file_path is not None:
cache = {'df': df, 'parameter_names': parameter_names}
with open(cache_file_path, 'wb') as f:
pickle.dump(cache, f, protocol=2)
return df, parameter_names
