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]
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
class MainNode(Node): 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 point_cloud_publisher(self): self.pcd_pub.publish(self.point_cloud.pcd) def coverable_point_cloud_publisher(self): self.coverable_pcd_pub.publish(self.coverable_point_cloud.pcd) def traversable_point_cloud_publisher(self): self.traversable_pcd_pub.publish(self.traversable_point_cloud.pcd) def inaccessible_point_cloud_publisher(self): self.inaccessible_pcd_pub.publish(self.inaccessible_point_cloud.pcd) def visited_point_cloud_publisher(self): self.visited_pcd_pub.publish(self.visited_points_pcd) def visited_ground_point_cloud_publisher(self): self.visited_ground_pcd_pub.publish(self.visited_ground_points_pcd) def path_publisher(self): path_msg = ROSMessage.path(self.path) self.path_pub.publish(path_msg) def animated_path_publisher(self): '''Publishes the path point by point to create an animation in RViz. ''' self.animation_iteration += 10 if self.animation_iteration >= len(self.path): self.path_publisher() return path_msg = ROSMessage.path(self.path[0:self.animation_iteration]) self.path_pub.publish(path_msg) new_path = self.path[self.animation_iteration - 10:self.animation_iteration] self.coverable_point_cloud.visit_path(new_path) #point = self.path[self.animation_iteration] #self.coverable_point_cloud.visit_path_to_position(point, self.path[self.animation_iteration-1]) self.visited_ground_points_pcd = self.coverable_point_cloud.get_covered_points_as_pcd( ) self.visited_ground_point_cloud_publisher() def animated_segment_publisher(self): '''Publishes the path point by point to create an animation in RViz. ''' if self.animation_iteration >= len(self.segments): #self.path_publisher() self.print("DONE!") return current_path = np.empty((0, 3)) for idx in range(self.animation_iteration): current_path = np.append(current_path, self.segments[idx].path, axis=0) path_msg = ROSMessage.path(current_path) self.path_pub.publish(path_msg) latest = self.segments[self.animation_iteration].path self.coverable_point_cloud.visit_path(latest) self.visited_ground_points_pcd = self.coverable_point_cloud.get_covered_points_as_pcd( ) self.visited_ground_point_cloud_publisher() self.animation_iteration += 1 self.marker_publisher(self.animation_iteration) def marker_publisher(self, max=None): self.markers_msg = visualization_msgs.MarkerArray() for idx, marker in enumerate(self.markers[0:max]): stamp = self.get_clock().now().to_msg() msg = ROSMessage.point_marker(self.last_id, stamp, marker["point"], marker["color"], str(idx)) self.markers_msg.markers.append(msg) self.last_id += 1 self.markers_pub.publish(self.markers_msg) def clicked_point_cb(self, msg): self.print(msg) point = np.array([msg.point.x, msg.point.y, msg.point.z]) self.print(self.traversable_point_cloud.distance_to_nearest(point)) def print(self, object_to_print): self.get_logger().info(str(object_to_print))