def main(): #### STEP 1 - Get classified pointcloud #### environment = PointCloudEnvironment(my_print, TERRAIN_ASSESSMENT_FILE, POINTCLOUD_FILE) coverable_points = environment.coverable_pcd.points traversable_points = environment.traversable_pcd.points motion_planner = MotionPlanner(my_print, environment.traversable_pcd) ### TIME_LIMIT = 400 start_point = np.array([28.6, -6.7, -10.3]) goal_coverage = 0.97 paths_markers = [] #Get CPP path for pub_path in ALL_PATHS: if not pub_path["do_calc"]: continue coverable_pcd = PointCloud(my_print, points=coverable_points) if pub_path["cpp"] == "BA*": cpp = BAstar(my_print, motion_planner, coverable_pcd, time_limit=TIME_LIMIT, parameters=pub_path["param"]) if pub_path["cpp"] == "Inward Spiral": cpp = Spiral(my_print, motion_planner, coverable_pcd, time_limit=TIME_LIMIT, parameters=pub_path["param"]) if pub_path["cpp"] == "Sampled BA*": cpp = RandomBAstar3(my_print, motion_planner, coverable_pcd, time_limit=TIME_LIMIT, parameters=pub_path["param"]) ns = pub_path["ns"] pub_path["path"] = cpp.get_cpp_path(start_point, goal_coverage=goal_coverage) pub_path["markers"] = cpp.points_to_mark pub_path["stats"] = cpp.print_stats(pub_path["path"]) save_data(ALL_PATHS) print("DONE!")
def main(): #with open(RESULTS_FILE, 'rb') as cached_pcd_file: # cache_data = pickle.load(cached_pcd_file) # pprint.pprint(cache_data) #return with open(RESULTS_FILE, 'rb') as cached_pcd_file: cache_data = pickle.load(cached_pcd_file) ALGORITHMS = deepcopy(cache_data) for alg in ALGORITHMS: ALGORITHMS[alg]["do_hyper"] = False ALGORITHMS[alg][ "cpp"] = lambda print, motion_planner, cov_points, time_limit, parameters: RandomBAstar3( print, motion_planner, PointCloud( print, points=cov_points), time_limit, parameters) #### STEP 1 - Get classified pointcloud #### environment = PointCloudEnvironment(my_print, TERRAIN_ASSESSMENT_FILE, POINTCLOUD_FILE) coverable_points = environment.coverable_pcd.points traversable_points = environment.traversable_pcd.points motion_planner = MotionPlanner(my_print, environment.traversable_pcd) #If from terrain assessment file: #with open(TERRAIN_ASSESSMENT_FILE, 'rb') as cached_pcd_file: # cache_data = pickle.load(cached_pcd_file) # coverable_points = cache_data["coverable_points"] # traversable_points = cache_data["traversable_points"] #traversable_pcd = PointCloud(my_print, points= traversable_points) #motion_planner = MotionPlanner(my_print, traversable_pcd) #### STEP 2 - Hyper parameters #### for algorithm_key, algorithm in ALGORITHMS.items(): if algorithm["do_hyper"]: trials = Trials() hyper_optimizer = HyptoOptimizer(save_data, algorithm, my_print, HYPER_START_POS, motion_planner, coverable_points) opt_param = fmin( hyper_optimizer.hyper_test_newest_sampled_bastar_param, space=(hp.uniform('ba_exploration', 0.75, 0.95), hp.uniform('max_distance', 1, 5), hp.uniform('max_distance_part_II', 4, 10), hp.uniform('min_bastar_cost_per_coverage', 5000, 10000), hp.uniform('min_spiral_cost_per_coverage', 10000, 20000), hp.uniform('step_size', 0.5, 1.0), hp.uniform('visited_threshold', 0.25, 0.5)), algo=tpe.suggest, max_evals=HYPER_MAX_EVAL, trials=trials) print(trials.statuses()) algorithm["opt_param"] = opt_param algorithm["hyper_data"] = trials.trials ALGORITHMS[algorithm_key] = algorithm save_data(ALGORITHMS) #### STEP 3 - Full tests #### for start_point_nr in range(NUMBER_OF_START_POINTS): #start_point = get_random_point(traversable_points) start_point = start_points[start_point_nr] print("Start point " + str(start_point_nr) + ": " + str(start_point)) for algorithm_key, algorithm in ALGORITHMS.items(): if algorithm["do_experiment"]: experimenter = Experimenter(algorithm, print) parameters = None if "opt_param" in algorithm: parameters = algorithm["opt_param"] cpp = algorithm["cpp"](my_print, motion_planner, coverable_points, algorithm["experiment_time_limit"], parameters) if "sample_specific_stats" in algorithm: experimenter.perform_sample_cpp(cpp, start_point, start_point_nr) algorithm["sample_specific_stats"].append( experimenter.sample_specific_stats) else: experimenter.perform_cpp(cpp, start_point, start_point_nr) algorithm["experiment_results"].append(experimenter.results) ALGORITHMS[algorithm_key] = algorithm save_data(ALGORITHMS)
def __init__(self): super().__init__('MainNode') #Publishers: self.markers_pub = self.create_publisher(visualization_msgs.MarkerArray, 'marker', 3000) self.markers_path_pub = self.create_publisher(visualization_msgs.Marker, 'path_markers', 3000) self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd', 10) self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'coverable_pcd', 100) self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_pcd', 100) self.visited_ground_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_ground_pcd', 100) self.traversable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'traversable_pcd', 100) self.inaccessible_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'inaccessible_pcd', 100) self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10) with open(FILE, 'rb') as cached_pcd_file: cache_data = pickle.load(cached_pcd_file) all_results = deepcopy(cache_data) param = list(filter(lambda x: x["ns"] == PATH_NS, all_results))[0]["param"] cpp_name = list(filter(lambda x: x["ns"] == PATH_NS, all_results))[0]["cpp"] #Create Environment environment = PointCloudEnvironment(self.print, "garage_terrain_assessment.dictionary", "garage.pcd", False) point_cloud = environment.full_pcd traversable_point_cloud = environment.traversable_pcd coverable_point_cloud = environment.coverable_pcd motion_planner = MotionPlanner(self.print, traversable_point_cloud) TIME_LIMIT = 400 goal_coverage = 0.97 start_point = np.array([28.6, -6.7, -10.3]) #garage #start_point = np.array([-53.7, 54.2, -2.7]) #bridge #start_point = np.array([-20.7, 43, -1]) #cross if cpp_name == "BA*": cpp = BAstar(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param) if cpp_name == "Inward Spiral": cpp = Spiral(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param) if cpp_name == "Sampled BA*": cpp = RandomBAstar3(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param) path = cpp.get_cpp_path(start_point, goal_coverage=goal_coverage) all_segments = cpp.all_segments all_movements = cpp.all_movements markers = cpp.points_to_mark self.pcd_pub.publish(point_cloud.point_cloud(point_cloud.points, 'my_frame')) self.coverable_pcd_pub.publish(coverable_point_cloud.point_cloud(coverable_point_cloud.points, 'my_frame')) self.traversable_pcd_pub.publish(traversable_point_cloud.point_cloud(traversable_point_cloud.points, 'my_frame')) self.markers_msg = visualization_msgs.MarkerArray() self.last_id = 0 ANIMATION = True if not ANIMATION: #Only markers for idx, marker in enumerate(markers): stamp = self.get_clock().now().to_msg() msg = ROSMessage.point_marker(self.last_id, stamp, marker["point"], marker["color"], "markers") self.markers_msg.markers.append(msg) self.last_id += 1 #Add segments coverable_point_cloud.covered_points_idx = np.array([]) for idx, segment in enumerate(all_segments): if len(segment.path) == 0: #all_movements.pop(idx) continue stamp = self.get_clock().now().to_msg() msg = ROSMessage.line_marker(self.last_id, stamp, segment.path, [0.1,1.0,1.0], "segment"+str(idx)) self.markers_msg.markers.append(msg) self.last_id += 1 coverable_point_cloud.visit_path(segment.path) if ANIMATION: visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd() self.visited_ground_pcd_pub.publish(visited_ground_points_pcd) self.markers_pub.publish(self.markers_msg) time.sleep(1) #Add Movements for idx, move in enumerate(all_movements): stamp = self.get_clock().now().to_msg() msg = ROSMessage.line_marker(self.last_id, stamp, move.path, [1.0,0.5,0.5], "move"+str(idx)) self.markers_msg.markers.append(msg) self.last_id += 1 visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd() self.visited_ground_pcd_pub.publish(visited_ground_points_pcd) self.markers_pub.publish(self.markers_msg) return current_max = 0 while current_max < len(path): self.markers_msg = visualization_msgs.MarkerArray() stamp = self.get_clock().now().to_msg() msg = ROSMessage.line_marker(0, stamp, path[0:current_max], [0.1,1.0,1.0], PATH_NS) self.markers_msg.markers = [msg] current_max += 10 self.markers_pub.publish(self.markers_msg)
def __init__(self): super().__init__('MainNode') #Publishers: self.markers_pub = self.create_publisher( visualization_msgs.MarkerArray, 'marker', 3000) self.markers_path_pub = self.create_publisher( visualization_msgs.Marker, 'path_markers', 3000) self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd', 10) self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'coverable_pcd', 100) self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_pcd', 100) self.visited_ground_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'visited_ground_pcd', 100) self.traversable_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'traversable_pcd', 100) self.inaccessible_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'inaccessible_pcd', 100) self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10) with open(FILE, 'rb') as cached_pcd_file: cache_data = pickle.load(cached_pcd_file) self.prev_file = deepcopy(cache_data) #Create Environment environment = PointCloudEnvironment( self.print, "garage_terrain_assessment.dictionary", "garage.pcd", False) point_cloud = environment.full_pcd traversable_point_cloud = environment.traversable_pcd coverable_point_cloud = environment.coverable_pcd motion_planner = MotionPlanner(self.print, traversable_point_cloud) TIME_LIMIT = 400 start_point = np.array([28.6, -6.7, -10.3]) goal_coverage = 0.97 paths_markers = [] #Get CPP path for pub_path in ALL_PATHS: if not pub_path["do_calc"]: continue coverable_pcd = PointCloud(self.print, points=coverable_point_cloud.points) if pub_path["cpp"] == "BA*": cpp = BAstar(self.print, motion_planner, coverable_pcd, time_limit=TIME_LIMIT, parameters=pub_path["param"]) if pub_path["cpp"] == "Inward Spiral": cpp = Spiral(self.print, motion_planner, coverable_pcd, time_limit=TIME_LIMIT, parameters=pub_path["param"]) if pub_path["cpp"] == "Sampled BA*": cpp = RandomBAstar3(self.print, motion_planner, coverable_pcd, time_limit=TIME_LIMIT, parameters=pub_path["param"]) ns = pub_path["ns"] pub_path["path"] = cpp.get_cpp_path(start_point, goal_coverage=goal_coverage) pub_path["markers"] = cpp.points_to_mark pub_path["stats"] = cpp.print_stats(pub_path["path"]) save_data(ALL_PATHS) #self.print(path_msg) #paths_markers.append(path_msg) #path_pub = self.create_publisher(nav_msgs.Path, ns + '/path', 10) #self.markers_pub.publish(path_msg) # #self.markers_msg = visualization_msgs.MarkerArray() # #for idx, msg in enumerate(paths_markers): # stamp = self.get_clock().now().to_msg() # self.markers_msg.markers.append(msg) # #self.last_id += 1 # markers_pub = self.create_timer(5, self.marker_publisher) #self.markers_pub.publish(self.markers_msg) for idx, pub_path in enumerate(ALL_PATHS): if pub_path.get("stats", False) is False: pub_path["stats"] = self.prev_file[idx]["stats"] self.print("=" * 20) self.print(pub_path["ns"]) self.print(pub_path["stats"]) self.pcd_pub.publish( point_cloud.point_cloud(point_cloud.points, 'my_frame')) self.coverable_pcd_pub.publish( coverable_point_cloud.point_cloud(coverable_point_cloud.points, 'my_frame')) self.traversable_pcd_pub.publish( traversable_point_cloud.point_cloud(traversable_point_cloud.points, 'my_frame'))
def __init__(self): super().__init__('MainNode') #Publishers: self.markers_pub = self.create_publisher( visualization_msgs.MarkerArray, 'marker', 3000) self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd', 10) self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'coverable_pcd', 100) self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_pcd', 100) self.visited_ground_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'visited_ground_pcd', 100) self.traversable_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'traversable_pcd', 100) self.inaccessible_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'inaccessible_pcd', 100) self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10) #Varaiables for publishers self.last_id = 0 timer_period = 5 animation_time_period = 0.01 self.animation_iteration = 0 self.path = [] #Subscribers: self.rviz_sub = self.create_subscription(geometry_msgs.PointStamped, "clicked_point", self.clicked_point_cb, 100) #environment = PointCloudEnvironment(self.print, "cached_coverable_points.dictionary", "pointcloud.pcd") #environment = MapEnvironment(self.print, "simple_open.dictionary", "src/exjobb/maps/map_simple_open.png", 0.015) #environment = MapEnvironment(self.print, "map_ipa_apartment.dictionary", "src/exjobb/maps/map_ipa_apartment.png", 0.05) NEW_POINTCLOUD = False if NEW_POINTCLOUD: environment = MapEnvironment( self.print, "simple_open.dictionary", "src/exjobb/maps/map_simple_open.png", 0.015) self.point_cloud = PointCloud(self.print, file="cross.pcd", new_point_cloud=True) else: #environment = PointCloudEnvironment(self.print, "pointcloud2.dictionary", "pointcloud2.pcd", False) #x,y = #351451.84, 4022966.5 Street #environment = PointCloudEnvironment(self.print, "pointcloud3.dictionary", "pointcloud3.pcd", False) #x,y = (351609.25, 4022912.0) Same Underground garage one floor #environment = PointCloudEnvironment(self.print, "pointcloud4.dictionary", "pointcloud4.pcd", False) #x,y = (326815.75, 4152473.25) Busy street, with cars #environment = PointCloudEnvironment(self.print, "cached_coverable_points.dictionary", "pointcloud.pcd") #environment = MapEnvironment(self.print, "map_ipa_apartment.dictionary", "src/exjobb/maps/map_ipa_apartment.png", 0.05) #environment = PointCloudEnvironment(self.print, "bridge_2_small.dictionary", "bridge_2_small.pcd", False) #environment = PointCloudEnvironment(self.print, "cross_terrain_assessment.dictionary", "cross.pcd", False) #environment = PointCloudEnvironment(self.print, "pre_garage_terrain_assessment.dictionary", "garage.pcd", False)' environment = PointCloudEnvironment( self.print, "garage_terrain_assessment.dictionary", "garage.pcd", False) #environment = PointCloudEnvironment(self.print, "bridge_terrain_assessment.dictionary", "bridge_2.pcd", False) #environment = MapEnvironment(self.print, "simple_open.dictionary", "src/exjobb/maps/map_simple_open.png", 0.015) self.point_cloud = environment.full_pcd #traversable_points, coverable_points, inaccessible_points = self.do_terrain_assessment() #self.traversable_point_cloud = PointCloud(self.print, points= traversable_points) #self.coverable_point_cloud = PointCloud(self.print, points= coverable_points) #self.inaccessible_point_cloud = PointCloud(self.print, points= inaccessible_points) # self.point_cloud.pcd = self.point_cloud.point_cloud( self.point_cloud.points, 'my_frame') self.traversable_point_cloud = environment.traversable_pcd self.traversable_point_cloud.pcd = self.traversable_point_cloud.point_cloud( self.traversable_point_cloud.points, 'my_frame') self.coverable_point_cloud = environment.coverable_pcd self.coverable_point_cloud.pcd = self.coverable_point_cloud.point_cloud( self.coverable_point_cloud.points, 'my_frame') self.inaccessible_point_cloud = environment.inaccessible_pcd self.inaccessible_point_cloud.pcd = self.inaccessible_point_cloud.point_cloud( self.inaccessible_point_cloud.points, 'my_frame') if SMALL_POINT_CLOUD: bbox = o3d.geometry.AxisAlignedBoundingBox([10, 15, -5.3], [15, 21, 10]) trav_points_idx = bbox.get_point_indices_within_bounding_box( self.traversable_point_cloud.raw_pcd.points) self.traversable_point_cloud = PointCloud( self.print, points=self.traversable_point_cloud.points[trav_points_idx]) cov_points_idx = bbox.get_point_indices_within_bounding_box( self.coverable_point_cloud.raw_pcd.points) self.coverable_point_cloud = PointCloud( self.print, points=self.coverable_point_cloud.points[cov_points_idx]) self.markers = [] motion_planner = MotionPlanner(self.print, self.traversable_point_cloud) if PUBLISH_INACCESSIBLE_PCD: inaccessible_pcd_pub = self.create_timer( timer_period, self.inaccessible_point_cloud_publisher) if MOTION_PLANNER_TEST: #start_pos = [5.0625 , 91.05000305, -32.58319855] #end_pos = [ 0.8125 , 93.30000305, -32.33319855] end_pos = np.array([6.05999994, -13., -5.71468687]) start_pos = np.array([28.6, -6.7, -10.3]) start_point = self.traversable_point_cloud.find_k_nearest( start_pos, 1)[0] end_point = self.traversable_point_cloud.find_k_nearest( end_pos, 1)[0] self.path = motion_planner.Astar(start_point, end_point) self.markers.append({"point": start_point, "color": RED}) self.markers.append({"point": end_point, "color": BLUE}) if self.path is False: self.path = [] if CPP_TEST: random_idx = np.random.choice(len( self.traversable_point_cloud.points), 1, replace=False)[0] #start_point = [-14, -16, -3.6] # #start_point = [-23, 30, -0.9] start_point = self.traversable_point_cloud.points[random_idx] #SAMPLED BA* #cost 4953, length: 3684, rotation: 1269 ######real: cost: ?? lkength: 3235, rotation: 1108 sparam1 = { 'ba_exploration': 0.90756041115558, 'max_distance': 4.78202945337845, 'max_distance_part_II': 6.75513650527977, 'min_bastar_cost_per_coverage': 8192.530314616084, 'min_spiral_cost_per_coverage': 12157.969167186768, 'step_size': 0.562061544696692, 'visited_threshold': 0.279490436505789 } #cost 4615, length: 3294, rotation: 1321 ######real: cost: ??, length: 3334, rotation: 1304 sparam2 = { 'ba_exploration': 0.816319265003861, 'max_distance': 1.02476727664307, 'max_distance_part_II': 4.76356301411862, 'min_bastar_cost_per_coverage': 6616.530314616084, 'min_spiral_cost_per_coverage': 19277.969167186768, 'step_size': 0.950568870175564, 'visited_threshold': 0.484179597225153 } #cost 4261, length: 3158, rotation: 1103 #######real: cost: ??, length: 3078, rotation: 1114 sparam3 = { 'ba_exploration': 0.853031300592955, 'max_distance': 3.89663024793223, 'max_distance_part_II': 4.80685526433465, 'min_bastar_cost_per_coverage': 9312.530314616084, 'min_spiral_cost_per_coverage': 13196.969167186768, 'step_size': 0.636195719728099, 'visited_threshold': 0.337665370485907 } #length: 3596, rotation: 1296, 97% - annan step size (0.6..) #real: cost: 4306, length: 3073, rotation: 1233 param4 = { 'ba_exploration': 0.8653615601139727, 'max_distance': 4.129493635268686, 'max_distance_part_II': 6.935911381739787, 'min_bastar_cost_per_coverage': 8238.530314616084, 'min_spiral_cost_per_coverage': 13644.969167186768, 'step_size': 0.54868363557903, 'visited_threshold': 0.3730115058138923 } #cost: 5797, length: 4643, rotation: 1154, 97% - annan step size (0.6..) #real: cost: 6422, length: 5116, rotation: 1306 param_best_brdige = { 'ba_exploration': 0.939978646944692, 'max_distance': 4.49053749147136, 'max_distance_part_II': 7.05948312639, 'min_bastar_cost_per_coverage': 12772.530314616084, 'min_spiral_cost_per_coverage': 25988.969167186768, 'step_size': 0.618705451980032, 'visited_threshold': 0.38872474480067 } #cost: 3001, length: 2186, rotation: 815 #real: cost: 3083, length: 2281, rotation: 802 param_best_cross = { 'ba_exploration': 0.863145455156051, 'max_distance': 1.69280755868826, 'max_distance_part_II': 4.48375188984703, 'min_bastar_cost_per_coverage': 6488.530314616084, 'min_spiral_cost_per_coverage': 8141.257661974652297, 'step_size': 0.553977048496769, 'visited_threshold': 0.38872474480067 } #BASTAR: #cost: 16062, lenth: 10575 rotation: 5487 param1 = { 'angle_offset': 3.44800051788481, 'step_size': 0.963400677899873, 'visited_threshold': 0.257015802906527 } #cost: 7583, lenth: 4452 rotation: 3131 param2 = { 'angle_offset': 3.78341027362029, 'step_size': 0.601687134922371, 'visited_threshold': 0.328108983656107 } #cost: 5013, lenth: 3049 rotation: 1964 param3 = { 'angle_offset': 5.27158130667689, 'step_size': 0.517468289229711, 'visited_threshold': 0.455659073558674 } #cost: 4238, lenth: 2896 rotation: 1342 param4 = { 'angle_offset': 4.64664343656672, 'step_size': 0.633652049936913, 'visited_threshold': 0.472819723019576 } #cost: 3262, lenth: 2249 rotation: 1013 param_best_cross = { 'angle_offset': 4.70135588957793, 'step_size': 0.523646671416283, 'visited_threshold': 0.403681713288835 } #cost: 6385, lenth: 4562 rotation: 1823 param_best_brdige = { 'angle_offset': 5.33881157053433, 'step_size': 0.55692737194204, 'visited_threshold': 0.453169184364576 } #SPIRAL: #cost: 14292, lenth: 7523 rotation: 6769 param1 = { 'step_size': 0.999314930298507, 'visited_threshold': 0.32443603324225 } #cost: 7431, lenth: 3990 rotation: 3441 param2 = { 'step_size': 0.825030992319859, 'visited_threshold': 0.433448258850281 } #cost: 6466, lenth: 3218 rotation: 3248 param3 = { 'step_size': 0.521396930930628, 'visited_threshold': 0.47473068968531 } #cost: 5787, lenth: 3101 rotation: 2686 param4 = { 'step_size': 0.627870706339337, 'visited_threshold': 0.498775709725593 } #cost: 7213, lenth: 4440 rotation: 2773 param_best_brdige = { 'step_size': 0.737114020263598, 'visited_threshold': 0.483088877473477 } #cost: 4054, lenth: 2239 rotation: 1815 param_best_cross = { 'step_size': 0.664671825076571, 'visited_threshold': 0.499669038773602 } #param = {'step_size': 0.5, # 'visited_threshold': 0.4} start_point = [-20.7, 43, -1] #start_point = np.array([28.6, -6.7, -10.3]) #garage start_point = np.array([-53.7, 54.2, -2.7]) #bridge #start_point = np.array([-20.7, 43, -1]) #cross #start_point = np.array([15.6, -16.7, -5.3]) #start_point = np.array([0.6,0.6,0]) #start_points = {} #for n in range(10): # random_idx = np.random.choice(len(self.traversable_point_cloud.points), 1, replace=False)[0] # start_points[n] = self.traversable_point_cloud.points[random_idx] # #self.markers.append( {"point": self.traversable_point_cloud.points[random_idx], "color": RED} ) #self.print(start_points) self.cpp = RandomBAstar3(self.print, motion_planner, self.coverable_point_cloud, time_limit=300, parameters=sparam4) self.path = self.cpp.get_cpp_path(start_point, goal_coverage=0.97) #self.path = self.cpp.breadth_first_search(start_point) #self.print(self.cpp.print_results()) #self.path = self.cpp.get_cpp_node_path(start_point) self.print(self.cpp.print_stats(self.path)) for marker in self.cpp.points_to_mark: self.markers.append(marker) #self.markers.append( {"point": self.path[-1], "color": RED} ) #self.points_to_mark = [self.path[-1]] if PUBLISH_FULL_PCD: #pcd_pub = self.create_timer(timer_period, self.point_cloud_publisher) self.point_cloud_publisher() if PUBLISH_GROUND_PCD: #coverable_pcd_pub = self.create_timer(timer_period, self.coverable_point_cloud_publisher) self.coverable_point_cloud_publisher() if PUBLISH_TRAVERSABLE_PCD: #traversable_pcd_pub = self.create_timer(timer_period, self.traversable_point_cloud_publisher) self.traversable_point_cloud_publisher() #HYPER_START_POS = np.array([-53.7, 54.2, -2.7]) #start_points = { # 0: np.array([-43.10443115, 3.99802136, 4.46702003]), # 1: np.array([ 21.61431885, -33.00197983, -2.77298403]), # 2: np.array([-34.51068115, 12.49802208, -4.17298126]), # 3: np.array([ 15.9268198 , -36.00197983, -2.6929822 ]), # 4: np.array([38.98931885, 45.49802399, 1.19701743]), # 5: np.array([ 3.73931861, 40.74802399, 2.83701849]), # 6: np.array([ 15.5205698 , -31.50197792, -2.8729825 ]), # 7: np.array([-16.44818115, -19.25197792, -3.58298159]), # 8: np.array([10.52056885, 42.74802399, 2.46701956]), # 9: np.array([53.89556885, 35.99802399, 0.33701676])} #for point in start_points.values(): # self.markers.append({ # "point": point, # "color": [0.0,0.0,1.0] # }) #self.markers.append({ # "point": HYPER_START_POS, # "color": [0.0,1.0,0.0] # }) #CPP_TEST = True if PUBLISH_MARKERS and len(self.markers): #for marker in self.cpp.points_to_mark: # self.markers.append(marker) markers_pub = self.create_timer(timer_period, self.marker_publisher) if PUBLISH_PATH and len(self.path) > 0 and not PUBLISH_PATH_ANIMATION: path_pub = self.create_timer(timer_period, self.path_publisher) if PUBLISH_VISITED_PCD: self.point_cloud.visit_path(self.path) self.visited_points_pcd = self.point_cloud.get_covered_points_as_pcd( ) visited_pcd_pub = self.create_timer( timer_period, self.visited_point_cloud_publisher) if PUBLISH_VISITED_GROUND_PCD and len(self.path): #self.coverable_point_cloud = PointCloud(self.print, points= coverable_points) self.coverable_point_cloud.visit_path(self.path) self.visited_ground_points_pcd = self.coverable_point_cloud.get_covered_points_as_pcd( ) visited_ground_pcd_pub = self.create_timer( timer_period, self.visited_ground_point_cloud_publisher) if PUBLISH_PATH_ANIMATION and len(self.path) > 0: time.sleep(8) self.coverable_point_cloud.covered_points_idx = np.array([]) path_pub = self.create_timer(animation_time_period, self.animated_path_publisher) if PUBLISH_SEGMENTS_ANIMATION and len(self.cpp.all_segments) > 0: time.sleep(8) self.coverable_point_cloud.covered_points_idx = np.array([]) self.segments = self.cpp.all_segments path_pub = self.create_timer(animation_time_period, self.animated_segment_publisher)
"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: RandomBAstar3(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 __init__(self): super().__init__('MainNode') #Publishers: self.markers_pub = self.create_publisher( visualization_msgs.MarkerArray, 'marker', 3000) self.markers_path_pub = self.create_publisher( visualization_msgs.Marker, 'path_markers', 3000) self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd', 10) self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'coverable_pcd', 100) self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_pcd', 100) self.visited_ground_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'visited_ground_pcd', 100) self.traversable_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'traversable_pcd', 100) self.inaccessible_pcd_pub = self.create_publisher( sensor_msgs.PointCloud2, 'inaccessible_pcd', 100) self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10) #Create Environment environment = PointCloudEnvironment( self.print, PCD_DATA[PCD]["terr_assessment_file"], PCD_DATA[PCD]["pcd_file"], False) point_cloud = environment.full_pcd traversable_point_cloud = environment.traversable_pcd coverable_point_cloud = environment.coverable_pcd motion_planner = MotionPlanner(self.print, traversable_point_cloud) parameters = list( filter(lambda x: x["pcd"] == PCD and x["cpp"] == ALGORITHM, PARAMETER_DATA))[0]["param"] if ALGORITHM == "BA*": cpp = BAstar(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters=parameters) if ALGORITHM == "Inward Spiral": cpp = Spiral(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters=parameters) if ALGORITHM == "Sampled BA*": cpp = RandomBAstar3(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters=parameters) self.path = cpp.get_cpp_path(PCD_DATA[PCD]["start_point"], goal_coverage=GOAL_COVERAGE) self.print(cpp.print_stats(self.path)) markers_pub = self.create_timer(5, self.marker_publisher) self.pcd_pub.publish( point_cloud.point_cloud(point_cloud.points, 'my_frame')) self.coverable_pcd_pub.publish( coverable_point_cloud.point_cloud(coverable_point_cloud.points, 'my_frame')) self.traversable_pcd_pub.publish( traversable_point_cloud.point_cloud(traversable_point_cloud.points, 'my_frame'))