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")
