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