def map_callback(self, msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
self.occupancy = StochOccupancyGrid2D(
self.map_resolution,
self.map_width,
self.map_height,
self.map_origin[0],
self.map_origin[1],
10, # Increasing window size inflates obstacles
self.map_probs)
self.occupancy_strict = StochOccupancyGrid2D(
self.map_resolution,
self.map_width,
self.map_height,
self.map_origin[0],
self.map_origin[1],
6, # Increasing window size inflates obstacles
self.map_probs)
if self.x_g is not None:
# if we have a goal to plan to, replan
self.replan() # new map, need to replan
def deflate_map(self):
rospy.loginfo("[NAVIGATOR]: Deflating Map")
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
self.occupancy = StochOccupancyGrid2D(
self.map_resolution, self.map_width, self.map_height,
self.map_origin[0], self.map_origin[1], 8, self.map_probs)
def map_callback(self, msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
self.occupancy = StochOccupancyGrid2D(
self.map_resolution, self.map_width, self.map_height,
self.map_origin[0], self.map_origin[1], 8, self.map_probs)
#binary_map = (self.map_probs > 90)
#mask = scipy.binary_dilation(binary_map)
#inflated_map = 100*(mask ^ (self.map_probs > 90)) + self.map_probs
#print(self.map_width)
#print(self.map_height)
#print(self.map_resolution)
if self.x_g is not None:
# if we have a goal to plan to, replan
if (self.mode is not Mode.BACKING_UP) and (
self.mode
is not Mode.INFLATE) and (self.mode
is not Mode.INFLATE_ALIGN):
rospy.loginfo("replanning because of new map")
self.replan() # new map, need to replan
def map_callback(self, msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
#self.occupancy = StochOccupancyGrid2D(self.map_resolution,
#self.map_width,
#self.map_height,
#self.map_origin[0],
#self.map_origin[1],
#8,
#self.map_probs)
self.occupancy = StochOccupancyGrid2D(
self.map_resolution,
self.map_width,
self.map_height,
self.map_origin[0],
self.map_origin[1],
8, # NOTE: Made the window size larger
self.map_probs) # NOTE: Made the probability lower
if self.x_g is not None:
# if we have a goal to plan to, replan
rospy.loginfo("NAVIGATOR: replanning because of new map")
self.replan() # new map, need to replan
def map_callback(self, msg):
self.map_probs = msg.data
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
self.occupancy = StochOccupancyGrid2D(
self.map_resolution, self.map_width, self.map_height,
self.map_origin[0], self.map_origin[1], 8, self.map_probs)
self.occupancy_updated = True
def map_callback(self, msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
self.occupancy = StochOccupancyGrid2D(
self.map_resolution, self.map_width, self.map_height,
self.map_origin[0], self.map_origin[1], 8, self.map_probs)
self.new_map = True
def map_callback(self, msg):
"""
receives new map info and updates the map
"""
self.map_probs = msg.data
# if we've received the map metadata and have a way to update it:
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs) > 0:
self.occupancy = StochOccupancyGrid2D(
self.map_resolution, self.map_width, self.map_height,
self.map_origin[0], self.map_origin[1], 8, self.map_probs)
if self.x_g is not None:
# if we have a goal to plan to, replan
rospy.loginfo("replanning because of new map")
self.replan() # new map, need to replan
def inflate_map(self):
rospy.loginfo("[NAVIGATOR]: Inflating Map")
# convert to matrix
map_matrix = np.reshape(self.map_probs,
(384, 384)) #TODO: Magic numbers
map_matrix_inflated = morpho.grey_dilation(map_matrix, size=(3, 3))
# flatten back to list
self.map_probs_inflated = map_matrix_inflated.flatten().tolist()
if self.map_width > 0 and self.map_height > 0 and len(
self.map_probs_inflated) > 0:
self.occupancy = StochOccupancyGrid2D(self.map_resolution,
self.map_width,
self.map_height,
self.map_origin[0],
self.map_origin[1], 8,
self.map_probs_inflated)