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
