Path Planner Application Manual - Auburn University



Path Planner Application Manual

[pic]

User Manual 2

The Toolbar 2

Developer Manual 3

Introduction 3

Representing the Map 3

Standard Template Library 4

The CPathPlannerApp Class 4

The PathPlannerBase Interface 5

The PlannerNode Class 7

The Algorithms 8

Breadth-First 10

Best-First 12

Dijkstra 14

A* 17

A* Additions 22

A* On Node Mesh 22

User Manual

The Toolbar

The [pic] (Open CollisionMap) button allows the user to open a collision map. The collision map is the map on which the planning occurs. The application open Data\CollisionMap\CollisionMapBop.bmp as default relative to the application executable.

The [pic] (Application Settings) button brings up the application settings dialog. The dialog allows the user to select a planning algorithm, specify the start and goal coordinates, and specify/view other application settings.

The [pic] (Create Planner) button initializes an instance of the planner selected in the application settings dialog. If the selected planner has not been instantiated yet, it creates an instance of it.

The [pic] (Destroy Planner) button destructs the instance of the planner specified in the application settings dialog.

The [pic] (Start Planner) button signals the application that the planer should start the planning process. If an instance of the planner does not exist, one is created.

The [pic] (Pause Planner) button pauses the planning process.

The [pic] (Stop Planner) button stops the planner.

The [pic] (Planner settings) button allows the user to bring up planner specific settings. Note that some planners may not require additional settings. These settings are local to the planner and if the planner is destructed, the default settings will be restored.

The [pic] (Planner Information) button exposes the statistics and information about the selected planner.

The [pic] (Draw Progress) button toggles the drawings performed by the planner. When the button is not pressed, the planner will only be allowed to draw after it has completed the planning process. This feature is useful when timing the performance of a planner.

The [pic] (About Help) button brings up the about help dialog.

Developer Manual

Introduction

The path planner application is designed to allow for convenient creation, demonstration, and analysis of different path planning algorithms. In this manual the term path planner refers to the application component that performs the planning, and the term path planner application refers to the application itself. Developers can create a new planning algorithm and study its characteristics.

The application settings dialog allows the user to specify settings for the application. For example, it allows you to select a planning algorithm and specify some global settings such as the start and the goal coordinate. Once an algorithm is selected, the run controls can be used to run, pause, and stop the planner.

This tutorial covers planning algorithms that are derivatives of the Breadth-First algorithms. Even the A* algorithm is a modified version of the Breadth-First algorithm. The ultimate goal of this tutorial is to help you understand the A* algorithm. Instead of jumping right into A* we will first implement Breadth-First, Dijkstra, and Best-First. These algorithms are similar in many ways and by implementing them first, you will be able to better understand the A* algorithm. Implementing A* is not a hard task but it is more important to understand why it was developed and how it is different from other algorithms.

As you go through the tutorial be sure to compare your algorithms to the completed version to make sure you are on a right track. Keep in mind that writing code that performs many operations and loops many times can be harder to debug. Expect to run into bugs but use caution to avoid them.

Representing the Map

The collision map is a bitmap that represents the map the path planning occurs on. The black regions represent obstacles. The white and gray regions represent the passable regions. They represent regions with cost 1, 2, 3 and 4 respectively. The lighter the color the less it costs. These regions represent costs such as resources or the risk involved. Not all planning algorithms understand weighted regions and assume that all passable regions are weighted equally.

To represent the small-scale version of the world, many games use a graph/mesh of points (waypoints), polygons, or volumes. The nodes of such graph contain information such as the location of the node and a list of connections to other nodes. For example, two connected waypoints can represent a simple hallway. A simple hall way can also be represented as a convex polygon. Either way, you can use the data in these graphs to resolve paths between two points. The nodes of the graph can contain additional data to indicate that the entity should be climbing or crawl through a region. Tile based Games such as War Craft III and Age of Empires simply use a 2D grid. Here we assume that the world is represented as a 2D grid. Please note that a 2D grid is a specific case of a node graph where all the nodes are aligned and are connected to no more than eight immediate neighbors. A mesh of triangles (navigation mesh) is a specific case of a node graph that can have up to three connections. The algorithms covered here can be easily modified to work on an arbitrary graph of nodes.

Standard Template Library

If you are not familiar with the STL, I recommend that you go to read up on it. You can even look through Microsoft’s newer STL documentations (7.0 and up) for sample code. Here are the list of classes and functions you should be familiar with:

|list (doubly linked list) |

|push_front, push_back |

|pop_front, pop_back |

|front, back |

|begin, end |

|empty, clear |

