def to_PoseStamped(klampt_se3, stamp='now'): """From Klamp't se3 element to ROS PoseStamped """ if stamp == 'now': stamp = rospy.Time.now() elif isinstance(stamp, (int, float)): stamp = rospy.Time(stamp) ros_pose = PoseStamped() ros_pose.header.stamp = stamp ros_pose.orientation = to_Quaternion(klampt_se3[0]) ros_pose.position = to_Point(klampt_se3[0]) return ros_pose
def find_frontier(self, msg): """ Returns a dictionary of frontiers given by {'key',(x,y,size)} in the occupancy grid. :return {'key',(x,y,size)} A dictionary of frontiers and their sizes. """ # Get a map from the map_expander node mapdata = FindFrontier.request_map() # Begins counting the number of centroids centroid_number = 0 # Establishes the dictionary of frontiers and the list of visited points # frontiers = {} visited_points = [] frontier = [-1515, -1515] priority = 0 [current_x, current_y] = FindFrontier.world_to_grid(mapdata, msg.start.pose.position) # Loop through all cells in the occupancy grid for h in range(mapdata.info.height - 1): for w in range(mapdata.info.width - 1): # Calculates index of the point in the grid this_index = FindFrontier.grid_to_index(mapdata, w, h) # If the point has not been checked if this_index not in visited_points: # Add this point to the list of visited points visited_points.append(this_index) # If this point is unknown if mapdata.data[this_index] == -1: # Increase the total number of centroids by one centroid_number += 1 # Set the initial size to one size = 1 # Adds to the total x and y for averaging later total_x = w total_y = h # Add any adjacent unknown points to the list of unknowns list_of_unknown_points = FindFrontier.unknown_neighbors_of_8( w, h, mapdata, []) # While there are unknown neighbors while not len(list_of_unknown_points) == 0: # Get a point from the list this = list_of_unknown_points.pop this_x = this[0] this_y = this[1] # Increase the size by one size += 1 # Add the current x and y to the totals total_x += this_x total_y += this_y # Add any adjacent unknown points to the list of unknowns list_of_unknown_points.append( FindFrontier.unknown_neighbors_of_8( this_x, this_y, mapdata, visited_points)) # Get the index for this point this_index = FindFrontier.grid_to_index( mapdata, this_x, this_y) # Add this point to the visited points list visited_points.append(this_index) # Calculate the centroid using the total xs and ys centroid_x = total_x / size centroid_y = total_y / size centroid_distance = FindFrontier.euclidean_distance( centroid_x, centroid_y, current_x, current_y) centroid_priority = size / centroid_distance if centroid_priority > priority: priority = centroid_priority frontier = (centroid_x, centroid_y) # Add the frontier to the dictionary # frontiers[centroid_number] = (centroid_x, centroid_y, size) # Return the dictionary best_point = PoseStamped() best_point.header.frame_id = "frontier" best_point.position = [frontier[0], frontier[1], 0] best_point.orientation = [0, 0, 0, 0] return best_point