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)