def setLegalMoveBaseGoal(self,point, threshold=65):
"""
This function finds a legal place to make sure that the robot is capable of moving to the location
:param Point : <x,y> touple representing the locaiton in the gMap
"""
frontierMap = tools.dialateOccupancyMap(self._map, len(self._map), len(self._map[0]))
for i in xrange(3):
frontierMap = tools.dialateOccupancyMap(frontierMap, len(frontierMap), len(frontierMap[0]))
px, py = point
self.painter.paintGoal(self.goalTopic, self.convertMapToGlobal(point))
if threshold > frontierMap[py][px] > -1:
return point
raise FrontierException("The frontier you have selected is not within your visiting capabilities")
def updateMap(self, msg):
"""
This is the function that is called whenever the "/map" topic is published to
:param msg:
:return:
"""
newMap = tools.lMaptoLLMap(msg.data, msg.info.height, msg.info.width)
# Updates the map with the map data, height and width.
for i in xrange(2): # Dilates the map by a semi-arbitrary value.
newMap = tools.dialateOccupancyMap(newMap, len(self._map[0]),len(self._map))
self._map = newMap
self._max_h = msg.info.height # Updates max height
self._max_w = msg.info.width # Updates max width
self._res = msg.info.resolution
self._pose = msg.info.origin # Updates the pose to be origin
self._mapSet = True
msg.data = tools.llMaptoLMap(self._map, self._max_h, self._max_w)
self._DMap.publish(msg)
self._updateLocation()#Updates the location
