def Simple_JPS_Planner(_map, start, goal):
#_map : [] grid map
#start: position
#goal : pose
#Open_list = [current jump point1: {'LeftDown': False, 'LeftUp': False,
# 'RightDown': False, 'RightUp': False,
# 'Right': False, 'Left': False,
# 'Up': False, 'Down': False},
# current jump point2: {'LeftDown': False, 'LeftUp': False,
# 'RightDown': False, 'RightUp': False,
# 'Right': False, 'Left': False,
# 'Up': False, 'Down': False}]
import JPS
width = _map.info.width
height = _map.info.height
start_num = maplib.position_num(_map, start)
end_num = maplib.position_num(_map, goal.position)
DENSITY = 5
raw_field = JPS_GenerateMap(_map.data, width, height)
field = JPS.generate_field(
raw_field, (lambda cell: True if cell > DENSITY else False), True)
(start_row, start_colomn, end_row,
end_colomn) = Generate_Cor(start_num, end_num)
path = JPS.jps(field, start_row, start_colomn, end_row, end_colomn)
path = JPS.get_full_path(path)
print len(path)
return path
def Simple_JPS_Planner(_map, start, goal):
#_map : [] grid map
#start: position
#goal : pose
#Open_list = [current jump point1: {'LeftDown': False, 'LeftUp': False,
# 'RightDown': False, 'RightUp': False,
# 'Right': False, 'Left': False,
# 'Up': False, 'Down': False},
# current jump point2: {'LeftDown': False, 'LeftUp': False,
# 'RightDown': False, 'RightUp': False,
# 'Right': False, 'Left': False,
# 'Up': False, 'Down': False}]
import JPS
width = _map.info.width
height = _map.info.height
start_num = maplib.position_num(_map, start)
end_num = maplib.position_num(_map, goal.position)
DENSITY = 5
raw_field = JPS_GenerateMap(_map.data, width, height)
field = JPS.generate_field(raw_field, (lambda cell: True if cell > DENSITY else False), True)
(start_row, start_colomn, end_row, end_colomn) = Generate_Cor(start_num, end_num)
path = JPS.jps(field, start_row, start_colomn, end_row, end_colomn)
path = JPS.get_full_path(path)
print len(path)
return path
def __init__(self, mapTopicName = '/map_yellow'):
self.mapName = mapTopicName
# Default stuff
self.goal = Point(1,1,0) # default
self.start = Point(1,1,0) # default
# Automatic subscribers
rospy.Subscriber(mapTopicName, OccupancyGrid, self.mapRecieved, queue_size=1)
rospy.Subscriber('/move_base_simple/yellowgoal', PoseStamped, self.goalRecieved, queue_size=2)
rospy.Subscriber('/yellowinitialpose', PoseWithCovarianceStamped, self.startRecieved, queue_size=2)
# For keeping Updates on robot position
self.Jeep = JPS()
# Flag to prevent calculations on non-existent maps
self.hasMap = False
def main():
if len(sys.argv) == 2:
configFileName = sys.argv[1]
else:
configFileName = "office.config"
cfgReader = ConfigFileReader.ConfigFileReader(configFileName)
ret, height, width, numRobots, R, baseX, baseY, initLocs, obstacles = cfgReader.readCfg(
)
if ret == -1:
print 'readCfg() Unsuccessful!'
sys.exit(-1)
else:
print 'Read the config file', configFileName
k = 2
T = 100000
algo = CAC.CAC(height, width, obstacles, numRobots, initLocs, T)
if height <= 10:
xoffset = 300
else:
xoffset = 100
if width <= 10:
yoffset = 300
else:
yoffset = 100
maxScreenHeight = 700
cellSize = int(floor(maxScreenHeight / (height + 2)))
root = Tk()
gui = GridUI.GridUI(root, height, width, cellSize, algo.gridworld,
algo.robots, algo.frontier)
guiHeight = str((height + 2) * cellSize)
guiWidth = str((width + 2) * cellSize)
xOffset = str(xoffset)
yOffset = str(yoffset)
geometryParam = guiWidth + 'x' + guiHeight + '+' + xOffset + '+' + yOffset
root.geometry(geometryParam)
ALGOFLAG = 1
if ALGOFLAG == 1:
astar = Dijkstra.Dijkstra()
path, covered, cost = astar.aStarSearch(algo.gridworld, (0, 0),
(35, 40))
cost = cost[(35, 40)]
if ALGOFLAG == 2:
astar = AStar.AStar()
path, covered, cost = astar.aStarSearch(algo.gridworld, (0, 0),
(35, 40))
cost = cost[(35, 40)]
if ALGOFLAG == 3:
astar = AStar_arjun.AStar()
path, cost = astar.aStarSearch(algo.gridworld, (0, 0), (35, 40))
cost = cost[(35, 40)]
if ALGOFLAG == 4:
astar = JPS.JPS()
covered = astar.jps(algo.gridworld, (0, 0), (35, 40))
path, cost = astar.full_path(covered)
print 'cost:', cost
if TIMER == True:
print("--- %s seconds ---" % (time.time() - start_time))
def run():
gui.redraw(height, width, cellSize, algo.gridworld, algo.robots, path)
#gui.redraw(height, width, cellSize, algo.gridworld, algo.robots, covered)
root.after(1, run)
root.after(1, run)
root.mainloop()
class MapHolster:
# Reads map meta data and saves to object constants
def __init__(self, mapTopicName = '/map_yellow'):
self.mapName = mapTopicName
# Default stuff
self.goal = Point(1,1,0) # default
self.start = Point(1,1,0) # default
# Automatic subscribers
rospy.Subscriber(mapTopicName, OccupancyGrid, self.mapRecieved, queue_size=1)
rospy.Subscriber('/move_base_simple/yellowgoal', PoseStamped, self.goalRecieved, queue_size=2)
rospy.Subscriber('/yellowinitialpose', PoseWithCovarianceStamped, self.startRecieved, queue_size=2)
# For keeping Updates on robot position
self.Jeep = JPS()
# Flag to prevent calculations on non-existent maps
self.hasMap = False
# Reads the tf stack and returns the global coordinates of the robot
def getCurrentPosition(self):
return self.Jeep.getCurrentPosition()
# Reads the tf stack and returns the global orientation of the robot
def getCurrentOrientation(self):
return self.JEEP.getCurrentOrientation()
#####################################################
########## Grid Cell Utilities ##################
#####################################################
# makes a grid cell with the x/y indices from the map
# can also take a Point object
#GLOBAL COORDINATES IN REAL LIFE
def newGridCell(self,x, y = None):
#X is actually a Point object in this case
if y == None:
y = x.y
x = x.x
gx = self.gridOrigin.x + x*self.gridResolution
gy = self.gridOrigin.y + y*self.gridResolution
return Point(gx, gy, 0)
# Converts a list of "points" in map indices to XY grid cells
def convertPointsToCells(self, points):
cells = []
for pt in points:
cells += [self.newGridCell(pt)]
return cells
# converts a random cell to be the nearest map Point(whole numbers!)
# ABSTRACTED POINTS
def convertCellToPoint(self, aCell):
mx = math.trunc((aCell.x - self.gridOrigin.x + self.gridResolution/2) / self.gridResolution)
my = math.trunc((aCell.y - self.gridOrigin.y + self.gridResolution/2) / self.gridResolution)
return Point(mx, my,0)
#####################################################
###### GridCell message Utilities ###############
#####################################################
# Takes cells as a list of Points and sends them to rviz
def makeGridCells(self, cells):
newHeader = Header(1,rospy.get_rostime(),'map')
return GridCells(newHeader, self.gridResolution, self.gridResolution, cells)
# Helper to make GridCells message
def makeMessage(self, points):
return self.makeGridCells(self.convertPointsToCells(points))
#####################################################
########## Map Utilities ##################
#####################################################
# returns the value at the given point
# can also take a Point object
def readMapPoint(self, x, y = None):
#X is actually a Point object in this case
if y == None:
y = x.y
x = x.x
width = self.currentMap.info.width
try:
return self.currentMap.data[y*width + x]
except IndexError:
return -1 # undefined
# Gets a list of the adjacent points that have low
# chances of being obstacles
# -can also take a Point object
# -optional tolerance, default 100
def getEightAdjacentPoints(self, x, y = None, tol = 100):
# X is actually a Point object in this case
if y == None:
y = math.trunc(x.y)
x = math.trunc(x.x)
# Make a list of the 8 adjacent points
pointsToCheck = []
for ax in range(x-1,x+2):
for ay in range(y-1,y+2):
pointsToCheck += [Point(ax,ay,0)]
pointsToCheck.remove(Point(x,y,0)) # Center
# A list of points that are adjacent and need
# to be checked for obstacles
pointsWithoutObstacles = []
for pt in pointsToCheck:
if self.readMapPoint(pt) < tol and self.readMapPoint(pt) != -1:
pointsWithoutObstacles += [pt]
return pointsWithoutObstacles
#####################################################
########## Subscriber Callbacks ##################
#####################################################
def mapRecieved(self, aMap):
self.currentMap = aMap
self.mapInfo = aMap.info
self.mapOrigin = aMap.info.origin.position
self.gridResolution = aMap.info.resolution # assume square map
self.gridOrigin = Point( self.mapOrigin.x + self.gridResolution/2 , self.mapOrigin.y + self.gridResolution/2 , 0)
self.hasMap = True
#print "Holster got:" + self.mapName
def goalRecieved(self, aStampedPose):
if self.hasMap:
self.goal = self.convertCellToPoint(aStampedPose.pose.position)
def startRecieved(self, aPoseWithCovarianceStamped):
if self.hasMap:
self.start = self.convertCellToPoint(aPoseWithCovarianceStamped.pose.pose.position)