|size |

| |

|vector (dynamically growable array) |

|operator [] |

|inser, erase |

|empty, clear |

|size |

| |

|// This data structure is not as critical to know |

|map (internally represented as a binary search tree, it can be used for fast searches |

|for an object paired with or mapped to a unique key) |

|operator [] // for convenient insertion (and retrieval) of objects using a key |

|find // checking if a value has been mapped to a key |

Once you implemented the algorithms you can go back and optimized your code by using your own data structures. This will payoff significantly for the list data structure, but not as much for the vectors. Since STL linked lists cannot require you to only put data in it that have a next and a previous pointer, every time you insert a node into an STL’s list, additional memory is allocated that contains the next and previous pointers in addition the data.

The CPathPlannerApp Class

This class is the application’s main class. It extends the CWinApp class, which store a pointer to the window. Once the application has been initialized, it acts as a state machine that provides the planner with some processing time. Here are some of the more important members of this class

static CPathPlannerApp *instance;

ApplicationState applicationState;

PathPlannerBase *planner;

int **collisionMapData;

This class is a singleton, which means that the member variable instance points to the only instance of the class. The member applicationState stores the current state of the application and is used to decide what actions are valid in different stages of the planning, as well as which function of the planner should be called. The member planner stores a pointer to an instance of the planner that is specified in the application settings dialog. collisionMapData is a dynamically allocated 2D array that stores the data of the collision map. This array is populated by 1, 2, 3, 4 and –1 to indicate the weight (or cost) at different locations of the terrain. Regions that represent obstacles are set to –1. The line bellow demonstrated how to retrieve the terrain cost at the coordinate (x, y).

int mapData = CPathPlannerApp::instance->collisionMapData[y][x];

Please note that the first index of the array is y and the second index is x (as typically expected).

The PathPlannerBase Interface

The application communicates with a planner through the PathPlannerBase Interface. Every planner should extend PathPlannerBase. The Interface defines the following functions:

virtual void PlanPath(int ix, int iy, int gx, int gy);

virtual void Run();

virtual bool IsDone();

virtual void Draw(CDC *dc);

virtual void Settings();

virtual void ShowInfo();

virtual void CleanUp();

First thing you should notice is that all the functions are virtual and meant to be overridden. When the [pic] button is pressed the application initializes an instance of the currently selected planner as indicated in the applications settings dialog. If an instance of the planner does not exist, a new one is constructed. Otherwise, the existing instance is reused (CleanUp is called). Here is what happens when the application is in the run state:

void CPathPlannerApp::StateRun(){

runCount++;

//Allow the planner to run. The planner should try to spend //approximately as much time as indicated by the timeSlice

planner->Run();

if (drawProgress){

// cause a repaint

pFrame->GetChildView()->Invalidate(FALSE);

}

if (planner->IsDone()){

applicationState = STATE_RUN_END;

}

}

The sections bellow explain what the overwritten functions should do

PlanPath (…)

The PlanPath(…) function is used to tell the planner that the planning process is about to begin. It is called when the [pic] button is pressed. In this function, you can perform some initialization and store the start and goal points.

Run ()

Immediately after PlanPath(…) is called, the Run functions is called. The application repeatedly calls the Run function until the planer indicates that it no longer needs processing time. The Run function is where the planning occurs. It is extremely important to note that the Run cannot hold on to the thread for a long time. You are responsible for making sure that it returns after spending approximately CPathPlannerApp::instance->timeSlice. This is critical because if the run function takes too long, the application will not get the chance to respond to events such as resizing, moving, and most importantly painting. You can think of the application having a game loop. Any single function called by the application cannot take too long because the game will not be responsive. In other words, the planning process must be split across frames. It may sound like this will significantly complicate the planner code, but that is not the case. Algorithms such as Breadth-First, Dijkstra, Best-First, and A* can easily be split across frames. If you store the state of the planning when the Run function returns, you can continue the process the next time the function is invoked. Since this function is called repeatedly by the application, it is important not to perform any initialization in it. Initializations can be done in PlanPath or and some in the constructor.

IsDone ()

This function is called by the application to find out whether or not the planner has completed the process. This function should return false if you wish the run function to get called again, and true to indicate that the planer is done.

Draw (…)

When drawProgress is true and the run function returns, the application invalidates the content of the window, which in turn causes the Draw function of the planner to get called. The function can draw any debugging/visualization data on the map to allow the user to see what the planner is doing. Please note that you should never call Draw directly. Following this guideline allows the user to enable/disable the drawings of the planner in order to speed up or time the process. The draw function can, for example, you can call set pixel to set a pixel (or grid cell) to a specific color.

