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