Exemplo n.º 1
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]))
Exemplo n.º 2
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]))
Exemplo n.º 3
0
def eightTest(s, g, h):
    return ucSearch.smSearch(EightPuzzleSM(g),
                             s,
                             maxNodes=200000,
                             heuristic=lambda s: h(s, g))
Exemplo n.º 4
0
def numberCostTest(s, g, h):
    return ucSearch.smSearch(NumberTestCostSM(g), s, heuristic=h)
Exemplo n.º 5
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]))
Exemplo n.º 6
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]))
Exemplo n.º 7
0
def eightTest(s, g, h):
    return ucSearch.smSearch(EightPuzzleSM(g), s, maxNodes = 200000,
                             heuristic = lambda s: h(s, g))
Exemplo n.º 8
0
def numberCostTest(s, g, h):
    return ucSearch.smSearch(NumberTestCostSM(g), s, heuristic = h)