dc->SetPixel(x, y, RGB(0, 0, 255));

Settings ()

When the planner settings button [pic] is pressed on the toolbar, the application calls the settings function of the planner. This allows the planner to popup a dialog box in order to collect planner specific settings. Note that these settings are local to the planner and not saved when the planner is destroyed.

ShowInfo ()

When the planner Information button [pic] is pressed on the toolbar, the application calls the showInfo function of the planner. This allows the planner to display statistics/summery of the planning process.

CleanUp ()

This function is called when the application decides to reuse the instance of the planner. A planner is reused when [pic] is pressed and an instance of the planner has already been created (or [pic] is grayed out). You should also call this function from the destructor of your planner as well. It is your responsibility to ensure that the cleanup function does not fail if it is called when the planner has not done any planning.

The PlannerNode Class

A path can be described as an ordered list of sub-destinations. Instances of the PlannerNode class are used to represent a path between the start point and an arbitrary point on the map, which may be the goal. The node class needs to store a position as well as a parent pointer. By chaining nodes together using the parent (or previous) pointer, we can represent a path (or a candidate solution). Note that this node class is not exactly a waypoint node. A waypoint node is used to represent the connectivity of a map, whereas the PlannerNode represents a specific path through a map and is used strictly by a planning algorithm. A clear distinction between them is that a waypoint node only needs sibling pointers, but a PlannerNode only needs a parent pointer. The PlannerNodes are used to keep track of how we got to a spot on the map as opposed to where we can possibly go from a spot on the map.

|A linked list of nodes that represent a path from goal to start |

The Algorithms

During the planning process we will come across many different paths that may lead us to the goal, and therefore need to keep track of them. By keeping track of them, we will be able to come back to them in order to create additional paths that extend them. By creating additional paths that extend existing paths, we will be able to work our way through the map in search of the goal. The open list (green) will keep track of paths that are open for consideration (or being extended), and another list, the closed list (blue), will keep track of paths that we have already considered.

|[pic] |

|Figure 1 |

Figure 1 is a snapshot of a planner in process. Nodes in the open list are green and nodes in the closed list are blue. The open list contains paths that are yet to be extended. The closed list contains paths that used to be in the open list. They have already been extended and check to see if they had ran into the goal. It is important to note that each node in the open list indicates the head of a path that ends at the start point (upper left corner in the image above).

|[pic] |

|Figure 2 |

Figure 2 shows two different paths (red and pink) that partially overlap. Each path is a linked list of nodes that start at a green spot (a node in open) and end in the start point. The paths in the image are only two of the many different paths.

The different algorithms such as Breadth-First, Dijkstra, Best-First, and A* have a lot in common. They all share the pseudo code below, which is absolutely fundamental to understand.

| |

|Create a node for the start point and push it into the open list |

|While the open list is not empty |

|Pop a node from the open list and call it currentNode |

|Check to see if it is at the goal. If so, we can exit the loop |

|Create the successors of currentNode and push them into the open list. |

|Put the currentNode in to the closed list |

|Pseudo-code 1 |

The idea is simple. We want to examine the map in order to find a path between the start and the goal. We start traversing the map and for every spot we reach we will create a node that points to its parent. We will repeat this until we reach the goal. Once we find the goal, we will follow the parent pointer of the node that reached the goal in order to find out how we got to the goal. The root node will be the only node without a parent.

The algorithms first create a root node and push it into the open list. They then loop until they have examined every node in the open list. Note that the first iteration of the loop the only node in the open list, the root node, is popped from the list. This means that if we fail to push additional nodes into the list, the loop condition will evaluate to false. In the body of the loop, the algorithms attempt to create the successors of the currentNode. These successors are in essence extensions to the path that currentNode indicate. Once the successors are generated and pushed into the open list, the currentNode is pushed into the closed list.

Before creating a successor node, the algorithms make sure that there is no more than one node for any given spot of the map. This is to prevent unnecessary re-exploration of different parts of the map. For example, there is no need to store more than one path between the start and a specific point on the map. To prevent re-exploration of the map, before creating a successor node, we have to see if we have already made a node for that spot of the map. In order to do so, they have to check the open and the close list. This step can be very expensive (unless you use a data structure that offers fast lookups such as hashTable or hashMap), but is still a sound thing to do. Not catching redundant nodes can result in substantial consumption of memory as well as substantially longer time to find the goal.

The main difference between the different algorithms is in which node (or path) they decide to bring out of the open list. Breadth-First brings out the one that has been waiting the longest; Dijkstra brings out the one that is the cheapest and Best-First chooses the one that is closest to the goal. Instead of using time, cost, or distance, A* uses a combination of cost and distance.

