def getNextValues(self, state, inp):
(map, sensors) = inp
# Make a model for planning in this particular map
dynamicsModel = gridDynamics.GridDynamics(map)
# Find the indices for the robot's current location and goal
currentIndices = map.pointToIndices(sensors.odometry.point())
goalIndices = map.pointToIndices(self.goalPoint)
if timeToReplan(state, currentIndices, map, goalIndices):
# Define heuristic to be Euclidean distance
def h(s):
return self.goalPoint.distance(map.indicesToPoint(s))
# Define goal test
def g(s):
return s == goalIndices
# Make a new plan
plan = ucSearch.smSearch(dynamicsModel,
currentIndices,
g,
heuristic=h,
maxNodes=5000)
# Clear the old path from the map
if state: map.undrawPath(state)
if plan:
# The call to the planner succeeded; extract the list
# of subgoals
state = [s[:2] for (a, s) in plan]
print 'New plan', state
# Draw the plan
map.drawPath(state)
else:
# The call to the plan failed
# Just show the start and goal indices, for debugging
map.drawPath([currentIndices, goalIndices])
state = None
if not state or (currentIndices == state[0] and len(state) == 1):
# If we don't have a plan or we've already arrived at the
# goal, just ask the move machine to stay at the current pose.
return (state, sensors.odometry)
elif currentIndices == state[0] and len(state) > 1:
# We have arrived at the next subgoal in our plan; so we
# Draw that square using the color it should have in the map
map.drawSquare(state[0])
# Remove that subgoal from the plan
state = state[1:]
# Redraw the rest of the plan
map.drawPath(state)
# Return the current plan and a subgoal in world coordinates
return (state, map.indicesToPoint(state[0]))
def getNextValues(self, state, inp):
(map, sensors) = inp
if self.useCostDynamics:
dynamicsModel = gridDynamics.GridCostDynamicsSM(map)
scale = 10.0 / (1.0 - 0.5)**4
else:
scale = 1
dynamicsModel = gridDynamics.GridDynamics(map)
goalIndices = map.pointToIndices(self.goalPoint)
currentIndices = map.pointToIndices(sensors.odometry)
return newPathAndSubgoal(map, sensors, self.goalPoint, dynamicsModel,
state, timeToReplanDynamicMap, scale)
def getNextValues(self, state, inp):
(goalIndices, (map, sensors)) = inp
if self.useCostDynamics:
dynamicsModel = gridDynamics.GridCostDynamicsSM(map)
scale = 10.0 / (1.0 - 0.5)**4
else:
scale = 1
dynamicsModel = gridDynamics.GridDynamics(map)
currentIndices = map.pointToIndices(sensors.odometry)
(path, subgoal) = newPathAndSubgoal(map, sensors, goalIndices,
dynamicsModel, state,
timeToReplanDynamicMapAndGoal,
scale)
return (path, (subgoal, path == None))
def __init__(self, goalPoint, worldPath, gridSquareSize, mapClass):
"""
@param goalPoint: instance of util.Point, representing the
desired robot location in odometry coordinates.
@param worldPath: pathname of a file containing a soar world
description, which will be used to construct the gridmap for
planning
@param gridSquareSize: size of the grid squares in the map to
be constructed
@param mapClass: a subclass of C{gridMap.GridMap}; it needs
to take a path and a grid square size as input in its
initializer.
"""
self.worldMap = mapClass(worldPath, gridSquareSize)
self.dynamicsModel = gridDynamics.GridDynamics(self.worldMap)
self.goalPoint = goalPoint
self.startState = None