def get_coverage(self, cpp, path): tmp_pcd = PointCloud(self.print, points=cpp.coverable_pcd.points) tmp_pcd.visit_path(path) return tmp_pcd.get_coverage_efficiency()
class RandomSpiralSegment: """A class to generate a segment of Spiral path. Used in Sample-Based BAstar CPP Algorithm. """ def __init__(self, print, motion_planner, starting_point, visited_waypoints): """ Args: print: function for printing messages motion_planner: Motion Planner of the robot wihch also has the Point Cloud starting_point: A [x,y,z] np.array of the start position of the robot visited_waypoints: A Nx3 array with points that has been visited and should be avoided """ self.visited_waypoints = visited_waypoints self.start = starting_point self.print = print self.pcd = PointCloud(print, points=motion_planner.traversable_points) self.motion_planner = motion_planner current_angle = 0 self.path = np.empty((0, 3)) next_starting_point = starting_point while True: local_spiral_path, current_position = self.get_path_until_dead_zone( next_starting_point, current_angle) self.path = np.append(self.path, local_spiral_path, axis=0) self.visited_waypoints = np.append(self.visited_waypoints, local_spiral_path, axis=0) next_starting_point = self.wavefront_algorithm(current_position) if next_starting_point is False: break if np.linalg.norm(next_starting_point - current_position ) > RANDOM_BASTAR_VARIANT_DISTANCE: break path_to_next_starting_point = self.motion_planner.Astar( current_position, next_starting_point) if path_to_next_starting_point is False: break self.path = np.append(self.path, path_to_next_starting_point, axis=0) current_position = next_starting_point if len(self.path) >= 2: current_angle = self.get_angle(self.path[-2], current_position) self.end = current_position self.print("Length of path: " + str(len(self.path))) if len(self.path) > 1: self.pcd.visit_path(self.path) self.covered_points_idx = self.pcd.covered_points_idx self.coverage = self.pcd.get_coverage_efficiency() self.pcd = None self.motion_planner = None def wavefront_algorithm(self, start_position): """Using Wavefront algorithm to find the closest obstacle free uncovered position. Args: start_position: A [x,y,z] np.array of the start position of the search Returns: An obstacle free uncovered position. """ last_layer = np.array([start_position]) visited = np.array([start_position]) visited_points = np.append(self.visited_waypoints, self.path, axis=0) while len(last_layer): new_layer = np.empty((0, 3)) for pos in last_layer: neighbours = self.get_neighbours(pos) for neighbour in neighbours: if self.has_been_visited(neighbour, visited): continue if not self.motion_planner.is_valid_step(pos, neighbour): continue if not self.has_been_visited(neighbour, visited_points): return neighbour visited = np.append(visited, [neighbour], axis=0) new_layer = np.append(new_layer, [neighbour], axis=0) last_layer = new_layer self.print( "FAIL. No new uncovered obstacle free positions could be found.") return False def get_path_until_dead_zone(self, current_position, current_angle): """Covers the area in an inward spiral motion until a dead zone is reached. Args: current_position: A [x,y,z] np.array of the start position of the search current_angle: A float value representing the starting angle in radians Returns: New part of the path with waypoints and the position of the robot at the end of the path. """ local_path = np.array([current_position]) dead_zone_reached = False visited_points = np.append(self.visited_waypoints, self.path, axis=0) while not dead_zone_reached: dead_zone_reached = True neighbours = self.get_neighbours_for_spiral( current_position, current_angle) for neighbour in neighbours: if self.is_blocked(current_position, neighbour, visited_points): continue local_path = np.append(local_path, [neighbour], axis=0) visited_points = np.append(visited_points, [neighbour], axis=0) current_angle = self.get_angle(current_position, neighbour) current_position = neighbour dead_zone_reached = False break return local_path, current_position def get_angle(self, from_pos, to_pos): """Calculates the angle of the robot after making a step Args: from_pos: A [x,y,z] np.array of the start position to_pos: A [x,y,z] np.array of the end position Returns: An angle in radians """ vec = to_pos[0:2] - from_pos[0:2] return np.angle(vec[0] + vec[1] * 1j) def is_in_list(self, list, array): """Checks if an array is in a list by checking if it has values close to it. Args: list: list with arrays array: array to check Returns: True if it finds the array in the list """ diffs = np.linalg.norm(list - array, axis=1) return np.any(diffs < 0.05) def get_neighbours_for_spiral(self, current_position, current_angle): """Finds neighbours of a given position. And return them in the order to create the inward spiral motion. Args: current_position: A [x,y,z] np.array of the start position current_angle: A float value representing the starting angle in radians Returns: List of neighbours in specific order to get the inward spiral motion, """ directions = [] for direction_idx in range(8): angle = direction_idx / 8 * np.pi * 2 + current_angle x = current_position[0] + np.cos(angle) * SPIRAL_STEP_SIZE y = current_position[1] + np.sin(angle) * SPIRAL_STEP_SIZE z = current_position[2] pos = np.array([x, y, z]) directions.append(self.pcd.find_k_nearest(pos, 1)[0]) right, forwardright, forward, forwardleft, left, backleft, back, backright = directions return [ backright, right, forwardright, forward, forwardleft, left, backleft ] def get_neighbours(self, current_position): """Finds all neighbours of a given position. Args: current_position: A [x,y,z] np.array of the start position Returns: All 8 neighbours of the given position """ directions = [] for direction_idx in range(8): angle = direction_idx / 8 * np.pi * 2 x = current_position[0] + np.cos(angle) * SPIRAL_STEP_SIZE y = current_position[1] + np.sin(angle) * SPIRAL_STEP_SIZE z = current_position[2] pos = np.array([x, y, z]) directions.append(self.pcd.find_k_nearest(pos, 1)[0]) return directions def has_been_visited(self, point, path=None): """Checks if a point has been visited. Looks if the distance to a point in the path is smaller than RANDOM_BASTAR_VISITED_TRESHOLD. Args: point: A [x,y,z] np.array of the point that should be checked. path (optional): Specific path. Defaults to None. Returns: True if the point has been classified as visited """ if path is None: path = self.path distances = np.linalg.norm(path - point, axis=1) return np.any(distances <= RANDOM_BASTAR_VISITED_TRESHOLD) def is_blocked(self, from_point, to_point, path=None): """Checks if a step is valid by looking if the end point has been visited or is an obstacle. Args: from_point: A [x,y,z] np.array of the start position to_point: A [x,y,z] np.array of the end position path (optional): Specific path. Defaults to None. Returns: True if the point has been classified as blocked """ if path is None: path = self.path if self.has_been_visited(to_point, path): return True if not self.motion_planner.is_valid_step(from_point, to_point): return True return False
class RandomBAstar2(CPPSolver): ''' Solving the Coverage Path Planning Problem with Random Sample BAstar with Inward Spiral ''' def __init__(self, print, motion_planner, coverable_pcd, time_limit=None, parameters=None): ''' Args: print: function for printing messages motion_planner: Motion Planner of the robot wihch also has the Point Cloud ''' self.print = print super().__init__(print, motion_planner, coverable_pcd, time_limit) self.name = "Random BAstar" if parameters is None: self.max_distance = RANDOM_BASTAR_VARIANT_DISTANCE self.max_distance_part_II = RANDOM_BASTAR_VARIANT_DISTANCE_PART_II self.nbr_of_angles = RANDOM_BASTAR_NUMBER_OF_ANGLES self.ba_exploration = RANDOM_BASTAR_PART_I_COVERAGE self.coverage_2 = COVEREAGE_EFFICIENCY_GOAL self.min_spiral_coverage = RANDOM_BASTAR_MIN_SPIRAL_LENGTH self.min_bastar_coverage = RANDOM_BASTAR_MIN_COVERAGE self.max_iterations = RANDOM_BASTAR_MAX_ITERATIONS self.step_size = BASTAR_STEP_SIZE self.visited_threshold = BASTAR_VISITED_TRESHOLD else: self.max_distance = parameters["max_distance"] self.max_distance_part_II = parameters["max_distance_part_II"] self.ba_exploration = parameters["ba_exploration"] self.min_spiral_coverage = parameters["min_spiral_coverage"] self.min_bastar_coverage = parameters["min_bastar_coverage"] self.step_size = parameters["step_size"] self.visited_threshold = parameters["visited_threshold"] self.randombastar_stats = {} self.randombastar_stats_over_time = [] def get_cpp_path(self, start_point, goal_coverage=None): """Generates a path that covers the area using BAstar Algorithm. Args: start_point: A [x,y,z] np.array of the start position of the robot Returns: Nx3 array with waypoints """ self.start_tracking() self.move_to(start_point) self.all_segments = [] self.visited_waypoints = np.empty((0, 3)) self.tmp_coverable_pcd = PointCloud(self.print, points=self.coverable_pcd.points) self.explored_pcd = PointCloud(self.print, points=self.coverable_pcd.points) self.uncovered_coverable_points_idx = np.arange( len(self.tmp_coverable_pcd.points)) iter = 0 #### PART 0 - Border #### #self.print("PART 0 - Covering border") #coverable_pcd = PointCloud(self.print, points=self.coverable_pcd.points) #border_segment = RandomBorderSegment(self.print, self.motion_planner, start_point, self.visited_waypoints, coverable_pcd, self.max_distance_part_II, self.step_size, self.visited_threshold, self.time_left()) #self.add_segment(border_segment) #self.explored_pcd.covered_points_idx = np.unique(np.append(self.explored_pcd.covered_points_idx, border_segment.covered_points_idx)) #self.print("Coverage of border: " + str(border_segment.coverage)) #self.print("Border cost: "+ str(self.get_cost_per_coverage(border_segment))) #### PART 1 - BA* #### self.print("PART 1 - Covering with BA*") coverage = self.tmp_coverable_pcd.get_coverage_efficiency() exploration = self.explored_pcd.get_coverage_efficiency() while exploration < self.ba_exploration and coverage < goal_coverage and not self.time_limit_reached( ): iter += 1 #self.print("Uncovered points: " + str(len(self.uncovered_coverable_points_idx))) random_point = self.get_random_uncovered_point( ignore_list=self.explored_pcd.covered_points_idx) if random_point is False: break closest_border_point, _ = self.find_closest_border( random_point, self.step_size, self.visited_threshold, self.visited_waypoints) BA_segments_from_point = [] for angle_idx in range(4): angle_offset = angle_idx * 2 * np.pi / 4 coverable_pcd = PointCloud(self.print, points=self.coverable_pcd.points) new_BAstar_path = BAStarSegment( self.print, self.motion_planner, closest_border_point, angle_offset, self.visited_waypoints, coverable_pcd, self.max_distance, self.step_size, self.visited_threshold, self.time_left()) BA_segments_from_point.append(new_BAstar_path) #if new_BAstar_path.coverage == 0: # break self.min_bastar_coverage = 0 #self.print([self.get_cost_per_coverage(a) for a in BA_segments_from_point]) #accepted_segments = list(filter(lambda x: x.coverage > self.min_bastar_coverage, BA_segments_from_point)) accepted_segments = list( filter(lambda x: self.get_cost_per_coverage(x) < 4500, BA_segments_from_point)) self.print([ self.get_cost_per_coverage(a) for a in BA_segments_from_point ]) if not accepted_segments: coverable_pcd = PointCloud(self.print, points=self.coverable_pcd.points) spiral_segment = RandomSpiralSegment( self.print, self.motion_planner, closest_border_point, self.visited_waypoints, coverable_pcd, self.max_distance_part_II, self.step_size, self.visited_threshold, self.time_left()) cost_per_coverage = self.get_cost_per_coverage(spiral_segment) self.print("cost_per_coverage spiral: " + str(cost_per_coverage)) if cost_per_coverage < 15000: self.add_segment(spiral_segment) best_BA_segment = spiral_segment self.print("COVERING WITH SPIRAL INSTEAD") else: best_BA_segment = max(BA_segments_from_point, key=operator.attrgetter("coverage")) else: #best_BA_segment = max(BA_segments_from_point, key=operator.attrgetter("coverage")) costs = [ self.get_cost_per_coverage(segment) for segment in accepted_segments ] #self.print(costs) best_BA_segment_idx = np.argmin(costs) best_BA_segment = accepted_segments[best_BA_segment_idx] self.add_segment(best_BA_segment) coverage = self.tmp_coverable_pcd.get_coverage_efficiency() self.print_update(coverage) self.randombastar_stats_over_time.append({ "time": timeit.default_timer() - self.start_time, "coverage": coverage, "iteration": iter, "path": best_BA_segment.path, "segment": best_BA_segment }) self.print( str(iter) + "- bastar coverage: " + str(best_BA_segment.coverage)) self.explored_pcd.covered_points_idx = np.unique( np.append(self.explored_pcd.covered_points_idx, best_BA_segment.covered_points_idx)) exploration = self.explored_pcd.get_coverage_efficiency() self.print("exploration: " + str(exploration)) self.print("Number of found paths: " + str(len(self.all_segments))) self.randombastar_stats["Part1_segments"] = len(self.all_segments) self.randombastar_stats["Part1_coverage"] = coverage self.randombastar_stats["Part1_iterations"] = iter self.randombastar_stats["Part1_time"] = timeit.default_timer( ) - self.start_time total = 0 for segment in self.all_segments[1:]: total += self.get_cost_per_coverage(segment) self.print("Bastar ost: " + str(total / self.randombastar_stats["Part1_segments"])) #### PART 2 - Inward Spiral #### self.print("PART 2 - Covering with Inward spiral") self.explored_pcd.covered_points_idx = self.tmp_coverable_pcd.covered_points_idx iter = 0 while coverage < goal_coverage and not self.time_limit_reached(): iter += 1 #self.print("Uncovered points: " + str(len(self.uncovered_coverable_points_idx))) random_point = self.get_random_uncovered_point() if random_point is False: break closest_border_point, _ = self.find_closest_border( random_point, self.step_size, self.visited_threshold, self.visited_waypoints) coverable_pcd = PointCloud(self.print, points=self.coverable_pcd.points) spiral_segment = RandomSpiralSegment( self.print, self.motion_planner, closest_border_point, self.visited_waypoints, coverable_pcd, self.max_distance_part_II, self.step_size, self.visited_threshold, self.time_left()) self.print( str(iter) + "- spiral coverage: " + str(spiral_segment.coverage)) self.print(self.get_cost_per_coverage(spiral_segment)) if self.get_cost_per_coverage(spiral_segment) > 15000: close_coverable_points_idx = spiral_segment.covered_points_idx if not len(close_coverable_points_idx): close_coverable_points_idx = self.tmp_coverable_pcd.points_idx_in_radius( closest_border_point, self.visited_threshold) self.uncovered_coverable_points_idx = self.delete_values( self.uncovered_coverable_points_idx, close_coverable_points_idx) #self.print("Too short spiral") continue self.add_segment(spiral_segment) coverage = self.tmp_coverable_pcd.get_coverage_efficiency() self.randombastar_stats_over_time.append({ "time": timeit.default_timer() - self.start_time, "coverage": coverage, "iteration": iter, "path": spiral_segment.path, "segment": spiral_segment }) #self.print("length: " + str(len(spiral_segment.path))) self.print_update(coverage) #self.print("Uncovered points: " + str(len(self.uncovered_coverable_points_idx))) self.randombastar_stats["Part2_segments"] = len( self.all_segments) - self.randombastar_stats["Part1_segments"] self.randombastar_stats["Part2_coverage"] = coverage self.randombastar_stats["Part2_iterations"] = iter self.randombastar_stats["Part2_time"] = timeit.default_timer( ) - self.start_time total = 0 if self.randombastar_stats["Part2_segments"]: for segment in self.all_segments[ self.randombastar_stats["Part1_segments"]:]: total += self.get_cost_per_coverage(segment) self.print("spiral ost: " + str(total / self.randombastar_stats["Part2_segments"])) paths_to_visit_in_order = self.traveling_salesman(self.all_segments) self.follow_paths(start_point, paths_to_visit_in_order) #self.print_stats(self.path) self.print(self.randombastar_stats) return self.path def get_cost_per_coverage(self, segment): if segment.coverage == 0: return np.Inf def get_length_of_path(path): ''' Calculates length of the path in meters ''' length = 0 for point_idx in range(len(path) - 1): length += np.linalg.norm(path[point_idx] - path[point_idx + 1]) return length def get_total_rotation(path): ''' Calculates the total rotation made by the robot while executing the path ''' rotation = 0 for point_idx in range(len(path) - 2): prev = (path[point_idx + 1] - path[point_idx]) / np.linalg.norm(path[point_idx] - path[point_idx + 1]) next = (path[point_idx + 2] - path[point_idx + 1] ) / np.linalg.norm(path[point_idx + 2] - path[point_idx + 1]) dot_product = np.dot(prev, next) curr_rotation = np.arccos(dot_product) if not np.isnan(curr_rotation): rotation += abs(curr_rotation) return rotation length_of_path = get_length_of_path(segment.path) rotation = get_total_rotation(segment.path[:, 0:2]) return (length_of_path + rotation) / segment.coverage def traveling_salesman(self, paths): """Using Traveling Salesman Algorithm to order the path in an order that would minimise the total length of the path. Args: paths: List of paths of types RandomSpiralSegment and BAStarSegment Returns: Ordered list of paths """ tree = Tree("BAstar paths") start_nodes_idx = [] end_nodes_idx = [] def get_weight(from_idx, to_idx): from_point = tree.nodes[from_idx] to_point = tree.nodes[to_idx] return 100 + np.linalg.norm( to_point[0:2] - from_point[0:2]) + 10 * abs(to_point[2] - from_point[2]) for path in paths: start_point = path.start end_point = path.end start_point_node_idx = tree.add_node(start_point) start_nodes_idx.append(start_point_node_idx) for node_idx, point in enumerate(tree.nodes[:-1]): tree.add_edge(start_point_node_idx, node_idx, get_weight(start_point_node_idx, node_idx)) end_point_node_idx = tree.add_node(end_point) end_nodes_idx.append(end_point_node_idx) for node_idx, point in enumerate(tree.nodes[:-2]): tree.add_edge(end_point_node_idx, node_idx, get_weight(end_point_node_idx, node_idx)) tree.add_edge(start_point_node_idx, end_point_node_idx, 0) traveling_Salesman_path = tree.get_traveling_salesman_path() self.print(traveling_Salesman_path) paths_in_order = [] current_position = np.array([0, 0, 0]) for node_idx in traveling_Salesman_path: if np.array_equal(tree.nodes[node_idx], current_position): continue if node_idx in start_nodes_idx: path_idx = start_nodes_idx.index(node_idx) current_path = paths[path_idx] paths_in_order.append(current_path) elif node_idx in end_nodes_idx: path_idx = end_nodes_idx.index(node_idx) current_path = paths[path_idx] current_path.path = np.flip(current_path.path, 0) current_path.end = current_path.start current_path.start = tree.nodes[node_idx] paths_in_order.append(current_path) else: self.print("Not start or end point..") current_position = current_path.end return paths_in_order def add_segment(self, segment): self.all_segments.append(segment) self.visited_waypoints = np.append(self.visited_waypoints, segment.path, axis=0) self.tmp_coverable_pcd.covered_points_idx = np.unique( np.append(self.tmp_coverable_pcd.covered_points_idx, segment.covered_points_idx)) self.uncovered_coverable_points_idx = self.delete_values( self.uncovered_coverable_points_idx, segment.covered_points_idx) def get_covered_points_idx_from_paths(self, paths): """Finds indices of all covered points by the given paths Args: paths: Generated Paths of class RandomBAstarSegment Returns: List of points indices that has been covered """ covered_points_idx = np.array([]) for path in paths: covered_points_idx = np.unique( np.append(covered_points_idx, path.covered_points_idx, axis=0)) return covered_points_idx def follow_paths(self, start_position, paths_to_visit_in_order): """Connects all paths with Astar and make the robot walk through the paths. Args: start_position: A [x,y,z] np.array of the start position of the robot paths_to_visit_in_order: Ordered list of paths of types RandomSpiralSegment and BAStarSegment """ current_position = start_position for idx, path in enumerate(paths_to_visit_in_order): self.print("Moving to start of path " + str(idx + 1) + " out of " + str(len(paths_to_visit_in_order))) path_to_next_starting_point = self.motion_planner.Astar( current_position, path.start) self.follow_path(path_to_next_starting_point) self.path = np.append(self.path, path.path, axis=0) self.coverable_pcd.covered_points_idx = np.unique( np.append(self.coverable_pcd.covered_points_idx, path.covered_points_idx, axis=0)) current_position = self.path[-1] def get_random_uncovered_point(self, ignore_list=None, iter=False): """Returns a random uncovered point Args: visited_waypoints: Nx3 array with waypoints that has been visited iter (bool, optional): Integer for random seed. Defaults to False. Returns: A [x,y,z] position of an unvisited point. """ uncovered_coverable_points_idx = self.uncovered_coverable_points_idx if iter is not False: np.random.seed(20 * iter) if ignore_list is not None: uncovered_coverable_points_idx = self.delete_values( uncovered_coverable_points_idx, ignore_list) while len(uncovered_coverable_points_idx ) and not self.time_limit_reached(): random_idx = np.random.choice(len(uncovered_coverable_points_idx), 1, replace=False)[0] random_uncovered_coverable_point_idx = uncovered_coverable_points_idx[ random_idx] random_uncovered_coverable_point = self.coverable_pcd.points[ random_uncovered_coverable_point_idx] closest_traversable_point = self.traversable_pcd.find_k_nearest( random_uncovered_coverable_point, 1)[0] if self.has_been_visited(closest_traversable_point, self.visited_threshold, self.visited_waypoints): close_coverable_points_idx = self.tmp_coverable_pcd.points_idx_in_radius( random_uncovered_coverable_point, ROBOT_RADIUS) self.uncovered_coverable_points_idx = self.delete_values( self.uncovered_coverable_points_idx, close_coverable_points_idx) #self.print("Has been visited. Removing " + str(len(close_coverable_points_idx))) closest_not_visited = self.find_closest_traversable( closest_traversable_point, self.step_size, self.visited_threshold, self.visited_waypoints, self.step_size * 10) if closest_not_visited is False: #self.print("BFS could not find an unvisited close") continue return closest_not_visited continue if not self.accessible(random_uncovered_coverable_point, self.visited_waypoints): close_coverable_points_idx = self.tmp_coverable_pcd.points_idx_in_radius( random_uncovered_coverable_point, ROBOT_RADIUS) self.uncovered_coverable_points_idx = self.delete_values( self.uncovered_coverable_points_idx, close_coverable_points_idx) #self.print("Inaccessible. Removing " + str(len(close_coverable_points_idx))) continue break if len(uncovered_coverable_points_idx ) and not self.time_limit_reached(): return closest_traversable_point return False def delete_values(self, array, values): ''' Removes specific values from an array with unique values Args: array: NumPy array with unique values to remove values from values: NumPy array with values that should be removed. ''' return array[np.isin(array, values, assume_unique=True, invert=True)] def delete_values_not_unique(self, array, values): ''' Removes specific values from an array Args: array: NumPy array to remove values from values: NumPy array with values that should be removed. ''' return array[np.isin(array, values, invert=True)]