Breadth-First

Breadth-First tries to find a path from start to goal by examining the search space ply-by-ply. This behavior is because of the fact that for every iteration of the while loop, it pops the node that has been waiting the longest. In order to do so efficiently, you can simply emulate a queue. Every time a successor is generated, push it to the back of the list and always pop from the front of the list. The pseudo-code bellow shows the significant details. The terms defined here will be using in other pseudo-codes.

Note that Breadth-First is not the perfect algorithm for path planning. As you can see it generates many nodes before it finds the goal. This means it uses a lot of memory and CPU. In addition, Breadth-First has no concept of cost or even distance. Even though it finds a path to the goal that has fewest possible nodes in between, it does not find the optimal solution in terms of distance. If you run Breadth-First for different coordinates, you will see that the path it finds goes diagonal for no reason. The path that Breadth-First finds depends heavily on the order in which the successor nodes are created.

list open // sorted list of nodes open for consideration

list closed // list of nodes that we have already considered

list solution // the nodes along the path from start to goal

Node currentNode // the node we are currently working on

Node successorNode // one of the children of currentNode

Node rootNode // the start node

Point startPoint // a pair of x and y that contain the start coordinates

Point goalPoint // a pair of x and y that contain the goal coordinates

Point nearbyPoint // a pair of x and y that contain one of the 8 possible

// coordinates around the currentNode

|Breadth-First Pseudo-Code |

| |

|1)create the rootNode: |

|- set its x and y to the startPoint |

|- set its parent to NULL |

| |

|2)push rootNode in to open |

| |

|3)while open is not empty |

| |

|1)pop the node that has been waiting the longest from open and assign it to |

|currentNode |

| |

|2)if currentNode's x and y are equal to the goalPoint, then |

|// Note: we can traverse through the parents of currentNode |

|- push the nodes that are part of the path in to solution |

|- break from step 3 |

| |

|// Note: a node has 8 points around it which can be used to create min of 0 and |

|// max of 8 successor |

|3)for every nearbyPoint around the currentNode do the following |

| |

|1)if this nearbyPoint is in a spot that is illegal such as a wall, then |

|- skip to the next nearbyPoint |

| |

|2)if a node for this nearbyPoint has been created before, then |

|- skip to the next nearbyPoint |

| |

|3)create successorNode: |

|- set its x and y to the nearbyPoint |

|- set its parent to be currentNode |

| |

|4)push successorNode in to open |

| |

|4)push the currentNode in to closed |

| |

|4)if the while loop completes without finding the goal, the goalPoint is unreachable |

Programming Hints

• Given a coordinate, how can you compute the coordinates of its neighbors? Since the world is represented as a grid, you can use a lookup table or a nested for loop. A point on the grid can have up to 8 points around it. If using a for loop, be careful to note that a 3 by 3 loop results in 9 pairs of values, and that you need to skip one of them.

• I strongly suggest that you compute the coordinates of a nearByPoint and store it in variables such as nearbyPointX and nearbyPointY, or successorX and successorY. Not doing so will make your code harder to read and a good candidate for very hard to find bugs.

• If you find a node that represents the goal position, you can stop the search process. You can now store the solution to the problem, which is an ordered list of nodes from start to goal. You can use a vector or a list.

• For any spot of the map there should be at most 1 node, which will be in either the open or the closed list. You should NOT generate nodes for spots on the map that already have a node generated for them. Every time you go to make a node you have to check all the nodes you have ever made before! This is an expensive check. Where are all the nodes you have ever made before? All in open? All in closed? Important to note that some are in open and some in closed. Later on you can replace a open/closed list with a hash table to significantly speed up this step.

• Your algorithm should be split across frames. That is, it should not spend more than CPathPlannerApp::instance->timeSlice milliseconds in the Run() function. When Run is called, do a GetTickCount() to store when you entered the function. Every pass of the loop check to see if you have spent more than the allowed time in which case the function should return. The application keeps calling the run function until isDone() returns true. You should insert the check somewhere in your while loop so if time slice is –1, it does the loop exactly once. This will allow the while loop to return every iteration and cause a repaint.

• When your algorithm is totally done, you should set the isDone flag to true so that the app does not keep calling your run function anymore. Note that the planner is done when it either finds the goal or open becomes empty.

• Be sure to clean up ALL nodes and other memory that you allocated. Please note that your clean up functions should be robust. As mentioned earlier, the application calls the cleanup before PlanPath to ensure the planer is ready to go.

