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
# 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 eightTest(s, g, h):
return ucSearch.smSearch(EightPuzzleSM(g),
s,
maxNodes=200000,
heuristic=lambda s: h(s, g))
def numberCostTest(s, g, h):
return ucSearch.smSearch(NumberTestCostSM(g), s, heuristic=h)
def newPathAndSubgoal(worldMap, sensorInput, goalPoint, dynamicsModel, path,
timeToReplan, scale = 1):
"""
This procedure does the primary work of both replanner classes.
It tests to see if the current plan is empty or invalid. If so,
it calls the planner to make a new plan. Then, given a plan, if
the robot has reached the first grid cell in the plan, it removes
that grid cell from the front of the plan. Finally, it gets the
the center of the current first grid-cell in the plan, in odometry
coordinates, and generates that as the subgoal.
It uses a heuristic in the planning, which is the Cartesian
distance between the current location of the robot in odometry
coordinates (determined by finding the center of the grid square)
and the goal location.
Whenever a new plan is made, it is drawn into the map. Whenever a
subgoal is achieved, it is removed from the path drawn in the map.
@param worldMap: instance of a subclass of C{gridMap.GridMap}
@param sensorInput: instance of C{io.SensorInput}, containing current
robot pose
@param goalPoint: instance of C{util.Point}, specifying goal
@param dynamicsModel: a state machine that specifies the
transition dynamics for the robot in the grid map
@param path: the path (represented as a list of pairs of indices
in the map) that the robot is currently
following. Can be C{None} or C{[]}.
@param timeToReplan: a procedure that takes C{path}, the robot's
current indices in the grid, the map, and the
indices of the goal, and returns C{True} or
C{False} indicating whether a new plan needs to
be constructed.
@returns: a tuple C{(path, subgoal)}, where C{path} is a list of
pairs of indices indicating a path through the
grid, and C{subgoal} is an instance of
C{util.Point} indicating the point in odometry
coordinates that the robot should drive to.
"""
currentIndices = worldMap.pointToIndices(sensorInput.odometry.point())
if isinstance(goalPoint, tuple):
goalIndices = goalPoint
goalPoint = worldMap.indicesToPoint(goalIndices)
else:
goalIndices = worldMap.pointToIndices(goalPoint)
if timeToReplan(path, currentIndices, worldMap, goalIndices):
if path: worldMap.undrawPath(path)
def h(s):
return scale * goalPoint.distance(worldMap.indicesToPoint(s))
plan = ucSearch.smSearch(dynamicsModel,
currentIndices,
lambda s: s == goalIndices,
heuristic = h,
maxNodes = 20000)
if plan:
path = [s for (a, s) in plan]
worldMap.drawPath(path)
print 'New plan', path
else:
worldMap.drawPath([currentIndices, goalIndices])
worldMap.drawWorld()
path = None
if not path:
return (path, sensorInput.odometry)
else:
if currentIndices == path[0] and len(path) > 1:
worldMap.drawSquare(path[0])
path = path[1:]
worldMap.drawPath(path)
return (path, worldMap.indicesToPoint(path[0]))
def newPathAndSubgoal(worldMap,
sensorInput,
goalPoint,
dynamicsModel,
path,
timeToReplan,
scale=1):
"""
This procedure does the primary work of both replanner classes.
It tests to see if the current plan is empty or invalid. If so,
it calls the planner to make a new plan. Then, given a plan, if
the robot has reached the first grid cell in the plan, it removes
that grid cell from the front of the plan. Finally, it gets the
the center of the current first grid-cell in the plan, in odometry
coordinates, and generates that as the subgoal.
It uses a heuristic in the planning, which is the Cartesian
distance between the current location of the robot in odometry
coordinates (determined by finding the center of the grid square)
and the goal location.
Whenever a new plan is made, it is drawn into the map. Whenever a
subgoal is achieved, it is removed from the path drawn in the map.
@param worldMap: instance of a subclass of C{gridMap.GridMap}
@param sensorInput: instance of C{io.SensorInput}, containing current
robot pose
@param goalPoint: instance of C{util.Point}, specifying goal
@param dynamicsModel: a state machine that specifies the
transition dynamics for the robot in the grid map
@param path: the path (represented as a list of pairs of indices
in the map) that the robot is currently
following. Can be C{None} or C{[]}.
@param timeToReplan: a procedure that takes C{path}, the robot's
current indices in the grid, the map, and the
indices of the goal, and returns C{True} or
C{False} indicating whether a new plan needs to
be constructed.
@returns: a tuple C{(path, subgoal)}, where C{path} is a list of
pairs of indices indicating a path through the
grid, and C{subgoal} is an instance of
C{util.Point} indicating the point in odometry
coordinates that the robot should drive to.
"""
currentIndices = worldMap.pointToIndices(sensorInput.odometry.point())
if isinstance(goalPoint, tuple):
goalIndices = goalPoint
goalPoint = worldMap.indicesToPoint(goalIndices)
else:
goalIndices = worldMap.pointToIndices(goalPoint)
if timeToReplan(path, currentIndices, worldMap, goalIndices):
if path: worldMap.undrawPath(path)
def h(s):
return scale * goalPoint.distance(worldMap.indicesToPoint(s))
plan = ucSearch.smSearch(dynamicsModel,
currentIndices,
lambda s: s == goalIndices,
heuristic=h,
maxNodes=20000)
if plan:
path = [s for (a, s) in plan]
worldMap.drawPath(path)
print 'New plan', path
else:
worldMap.drawPath([currentIndices, goalIndices])
worldMap.drawWorld()
path = None
if not path:
return (path, sensorInput.odometry)
else:
if currentIndices == path[0] and len(path) > 1:
worldMap.drawSquare(path[0])
path = path[1:]
worldMap.drawPath(path)
return (path, worldMap.indicesToPoint(path[0]))
def eightTest(s, g, h):
return ucSearch.smSearch(EightPuzzleSM(g), s, maxNodes = 200000,
heuristic = lambda s: h(s, g))
def numberCostTest(s, g, h):
return ucSearch.smSearch(NumberTestCostSM(g), s, heuristic = h)