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 ros_shutdown_callback(self): """ This function is called when the node receives an interrupt signal (KeyboardInterrupt). """ print_info("asked to shutdown, terminating run") self.write_event(self.get_clock().now(), 'ros_shutdown') self.write_event(self.get_clock().now(), 'supervisor_finished')
def send_goal(self): print_info(f"goal {self.goal_sent_count + 1} / {self.num_goals}") if not self.navigate_to_pose_action_client.wait_for_server( timeout_sec=5.0): self.write_event(self.get_clock().now(), 'failed_to_communicate_with_navigation_node') raise RunFailException( "navigate_to_pose action server not available") if len(self.traversal_path_poses) == 0: self.write_event(self.get_clock().now(), 'insufficient_number_of_poses_in_traversal_path') raise RunFailException( "insufficient number of poses in traversal path, can not send goal" ) goal_msg = NavigateToPose.Goal() goal_msg.pose.header.stamp = self.get_clock().now().to_msg() goal_msg.pose.header.frame_id = self.fixed_frame goal_msg.pose.pose = self.traversal_path_poses.popleft() self.current_goal = goal_msg self.navigate_to_pose_action_goal_future = self.navigate_to_pose_action_client.send_goal_async( goal_msg) self.navigate_to_pose_action_goal_future.add_done_callback( self.goal_response_callback) self.write_event(self.get_clock().now(), 'target_pose_set') self.goal_sent_count += 1
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 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 write_event(self, stamp, event): print_info("t: {t}, event: {event}".format(t=nanoseconds_to_seconds( stamp.nanoseconds), event=str(event))) try: with open(self.run_events_file_path, 'a') as run_events_file: run_events_file.write("{t}, {event}\n".format( t=nanoseconds_to_seconds(stamp.nanoseconds), event=str(event))) except IOError as e: self.get_logger().error( "slam_benchmark_supervisor.write_event: could not write event to run_events_file: {t} {event}" .format(t=nanoseconds_to_seconds(stamp.nanoseconds), event=str(event))) self.get_logger().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 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 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())
def save_voronoi_plot_and_trajectory(self, plot_file_path, segments, graph=None, do_not_recompute=False, timeout=120, max_nodes=2000, min_radius=None): plot_file_path = path.expanduser(plot_file_path) if path.exists(plot_file_path) and do_not_recompute: print_info( "do_not_recompute: will not recompute the voronoi plot {}". format(plot_file_path)) return if not path.exists(path.dirname(plot_file_path)): os.makedirs(path.dirname(plot_file_path)) if min_radius is None: min_radius = 4 * self.resolution if graph is None: graph = self.voronoi_graph.subgraph( filter( lambda n: self.voronoi_graph.nodes[n]['radius'] >= min_radius, self.voronoi_graph.nodes)) import matplotlib.pyplot as plt fig, ax = plt.subplots() fig.set_size_inches(*cm_to_body_parts(10 * self.map_size_meters)) start_time = time.time() print("plotting. nodes: {}/{}".format( min(max_nodes, graph.number_of_nodes()), graph.number_of_nodes())) for (x_1, y_1), (x_2, y_2) in segments: ax.plot((x_1, x_2), (y_1, y_2), color='blue', linewidth=1.5) num_nodes = 0 nth = max(1, graph.number_of_nodes() // max_nodes) for node_index, node_data in graph.nodes.data(): num_nodes += 1 if num_nodes % nth: continue x_1, y_1 = node_data['vertex'] radius_1 = node_data['radius'] if radius_1 < min_radius: continue # plot leaf vertices if len(list(graph.neighbors(node_index))) == 1: ax.scatter(x_1, y_1, color='red', s=4.0, marker='o') # plot chain vertices if len(list(graph.neighbors(node_index))) == 3: ax.scatter(x_1, y_1, color='blue', s=4.0, marker='o') # plot other vertices if len(list(graph.neighbors(node_index))) == 2: ax.scatter(x_1, y_1, color='grey', s=2.0, marker='o') # plot segments for neighbor_index in graph.neighbors(node_index): if neighbor_index < node_index: radius_2 = graph.nodes[neighbor_index]['radius'] if radius_2 > min_radius: x_2, y_2 = graph.nodes[neighbor_index]['vertex'] ax.plot((x_1, x_2), (y_1, y_2), color='black', linewidth=1.0) # plot circles ax.add_artist( plt.Circle(node_data['vertex'], radius_1, color='grey', fill=False, linewidth=0.2)) if time.time() - start_time > timeout: print("timeout") break # plot vertical and horizontal wall points _, north_bitmap, south_bitmap, west_bitmap, east_bitmap = self.edge_bitmaps( lambda pixel: pixel != self.free_rgb) h_wall_points_pixels_set = set() v_wall_points_pixels_set = set() w, h = north_bitmap.shape for x in range(w): for y in range(h): if north_bitmap[x, y] or south_bitmap[x, y]: h_wall_points_pixels_set.add((x, y)) h_wall_points_pixels_set.add((x + 1, y)) if west_bitmap[x, y] or east_bitmap[x, y]: v_wall_points_pixels_set.add((x, y)) v_wall_points_pixels_set.add((x, y + 1)) h_wall_points_meters = self.image_y_up_to_map_frame_coordinates( np.array(list(h_wall_points_pixels_set))) v_wall_points_meters = self.image_y_up_to_map_frame_coordinates( np.array(list(v_wall_points_pixels_set))) ax.scatter(h_wall_points_meters[:, 0], h_wall_points_meters[:, 1], s=15.0, marker='_') ax.scatter(v_wall_points_meters[:, 0], v_wall_points_meters[:, 1], s=15.0, marker='|') fig.savefig(plot_file_path) plt.close(fig)
def gridmap_to_mesh(grid_map_info_file_path, mesh_file_path, do_not_recompute=False, map_floor_height=0.0, wall_height=2.0): if path.exists(mesh_file_path): if do_not_recompute: print_info("do_not_recompute: will not recompute the output mesh {}".format(mesh_file_path)) return else: print_info("overriding mesh {}".format(mesh_file_path)) if not path.exists(path.dirname(mesh_file_path)): os.makedirs(path.dirname(mesh_file_path)) m = GroundTruthMap(grid_map_info_file_path) pixels = m.map_image.load() # occupied_bitmap contains 1 where pixels are occupied (black color value -> wall) # occupied_bitmap coordinates have origin in the image bottom-left with y-up rather than top-left with y-down, # so it is in between image coordinates and map frame coordinates w, h = m.map_image.size occupied_bitmap = np.zeros((w+1, h+1), dtype=int) for y in range(h): for x in range(w): occupied_bitmap[x, h-1-y] = pixels[x, y] == (0, 0, 0) # span the image with kernels to find vertical walls, north -> +1, south -> -1 vertical_edges = ndimage.convolve(occupied_bitmap, np.array([[1, -1]]), mode='constant', cval=0, origin=np.array([0, -1])) north_bitmap = vertical_edges == 1 south_bitmap = vertical_edges == -1 # span the image with kernels to find horizontal walls, west -> +1, east -> -1 horizontal_edges = ndimage.convolve(occupied_bitmap, np.array([[1], [-1]]), mode='constant', cval=0, origin=np.array([-1, 0])) west_bitmap = horizontal_edges == 1 east_bitmap = horizontal_edges == -1 vertices = list() normals = list() triangles = list() # make the mesh for the north and south walls for y in range(north_bitmap.shape[1]): north_wall_start: Optional[np.array] = None south_wall_start: Optional[np.array] = None for x in range(north_bitmap.shape[0]): if north_wall_start is None and north_bitmap[x, y]: # wall goes up north_wall_start = m.image_y_up_to_map_frame_coordinates(np.array((x, y))) if north_wall_start is not None and not north_bitmap[x, y]: # wall goes down north_wall_end = m.image_y_up_to_map_frame_coordinates(np.array((x, y))) v, n, t = wall_face(*north_wall_start, *north_wall_end, map_floor_height, wall_height, '-y') vertices += v normals += n triangles += t north_wall_start = None if south_wall_start is None and south_bitmap[x, y]: # wall goes up south_wall_start = m.image_y_up_to_map_frame_coordinates(np.array((x, y))) if south_wall_start is not None and not south_bitmap[x, y]: # wall goes down south_wall_end = m.image_y_up_to_map_frame_coordinates(np.array((x, y))) v, n, t = wall_face(*south_wall_start, *south_wall_end, map_floor_height, wall_height, 'y') vertices += v normals += n triangles += t south_wall_start = None # make the mesh for the west and east walls for x in range(west_bitmap.shape[0]): west_wall_start: Optional[np.array] = None east_wall_start: Optional[np.array] = None for y in range(west_bitmap.shape[1]): if west_wall_start is None and west_bitmap[x, y]: # wall goes up west_wall_start = m.image_y_up_to_map_frame_coordinates(np.array((x, y))) if west_wall_start is not None and not west_bitmap[x, y]: # wall goes down west_wall_end = m.image_y_up_to_map_frame_coordinates(np.array((x, y))) v, n, t = wall_face(*west_wall_start, *west_wall_end, map_floor_height, wall_height, '-x') vertices += v normals += n triangles += t west_wall_start = None if east_wall_start is None and east_bitmap[x, y]: # wall goes up east_wall_start = m.image_y_up_to_map_frame_coordinates(np.array((x, y))) if east_wall_start is not None and not east_bitmap[x, y]: # wall goes down east_wall_end = m.image_y_up_to_map_frame_coordinates(np.array((x, y))) v, n, t = wall_face(*east_wall_start, *east_wall_end, map_floor_height, wall_height, 'x') vertices += v normals += n triangles += t east_wall_start = None # make the mesh for the top of the walls for y in range(occupied_bitmap.shape[1]): x_min, y_min = None, None for x in range(occupied_bitmap.shape[0]): if x_min is None and occupied_bitmap[x, y]: # wall goes up x_min, y_min = x, y if x_min is not None and not occupied_bitmap[x, y]: # wall goes down # find the rectangle covering the most wall by checking each horizontal line x_max = x y_max = None subbitmap = occupied_bitmap[x_min:x_max, y_min:h+1] for square_h in range(subbitmap.shape[1]): if not np.all(subbitmap[:, square_h]): y_max = y_min + square_h break # once found the square, delete the corresponding pixels (so they won't be checked again) occupied_bitmap[x_min:x_max, y_min:y_max] = 0 v, n, t = wall_top(*m.image_y_up_to_map_frame_coordinates(np.array((x_min, y_min))), *m.image_y_up_to_map_frame_coordinates(np.array((x_max, y_max))), wall_height) vertices += v normals += n triangles += t x_min = None mesh = cd.Collada() effect = cd.material.Effect("effect0", [], "phong", diffuse=(1, 0, 0), specular=(0, 1, 0)) mat = cd.material.Material("material0", "mymaterial", effect) mesh.effects.append(effect) mesh.materials.append(mat) cd_vertices_list = list() for index, vertex in enumerate(vertices): cd_vertices_list += vertex.ls vertex.index = index cd_normals_list = list() for index, normal in enumerate(normals): cd_normals_list += normal.ls normal.index = index cd_triangles_list = list() for triangle in triangles: cd_triangles_list += triangle.ls vert_src = cd.source.FloatSource("cubeverts-array", np.array(cd_vertices_list), ('X', 'Y', 'Z')) normal_src = cd.source.FloatSource("cubenormals-array", np.array(cd_normals_list), ('X', 'Y', 'Z')) geom = cd.geometry.Geometry(mesh, "geometry0", "mycube", [vert_src, normal_src]) input_list = cd.source.InputList() input_list.addInput(0, 'VERTEX', "#cubeverts-array") input_list.addInput(1, 'NORMAL', "#cubenormals-array") triset = geom.createTriangleSet(np.array(cd_triangles_list), input_list, "materialref") geom.primitives.append(triset) mesh.geometries.append(geom) matnode = cd.scene.MaterialNode("materialref", mat, inputs=[]) geomnode = cd.scene.GeometryNode(geom, [matnode]) node = cd.scene.Node("node0", children=[geomnode]) myscene = cd.scene.Scene("myscene", [node]) mesh.scenes.append(myscene) mesh.scene = myscene mesh.write(mesh_file_path)
parser.add_argument( '-p', dest='save_visualization_plots', help='If set, the Voronoi visualization plots are computed and saved.', action='store_true', default=False, required=False) args = parser.parse_args() environment_folders = sorted( filter(path.isdir, glob.glob(path.expanduser(args.environment_folders)))) dump_path = args.dump_path print_info("computing environment data {}%".format(0)) recompute_data = args.recompute_data save_visualization_plots = args.save_visualization_plots recompute_plots = True for progress, environment_folder in enumerate(environment_folders): print_info("computing environment data {}% {}".format( 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",
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
vert_src = cd.source.FloatSource("cubeverts-array", np.array(cd_vertices_list), ('X', 'Y', 'Z')) normal_src = cd.source.FloatSource("cubenormals-array", np.array(cd_normals_list), ('X', 'Y', 'Z')) geom = cd.geometry.Geometry(mesh, "geometry0", "mycube", [vert_src, normal_src]) input_list = cd.source.InputList() input_list.addInput(0, 'VERTEX', "#cubeverts-array") input_list.addInput(1, 'NORMAL', "#cubenormals-array") triset = geom.createTriangleSet(np.array(cd_triangles_list), input_list, "materialref") geom.primitives.append(triset) mesh.geometries.append(geom) matnode = cd.scene.MaterialNode("materialref", mat, inputs=[]) geomnode = cd.scene.GeometryNode(geom, [matnode]) node = cd.scene.Node("node0", children=[geomnode]) myscene = cd.scene.Scene("myscene", [node]) mesh.scenes.append(myscene) mesh.scene = myscene mesh.write(mesh_file_path) if __name__ == '__main__': environment_folders = sorted(glob.glob(path.expanduser("~/ds/performance_modelling/test_datasets/dataset/*"))) print_info("gridmap_to_mesh {}%".format(0)) for progress, environment_folder in enumerate(environment_folders): print_info("gridmap_to_mesh {}% {}".format((progress + 1)*100//len(environment_folders), environment_folder)) map_info_file_path = path.join(environment_folder, "data", "map.yaml") result_mesh_file_path = path.join(environment_folder, "data", "meshes", "extruded_map.dae") gridmap_to_mesh(map_info_file_path, result_mesh_file_path)
def absolute_error_vs_voronoi_radius(estimated_poses_file_path, ground_truth_poses_file_path, ground_truth_map, 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 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 start_time = time.time() ground_truth_poses_df = pd.read_csv(ground_truth_poses_file_path) 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) print_info("compute_ground_truth_interpolated_poses", time.time() - start_time) # if no matching ground truth data points are found, the metrics can not be computed if len(trajectory_df.index) < 2: print_error("no matching ground truth data points were found") return trajectory_points_list = list() trajectory_df['abs_err'] = np.sqrt((trajectory_df['x_est'] - trajectory_df['x_gt'])**2 + (trajectory_df['y_est'] - trajectory_df['y_gt'])**2) for _, row in trajectory_df.iterrows(): x_gt, y_gt, abs_err = row['x_gt'], row['y_gt'], row['abs_err'] trajectory_points_list.append(np.array([x_gt, y_gt])) start_time = time.time() min_radius = 4 * ground_truth_map.resolution voronoi_graph = ground_truth_map.voronoi_graph.subgraph(filter( lambda n: ground_truth_map.voronoi_graph.nodes[n]['radius'] >= min_radius, ground_truth_map.voronoi_graph.nodes )) print_info("ground_truth_map.voronoi_graph", time.time() - start_time) voronoi_vertices_list = list() voronoi_radii_list = list() for node_index, node_data in voronoi_graph.nodes.data(): voronoi_vertices_list.append(node_data['vertex']) voronoi_radii_list.append(node_data['radius']) voronoi_vertices_array = np.array(voronoi_vertices_list) start_time = time.time() kdtree = scipy.spatial.cKDTree(voronoi_vertices_array) dist, indexes = kdtree.query(trajectory_points_list) print_info("kdtree", time.time() - start_time) trajectory_radii_list = list() for voronoi_vertex_index in indexes: trajectory_radii_list.append(voronoi_radii_list[voronoi_vertex_index]) trajectory_df['trajectory_radius'] = trajectory_radii_list # # plotting # import matplotlib.pyplot as plt # # # fig, ax = plt.subplots() # # ax.scatter(trajectory_df['trajectory_radius'], trajectory_df['abs_err']) # , color='red', s=4.0, marker='o') # # plt.xlabel("trajectory_radius") # # plt.ylabel("abs_err") # # plt.show() # # plt.cla() # # # # plt.plot(trajectory_df['t'], trajectory_df['abs_err']) # # plt.plot(trajectory_df['t'], trajectory_df['trajectory_radius']) # # plt.legend() # # plt.show() # # plt.cla() # # bins = np.linspace(0, 3, 4*5) # plt.hist(trajectory_df['abs_err'], bins=bins) # plt.hist(trajectory_df[(trajectory_df['trajectory_radius'] > 0.0) & (trajectory_df['trajectory_radius'] < 3.5)]['abs_err'], bins=bins) # plt.title("0.0 ~ 3.5") # plt.show() # plt.cla() # # plt.hist(trajectory_df['abs_err'], bins=bins) # plt.hist(trajectory_df[(trajectory_df['trajectory_radius'] > 3.5) & (trajectory_df['trajectory_radius'] < 5.0)]['abs_err'], bins=bins) # plt.title("3.5 ~ 5.0") # plt.show() # plt.cla() # # plt.hist(trajectory_df['abs_err'], bins=bins) # plt.hist(trajectory_df[(trajectory_df['trajectory_radius'] > 0.0) & (trajectory_df['trajectory_radius'] < 1.0)]['abs_err'], bins=bins) # plt.title("0.0 ~ 1.0") # plt.show() # plt.cla() # # bins = np.linspace(0, 9, 8*4) # plt.hist(trajectory_df['trajectory_radius'], bins=bins) # plt.hist(trajectory_df[(trajectory_df['abs_err'] > .3) & (trajectory_df['abs_err'] < .5)]['trajectory_radius'], bins=bins) # plt.show() # # # plot trajectory and distances from voronoi vertices # distance_segments = list() # for trajectory_point_index, voronoi_vertex_index in enumerate(indexes): # voronoi_vertex = voronoi_vertices_array[voronoi_vertex_index] # trajectory_point = trajectory_points_list[trajectory_point_index] # distance_segments.append((voronoi_vertex, trajectory_point)) # # trajectory_segments = zip(trajectory_points_list[0: -1], trajectory_points_list[1:]) # ground_truth_map.save_voronoi_plot_and_trajectory("~/tmp/vg_and_traj.svg", trajectory_segments + distance_segments) return trajectory_df
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
def compute_metrics(run_output_folder): metrics_result_dict = dict() # localization metrics estimated_correction_poses_path = path.join( run_output_folder, "benchmark_data", "estimated_correction_poses.csv") estimated_poses_path = path.join(run_output_folder, "benchmark_data", "estimated_poses.csv") ground_truth_poses_path = path.join(run_output_folder, "benchmark_data", "ground_truth_poses.csv") logs_folder_path = path.join(run_output_folder, "logs") print_info("trajectory_length") metrics_result_dict['trajectory_length'] = trajectory_length_metric( ground_truth_poses_path) print_info("relative_localization_correction_error") metrics_result_dict[ 'relative_localization_correction_error'] = relative_localization_error_metrics( path.join(logs_folder_path, "relative_localisation_correction_error"), estimated_correction_poses_path, ground_truth_poses_path) print_info("relative_localization_error") metrics_result_dict[ 'relative_localization_error'] = relative_localization_error_metrics( path.join(logs_folder_path, "relative_localisation_error"), estimated_poses_path, ground_truth_poses_path) print_info("absolute_localization_correction_error") metrics_result_dict[ 'absolute_localization_correction_error'] = absolute_localization_error_metrics( estimated_correction_poses_path, ground_truth_poses_path) print_info("absolute_localization_error") metrics_result_dict[ 'absolute_localization_error'] = absolute_localization_error_metrics( estimated_poses_path, ground_truth_poses_path) # computation metrics print_info("cpu_and_memory_usage") ps_snapshots_folder_path = path.join(run_output_folder, "benchmark_data", "ps_snapshots") metrics_result_dict['cpu_and_memory_usage'] = cpu_and_memory_usage_metrics( ps_snapshots_folder_path) # write metrics metrics_result_folder_path = path.join(run_output_folder, "metric_results") metrics_result_file_path = path.join(metrics_result_folder_path, "metrics.yaml") if not path.exists(metrics_result_folder_path): os.makedirs(metrics_result_folder_path) with open(metrics_result_file_path, 'w') as metrics_result_file: yaml.dump(metrics_result_dict, metrics_result_file, default_flow_style=False) # visualisation print_info("visualisation") visualisation_output_folder = path.join(run_output_folder, "visualisation") save_trajectories_plot(visualisation_output_folder, estimated_poses_path, estimated_correction_poses_path, ground_truth_poses_path)
def start_run(self): print_info("preparing to start run") # wait to receive sensor data from the environment (e.g., a simulator may need time to startup) waiting_time = 0.0 waiting_period = 0.5 while not self.received_first_scan and rclpy.ok(): time.sleep(waiting_period) rclpy.spin_once(self) waiting_time += waiting_period if waiting_time > 5.0: self.get_logger().warning( 'still waiting to receive first sensor message from environment' ) waiting_time = 0.0 # get the parameter robot_radius from the global costmap parameters_request = GetParameters.Request(names=['robot_radius']) parameters_response = self.call_service( self.global_costmap_get_parameters_service_client, parameters_request) self.robot_radius = parameters_response.values[0].double_value print_info("got robot radius") # get deleaved reduced Voronoi graph from ground truth map voronoi_graph = self.ground_truth_map.deleaved_reduced_voronoi_graph( minimum_radius=2 * self.robot_radius).copy() minimum_length_paths = nx.all_pairs_dijkstra_path( voronoi_graph, weight='voronoi_path_distance') minimum_length_costs = dict( nx.all_pairs_dijkstra_path_length(voronoi_graph, weight='voronoi_path_distance')) costs = defaultdict(dict) for i, paths_dict in minimum_length_paths: for j in paths_dict.keys(): if i != j: costs[i][j] = minimum_length_costs[i][j] # in case the graph has multiple unconnected components, remove the components with less than two nodes too_small_voronoi_graph_components = list( filter(lambda component: len(component) < 2, nx.connected_components(voronoi_graph))) for graph_component in too_small_voronoi_graph_components: voronoi_graph.remove_nodes_from(graph_component) if len(voronoi_graph.nodes) < 2: self.write_event( self.get_clock().now(), 'insufficient_number_of_nodes_in_deleaved_reduced_voronoi_graph' ) raise RunFailException( "insufficient number of nodes in deleaved_reduced_voronoi_graph, can not generate traversal path" ) # get greedy path traversing the whole graph starting from a random node traversal_path_indices = list() current_node = random.choice(list(voronoi_graph.nodes)) nodes_queue = set( nx.node_connected_component(voronoi_graph, current_node)) while len(nodes_queue): candidates = list( filter(lambda node_cost: node_cost[0] in nodes_queue, costs[current_node].items())) candidate_nodes, candidate_costs = zip(*candidates) next_node = candidate_nodes[int(np.argmin(candidate_costs))] traversal_path_indices.append(next_node) current_node = next_node nodes_queue.remove(next_node) # convert path of nodes to list of poses (as deque so they can be popped) self.traversal_path_poses = deque() for node_index in traversal_path_indices: pose = Pose() pose.position.x, pose.position.y = voronoi_graph.nodes[node_index][ 'vertex'] q = pyquaternion.Quaternion(axis=[0, 0, 1], radians=np.random.uniform( -np.pi, np.pi)) pose.orientation = Quaternion(w=q.w, x=q.x, y=q.y, z=q.z) self.traversal_path_poses.append(pose) # publish the traversal path for visualization traversal_path_msg = Path() traversal_path_msg.header.frame_id = self.fixed_frame traversal_path_msg.header.stamp = self.get_clock().now().to_msg() for traversal_pose in self.traversal_path_poses: traversal_pose_stamped = PoseStamped() traversal_pose_stamped.header = traversal_path_msg.header traversal_pose_stamped.pose = traversal_pose traversal_path_msg.poses.append(traversal_pose_stamped) self.traversal_path_publisher.publish(traversal_path_msg) # pop the first pose from traversal_path_poses and set it as initial pose self.initial_pose = PoseWithCovarianceStamped() self.initial_pose.header.frame_id = self.fixed_frame self.initial_pose.header.stamp = self.get_clock().now().to_msg() self.initial_pose.pose.pose = self.traversal_path_poses.popleft() self.initial_pose.pose.covariance = list( self.initial_pose_covariance_matrix.flat) self.num_goals = len(self.traversal_path_poses) # set the position of the robot in the simulator self.call_service(self.pause_physics_service_client, Empty.Request()) print_info("called pause_physics_service") time.sleep(1.0) robot_entity_state = EntityState(name=self.robot_entity_name, pose=self.initial_pose.pose.pose) set_entity_state_response = self.call_service( self.set_entity_state_service_client, SetEntityState.Request(state=robot_entity_state)) if not set_entity_state_response.success: self.write_event(self.get_clock().now(), 'failed_to_set_entity_state') raise RunFailException("could not set robot entity state") print_info("called set_entity_state_service") time.sleep(1.0) self.call_service(self.unpause_physics_service_client, Empty.Request()) print_info("called unpause_physics_service") time.sleep(1.0) # ask lifecycle_manager to startup all its managed nodes startup_request = ManageLifecycleNodes.Request( command=ManageLifecycleNodes.Request.STARTUP) startup_response: ManageLifecycleNodes.Response = self.call_service( self.lifecycle_manager_service_client, startup_request) if not startup_response.success: self.write_event(self.get_clock().now(), 'failed_to_startup_nodes') raise RunFailException("lifecycle manager could not startup nodes") self.write_event(self.get_clock().now(), 'run_start') self.run_started = True self.send_goal()
dest='base_run_folder', help= 'Folder in which the result of each run will be placed. Defaults to {}.' .format(default_base_run_folder), type=str, default=default_base_run_folder, required=False) parser.add_argument( '-s', dest='source_workspace_path', help='Path of the workspace directory. Defaults to {}.'.format( default_source_workspace_path), type=str, default=default_source_workspace_path, required=False) args = parser.parse_args() base_run_folder_arg = path.expanduser(args.base_run_folder) source_workspace_path_arg = path.expanduser(args.source_workspace_path) run_folders = list( filter(path.isdir, glob.glob(path.join(base_run_folder_arg, "*")))) for progress, run_folder in enumerate(run_folders): print_info("main: log_packages_and_repos {}% {}".format( (progress + 1) * 100 / len(run_folders), run_folder)) run_folder_log_dir_path = path.join(run_folder, "software_versions_log") log_packages_and_repos(source_workspace_path=source_workspace_path_arg, log_dir_path=run_folder_log_dir_path)
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
.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)) if __name__ == '__main__': print_info("Python version:", sys.version_info) default_base_run_folder = "~/ds/performance_modelling/output/test_slam/*" default_scans_file_name = "scans_gt.csv" default_topic_name = "/scan_gt" default_num_parallel_threads = 4 parser = argparse.ArgumentParser( formatter_class=argparse.RawTextHelpFormatter, description= 'Extracts laser scan messages from ros bags for each run and writes to a csv file.' ) parser.add_argument( '-r', dest='base_run_folder', help=
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 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 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 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
metrics_result_file_path = path.join(metrics_result_folder_path, "metrics.yaml") if not path.exists(metrics_result_folder_path): os.makedirs(metrics_result_folder_path) with open(metrics_result_file_path, 'w') as metrics_result_file: yaml.dump(metrics_result_dict, metrics_result_file, default_flow_style=False) # visualisation print_info("visualisation") visualisation_output_folder = path.join(run_output_folder, "visualisation") save_trajectories_plot(visualisation_output_folder, estimated_poses_path, estimated_correction_poses_path, ground_truth_poses_path) if __name__ == '__main__': run_folders = list( filter( path.isdir, glob.glob( path.expanduser( "~/ds/performance_modelling_output/test_localization/*")))) # last_run_folder = sorted(run_folders, key=lambda x: path.getmtime(x))[-1] # print("last run folder:", last_run_folder) for progress, run_folder in enumerate(run_folders): print_info("main: compute_metrics {}% {}".format( (progress + 1) * 100 / len(run_folders), run_folder)) compute_metrics(path.expanduser(run_folder))
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 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 execute_benchmark(benchmark_run_object, run_parameters_list, benchmark_configuration, base_run_folder, num_runs, ignore_executed_params_combinations, shuffle, args_parser=None): if not path.exists(base_run_folder): os.makedirs(base_run_folder) # 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(benchmark_configuration, 'r') as benchmark_configuration_file: benchmark_configuration = yaml.safe_load(benchmark_configuration_file) with open(run_parameters_list, 'r') as run_parameters_list_file: run_parameters_dict_list = yaml.safe_load(run_parameters_list_file) remaining_params_combinations = list() for parameters_combination_dict in run_parameters_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(run_parameters_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') log_file_path = path.join( base_run_folder, "session_{session_id}_log.csv".format(session_id=session_id)) for n_executed_runs, parameters_combination_dict in enumerate( remaining_params_combinations): # 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("\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, parameters_combination_dict=parameters_combination_dict, benchmark_configuration_dict=benchmark_configuration, args_parser=args_parser, ) 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")