• Write the draw function. Walk the open and closed list and draw a pixel for every node.

Best-First

Best-First uses problem specific knowledge about the problem to speed up the search process. Best-First is a heuristic search (as opposed to exhaustive) and tries to head right for the goal. Instead of keeping the open list sorted by time or cost, it uses the distance of each node to the goal. The distance-to-goal serves as a heuristic that promotes the regions of the map that are more likely to lead to the goal. This causes the algorithm to head directly to the goal. It is important to note that heading towards the goal is not always the right thing to do. Just because you are getting closer to the goal does not mean that you will not go down a dead end track. Yet generally speaking, it is a good rule of thumb (or estimate) to head towards the goal first.

Best-First is extremely faster than Breadth-First and Dijkstra. It typically creates very few nodes and finds a rather straight path to the goal. Best-First suffers from a few disadvantages. First, it does not understand terrain cost. It also heads to the goal and can find a not so optimal path because it is only concerned with getting closer to the goal, and not how far a path might have traveled before getting close to the goal.

|Best-First Pseudo-Code |

| |

|1)create the rootNode: |

|- set its x and y to the startPoint |

|- set its parent to NULL |

|- set its heuristicCost = distance of startPoint from the goalPoint |

| |

|2)push rootNode in to open |

| |

|3)while open is not empty |

| |

|1)pop the node with the best heuristicCost from open and assign it to currentNode |

| |

|2)if currentNode's x and y are equal to the goalPoint, then |

|// Note: we can traverse through the parents of currentNode |

|- push the nodes that are part of the path in to solution |

|- break from step 3 |

| |

|// Note: a node has 8 points around it which can be used to create min of 0 and |

|// max of 8 successor |

|3)for every nearbyPoint around the currentNode do the following |

| |

|1)if this nearbyPoint is in a spot that is illegal such as a wall, then |

|- skip to the next nearbyPoint |

| |

|2)if a node for this nearbyPoint has been created before, then |

|- skip to the next nearbyPoint |

| |

|3)create successorNode: |

|- set its x and y to the nearbyPoint |

|- set its parent to be currentNode |

|- set its heuristicCost = distance of nearbyPoint from the goalPoint |

| |

|4)push successorNode in to open |

| |

|4)push the currentNode in to closed |

| |

|4)if the while loop completes without finding the goal, the goalPoint is unreachable |

Programming Hints

• Do we need to use a queue or a priority queue? How can you sort the open list (or keep it sorted)? First, use a vector instead of a list for you open. Check out the SmartInsert function. SmartInsert is basically a binary search/insert function that will call the compare() function of the Node class as many times as it needs to in order to find the right place to insert the node in the open vector. You should write the Node::BestFirstCompare().

• When using a vector, you should pop from the back instead of the front. Vectors have substantial penalty for removing from the front (all the data in the vector have to get moved)

• Use a vector for open and a list for close

• Write the computeHeuristic() function

• Write the BestFirstCompare () function in the Node class. What should it compare for Best-First? Also make sure SmartInsert calls this compare function.

• Run it and see if it likes to get closer and closer to the goal. Does it do what you expect it to do? Compare it to the completed version.

Dijkstra

This algorithm is similar to Breadth-First but find the most optimal solution. The problem with Breadth-First was that as it went along, it only cared about plies. Dijkstra goes a step further and keeps track of how much a path costs and every iteration of the loop it pops the cheapest path. This means that every node needs to store a cost that indicates how much we have paid to get to it from the start node. When Dijkstra goes to generate a successor node, it adds the cost of reaching the parent of the successor node to the cost of getting from the parent to the successor node. Note that the parent of the successor node is the current node. Also, note that when going diagonal on a grid map, we have to add some additional cost since a little longer distance is traveled.

|In general |

|Given cost = parent’s cost + cost of going from the parent to this successor |

|When moving left, right, up, or down |

|Given cost = parent’s cost + mapData[y][x] |

|When moving diagonal |

|Given cost = parent’s cost + mapData[y][x] * √2 |

|[pic] |a = b = 1 unit |

| |c2 = a2 + b2 |

| |c = √ (a2 + b2) |

| |c = √2 = 1.4142… |

Dijkstra finds a better solution than Breadth-First. It can also take into account regions with different weights. If you run Dijkstra with diagonal penalty enabled (through the planner settings dialog), it will find the most optimal path. In fact, no other algorithms can find a more optimal solution than Dijkstra does. At the very best they can hope to be as good.

