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)
all_results = deepcopy(cache_data)
path = list(filter(lambda x: x["ns"] == PATH_NS,
all_results))[0]["path"]
#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)
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'))
current_max = 0
while current_max < len(path):
self.markers_msg = visualization_msgs.MarkerArray()
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(0, stamp, path[0:current_max],
[0.1, 1.0, 1.0], PATH_NS)
self.markers_msg.markers = [msg]
current_max += 10
self.markers_pub.publish(self.markers_msg)
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 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"]
#### 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.25,
0.5)),
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.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):
self.start_tracking()
coverage = 0
self.move_to(start_point)
self.starting_point_list = np.array([start_point])
starting_point = start_point
current_position = start_point
self.motion_planner = MotionPlanner(self.logger, self.pcd)
self.cover_local_area_time = 0
self.backtracking_time = 0
self.motion_planner_time = 0
self.rest_time = 0
self.b_time = 0
self.sort_time = 0
self.visited_time = 0
self.valid_time = 0
self.next_starting_point_time = 0
break_loop = False
self.print("build_RRT_tree")
tree = self.build_RRT_tree(start_point)
self.possible_positions = tree.nodes
self.print(self.possible_positions)
self.print(len(self.possible_positions))
#return self.possible_positions
self.pcd.visited_points_idx = np.array([])
coverage = 0
self.points_to_mark = self.possible_positions
while coverage < COVEREAGE_EFFICIENCY_GOAL:
cover_local_area = timeit.default_timer()
path_length, current_position = self.cover_local_area(
starting_point)
self.cover_local_area_time += timeit.default_timer(
) - cover_local_area
if path_length == 0:
self.print("No path found when covering local area!")
#break
backtracking = timeit.default_timer()
backtracking_list = self.get_sorted_backtracking_list_simple(
self.path)
self.backtracking_time += timeit.default_timer() - backtracking
if len(backtracking_list) == 0:
self.print("backtracking_list empty")
break
next_starting_point_time = timeit.default_timer()
next_starting_point, backtracking_list = backtracking_list[
0], backtracking_list[1:]
#next_starting_point, next_starting_point_idx, visiting_rate = self.get_next_starting_point(backtracking_list)
self.next_starting_point_time += timeit.default_timer(
) - next_starting_point_time
motion_planner = timeit.default_timer()
path_to_next_starting_point = self.motion_planner.Astar(
current_position, next_starting_point)
self.motion_planner_time += timeit.default_timer() - motion_planner
#if path_to_next_starting_point is False:
#go back
#break
#current_position = self.path[-2]
while path_to_next_starting_point is False:
self.print("No path found when planning Astar!")
self.starting_point_list = np.append(self.starting_point_list,
[current_position],
axis=0)
self.starting_point_list = np.append(self.starting_point_list,
[next_starting_point],
axis=0)
self.print("backtracking_list: " + str(backtracking_list))
backtracking_list = np.delete(backtracking_list,
next_starting_point_idx,
axis=0)
if len(backtracking_list) == 0:
break_loop = True
break
self.print("backtracking_list: " + str(backtracking_list))
self.print("previous: " + str(next_starting_point))
next_starting_point, next_starting_point_idx, visiting_rate = self.get_next_starting_point(
backtracking_list, visiting_rate)
self.print("new: " + str(next_starting_point))
path_to_next_starting_point = self.motion_planner.Astar(
current_position, next_starting_point)
if break_loop:
break
rest = timeit.default_timer()
self.follow_path(path_to_next_starting_point)
starting_point = next_starting_point
#self.starting_point_list = np.append(self.starting_point_list, [next_starting_point], axis=0 )
coverage = self.pcd.get_coverage_efficiency()
self.print("coverage" + str(coverage))
self.rest_time += timeit.default_timer() - rest
#if new_coverage - coverage < 0.01 and len(backtracking_list) > 0:
# self.print("Too small area")
# self.path = self.path[0 : current_path_length]
# self.pcd.visited_points_idx = self.pcd.visited_points_idx[0 : current_path_length]
# starting_point, backtracking_list = backtracking_list[0], backtracking_list[1:]
# continue
#if backtracking_list and path_to_cover_local_area:
# critical_point = path_to_cover_local_area[-1]
# next_starting_point = self.get_next_starting_point(backtracking_list)
# path_to_next_starting_point = self.find_path(critical_point, next_starting_point)
# path = np.append( path, path_to_next_starting_point )
#self.print("path: " + str(self.path))
self.print("cover_local_area_time" + str(self.cover_local_area_time))
self.print("motion_planner_time" + str(self.motion_planner_time))
self.print("backtracking_time" + str(self.backtracking_time))
self.print("rest_time" + str(self.rest_time))
self.print("sort_time" + str(self.sort_time))
self.print("b_time" + str(self.b_time))
self.print("visited_time" + str(self.visited_time))
self.print("valid_time" + str(self.valid_time))
self.print("next_starting_point_time: " +
str(self.next_starting_point_time))
self.motion_planner.print_times()
self.print_stats(self.path)
return self.path
class BAstarRRT(CPPSolver):
def __init__(self, logger, motion_planner):
self.logger = logger
super().__init__(logger, motion_planner)
self.name = "BAstarRRT"
def get_cpp_path(self, start_point):
self.start_tracking()
coverage = 0
self.move_to(start_point)
self.starting_point_list = np.array([start_point])
starting_point = start_point
current_position = start_point
self.motion_planner = MotionPlanner(self.logger, self.pcd)
self.cover_local_area_time = 0
self.backtracking_time = 0
self.motion_planner_time = 0
self.rest_time = 0
self.b_time = 0
self.sort_time = 0
self.visited_time = 0
self.valid_time = 0
self.next_starting_point_time = 0
break_loop = False
self.print("build_RRT_tree")
tree = self.build_RRT_tree(start_point)
self.possible_positions = tree.nodes
self.print(self.possible_positions)
self.print(len(self.possible_positions))
#return self.possible_positions
self.pcd.visited_points_idx = np.array([])
coverage = 0
self.points_to_mark = self.possible_positions
while coverage < COVEREAGE_EFFICIENCY_GOAL:
cover_local_area = timeit.default_timer()
path_length, current_position = self.cover_local_area(
starting_point)
self.cover_local_area_time += timeit.default_timer(
) - cover_local_area
if path_length == 0:
self.print("No path found when covering local area!")
#break
backtracking = timeit.default_timer()
backtracking_list = self.get_sorted_backtracking_list_simple(
self.path)
self.backtracking_time += timeit.default_timer() - backtracking
if len(backtracking_list) == 0:
self.print("backtracking_list empty")
break
next_starting_point_time = timeit.default_timer()
next_starting_point, backtracking_list = backtracking_list[
0], backtracking_list[1:]
#next_starting_point, next_starting_point_idx, visiting_rate = self.get_next_starting_point(backtracking_list)
self.next_starting_point_time += timeit.default_timer(
) - next_starting_point_time
motion_planner = timeit.default_timer()
path_to_next_starting_point = self.motion_planner.Astar(
current_position, next_starting_point)
self.motion_planner_time += timeit.default_timer() - motion_planner
#if path_to_next_starting_point is False:
#go back
#break
#current_position = self.path[-2]
while path_to_next_starting_point is False:
self.print("No path found when planning Astar!")
self.starting_point_list = np.append(self.starting_point_list,
[current_position],
axis=0)
self.starting_point_list = np.append(self.starting_point_list,
[next_starting_point],
axis=0)
self.print("backtracking_list: " + str(backtracking_list))
backtracking_list = np.delete(backtracking_list,
next_starting_point_idx,
axis=0)
if len(backtracking_list) == 0:
break_loop = True
break
self.print("backtracking_list: " + str(backtracking_list))
self.print("previous: " + str(next_starting_point))
next_starting_point, next_starting_point_idx, visiting_rate = self.get_next_starting_point(
backtracking_list, visiting_rate)
self.print("new: " + str(next_starting_point))
path_to_next_starting_point = self.motion_planner.Astar(
current_position, next_starting_point)
if break_loop:
break
rest = timeit.default_timer()
self.follow_path(path_to_next_starting_point)
starting_point = next_starting_point
#self.starting_point_list = np.append(self.starting_point_list, [next_starting_point], axis=0 )
coverage = self.pcd.get_coverage_efficiency()
self.print("coverage" + str(coverage))
self.rest_time += timeit.default_timer() - rest
#if new_coverage - coverage < 0.01 and len(backtracking_list) > 0:
# self.print("Too small area")
# self.path = self.path[0 : current_path_length]
# self.pcd.visited_points_idx = self.pcd.visited_points_idx[0 : current_path_length]
# starting_point, backtracking_list = backtracking_list[0], backtracking_list[1:]
# continue
#if backtracking_list and path_to_cover_local_area:
# critical_point = path_to_cover_local_area[-1]
# next_starting_point = self.get_next_starting_point(backtracking_list)
# path_to_next_starting_point = self.find_path(critical_point, next_starting_point)
# path = np.append( path, path_to_next_starting_point )
#self.print("path: " + str(self.path))
self.print("cover_local_area_time" + str(self.cover_local_area_time))
self.print("motion_planner_time" + str(self.motion_planner_time))
self.print("backtracking_time" + str(self.backtracking_time))
self.print("rest_time" + str(self.rest_time))
self.print("sort_time" + str(self.sort_time))
self.print("b_time" + str(self.b_time))
self.print("visited_time" + str(self.visited_time))
self.print("valid_time" + str(self.valid_time))
self.print("next_starting_point_time: " +
str(self.next_starting_point_time))
self.motion_planner.print_times()
self.print_stats(self.path)
return self.path
def build_RRT_tree(self, start_point):
tree = Tree()
tree.add_node(start_point)
nbr_of_points_in_pcd = len(self.pcd.points)
for i in range(MAX_ITERATIONS):
#self.print(i)
random_point = self.pcd.points[np.random.randint(
nbr_of_points_in_pcd)]
new_point_1, status = self.extend(tree, random_point)
#self.print("status: " + str(status))
if status == TRAPPED:
continue
#self.print(new_point_1)
self.pcd.visit_position(new_point_1)
if i % GOAL_CHECK_FREQUENCY == 0:
coverage = self.pcd.get_coverage_efficiency()
self.print("Coverage: " + str(round(coverage * 100, 2)) + "%")
if coverage > RRT_COVEREAGE_EFFICIENCY_GOAL:
self.print("Coverage reached")
return tree
self.logger.warn("Failed to cover")
return tree
def get_next_starting_point(self,
sorted_backtracking_list,
visiting_rate_start=0.7):
next_starting_point = False
visiting_rate = visiting_rate_start
while next_starting_point is False:
visiting_rate += 0.05
for idx, potential_starting_point in enumerate(
sorted_backtracking_list):
visiting_rate_in_area = self.pcd.get_visiting_rate_in_area(
potential_starting_point, 2 * ROBOT_RADIUS)
if visiting_rate_in_area < visiting_rate:
next_starting_point = potential_starting_point
break
return next_starting_point, idx, visiting_rate
def cover_local_area(self, start_point):
path_found = False
best_path = np.empty((0, 3))
path_before = self.path
#self.print("start: " + str(start_point))
for angle_idx in range(1):
current_position = start_point
angle_offset = angle_idx * np.pi / 4
current_path = path_before
critical_point_found = False
path_length = 0
local_path = np.empty((0, 3))
while not critical_point_found:
critical_point_found = True
self.print("current_position: " + str(current_position))
for neighbour in self.get_neighbours(current_position,
angle_offset):
#if self.is_blocked(current_position, neighbour, current_path):
# continue
if self.is_blocked(neighbour, current_position):
continue
self.print("neighbour: " + str(neighbour))
current_position = neighbour
current_path = np.append(current_path, [neighbour], axis=0)
path_length += 1
critical_point_found = False
#self.print("path: " + str(self.path))
break
new_local_path = current_path[len(path_before) - 1:]
if len(best_path) < len(new_local_path):
best_path = new_local_path
self.pcd.visit_path(best_path, ROBOT_RADIUS)
self.path = np.append(self.path, best_path, axis=0)
current_position = best_path[-1]
return path_length, current_position
#path_to_cover_local_area = []
def get_sorted_backtracking_list_simple(self, path):
backtracking_list = np.empty((0, 3))
for position in self.possible_positions:
if not self.has_been_visited(position):
backtracking_list = np.append(backtracking_list, [position],
axis=0)
current_position = path[-1]
distances = np.linalg.norm(backtracking_list - current_position,
axis=1)
distances = distances[distances > 0.1]
sorted_idx = np.argsort(distances)
return backtracking_list[sorted_idx]
def get_sorted_backtracking_list(self, path):
backtracking_list = np.empty((0, 3))
#self.print("Backtracking_list 1: " + str(backtracking_list))
for point in path:
#self.print("Backtracking_list 2: " + str(backtracking_list))
def b(si, sj):
b_t = timeit.default_timer()
#self.print("si, sj: " + str((si, sj)))
#self.print("si valid: " + str(self.is_valid_step(point, si)))
#self.print("sj valid: " + str(self.is_valid_step(point, sj)))
if not self.is_blocked(point, si) and self.is_blocked(
point, sj):
#self.print("point: " + str(point))
#self.print("not bloacked: " + str(si))
#self.print("bloacked: " + str(sj))
self.b_time += timeit.default_timer() - b_t
return True
#self.print("Return 0")
self.b_time += timeit.default_timer() - b_t
return False
sort = timeit.default_timer()
neighbours = self.get_neighbours(point)
s1 = neighbours[6] #east
s2 = neighbours[2] #northeast
s3 = neighbours[0] #north
s4 = neighbours[3] #northwest
s5 = neighbours[7] #west
s6 = neighbours[5] #southwest
s7 = neighbours[1] #south
s8 = neighbours[4] #southeast
self.sort_time += timeit.default_timer() - sort
self.print("backtrack neighbours" + str(neighbours))
combinations = [(s1, s8), (s1, s2), (s5, s6), (s5, s4), (s7, s6),
(s7, s8)]
for c in combinations:
if b(c[0], c[1]):
backtracking_list = np.append(backtracking_list, [point],
axis=0)
break
#my = b(s1, s8) + b(s1,s2) + b(s5,s6) + b(s5,s4) + b(s7,s6) + b(s7,s8)
##self.print("my" + str(my))
#if my >= 1:
# backtracking_list = np.append( backtracking_list, [point], axis=0)
#self.print("Backtracking_list 3: " + str(backtracking_list))
#self.print("Length of backtracking_list: " + str(backtracking_list))
current_position = path[-1]
distances = np.linalg.norm(backtracking_list - current_position,
axis=1)
distances = distances[distances > 0.1]
sorted_idx = np.argsort(distances)
#closest_point_idx = np.argmin(distances)
#self.backtrack_list = backtracking_list
return backtracking_list[
sorted_idx] #backtracking_list[closest_point_idx]
def get_neighbours(self, current_position, angle_offset=np.pi / 4):
directions = []
for direction_idx in range(8):
angle = direction_idx / 8 * np.pi * 2 + angle_offset
x = current_position[0] + np.cos(angle) * CELL_STEP_SIZE
y = current_position[1] + np.sin(angle) * CELL_STEP_SIZE
z = current_position[2]
pos = np.array([x, y, z])
#self.print("pos" + str(pos))
nearest_position = self.get_nearest_possible_positions(pos)
#self.print("nearest_position" + str(nearest_position))
directions.append(nearest_position)
east, northeast, north, northwest, west, southwest, south, southeast = directions
'''
east = self.motion_planner.new_point_towards(current_position, current_position + directions, CELL_STEP_SIZE)
northeast = self.motion_planner.new_point_towards(current_position, current_position +np.array([100,100,0]), CELL_STEP_SIZE)
north = self.motion_planner.new_point_towards(current_position, current_position +np.array([0,100,0]), CELL_STEP_SIZE)
northwest = self.motion_planner.new_point_towards(current_position,current_position + np.array([-100,100,0]), CELL_STEP_SIZE)
west = self.motion_planner.new_point_towards(current_position, current_position +np.array([-100,0,0]), CELL_STEP_SIZE)
southwest = self.motion_planner.new_point_towards(current_position, current_position +np.array([-100,-100,0]), CELL_STEP_SIZE)
south = self.motion_planner.new_point_towards(current_position,current_position + np.array([0,-100,0]), CELL_STEP_SIZE)
southeast = self.motion_planner.new_point_towards(current_position,current_position + np.array([100,-100,0]), CELL_STEP_SIZE)
'''
#return [north, east, south, west]
return [
north, south, northeast, northwest, southeast, southwest, east,
west
]
def has_been_visited(self, point, path=None):
if path is None:
path = self.path
#self.print("path: " + str(self.path) )
#self.print("self.path - point" + str(self.path - point))
distances = np.linalg.norm(path - point, axis=1)
#self.print("distances" + str(distances))
#self.print(np.any(distances <= STEP_SIZE) )
return np.any(distances <= VISITED_TRESHOLD)
def is_blocked(self, from_point, to_point, path=None):
if path is None:
path = self.path
visited = timeit.default_timer()
if self.has_been_visited(to_point, path):
self.visited_time += timeit.default_timer() - visited
return True
self.visited_time += timeit.default_timer() - visited
if np.array_equal(from_point, to_point):
return True
return False
valid = timeit.default_timer()
if not self.motion_planner.is_valid_step(from_point, to_point):
self.valid_time += timeit.default_timer() - valid
return True
self.valid_time += timeit.default_timer() - valid
return False
def get_nearest_possible_positions(self, point):
distances = np.linalg.norm(self.possible_positions - point, axis=1)
#self.print("distances" + str(distances))
nearest_idx = np.argmin(distances)
#self.print(self.possible_positions[nearest_idx])
return self.possible_positions[nearest_idx]
def extend(self, tree, extension_point):
nearest_node_idx, nearest_point = tree.nearest_node(extension_point)
neighbours = self.get_neighbours(nearest_point)
distances = np.linalg.norm(neighbours - extension_point, axis=1)
nearest_idx = np.argmin(distances)
configured_extension_point = neighbours[nearest_idx]
new_point = self.new_point_towards(nearest_point,
configured_extension_point,
RRT_STEP_SIZE)
#self.logger.info(str(np.linalg.norm(new_point - nearest_point)))
if self.motion_planner.is_valid_step(nearest_point, new_point):
distance = np.linalg.norm(new_point - nearest_point)
new_node_idx = tree.add_node(new_point)
tree.add_edge(nearest_node_idx, new_node_idx, distance)
#self.logger.info("New: " + str(new_node_idx) + ": " + str(new_point) + ", dist: " + str(distance))
#self.logger.info("New in tree: " + str(new_node_idx) + ": " + str(tree.nodes[new_node_idx]))
return new_point, ADVANCED
else:
return new_point, TRAPPED
def new_point_towards(self, start, end, step_length):
direction = (end - start) / np.linalg.norm(end - start)
return start + step_length * direction
def get_points_to_mark(self):
#return self.backtrack_list
return self.points_to_mark
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 __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)
#Create Environment
environment = MapEnvironment(self.print, "simple_open.dictionary",
"src/exjobb/maps/map_simple_open.png",
0.015)
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([5,0.55,0]) #spiral
start_point = np.array([0.55, 0.7, 0])
goal_coverage = 1
paths_markers = []
#Get CPP path
current = "BA*"
current_param = {
'angle_offset': 0,
'step_size': 0.5,
'visited_threshold': 0.375
}
if current == "BA*":
cpp = BAstar(self.print,
motion_planner,
coverable_point_cloud,
time_limit=TIME_LIMIT,
parameters=current_param)
if current == "Inward Spiral":
cpp = Spiral(self.print,
motion_planner,
coverable_point_cloud,
time_limit=TIME_LIMIT,
parameters=current_param)
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'))
path = cpp.get_cpp_path(start_point, goal_coverage=goal_coverage)
self.print(cpp.print_stats(path))
current_max = 0
coverable_point_cloud.covered_points_idx = np.array([])
while current_max < len(path):
self.markers_msg = visualization_msgs.MarkerArray()
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(0, stamp, path[0:current_max],
[0.1, 1.0, 1.0], "path")
self.markers_msg.markers = [msg]
current_max += 2
new_path = path[current_max - 2:current_max]
self.markers_pub.publish(self.markers_msg)
#if current_max > 2:
coverable_point_cloud.visit_path(new_path)
visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd(
)
self.visited_ground_pcd_pub.publish(visited_ground_points_pcd)
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)
all_results = deepcopy(cache_data)
param = list(filter(lambda x: x["ns"] == PATH_NS, all_results))[0]["param"]
cpp_name = list(filter(lambda x: x["ns"] == PATH_NS, all_results))[0]["cpp"]
#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
goal_coverage = 0.97
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
if cpp_name == "BA*":
cpp = BAstar(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param)
if cpp_name == "Inward Spiral":
cpp = Spiral(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param)
if cpp_name == "Sampled BA*":
cpp = RandomBAstar3(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param)
path = cpp.get_cpp_path(start_point, goal_coverage=goal_coverage)
all_segments = cpp.all_segments
all_movements = cpp.all_movements
markers = cpp.points_to_mark
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'))
self.markers_msg = visualization_msgs.MarkerArray()
self.last_id = 0
ANIMATION = True
if not ANIMATION:
#Only markers
for idx, marker in enumerate(markers):
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.point_marker(self.last_id, stamp, marker["point"], marker["color"], "markers")
self.markers_msg.markers.append(msg)
self.last_id += 1
#Add segments
coverable_point_cloud.covered_points_idx = np.array([])
for idx, segment in enumerate(all_segments):
if len(segment.path) == 0:
#all_movements.pop(idx)
continue
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(self.last_id, stamp, segment.path, [0.1,1.0,1.0], "segment"+str(idx))
self.markers_msg.markers.append(msg)
self.last_id += 1
coverable_point_cloud.visit_path(segment.path)
if ANIMATION:
visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd()
self.visited_ground_pcd_pub.publish(visited_ground_points_pcd)
self.markers_pub.publish(self.markers_msg)
time.sleep(1)
#Add Movements
for idx, move in enumerate(all_movements):
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(self.last_id, stamp, move.path, [1.0,0.5,0.5], "move"+str(idx))
self.markers_msg.markers.append(msg)
self.last_id += 1
visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd()
self.visited_ground_pcd_pub.publish(visited_ground_points_pcd)
self.markers_pub.publish(self.markers_msg)
return
current_max = 0
while current_max < len(path):
self.markers_msg = visualization_msgs.MarkerArray()
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(0, stamp, path[0:current_max], [0.1,1.0,1.0], PATH_NS)
self.markers_msg.markers = [msg]
current_max += 10
self.markers_pub.publish(self.markers_msg)
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'))
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 __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)
#Create Environment
environment = PointCloudEnvironment(
self.print, PCD_DATA[PCD]["terr_assessment_file"],
PCD_DATA[PCD]["pcd_file"], 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)
parameters = list(
filter(lambda x: x["pcd"] == PCD and x["cpp"] == ALGORITHM,
PARAMETER_DATA))[0]["param"]
if ALGORITHM == "BA*":
cpp = BAstar(self.print,
motion_planner,
coverable_point_cloud,
time_limit=TIME_LIMIT,
parameters=parameters)
if ALGORITHM == "Inward Spiral":
cpp = Spiral(self.print,
motion_planner,
coverable_point_cloud,
time_limit=TIME_LIMIT,
parameters=parameters)
if ALGORITHM == "Sampled BA*":
cpp = RandomBAstar3(self.print,
motion_planner,
coverable_point_cloud,
time_limit=TIME_LIMIT,
parameters=parameters)
self.path = cpp.get_cpp_path(PCD_DATA[PCD]["start_point"],
goal_coverage=GOAL_COVERAGE)
self.print(cpp.print_stats(self.path))
markers_pub = self.create_timer(5, self.marker_publisher)
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'))
