replanner

Module replanner

State machine classes for planning paths in a grid map.

Classes

Replanner
This replanner state machine has a fixed map, which it constructs at initialization time.

ReplannerWithDynamicMap
This replanner state machine has a dynamic map, which is an input to the state machine.

ReplannerWithDynamicMapAndGoal
This replanner state machine has a dynamic map and a dynamic goal, both of which are inputs to the state machine.

Functions

newPathAndSubgoal(worldMap, sensorInput, goalPoint, dynamicsModel, path, timeToReplan, scale=1)
This procedure does the primary work of both replanner classes.

timeToReplanStaticMap(plan, currentIndices, worldMap, goalIndices)
When the map is static, we just test for kidnapping.

adjacent((x1, y1), (x2, y2))

timeToReplanDynamicMap(plan, currentIndices, map, goalIndices)
Replan if the current plan is None, if the plan is invalid in the map (because it is blocked), or if the plan is empty and we are not at the goal (which implies that the last time we tried to plan, we failed).

timeToReplanDynamicMapAndGoal(plan, currentIndices, map, goalIndices)
Replan if the current plan is None, if the plan is invalid in the map (because it is blocked), if the plan is empty and we are not at the goal (which implies that the last time we tried to plan, we failed), or if the end of the plan is not the same as the goal indices (which means the goal changed).

planInvalidInMap(map, state)
Just checks to be sure the first two cells are occupiable.

timeToReplanDynamicMapWithKidnap(state, currentIndices, map, goalIndices)
Replan if the current plan is None, if the plan is invalid in the map (because it is blocked), or if the robot is not in a grid cell that is adjacent to the first one in the plan.

Variables

__package__ = None

Function Details

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.

Parameters:

worldMap - instance of a subclass of gridMap.GridMap
sensorInput - instance of io.SensorInput, containing current robot pose
goalPoint - instance of util.Point, specifying goal
dynamicsModel - a state machine that specifies the transition dynamics for the robot in the grid map
path - the path (represented as a list of pairs of indices in the map) that the robot is currently following. Can be None or [].
timeToReplan - a procedure that takes path, the robot's current indices in the grid, the map, and the indices of the goal, and returns True or False indicating whether a new plan needs to be constructed.

Returns:

a tuple (path, subgoal), where path is a list of pairs of indices indicating a path through the grid, and subgoal is an instance of util.Point indicating the point in odometry coordinates that the robot should drive to.

timeToReplanStaticMap(plan, currentIndices, worldMap, goalIndices)

When the map is static, we just test for kidnapping. Replan if the current plan is None or if the robot is not in a grid cell that is adjacent to the first one in the plan.

planInvalidInMap(map, state)

Just checks to be sure the first two cells are occupiable. In low noise conditions, it's good to check the whole plan, so failures are discovered earlier; but in high noise, we often have to get close to a location before we decide that it is really not safe to traverse.