A special complication occurs in A* that needs some extra code. When we did Breadth-First, since we went ply by ply, it would never find a better path to a spot on the map for which it had made a node already. Unlike Best-First and Breadth-First, you cannot simply say: “If a node for a spot of the map has been created already, forget about it”. Instead, you have to find out if the new path (i.e. successorNode) to that spot is better than the one that has been created before (i.e. oldNode). How do you know if a node is in fact better than another node? Check their given costs.

The proper pseudo-code for Dijkstra is:

| |

|Create a node for the start point and push it into the open list |

|While the open list is not empty |

|Pop a node from the open list and call it currentNode |

|Check to see if it is at the goal. If so, we can exit the loop |

|ONLY create and push a successor node into the open list if: |

|We have not already made a node for that spot of the map. |

|Or, a successor node is better than a node that has already been created. |

|Put the currentNode in to the closed list |

|Pseudo-code 2 |

Note: please be aware that it is not easy to see if the additional check (check labeled ii) is working or not. This is because it only affects path planning when diagonal penalty is ON. Even if it is turned on, the difference is not easy to detect. To test it, make sure you are using diagonal penalty of 1.4142 (do not use sqrt(2.0) or anything else), run on BOP, set goal coordinate to (10, 6), the given cost of the solution must be exactly 12.071 and not anything else.

Dijkstra suffers from a substantial problem. It does still take up a lot of memory and CPU time to find a path. Both Breadth-First and Dijkstra are Exhaustive searches. In other words, they do not take advantage of the location of the goal to spend more time in regions that are more likely to lead to the goal.

|Dijkstra Pseudo-Code (this version is simple, but not perfect) |

|(Note: It will work if the distance to the neighbors are exactly the same. In other words, it will work if you do not use diagonal |

|penalty. The problematic lines have been highlighted. You must implement the second version of Dikstra that is correct) |

| |

|1)create the rootNode: |

|- set its x and y to the startPoint |

|- set its parent to NULL |

|- set its givenCost = 0 |

| |

|2)push rootNode in to open |

| |

|3)while open is not empty |

| |

|1)pop the node with the best givenCost from open and assign it to currentNode |

| |

|2)if currentNode's x and y are equal to the goalPoint, then |

|// Note: we can traverse through the parents of currentNode |

|- push the nodes that are part of the path in to solution |

|- break from step 3 |

| |

|// Note: a node has 8 points around it which can be used to create min of 0 and |

|// max of 8 successor |

|3)for every nearbyPoint around the currentNode do the following |

| |

|1)if this nearbyPoint is in a spot that is illegal such as a wall, then |

|- skip to the next nearbyPoint |

| |

|2)if a node for this nearbyPoint has been created before, then |

|- skip to the next nearbyPoint |

| |

|3)create successorNode: |

|- set its x and y to the nearbyPoint |

|- set its parent to be currentNode |

|- set its givenCost = currentNode’s givenCoset + cost of spot |

|nearbyPoint is on |

| |

|4)push successorNode in to open |

| |

|4)push the currentNode in to closed |

| |

|4)if the while loop completes without finding the goal, the goalPoint is unreachable |

|Dijkstra Pseudo-Code (correct version) |

| |

|1)create the rootNode: |

|- set its x and y to the startPoint |

|- set its parent to NULL |

|- set its givenCost = 0 |

| |

|2)push rootNode in to open |

| |

|3)while open is not empty |

| |

|1)pop the node with the best givenCost from open and assign it to currentNode |

| |

|2)if currentNode's x and y are equal to the goalPoint, then |

|// Note: we can traverse through the parents of currentNode |

|- push the nodes that are part of the path in to solution |

|- break from step 3 |

| |

|// Note: a node has 8 points around it which can be used to create min of 0 and |

|// max of 8 successor |

|3)for every nearbyPoint around the currentNode do the following |

| |

|1)if this nearbyPoint is in a spot that is illegal such as a wall, then |

|- skip to the next nearbyPoint |

| |

|2)create successorNode: |

|- set its x and y to the nearbyPoint |

|- set its parent to be currentNode |

|- set its givenCost = currentNode’s givenCoset + cost of spot |

|nearbyPoint is on |

| |

|3)if a node for this nearbyPoint has been created before, then |

|// Note: to see whether or not a node is in fact better than the other, |

|// we have to compare their givenCost |

|- if successorNode is better than oldNode, then |

|- pop the oldNode and delete it |

|- else |

|- skip to the next nearbyPoint |

| |

|4)push successorNode in to open |

| |

|4)push the currentNode in to closed |

| |

|4)if the while loop completes without finding the goal, the goalPoint is unreachable |

Programming Hints

