def get_traversable_and_coverable_points(self):
desize = int(len(self.points) / 70000)
obstacle_points = self.get_points_that_are(OBSTACLE)[::desize, :]
coverable_points = self.get_points_that_are(COVERABLE)[::desize, :]
self.print("obst" + str(obstacle_points.shape))
self.print("coverable_points" + str(coverable_points.shape))
ground_points_idx = np.where(self.map.flatten() == COVERABLE)[0]
traversable_points_idx = ground_points_idx
full_pcd = self.map_to_full_pcd()
dist_matrix = distance_matrix(obstacle_points, coverable_points)
rows, cols = np.where(dist_matrix < 10 * self.resolution)
border_points = obstacle_points[rows]
for i, untraversable_point in enumerate(border_points):
#if i % 100 == 0:
# self.self.print("Working on border pos " + str(i) + " out of " + str(len(uncoverable_border_points)))
collision_risk_points = full_pcd.points_idx_in_radius(
untraversable_point, ROBOT_RADIUS)
traversable_points_idx = self.delete_values(
traversable_points_idx, collision_risk_points)
traversable_pcd = PointCloud(
print, points=full_pcd.points[traversable_points_idx.astype(int)])
coverable_points_idx_queue = ground_points_idx
coverable_points_idx_queue = self.delete_values(
coverable_points_idx_queue, traversable_points_idx)
false_coverable_points_idx = np.array([])
while len(coverable_points_idx_queue):
if len(coverable_points_idx_queue) % 1000 == 0:
self.print("coverable_points_idx_queue: " +
str(len(coverable_points_idx_queue)))
point_idx, coverable_points_idx_queue = coverable_points_idx_queue[
0], coverable_points_idx_queue[1:]
distance_to_nearest_traversable_point = traversable_pcd.distance_to_nearest(
full_pcd.points[point_idx])
if distance_to_nearest_traversable_point > ROBOT_RADIUS:
false_coverable_points_idx = np.append(
false_coverable_points_idx, point_idx)
real_coverable_points_idx = self.delete_values(
ground_points_idx, false_coverable_points_idx)
#self.print((len(real_coverable_points_idx), len(traversable_points_idx), len(false_coverable_points_idx), len(full_pcd.points)))
return full_pcd.points[traversable_points_idx], full_pcd.points[
real_coverable_points_idx]
def __init__(self,
print,
cache_dictionary,
pcd_file,
do_terrain_assessment=False):
self.full_pcd = PointCloud(print, file=pcd_file)
super().__init__(print, cache_dictionary, do_terrain_assessment)
def __init__(self, print, cache_dictionary, do_terrain_assessment=False):
self.cache_dictionary = cache_dictionary
self.print = print
if do_terrain_assessment:
self.print("Doing terrain assessment.")
traversable_points, coverable_points, inaccessible_points = self.do_terrain_assessment(
)
self.print("Saved terrain assessment in " +
str(self.cache_dictionary))
else:
traversable_points, coverable_points, inaccessible_points = self.get_terrain_assessment_data(
)
self.traversable_pcd = PointCloud(print, points=traversable_points)
self.coverable_pcd = PointCloud(print, points=coverable_points)
self.inaccessible_pcd = PointCloud(print, points=inaccessible_points)
def main():
#### STEP 1 - Get classified pointcloud ####
environment = PointCloudEnvironment(my_print, TERRAIN_ASSESSMENT_FILE,
POINTCLOUD_FILE)
coverable_points = environment.coverable_pcd.points
traversable_points = environment.traversable_pcd.points
motion_planner = MotionPlanner(my_print, environment.traversable_pcd)
###
TIME_LIMIT = 400
start_point = np.array([28.6, -6.7, -10.3])
goal_coverage = 0.97
paths_markers = []
#Get CPP path
for pub_path in ALL_PATHS:
if not pub_path["do_calc"]:
continue
coverable_pcd = PointCloud(my_print, points=coverable_points)
if pub_path["cpp"] == "BA*":
cpp = BAstar(my_print,
motion_planner,
coverable_pcd,
time_limit=TIME_LIMIT,
parameters=pub_path["param"])
if pub_path["cpp"] == "Inward Spiral":
cpp = Spiral(my_print,
motion_planner,
coverable_pcd,
time_limit=TIME_LIMIT,
parameters=pub_path["param"])
if pub_path["cpp"] == "Sampled BA*":
cpp = RandomBAstar3(my_print,
motion_planner,
coverable_pcd,
time_limit=TIME_LIMIT,
parameters=pub_path["param"])
ns = pub_path["ns"]
pub_path["path"] = cpp.get_cpp_path(start_point,
goal_coverage=goal_coverage)
pub_path["markers"] = cpp.points_to_mark
pub_path["stats"] = cpp.print_stats(pub_path["path"])
save_data(ALL_PATHS)
print("DONE!")
def map_to_full_pcd(self):
return PointCloud(print, points=self.points)
"sampled_bastar_param",
"hyper_time_limit":
250,
"hyper_min_coverage":
95,
"do_experiment":
True,
"experiment_time_limit":
400,
"experiment_results": [],
"sample_specific_stats": [],
"hyper_data": [],
"formatted_hyper_data": [],
"cpp":
lambda print, motion_planner, cov_points, time_limit, parameters:
RandomBAstar(print, motion_planner, PointCloud(
print, points=cov_points), time_limit, parameters),
'line':
'm',
'confidence_color': (1.0, 0.0, 1.0, 0.3)
}
}
###################
def my_print(text):
if PRINT:
return print(text)
else:
return
"hyper_test":
"step_param",
"hyper_time_limit":
250,
"hyper_min_coverage":
95,
"do_experiment":
True,
"experiment_time_limit":
400,
"experiment_results": [],
"hyper_data": [],
"formatted_hyper_data": [],
"cpp":
lambda print, motion_planner, cov_points, time_limit, parameters:
Spiral(print, motion_planner, PointCloud(print, points=cov_points),
time_limit, parameters),
'line':
'b',
'confidence_color': (0.0, 0.0, 1.0, 0.3)
}
}
###################
def my_print(text):
if PRINT:
return print(text)
else:
return
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
RESULTS_FILE = 'garage_max_coverage_3.dictionary'
HYPER_MAX_EVAL = 100
NUMBER_OF_START_POINTS = 10
HYPER_START_POS = np.array([28.6, -6.7, -10.3])
PRINT = False
ALGORITHMS = {
"BFS": {
"name": "BFS",
"do_hyper": True,
"hyper_time_limit": 250,
"hyper_min_coverage": 100,
"hyper_data": [],
"formatted_hyper_data": [],
"cpp": lambda print, motion_planner, cov_points, time_limit, parameters: BFS(print, motion_planner, PointCloud(print, points= cov_points), time_limit, parameters) ,
'line': 'b',
'confidence_color': (0.0, 0.0, 1.0, 0.3)
}
}
###################
def my_print(text):
if PRINT:
return print(text)
else:
return
def get_random_point(all_points):
return all_points[np.random.randint(len(all_points))]
class PointClassification():
''' Class for calculating points where a robot could go without collision.
'''
def __init__(self, print, pcd):
'''
Args:
print: function for printing messages
pcd: Point Cloud of the environment.
'''
self.pcd = pcd
self.print = print
def get_classified_points(self, ground_points_idx, uncoverable_border_points):
''' Given some points, find those which are traversable by the robot,
given robot size specifics. go through every point in the point cloud
that is classified as obstacle and filter out ground points that are
close.
Args:
ground_points_idx: indexes of points in the point cloud that are
classified as obstacle free.
'''
start = timeit.default_timer()
self.ground_point_cloud = PointCloud(self.print, points= self.pcd.points[ground_points_idx])
traversable_points_idx = ground_points_idx
false_uncoverable_idx = []
self.print("Starting search...")
for i, uncoverable_point in enumerate(uncoverable_border_points):
#self.print("Working on " + str(i) + " out of " + str(len(uncoverable_border_points)))
distance_to_nearest_ground_point = self.ground_point_cloud.distance_to_nearest(uncoverable_point)
if distance_to_nearest_ground_point < CELL_SIZE/2:
false_uncoverable_idx.append(i)
uncoverable_border_points = np.delete(uncoverable_border_points, false_uncoverable_idx, 0)
for i, untraversable_point in enumerate(uncoverable_border_points):
#if i % 100 == 0:
# self.print("Working on border pos " + str(i) + " out of " + str(len(uncoverable_border_points)))
collision_risk_points = self.pcd.points_idx_in_radius(untraversable_point, np.sqrt(1/2)*CELL_SIZE + 0.5*ROBOT_SIZE)
traversable_points_idx = self.delete_values(traversable_points_idx, collision_risk_points)
#Hindsight wrong classified points removal by hand:
wrong_points = np.empty((0,3))
#For pointcloud.pcd:
#wrong_points = np.append(wrong_points, [[24.395610809326172, 12.705216407775879, -5.311060428619385]], axis=0)
#wrong_points = np.append(wrong_points, [[-17.590679168701172, -3.7045161724090576, -6.118121147155762]], axis=0)
#For bridge.pcd:
wrong_points = np.append(wrong_points, [[-98.5624, 182.8, -30.83]], axis=0)
wrong_points = np.append(wrong_points, [[ 0.8125 , 93.30000305, -32.33319855]], axis=0)
wrong_points = np.append(wrong_points, [[-17.590679168701172, -3.7045161724090576, -6.118121147155762]], axis=0)
#For small bridge:
#wrong_points = np.append(wrong_points, [[ -5.11 , 15.47, -0.39]], axis=0)
#wrong_points = np.append(wrong_points, [[ -5.11 , 15.97, -0.39]], axis=0)
for wrong_point in wrong_points:
points_nearby = self.pcd.points_idx_in_radius(wrong_point, 0.5*ROBOT_SIZE)
traversable_points_idx = self.delete_values(traversable_points_idx, points_nearby)
traversable_pcd = PointCloud(self.print, points= self.pcd.points[traversable_points_idx.astype(int)])
coverable_points_idx_queue = ground_points_idx
coverable_points_idx_queue = self.delete_values(coverable_points_idx_queue, traversable_points_idx)
false_coverable_points_idx = np.array([])
while len(coverable_points_idx_queue):
#if len(coverable_points_idx_queue) % 1000 == 0:
# self.print("coverable_points_idx_queue: " + str(len(coverable_points_idx_queue)))
point_idx, coverable_points_idx_queue = coverable_points_idx_queue[0], coverable_points_idx_queue[1:]
point = self.pcd.points[point_idx]
distance_to_nearest_traversable_point = traversable_pcd.distance_to_nearest(self.pcd.points[point_idx])
if distance_to_nearest_traversable_point > ROBOT_SIZE/2:
false_coverable_points_idx = np.append(false_coverable_points_idx, point_idx)
real_coverable_points_idx = self.delete_values(ground_points_idx, false_coverable_points_idx)
stats = self.print_result(start, len(real_coverable_points_idx), len(traversable_points_idx), len(false_coverable_points_idx), len(self.pcd.points))
return self.pcd.points[traversable_points_idx], self.pcd.points[real_coverable_points_idx], self.pcd.points[false_coverable_points_idx.astype(int)], stats
def delete_values(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, assume_unique=True, invert=True) ]
def print_result(self, start, nbr_of_coverable, nbr_of_traversable, nbr_of_inaccessable, total_nbr_of_points):
''' Prints result data of the point classification.
'''
end = timeit.default_timer()
self.print("="*20)
self.print("POINT CLASSIFCATION")
self.print("Computational time: " + str(round(end - start, 1)) + " sec")
self.print("Number of COVERABLE points: " + str(nbr_of_coverable))
self.print("Number of TRAVERSABLE points: " + str(nbr_of_traversable))
self.print("Number of INACCESSABLE points: " + str(nbr_of_inaccessable))
self.print("Number of OBSTACLE points: " + str(total_nbr_of_points - nbr_of_inaccessable- nbr_of_coverable))
self.print("TOTAL Number of points: " + str(total_nbr_of_points))
return {
"Computational time": round(end - start, 1),
"Number of COVERABLE points": nbr_of_coverable,
"Number of TRAVERSABLE points": nbr_of_traversable,
"Number of INACCESSABLE points": nbr_of_inaccessable,
"Number of OBSTACLE points": total_nbr_of_points - nbr_of_inaccessable- nbr_of_coverable,
"TOTAL Number of points": total_nbr_of_points
}
def get_classified_points(self, ground_points_idx, uncoverable_border_points):
''' Given some points, find those which are traversable by the robot,
given robot size specifics. go through every point in the point cloud
that is classified as obstacle and filter out ground points that are
close.
Args:
ground_points_idx: indexes of points in the point cloud that are
classified as obstacle free.
'''
start = timeit.default_timer()
self.ground_point_cloud = PointCloud(self.print, points= self.pcd.points[ground_points_idx])
traversable_points_idx = ground_points_idx
false_uncoverable_idx = []
self.print("Starting search...")
for i, uncoverable_point in enumerate(uncoverable_border_points):
#self.print("Working on " + str(i) + " out of " + str(len(uncoverable_border_points)))
distance_to_nearest_ground_point = self.ground_point_cloud.distance_to_nearest(uncoverable_point)
if distance_to_nearest_ground_point < CELL_SIZE/2:
false_uncoverable_idx.append(i)
uncoverable_border_points = np.delete(uncoverable_border_points, false_uncoverable_idx, 0)
for i, untraversable_point in enumerate(uncoverable_border_points):
#if i % 100 == 0:
# self.print("Working on border pos " + str(i) + " out of " + str(len(uncoverable_border_points)))
collision_risk_points = self.pcd.points_idx_in_radius(untraversable_point, np.sqrt(1/2)*CELL_SIZE + 0.5*ROBOT_SIZE)
traversable_points_idx = self.delete_values(traversable_points_idx, collision_risk_points)
#Hindsight wrong classified points removal by hand:
wrong_points = np.empty((0,3))
#For pointcloud.pcd:
#wrong_points = np.append(wrong_points, [[24.395610809326172, 12.705216407775879, -5.311060428619385]], axis=0)
#wrong_points = np.append(wrong_points, [[-17.590679168701172, -3.7045161724090576, -6.118121147155762]], axis=0)
#For bridge.pcd:
wrong_points = np.append(wrong_points, [[-98.5624, 182.8, -30.83]], axis=0)
wrong_points = np.append(wrong_points, [[ 0.8125 , 93.30000305, -32.33319855]], axis=0)
wrong_points = np.append(wrong_points, [[-17.590679168701172, -3.7045161724090576, -6.118121147155762]], axis=0)
#For small bridge:
#wrong_points = np.append(wrong_points, [[ -5.11 , 15.47, -0.39]], axis=0)
#wrong_points = np.append(wrong_points, [[ -5.11 , 15.97, -0.39]], axis=0)
for wrong_point in wrong_points:
points_nearby = self.pcd.points_idx_in_radius(wrong_point, 0.5*ROBOT_SIZE)
traversable_points_idx = self.delete_values(traversable_points_idx, points_nearby)
traversable_pcd = PointCloud(self.print, points= self.pcd.points[traversable_points_idx.astype(int)])
coverable_points_idx_queue = ground_points_idx
coverable_points_idx_queue = self.delete_values(coverable_points_idx_queue, traversable_points_idx)
false_coverable_points_idx = np.array([])
while len(coverable_points_idx_queue):
#if len(coverable_points_idx_queue) % 1000 == 0:
# self.print("coverable_points_idx_queue: " + str(len(coverable_points_idx_queue)))
point_idx, coverable_points_idx_queue = coverable_points_idx_queue[0], coverable_points_idx_queue[1:]
point = self.pcd.points[point_idx]
distance_to_nearest_traversable_point = traversable_pcd.distance_to_nearest(self.pcd.points[point_idx])
if distance_to_nearest_traversable_point > ROBOT_SIZE/2:
false_coverable_points_idx = np.append(false_coverable_points_idx, point_idx)
real_coverable_points_idx = self.delete_values(ground_points_idx, false_coverable_points_idx)
stats = self.print_result(start, len(real_coverable_points_idx), len(traversable_points_idx), len(false_coverable_points_idx), len(self.pcd.points))
return self.pcd.points[traversable_points_idx], self.pcd.points[real_coverable_points_idx], self.pcd.points[false_coverable_points_idx.astype(int)], stats
def __init__(self):
super().__init__('MainNode')
#Publishers:
self.markers_pub = self.create_publisher(
visualization_msgs.MarkerArray, 'marker', 3000)
self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd',
10)
self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'coverable_pcd', 100)
self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'visited_pcd', 100)
self.visited_ground_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'visited_ground_pcd', 100)
self.traversable_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'traversable_pcd', 100)
self.inaccessible_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'inaccessible_pcd', 100)
self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10)
#Varaiables for publishers
self.last_id = 0
timer_period = 5
animation_time_period = 0.01
self.animation_iteration = 0
self.path = []
#Subscribers:
self.rviz_sub = self.create_subscription(geometry_msgs.PointStamped,
"clicked_point",
self.clicked_point_cb, 100)
#environment = PointCloudEnvironment(self.print, "cached_coverable_points.dictionary", "pointcloud.pcd")
#environment = MapEnvironment(self.print, "simple_open.dictionary", "src/exjobb/maps/map_simple_open.png", 0.015)
#environment = MapEnvironment(self.print, "map_ipa_apartment.dictionary", "src/exjobb/maps/map_ipa_apartment.png", 0.05)
NEW_POINTCLOUD = False
if NEW_POINTCLOUD:
environment = MapEnvironment(
self.print, "simple_open.dictionary",
"src/exjobb/maps/map_simple_open.png", 0.015)
self.point_cloud = PointCloud(self.print,
file="cross.pcd",
new_point_cloud=True)
else:
#environment = PointCloudEnvironment(self.print, "pointcloud2.dictionary", "pointcloud2.pcd", False) #x,y = #351451.84, 4022966.5 Street
#environment = PointCloudEnvironment(self.print, "pointcloud3.dictionary", "pointcloud3.pcd", False) #x,y = (351609.25, 4022912.0) Same Underground garage one floor
#environment = PointCloudEnvironment(self.print, "pointcloud4.dictionary", "pointcloud4.pcd", False) #x,y = (326815.75, 4152473.25) Busy street, with cars
#environment = PointCloudEnvironment(self.print, "cached_coverable_points.dictionary", "pointcloud.pcd")
#environment = MapEnvironment(self.print, "map_ipa_apartment.dictionary", "src/exjobb/maps/map_ipa_apartment.png", 0.05)
#environment = PointCloudEnvironment(self.print, "bridge_2_small.dictionary", "bridge_2_small.pcd", False)
#environment = PointCloudEnvironment(self.print, "cross_terrain_assessment.dictionary", "cross.pcd", False)
#environment = PointCloudEnvironment(self.print, "pre_garage_terrain_assessment.dictionary", "garage.pcd", False)'
environment = PointCloudEnvironment(
self.print, "garage_terrain_assessment.dictionary",
"garage.pcd", False)
#environment = PointCloudEnvironment(self.print, "bridge_terrain_assessment.dictionary", "bridge_2.pcd", False)
#environment = MapEnvironment(self.print, "simple_open.dictionary", "src/exjobb/maps/map_simple_open.png", 0.015)
self.point_cloud = environment.full_pcd
#traversable_points, coverable_points, inaccessible_points = self.do_terrain_assessment()
#self.traversable_point_cloud = PointCloud(self.print, points= traversable_points)
#self.coverable_point_cloud = PointCloud(self.print, points= coverable_points)
#self.inaccessible_point_cloud = PointCloud(self.print, points= inaccessible_points)
#
self.point_cloud.pcd = self.point_cloud.point_cloud(
self.point_cloud.points, 'my_frame')
self.traversable_point_cloud = environment.traversable_pcd
self.traversable_point_cloud.pcd = self.traversable_point_cloud.point_cloud(
self.traversable_point_cloud.points, 'my_frame')
self.coverable_point_cloud = environment.coverable_pcd
self.coverable_point_cloud.pcd = self.coverable_point_cloud.point_cloud(
self.coverable_point_cloud.points, 'my_frame')
self.inaccessible_point_cloud = environment.inaccessible_pcd
self.inaccessible_point_cloud.pcd = self.inaccessible_point_cloud.point_cloud(
self.inaccessible_point_cloud.points, 'my_frame')
if SMALL_POINT_CLOUD:
bbox = o3d.geometry.AxisAlignedBoundingBox([10, 15, -5.3],
[15, 21, 10])
trav_points_idx = bbox.get_point_indices_within_bounding_box(
self.traversable_point_cloud.raw_pcd.points)
self.traversable_point_cloud = PointCloud(
self.print,
points=self.traversable_point_cloud.points[trav_points_idx])
cov_points_idx = bbox.get_point_indices_within_bounding_box(
self.coverable_point_cloud.raw_pcd.points)
self.coverable_point_cloud = PointCloud(
self.print,
points=self.coverable_point_cloud.points[cov_points_idx])
self.markers = []
motion_planner = MotionPlanner(self.print,
self.traversable_point_cloud)
if PUBLISH_INACCESSIBLE_PCD:
inaccessible_pcd_pub = self.create_timer(
timer_period, self.inaccessible_point_cloud_publisher)
if MOTION_PLANNER_TEST:
#start_pos = [5.0625 , 91.05000305, -32.58319855]
#end_pos = [ 0.8125 , 93.30000305, -32.33319855]
end_pos = np.array([6.05999994, -13., -5.71468687])
start_pos = np.array([28.6, -6.7, -10.3])
start_point = self.traversable_point_cloud.find_k_nearest(
start_pos, 1)[0]
end_point = self.traversable_point_cloud.find_k_nearest(
end_pos, 1)[0]
self.path = motion_planner.Astar(start_point, end_point)
self.markers.append({"point": start_point, "color": RED})
self.markers.append({"point": end_point, "color": BLUE})
if self.path is False:
self.path = []
if CPP_TEST:
random_idx = np.random.choice(len(
self.traversable_point_cloud.points),
1,
replace=False)[0]
#start_point = [-14, -16, -3.6] #
#start_point = [-23, 30, -0.9]
start_point = self.traversable_point_cloud.points[random_idx]
#SAMPLED BA*
#cost 4953, length: 3684, rotation: 1269
######real: cost: ?? lkength: 3235, rotation: 1108
sparam1 = {
'ba_exploration': 0.90756041115558,
'max_distance': 4.78202945337845,
'max_distance_part_II': 6.75513650527977,
'min_bastar_cost_per_coverage': 8192.530314616084,
'min_spiral_cost_per_coverage': 12157.969167186768,
'step_size': 0.562061544696692,
'visited_threshold': 0.279490436505789
}
#cost 4615, length: 3294, rotation: 1321
######real: cost: ??, length: 3334, rotation: 1304
sparam2 = {
'ba_exploration': 0.816319265003861,
'max_distance': 1.02476727664307,
'max_distance_part_II': 4.76356301411862,
'min_bastar_cost_per_coverage': 6616.530314616084,
'min_spiral_cost_per_coverage': 19277.969167186768,
'step_size': 0.950568870175564,
'visited_threshold': 0.484179597225153
}
#cost 4261, length: 3158, rotation: 1103
#######real: cost: ??, length: 3078, rotation: 1114
sparam3 = {
'ba_exploration': 0.853031300592955,
'max_distance': 3.89663024793223,
'max_distance_part_II': 4.80685526433465,
'min_bastar_cost_per_coverage': 9312.530314616084,
'min_spiral_cost_per_coverage': 13196.969167186768,
'step_size': 0.636195719728099,
'visited_threshold': 0.337665370485907
}
#length: 3596, rotation: 1296, 97% - annan step size (0.6..)
#real: cost: 4306, length: 3073, rotation: 1233
param4 = {
'ba_exploration': 0.8653615601139727,
'max_distance': 4.129493635268686,
'max_distance_part_II': 6.935911381739787,
'min_bastar_cost_per_coverage': 8238.530314616084,
'min_spiral_cost_per_coverage': 13644.969167186768,
'step_size': 0.54868363557903,
'visited_threshold': 0.3730115058138923
}
#cost: 5797, length: 4643, rotation: 1154, 97% - annan step size (0.6..)
#real: cost: 6422, length: 5116, rotation: 1306
param_best_brdige = {
'ba_exploration': 0.939978646944692,
'max_distance': 4.49053749147136,
'max_distance_part_II': 7.05948312639,
'min_bastar_cost_per_coverage': 12772.530314616084,
'min_spiral_cost_per_coverage': 25988.969167186768,
'step_size': 0.618705451980032,
'visited_threshold': 0.38872474480067
}
#cost: 3001, length: 2186, rotation: 815
#real: cost: 3083, length: 2281, rotation: 802
param_best_cross = {
'ba_exploration': 0.863145455156051,
'max_distance': 1.69280755868826,
'max_distance_part_II': 4.48375188984703,
'min_bastar_cost_per_coverage': 6488.530314616084,
'min_spiral_cost_per_coverage': 8141.257661974652297,
'step_size': 0.553977048496769,
'visited_threshold': 0.38872474480067
}
#BASTAR:
#cost: 16062, lenth: 10575 rotation: 5487
param1 = {
'angle_offset': 3.44800051788481,
'step_size': 0.963400677899873,
'visited_threshold': 0.257015802906527
}
#cost: 7583, lenth: 4452 rotation: 3131
param2 = {
'angle_offset': 3.78341027362029,
'step_size': 0.601687134922371,
'visited_threshold': 0.328108983656107
}
#cost: 5013, lenth: 3049 rotation: 1964
param3 = {
'angle_offset': 5.27158130667689,
'step_size': 0.517468289229711,
'visited_threshold': 0.455659073558674
}
#cost: 4238, lenth: 2896 rotation: 1342
param4 = {
'angle_offset': 4.64664343656672,
'step_size': 0.633652049936913,
'visited_threshold': 0.472819723019576
}
#cost: 3262, lenth: 2249 rotation: 1013
param_best_cross = {
'angle_offset': 4.70135588957793,
'step_size': 0.523646671416283,
'visited_threshold': 0.403681713288835
}
#cost: 6385, lenth: 4562 rotation: 1823
param_best_brdige = {
'angle_offset': 5.33881157053433,
'step_size': 0.55692737194204,
'visited_threshold': 0.453169184364576
}
#SPIRAL:
#cost: 14292, lenth: 7523 rotation: 6769
param1 = {
'step_size': 0.999314930298507,
'visited_threshold': 0.32443603324225
}
#cost: 7431, lenth: 3990 rotation: 3441
param2 = {
'step_size': 0.825030992319859,
'visited_threshold': 0.433448258850281
}
#cost: 6466, lenth: 3218 rotation: 3248
param3 = {
'step_size': 0.521396930930628,
'visited_threshold': 0.47473068968531
}
#cost: 5787, lenth: 3101 rotation: 2686
param4 = {
'step_size': 0.627870706339337,
'visited_threshold': 0.498775709725593
}
#cost: 7213, lenth: 4440 rotation: 2773
param_best_brdige = {
'step_size': 0.737114020263598,
'visited_threshold': 0.483088877473477
}
#cost: 4054, lenth: 2239 rotation: 1815
param_best_cross = {
'step_size': 0.664671825076571,
'visited_threshold': 0.499669038773602
}
#param = {'step_size': 0.5,
# 'visited_threshold': 0.4}
start_point = [-20.7, 43, -1]
#start_point = np.array([28.6, -6.7, -10.3]) #garage
start_point = np.array([-53.7, 54.2, -2.7]) #bridge
#start_point = np.array([-20.7, 43, -1]) #cross
#start_point = np.array([15.6, -16.7, -5.3])
#start_point = np.array([0.6,0.6,0])
#start_points = {}
#for n in range(10):
# random_idx = np.random.choice(len(self.traversable_point_cloud.points), 1, replace=False)[0]
# start_points[n] = self.traversable_point_cloud.points[random_idx]
# #self.markers.append( {"point": self.traversable_point_cloud.points[random_idx], "color": RED} )
#self.print(start_points)
self.cpp = RandomBAstar3(self.print,
motion_planner,
self.coverable_point_cloud,
time_limit=300,
parameters=sparam4)
self.path = self.cpp.get_cpp_path(start_point, goal_coverage=0.97)
#self.path = self.cpp.breadth_first_search(start_point)
#self.print(self.cpp.print_results())
#self.path = self.cpp.get_cpp_node_path(start_point)
self.print(self.cpp.print_stats(self.path))
for marker in self.cpp.points_to_mark:
self.markers.append(marker)
#self.markers.append( {"point": self.path[-1], "color": RED} )
#self.points_to_mark = [self.path[-1]]
if PUBLISH_FULL_PCD:
#pcd_pub = self.create_timer(timer_period, self.point_cloud_publisher)
self.point_cloud_publisher()
if PUBLISH_GROUND_PCD:
#coverable_pcd_pub = self.create_timer(timer_period, self.coverable_point_cloud_publisher)
self.coverable_point_cloud_publisher()
if PUBLISH_TRAVERSABLE_PCD:
#traversable_pcd_pub = self.create_timer(timer_period, self.traversable_point_cloud_publisher)
self.traversable_point_cloud_publisher()
#HYPER_START_POS = np.array([-53.7, 54.2, -2.7])
#start_points = {
# 0: np.array([-43.10443115, 3.99802136, 4.46702003]),
# 1: np.array([ 21.61431885, -33.00197983, -2.77298403]),
# 2: np.array([-34.51068115, 12.49802208, -4.17298126]),
# 3: np.array([ 15.9268198 , -36.00197983, -2.6929822 ]),
# 4: np.array([38.98931885, 45.49802399, 1.19701743]),
# 5: np.array([ 3.73931861, 40.74802399, 2.83701849]),
# 6: np.array([ 15.5205698 , -31.50197792, -2.8729825 ]),
# 7: np.array([-16.44818115, -19.25197792, -3.58298159]),
# 8: np.array([10.52056885, 42.74802399, 2.46701956]),
# 9: np.array([53.89556885, 35.99802399, 0.33701676])}
#for point in start_points.values():
# self.markers.append({
# "point": point,
# "color": [0.0,0.0,1.0]
# })
#self.markers.append({
# "point": HYPER_START_POS,
# "color": [0.0,1.0,0.0]
# })
#CPP_TEST = True
if PUBLISH_MARKERS and len(self.markers):
#for marker in self.cpp.points_to_mark:
# self.markers.append(marker)
markers_pub = self.create_timer(timer_period,
self.marker_publisher)
if PUBLISH_PATH and len(self.path) > 0 and not PUBLISH_PATH_ANIMATION:
path_pub = self.create_timer(timer_period, self.path_publisher)
if PUBLISH_VISITED_PCD:
self.point_cloud.visit_path(self.path)
self.visited_points_pcd = self.point_cloud.get_covered_points_as_pcd(
)
visited_pcd_pub = self.create_timer(
timer_period, self.visited_point_cloud_publisher)
if PUBLISH_VISITED_GROUND_PCD and len(self.path):
#self.coverable_point_cloud = PointCloud(self.print, points= coverable_points)
self.coverable_point_cloud.visit_path(self.path)
self.visited_ground_points_pcd = self.coverable_point_cloud.get_covered_points_as_pcd(
)
visited_ground_pcd_pub = self.create_timer(
timer_period, self.visited_ground_point_cloud_publisher)
if PUBLISH_PATH_ANIMATION and len(self.path) > 0:
time.sleep(8)
self.coverable_point_cloud.covered_points_idx = np.array([])
path_pub = self.create_timer(animation_time_period,
self.animated_path_publisher)
if PUBLISH_SEGMENTS_ANIMATION and len(self.cpp.all_segments) > 0:
time.sleep(8)
self.coverable_point_cloud.covered_points_idx = np.array([])
self.segments = self.cpp.all_segments
path_pub = self.create_timer(animation_time_period,
self.animated_segment_publisher)
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 = 1
if False:
with open('cached_sampled_show.dictionary',
'rb') as cached_pcd_file:
cache_data = pickle.load(cached_pcd_file)
self.all_segments = cache_data["all_segments"]
else:
#### 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
random_point = self.get_random_uncovered_point(
ignore_list=self.explored_pcd.covered_points_idx,
iter=iter)
#self.points_to_mark.append({
# "point": random_point,
# "color": [1.0,0.0,1.0]
#})
if random_point is False:
break
closest_border_point, _ = self.find_closest_border(
random_point, self.step_size, self.visited_threshold,
self.visited_waypoints)
self.points_to_mark.append({
"point": closest_border_point,
"color": [0.0, 1.0, 0.0]
})
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)
accepted_segments = list(
filter(
lambda x: self.get_cost_per_coverage(x) < self.
min_bastar_cost_per_coverage, 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"))
#return best_BA_segment.path
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
})
#return best_BA_segment.path
#return best_BA_segment.path
self.print_segment_stats(best_BA_segment, "BA*", iter)
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
#### 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
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_segment_stats(spiral_segment, "Spiral", iter)
#self.print(self.get_cost_per_coverage(spiral_segment))
if self.get_cost_per_coverage(
spiral_segment) > self.min_spiral_cost_per_coverage:
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
# if False:
with open('cached_sampled_show.dictionary',
'wb') as cached_pcd_file:
cache_data = {"all_segments": self.all_segments}
pickle.dump(cache_data, cached_pcd_file)
total = np.empty((0, 3))
for segment in self.all_segments:
total = np.append(total, segment.path, axis=0)
#return total
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
PRINT = True
ALGORITHMS = {
"Sampled BA*": {
"name": "Sampled BA* & Inward Spiral",
"do_hyper": False,
"hyper_test": "sampled_bastar_param",
"hyper_time_limit": 250,
"hyper_min_coverage": 95,
"do_experiment": False,
"experiment_time_limit": 400,
"experiment_results": [],
"sample_specific_stats": [],
"hyper_data": [],
"formatted_hyper_data": [],
"cpp": lambda print, motion_planner, cov_points, time_limit, parameters: RandomBAstar(print, motion_planner, PointCloud(print, points= cov_points), time_limit, parameters),
'line': 'm',
'confidence_color': (1.0, 0.0, 1.0, 0.3)
},
"BA*": {
"name": "BA*",
"do_hyper": True,
"hyper_test": "step_param",
"hyper_time_limit": 250,
"hyper_min_coverage": 95,
"do_experiment": True,
"experiment_time_limit": 400,
"experiment_results": [],
"hyper_data": [],
"formatted_hyper_data": [],
"cpp": lambda print, motion_planner, cov_points, time_limit, parameters: BAstar(print, motion_planner, PointCloud(print, points= cov_points), time_limit, parameters) ,
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()
def __init__(self):
super().__init__('MainNode')
#Publishers:
self.markers_pub = self.create_publisher(
visualization_msgs.MarkerArray, 'marker', 3000)
self.markers_path_pub = self.create_publisher(
visualization_msgs.Marker, 'path_markers', 3000)
self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd',
10)
self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'coverable_pcd', 100)
self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'visited_pcd', 100)
self.visited_ground_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'visited_ground_pcd', 100)
self.traversable_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'traversable_pcd', 100)
self.inaccessible_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'inaccessible_pcd', 100)
self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10)
with open(FILE, 'rb') as cached_pcd_file:
cache_data = pickle.load(cached_pcd_file)
self.prev_file = deepcopy(cache_data)
#Create Environment
environment = PointCloudEnvironment(
self.print, "garage_terrain_assessment.dictionary", "garage.pcd",
False)
point_cloud = environment.full_pcd
traversable_point_cloud = environment.traversable_pcd
coverable_point_cloud = environment.coverable_pcd
motion_planner = MotionPlanner(self.print, traversable_point_cloud)
TIME_LIMIT = 400
start_point = np.array([28.6, -6.7, -10.3])
goal_coverage = 0.97
paths_markers = []
#Get CPP path
for pub_path in ALL_PATHS:
if not pub_path["do_calc"]:
continue
coverable_pcd = PointCloud(self.print,
points=coverable_point_cloud.points)
if pub_path["cpp"] == "BA*":
cpp = BAstar(self.print,
motion_planner,
coverable_pcd,
time_limit=TIME_LIMIT,
parameters=pub_path["param"])
if pub_path["cpp"] == "Inward Spiral":
cpp = Spiral(self.print,
motion_planner,
coverable_pcd,
time_limit=TIME_LIMIT,
parameters=pub_path["param"])
if pub_path["cpp"] == "Sampled BA*":
cpp = RandomBAstar3(self.print,
motion_planner,
coverable_pcd,
time_limit=TIME_LIMIT,
parameters=pub_path["param"])
ns = pub_path["ns"]
pub_path["path"] = cpp.get_cpp_path(start_point,
goal_coverage=goal_coverage)
pub_path["markers"] = cpp.points_to_mark
pub_path["stats"] = cpp.print_stats(pub_path["path"])
save_data(ALL_PATHS)
#self.print(path_msg)
#paths_markers.append(path_msg)
#path_pub = self.create_publisher(nav_msgs.Path, ns + '/path', 10)
#self.markers_pub.publish(path_msg)
#
#self.markers_msg = visualization_msgs.MarkerArray()
#
#for idx, msg in enumerate(paths_markers):
# stamp = self.get_clock().now().to_msg()
# self.markers_msg.markers.append(msg)
# #self.last_id += 1
#
markers_pub = self.create_timer(5, self.marker_publisher)
#self.markers_pub.publish(self.markers_msg)
for idx, pub_path in enumerate(ALL_PATHS):
if pub_path.get("stats", False) is False:
pub_path["stats"] = self.prev_file[idx]["stats"]
self.print("=" * 20)
self.print(pub_path["ns"])
self.print(pub_path["stats"])
self.pcd_pub.publish(
point_cloud.point_cloud(point_cloud.points, 'my_frame'))
self.coverable_pcd_pub.publish(
coverable_point_cloud.point_cloud(coverable_point_cloud.points,
'my_frame'))
self.traversable_pcd_pub.publish(
traversable_point_cloud.point_cloud(traversable_point_cloud.points,
'my_frame'))
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)]
def main():
#with open(RESULTS_FILE, 'rb') as cached_pcd_file:
# cache_data = pickle.load(cached_pcd_file)
# pprint.pprint(cache_data)
#return
#with open(RESULTS_FILE, 'rb') as cached_pcd_file:
# cache_data = pickle.load(cached_pcd_file)
# for alg in ALGORITHMS:
# if ALGORITHMS[alg]["do_hyper"]:
# ALGORITHMS[alg]["opt_param"] = cache_data[alg]["opt_param"]
with open(PREVIOUS_FILE, 'rb') as cached_pcd_file:
cache_data = pickle.load(cached_pcd_file)
ALGORITHMS = deepcopy(cache_data)
for alg in ALGORITHMS:
ALGORITHMS[alg]["do_hyper"] = False
ALGORITHMS[alg][
"cpp"] = lambda print, motion_planner, cov_points, time_limit, parameters: BAstar(
print, motion_planner, PointCloud(
print, points=cov_points), time_limit, parameters)
ALGORITHMS[alg]["experiment_results"] = []
#### STEP 1 - Get classified pointcloud ####
environment = PointCloudEnvironment(my_print, TERRAIN_ASSESSMENT_FILE,
POINTCLOUD_FILE)
coverable_points = environment.coverable_pcd.points
traversable_points = environment.traversable_pcd.points
motion_planner = MotionPlanner(my_print, environment.traversable_pcd)
#If from terrain assessment file:
#with open(TERRAIN_ASSESSMENT_FILE, 'rb') as cached_pcd_file:
# cache_data = pickle.load(cached_pcd_file)
# coverable_points = cache_data["coverable_points"]
# traversable_points = cache_data["traversable_points"]
#traversable_pcd = PointCloud(my_print, points= traversable_points)
#motion_planner = MotionPlanner(my_print, traversable_pcd)
#### STEP 2 - Hyper parameters ####
for algorithm_key, algorithm in ALGORITHMS.items():
if algorithm["do_hyper"]:
trials = Trials()
hyper_optimizer = HyptoOptimizer(save_data, algorithm, my_print,
HYPER_START_POS, motion_planner,
coverable_points)
if algorithm_key == "BA*":
opt_param = fmin(hyper_optimizer.hyper_test_bastar,
space=(hp.uniform('angle_offset', 0,
np.pi * 2),
hp.uniform('step_size', 0.5, 1),
hp.uniform('visited_threshold', 0.25,
0.5)),
algo=tpe.suggest,
max_evals=HYPER_MAX_EVAL,
trials=trials)
elif algorithm_key == "Inward Spiral":
opt_param = fmin(hyper_optimizer.hyper_test_inward_spiral,
space=(hp.uniform('step_size', 0.5, 1),
hp.uniform('visited_threshold', 0.5,
1)),
algo=tpe.suggest,
max_evals=HYPER_MAX_EVAL,
trials=trials)
elif algorithm_key == "Sampled BA*":
coverage_2 = algorithm["hyper_min_coverage"] / 100
opt_param = fmin(
hyper_optimizer.hyper_test_sampled_bastar_param,
space=(hp.uniform('coverage_1', 0.25, coverage_2),
hp.uniform('coverage_2', coverage_2 - 0.025,
coverage_2),
hp.uniform('max_distance', 1, 10),
hp.uniform('max_distance_part_II', 1,
20), hp.uniform('max_iterations', 30,
150),
hp.uniform('min_bastar_coverage', 0.005, 0.05),
hp.uniform('min_spiral_length', 2, 100),
hp.uniform('nbr_of_angles', 0.6,
8.4), hp.uniform('step_size', 0.66,
1.33),
hp.uniform('visited_threshold', 0.5, 1)),
algo=tpe.suggest,
max_evals=HYPER_MAX_EVAL,
trials=trials)
print(trials.statuses())
algorithm["opt_param"] = opt_param
algorithm["hyper_data"] = trials.trials
ALGORITHMS[algorithm_key] = algorithm
save_data(ALGORITHMS)
#### STEP 3 - Full tests ####
for start_point_nr in range(NUMBER_OF_START_POINTS):
#start_point = get_random_point(traversable_points)
start_point = start_points[start_point_nr]
print("Start point " + str(start_point_nr) + ": " + str(start_point))
for algorithm_key, algorithm in ALGORITHMS.items():
if algorithm["do_experiment"]:
experimenter = Experimenter(algorithm, print)
parameters = None
if "opt_param" in algorithm:
parameters = algorithm["opt_param"]
cpp = algorithm["cpp"](my_print, motion_planner,
coverable_points,
algorithm["experiment_time_limit"],
parameters)
if "sample_specific_stats" in algorithm:
experimenter.perform_sample_cpp(cpp, start_point,
start_point_nr)
algorithm["sample_specific_stats"].append(
experimenter.sample_specific_stats)
else:
experimenter.perform_cpp(cpp, start_point, start_point_nr)
algorithm["experiment_results"].append(experimenter.results)
ALGORITHMS[algorithm_key] = algorithm
save_data(ALGORITHMS)
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 __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
"hyper_time_limit":
250,
"hyper_min_coverage":
95,
"do_experiment":
True,
"experiment_time_limit":
400,
"experiment_results": [],
"sample_specific_stats": [],
"hyper_data": [],
"formatted_hyper_data": [],
"cpp":
lambda print, motion_planner, cov_points, time_limit, parameters:
RandomBAstar2(print, motion_planner,
PointCloud(print, points=cov_points), time_limit,
parameters),
'line':
'g',
'confidence_color': (0.0, 1.0, 0.0, 0.3)
}
}
###################
def my_print(text):
if PRINT:
return print(text)
else:
return
def main():
#with open(RESULTS_FILE, 'rb') as cached_pcd_file:
# cache_data = pickle.load(cached_pcd_file)
# pprint.pprint(cache_data)
#return
with open(RESULTS_FILE, 'rb') as cached_pcd_file:
cache_data = pickle.load(cached_pcd_file)
ALGORITHMS = deepcopy(cache_data)
for alg in ALGORITHMS:
ALGORITHMS[alg]["do_hyper"] = False
ALGORITHMS[alg][
"cpp"] = lambda print, motion_planner, cov_points, time_limit, parameters: RandomBAstar3(
print, motion_planner, PointCloud(
print, points=cov_points), time_limit, parameters)
#### STEP 1 - Get classified pointcloud ####
environment = PointCloudEnvironment(my_print, TERRAIN_ASSESSMENT_FILE,
POINTCLOUD_FILE)
coverable_points = environment.coverable_pcd.points
traversable_points = environment.traversable_pcd.points
motion_planner = MotionPlanner(my_print, environment.traversable_pcd)
#If from terrain assessment file:
#with open(TERRAIN_ASSESSMENT_FILE, 'rb') as cached_pcd_file:
# cache_data = pickle.load(cached_pcd_file)
# coverable_points = cache_data["coverable_points"]
# traversable_points = cache_data["traversable_points"]
#traversable_pcd = PointCloud(my_print, points= traversable_points)
#motion_planner = MotionPlanner(my_print, traversable_pcd)
#### STEP 2 - Hyper parameters ####
for algorithm_key, algorithm in ALGORITHMS.items():
if algorithm["do_hyper"]:
trials = Trials()
hyper_optimizer = HyptoOptimizer(save_data, algorithm, my_print,
HYPER_START_POS, motion_planner,
coverable_points)
opt_param = fmin(
hyper_optimizer.hyper_test_newest_sampled_bastar_param,
space=(hp.uniform('ba_exploration', 0.75,
0.95), hp.uniform('max_distance', 1, 5),
hp.uniform('max_distance_part_II', 4, 10),
hp.uniform('min_bastar_cost_per_coverage', 5000, 10000),
hp.uniform('min_spiral_cost_per_coverage', 10000,
20000), hp.uniform('step_size', 0.5, 1.0),
hp.uniform('visited_threshold', 0.25, 0.5)),
algo=tpe.suggest,
max_evals=HYPER_MAX_EVAL,
trials=trials)
print(trials.statuses())
algorithm["opt_param"] = opt_param
algorithm["hyper_data"] = trials.trials
ALGORITHMS[algorithm_key] = algorithm
save_data(ALGORITHMS)
#### STEP 3 - Full tests ####
for start_point_nr in range(NUMBER_OF_START_POINTS):
#start_point = get_random_point(traversable_points)
start_point = start_points[start_point_nr]
print("Start point " + str(start_point_nr) + ": " + str(start_point))
for algorithm_key, algorithm in ALGORITHMS.items():
if algorithm["do_experiment"]:
experimenter = Experimenter(algorithm, print)
parameters = None
if "opt_param" in algorithm:
parameters = algorithm["opt_param"]
cpp = algorithm["cpp"](my_print, motion_planner,
coverable_points,
algorithm["experiment_time_limit"],
parameters)
if "sample_specific_stats" in algorithm:
experimenter.perform_sample_cpp(cpp, start_point,
start_point_nr)
algorithm["sample_specific_stats"].append(
experimenter.sample_specific_stats)
else:
experimenter.perform_cpp(cpp, start_point, start_point_nr)
algorithm["experiment_results"].append(experimenter.results)
ALGORITHMS[algorithm_key] = algorithm
save_data(ALGORITHMS)
def get_cpp_path(self, start_point, angle_offset_fake = None, 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
"""
if goal_coverage is not None:
self.coverage_2 = goal_coverage
self.start_tracking()
self.path = np.array([start_point])
self.move_to(start_point)
current_position = start_point
total_nbr_of_points = len(self.coverable_pcd.points)
iter = 0
if DO_BASTAR_PLANNING:
Paths = []
uncovered_points = np.arange(total_nbr_of_points)
visited_waypoints = np.empty((0,3))
coverage_part_I = 0
#len(uncovered_points)/total_nbr_of_points > 0.05
while coverage_part_I < self.coverage_1 and iter <= self.max_iterations and coverage_part_I < self.coverage_2 and not self.time_limit_reached():
iter += 1
random_point = self.get_random_uncovered_point(visited_waypoints)
BAstar_paths_from_point = []
for angle_idx in range(self.nbr_of_angles):
angle_offset = angle_idx * 2*np.pi/self.nbr_of_angles
coverable_pcd = PointCloud(self.print, points=self.coverable_pcd.points)
new_BAstar_path = BAStarSegment(self.print, self.motion_planner, random_point, angle_offset, visited_waypoints, coverable_pcd, self.max_distance, self.step_size, self.visited_threshold)
BAstar_paths_from_point.append(new_BAstar_path)
if new_BAstar_path.coverage == 0:
break
best_BAstar_paths_from_point = max(BAstar_paths_from_point, key=operator.attrgetter("coverage"))
self.print(str(iter) + "- coverage: " + str(best_BAstar_paths_from_point.coverage))
if best_BAstar_paths_from_point.coverage > self.min_bastar_coverage:
Paths.append( best_BAstar_paths_from_point )
visited_waypoints = np.append(visited_waypoints, best_BAstar_paths_from_point.path, axis=0)
uncovered_points = self.delete_values(uncovered_points, best_BAstar_paths_from_point.covered_points_idx)
coverage_part_I = 1 - len(uncovered_points)/ total_nbr_of_points
self.print("Coverage part I: " + str(coverage_part_I))
self.randombastar_stats_over_time.append({
"time": timeit.default_timer() - self.start_time,
"coverage": coverage_part_I,
"iteration": iter,
"path": best_BAstar_paths_from_point.path,
"segment": best_BAstar_paths_from_point
})
self.print("Number of found paths: " + str(len(Paths)))
covered_points_idx = self.get_covered_points_idx_from_paths(Paths)
# with open('cached_sample_based_bastar.dictionary', 'wb') as cached_pcd_file:
# cache_data = { "covered_points_idx": covered_points_idx,
# "visited_waypoints": visited_waypoints,
# "paths": Paths,
# }
# pickle.dump(cache_data, cached_pcd_file)
#else:
# with open('cached_sample_based_bastar.dictionary', 'rb') as cached_pcd_file:
# cache_data = pickle.load(cached_pcd_file)
# covered_points_idx = np.unique(cache_data["covered_points_idx"])
# visited_waypoints = cache_data["visited_waypoints"]
# Paths = cache_data["paths"]
if not ONLY_PART_I:
self.coverable_pcd.covered_points_idx = covered_points_idx
coverage_part_II = len(covered_points_idx)/ total_nbr_of_points
self.randombastar_stats["Part1_segments"] = len(Paths)
self.randombastar_stats["Part1_coverage"] = coverage_part_II
self.randombastar_stats["Part1_iterations"] = iter
self.randombastar_stats["Part1_time"] = timeit.default_timer() - self.start_time
self.print("Coverage part II: " + str(coverage_part_II))
self.print("visited_waypoints: " + str(visited_waypoints))
iter = 0
failed_tries = 0
while coverage_part_II < self.coverage_2 and not self.time_limit_reached():
iter += 1
#if failed_tries > 100:
# break
random_uncovered_point = self.get_random_uncovered_point(visited_waypoints)
coverable_pcd = PointCloud(self.print, points=self.coverable_pcd.points)
spiral_path = RandomSpiralSegment(self.print, self.motion_planner, random_uncovered_point, visited_waypoints, coverable_pcd, self.max_distance_part_II, self.step_size, self.visited_threshold)
if len(spiral_path.path) < self.min_spiral_length:
visited_waypoints = np.append(visited_waypoints, [random_uncovered_point], axis=0)
failed_tries += 1
continue
failed_tries = 0
self.print("length: " + str(len(spiral_path.path)))
Paths.append(spiral_path)
visited_waypoints = np.append(visited_waypoints, spiral_path.path, axis=0)
covered_points_idx = np.unique(np.append(covered_points_idx, spiral_path.covered_points_idx))
self.print(len(covered_points_idx) / total_nbr_of_points)
coverage_part_II = len(covered_points_idx) / total_nbr_of_points
self.randombastar_stats_over_time.append({
"time": timeit.default_timer() - self.start_time,
"coverage": coverage_part_II,
"iteration": iter,
"path": spiral_path.path,
"segment": spiral_path
})
self.randombastar_stats["Part2_segments"] = len(Paths) - self.randombastar_stats["Part1_segments"]
self.randombastar_stats["Part2_coverage"] = coverage_part_II
self.randombastar_stats["Part2_iterations"] = iter
self.randombastar_stats["Part2_time"] = timeit.default_timer() - self.start_time
paths_to_visit_in_order = self.traveling_salesman(Paths)
self.follow_paths(current_position, paths_to_visit_in_order)
#self.print_stats(self.path)
#self.print(self.randombastar_stats)
#self.print(self.randombastar_stats_over_time)
return self.path
"hyper_test":
"step_param",
"hyper_time_limit":
250,
"hyper_min_coverage":
95,
"do_experiment":
True,
"experiment_time_limit":
400,
"experiment_results": [],
"hyper_data": [],
"formatted_hyper_data": [],
"cpp":
lambda print, motion_planner, cov_points, time_limit, parameters:
BAstar(print, motion_planner, PointCloud(print, points=cov_points),
time_limit, parameters),
'line':
'r',
'confidence_color': (1.0, 0.0, 0.0, 0.3)
}
}
###################
def my_print(text):
if PRINT:
return print(text)
else:
return
def map_to_traversable_and_coverable_pcd(self):
traversable_points, coverable_points = self.get_traversable_and_coverable_points(
)
return PointCloud(print, points=traversable_points), PointCloud(
print, points=coverable_points)
