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