• What should the given cost of a node be? Keep in mind that given cost is not the weight of the terrain for the spot of the map the node is on. It is rather the accumulated cost paid to get from the start node to a node. (It is based on the cost of the parent and the mapData at that spot)

• Make sure that the open list is sorted appropriately and that Node::DikstraCompare() is being used.

• First, get Dijkstra to work without diagonal penalty. The algorithm should look similar to Breadth-First on a map with uniform weights. On a map with different terrain weights, Dijkstra prefers less costly paths. Then add the code so that when diagonal penalty is on, and the planner is going diagonal, addition cost is added. In order to enable/disable the diagonal penalty option through the PlannerSettings dialog, you have to make sure your Settings() function is implemented properly. Compare your planner with the completed version. Note that when diagonal penalty is true, the paths do not have unnecessary zigzags.

A*

A* resolves most issues of Breadth-First, Best-First, and Dijkstra. It does not use a lot of memory, it is fast, and it can find optimal paths. A* pretty much combines Best-First and Dijkstra in the sense that it considers both the give cost (how much it cost to a node from the start) and heuristic cost (how close a node is to the goal). A* keeps the open list sorted by final cost.

|final cost = given cost + heuristic cost |

Note that the extra check we added to Dijkstra is necessary for A*. When doing A*, since the heuristic is only an estimate of how close the node is to the goal, it can come across many scenarios where it find a better path to a spot that has been visited already. When A* goes to create a successor node and realize that it has created a node with the same x and y already, it goes with the better one. Better means the cheaper or the one with the lower given cost not the final cost.

A* guarantees to find an optimal path if the heuristic value is not an overestimate. This means that if it costs 10 to get from start to goal, the heuristic does not say 15. Yet it would be okay for it to underestimate and say we will only have to pay 5 to get to the goal.

|A* Pseudo-Code |

|(Note: This pseudo code is easier to understand, but you should implement the next pseudo-code labeled “A* Pseudo-Code, Recycle |

|Redundant Nodes”) |

| |

|1)create the rootNode: |

|- set its x and y to the startPoint |

|- set its parent to NULL |

|- set its finalCost = givenCost + heuristicCost |

| |

|2)push rootNode in to open |

| |

|3)while open is not empty |

| |

|1)pop the node with the best finalCost from open and assign it to currentNode |

| |

|2)if currentNode's x and y are equal to the goalPoint, then |

|// Note: we can traverse through the parents of currentNode |

|- push the nodes that are part of the path in to solution |

|- break from step 3 |

| |

|// Note: a node has 8 points around it which can be used to create min of 0 and |

|// max of 8 successor |

|3)for every nearbyPoint around the currentNode do the following |

| |

|1)if this nearbyPoint is in a spot that is illegal such as a wall, then |

|- skip to the next nearbyPoint |

| |

|2)create successorNode: |

|- set its x and y to the nearbyPoint |

|- set its parent to be currentNode |

|- set its finalCost = givenCost + heuristicCost |

| |

|3)if a node for this nearbyPoint has been created before, then |

|// Note: to see whether or not a node is in fact better than the other, |

|// we have to compare their givenCost |

|- if successorNode is better than oldNode, then |

|- pop the oldNode and delete it |

|- else |

|- skip to the next nearbyPoint |

| |

|4)push successorNode in to open |

| |

|4)push the currentNode in to closed |

| |

|4)if the while loop completes without finding the goal, the goalPoint is unreachable |

Note that in the algorithm above, there are scenarios where a node is created but skipped because it is more expensive than a node we have previously made. There are also scenarios where we delete a node and create another node instead. This happens when we find a batter way to a spot on the map that we have visited (or created a node) before. The pseudo code bellow does not create nodes unless they are needed, and reuses some nodes when appropriate. It overrides the data of the node so that it is like a brand new node. It then inserts the updated node into the open list. By doing so, much fewer nodes are created, which reduces time and memory fragmentation.

|A* Pseudo-Code, Recycle Redundant Nodes |

| |

|1)create the rootNode: |

|- set its x and y to the startPoint |

|- set its parent to NULL |

|- set its finalCost = givenCost + heuristicCost |

| |

|2)push rootNode in to open |

| |

|3)while open is not empty |

| |

|1)pop the node with the best finalCost from open and assign it to currentNode |

| |

|2)if currentNode's x and y are equal to the goalPoint, then |

|// Note: we can traverse through the parents of currentNode |

|- push the nodes that are part of the path in to solution |

|- break from step 3 |

| |

|// Note: a node has 8 points around it which can be used to create min of 0 and |

|// max of 8 successor |

|3)for every nearbyPoint around the currentNode do the following |

| |

|1)if this nearbyPoint is in a spot that is illegal such as a wall, then |

|- skip to the next nearbyPoint |

| |

|2)if a node for this nearbyPoint has been created before, then |

|// Note: to see whether or not a node is in fact better than the other, |

|// we have to compare their givenCost not their finalCost |

|- if this nearbyPoint would make a node better than oldNode, then |

|// Note: by updating the oldNode, we avoid creating a new node for the |

|// nearbyPoint |

|- update the oldNode: |

|- set its parent to currentNode |

|- set its givenCost = currentNode’s givenCoset + cost of spot |

|nearbyPoint is on |

|- set its finalCost = givenCost + heuristicCost |

| |

|- if the oldNode is in closed, then |

|- pop it from closed and push in open |

|// Note: if the successor is in open already, any changes to its |

|// finalCost should affects its priority |

|- else |

|- skip to the next nearbyPoint |

| |

|3)create successorNode: |

|- set its x and y to the nearbyPoint |

|- set its parent to be currentNode |

|- set its finalCost = givenCost + heuristicCost |

| |

|4)push successorNode in to open |

| |

|4)push the currentNode in to closed |

| |

|4)if the while loop completes without finding the goal, the goalPoint is unreachable |

Programming Hints

• Even though both A* pseudo-codes are similar in behavior, A* Pseudo-Code Recycle Redundant Nodes creates less nodes by reusing redundant ones. You should do the more optimal one.

• Technically A* is the combination of Best-First and Dijkstra. Instead of using only the heuristic cost (for Best-First) or given cost (for Dijkstra) it uses the final cost.

• Write the AStarCompare() function.

• This step is the most tricky and important part. You must get this right! Instead of simply saying “don’t generate nodes for spots of the map that we have generated nodes for already”, we have to say “if we have generated a node for a spot of the map already, compare the paths so that we can go with the better (cheaper) one”

• Do we need to look in both the open and the closed list as we have been doing? (yes)

• How would you know if you have in FACT found a cheaper way? Could we check the final cost or it should be the given cost? Even though you will get the same result, you must base your decisions only on facts and not guesses (heuristics)

• What do you think we have to do if we find a cheaper path to a spot on the map? We can update the old node by replacing its data as if it is a different node. The new/updated node should be inserted in to the open list for reconsideration. If you are reusing a node, be sure to remove it from any list it was in already. Even if you are updating a node already in open, you have to remove and re-smartInsert it so that it is moved to the proper location in the list. What do you think we have to do if we find a more expensive path to a spot on the map? We should forget about the worse path. In order to make sure this A* specific check works properly, make sure when you run your planner on the BOP map, it re-explores the region of the map indicated in the following image.

|[pic] |

|The indicated region is re-explored by A* because better paths than existing paths are being found. This snap shot is taken from |

|the Bop map, heuristic weight of one, and no diagonal penalty. |

Make sure however you layout your logic, the five scenarios are handled properly. When you try to extend a path (create a node) and:

1. It would be a better path to a spot on the map, and the worse path is in open,

2. It would be a better path to a spot on the map, and the worse path is in close.

3. It would be a worse path to a spot on the map, and the better path is in open.

4. It would be a worse path to a spot on the map, and the better path is in close.

5. It would be a path to a spot on the map that has never been visited before.

• Does your A* work? Before moving on make sure it does! If it almost works, it’s probably entirely wrong. If you run it on the BOP map with heuristic weight of one and no diagonal penalty (starting at 1,1 searching for 98, 98), you must get exactly the same number of constructed nodes and runCount as the completed version. (It is ok for the numbers to be different for other maps or different settings because of the order in which successor nodes are created. Same holds for other algorithms. The numbers for, say, breadth fist doe not have to match the completed version, BUT A* with exact settings mentioned above must match the numbers)

• Once you have a working A*, add the heuristic weight. It will control the CPU and memory usage vs. optimality of the solution. f =g + (w * h). The lager the value of w, the more Best-First the A* algorithm will behave. The smaller the value of w, the more Dijkstra the A* algorithm will be.

• Make sure your diagonal penalty also works.

• Next, you can start optimizing your A* algorithm. Before starting to optimize the algorithm, make sure it is working perfectly.

• Best place to start is to use a better data structure for your open and specially closed list. Since the closed list search is the most expensive operation in of the planner, speeding it up can result in substantial improvement. You can implement a hash map or a hash table and use the coordinates of a node in order to compute as the key (or hash code). For the problem here you can compute a hash code by saying: (x ................
................

In order to avoid copyright disputes, this page is only a partial summary.

Google Online Preview   Download