INTRODUCTION
Overview
Download and Install
Quick Start
Documentation
Publications

NONFRAMEWORK CODE
Driver Interfaces
Drivers
Libraries
Utilities

FRAMEWORK CODE
Interfaces
Components
Libraries
Utilities

Full Software Listings

DEVELOPER
Tutorials
Examples
Dev Guide
Dashboard

PEOPLE
Contributors
Users

SourceForge.net Logo
Project
Download
Mailing lists

 

         

astar.h

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2009 Alex Brooks, Alexei Makarenko, Tobias Kaupp
00005  *
00006  * This copy of Orca is licensed to you under the terms described in
00007  * the LICENSE file included in this distribution.
00008  *
00009  */
00010  
00011 //
00012 // This class was originally written by Pham Ngoc Hai (h dot pham at acfr.usyd.edu.au).
00013 //
00014 
00015 #ifndef ORCA2_ASTAR_H
00016 #define ORCA2_ASTAR_H
00017 
00018 #include <deque>
00019 
00020 namespace hydropathplan
00021 {
00022 
00023 namespace astar {
00024 
00025 class AStarNode {
00026 public:
00027         AStarNode(int, int, double);                    // cell's postion (i,j), and weight
00028         ~AStarNode();
00029 
00030         double                  m_NodeWeight;           // The weight value of the cell. This is used to calculate 
00031                                                                                 // the moving cost to move to this cell from its adjacent node
00032                                                                                 // cost = m_NodeWeight*ASTAR_CELLSIZE (if orthogonal nodes)
00033                                                                                 // cost = sqrt(2)*m_NodeWeight*ASTAR_CELLSIZE (if diagonal nodes)
00034 
00035                                                                                 
00036         double                  f,g,h;                          // total cost, cost from start to note, heuristic to goal.
00037         int                             cell_i, cell_j;         // Cell(Node) position (i,j) in the matrix
00038                                                                                 // Note: In C/C++, the first index starts from 0
00039                                                                                 // then cell_i = 0,1,... and cell_i = 0,1,...
00040                                                                                 // In Matlab: the index stars from 1 => need to convert from Matlab
00041 
00042         int                             m_NodeID;                       // Unique Identification of each node (the index in mattrix)
00043                                                                                 // the 2x2 MAP(num_rows, num_cols) in C/C++ can be seen as an 1 dimentional
00044                                                                                 // array with mxn elements, then m_NodeID can be:
00045                                                                                 // m_NodeID = cell_j * m_iAStarMapRows + cell_i = cell_j*ASTAR_GRIDSIZE_H + cell_i;
00046         
00047         int                             m_NumOfChildren;
00048 
00049         AStarNode               *m_pParent;
00050         AStarNode               *m_pNext;                       //
00051         AStarNode               *m_pChildren[8];                // List of Neighbor around a node (8 neighbors)
00052 
00053 };
00054 
00055 //Tobias: my addition
00056 typedef std::deque<AStarNode*> AStarNodeDeque;
00057 
00058 class AStar {
00059 public:
00060         AStar(double *map, int map_rows, int map_cols, double map_cellsize, bool map_AllowDiagonal, double map_ObstacleWeight);
00061         ~AStar();
00062 
00063         bool                    run();                          // Run A* to find the shortest path
00064         void                    RUN_Initialize();       // Run A* -> the 1st step: initialize
00065         int                             RUN_Step();                     // Run A* step by step
00066 
00067 //protected:
00068         bool                    m_bAllowDiagonal;       // Permit to go Digonal or not
00069 
00070         double                  m_CellSize;                     // The size of each cell (-> calculate distance)
00071         double                  m_ObstacleWeight;       // Obstacle's weight -> to check whether or not a cell is obstacle.
00072 
00073         double                  *m_pAStarMAP;           //The A* Map stored in 2x2 matrix of doubles
00074                                                                                 // this map stores the weight of each cell
00075                                                                                 // e.x weight = 1 if free zone
00076                                                                                 // weight = ASTAR_OBSTACLE_COST if Obstacle
00077         int                             m_iAStarMapRows;        //Number of Rows of A* Map
00078         int                             m_iAStarMapCols;        //Number of Columns of A* Map
00079 
00080         int                             m_iStartNodeID;         // the ID (index of 1-dimention matrix) of the Start Node
00081         int                             m_iStartNode_i;         // the cell_i (index i) of the Start Node (in C/C++ matrix)
00082         int                             m_iStartNode_j;         // the cell_j (index j) of the Start Node (in C/C++ matrix)
00083 
00084         int                             m_iGoalNodeID;          // the ID (index of 1-dimention matrix) of the Goal Node
00085         int                             m_iGoalNode_i;          // the cell_i (index i) of the Goal Node (in C/C++ matrix)
00086         int                             m_iGoalNode_j;          // the cell_j (index j) of the Goal Node (in C/C++ matrix)
00087 
00088         // Nodes Lists
00089         AStarNode               *m_pBestNode;
00090         AStarNode               *m_pOpenList;
00091         AStarNode               *m_pClosedList;
00092 
00093         // Class Methods
00094         void                    AddToOpenList(AStarNode *);
00095         AStarNode               *GetBestNode();
00096         AStarNode               *IsNodeInList(AStarNode *, int);
00097         void                    ExpandChildNodes(AStarNode *);
00098         void                    LinkChildNodes(AStarNode *, AStarNode *);
00099 
00100         void                    ClearNodes();
00101 
00102 
00103         // Check whether can go from node to neighbor, basing on:
00104                 // neighbor may be an Obstacle, 
00105                 // or neighbor is located out of the limitation of the map
00106                 // or permiting Diagonal route from node to neighbor or not.
00107         int Check_ASNodeValid(AStarNode *node, AStarNode *neighbor);
00108 
00109 
00110         // Calculate the moving cost from node to its adjacent node (1 of 8 adjacent nodes)
00111         double Cost_2AdjacentNodes(AStarNode *node, AStarNode *neighbor); 
00112 
00113         // Calculate Mahattan heuristic h from node1 to node2 (Orthogonal moving only)
00114         // D is the minimum moving cost between a node and its adjacent (D is changable...)
00115         double ManhattanHeuristic(int node1_i, int node1_j, int node2_i, int node2_j, double D);
00116 
00117         // Calculate Diagonal heuristic h from node1 to node2 (Allow Diagonal+Orthogonal moving)
00118         double DiagonalHeuristic(int node1_i, int node1_j, int node2_i, int node2_j, double D);
00119 
00120         // Inline functions.
00121         // This function convert the index of 2-dimentions matrix to the index in the coressponding
00122         // 1-dimention matrix in C/C++ (index starts from 0, other than 1 in Matlab)
00123         inline int GetNodeID(int cell_i, int cell_j) { return cell_j * m_iAStarMapRows + cell_i; }
00124 };
00125 
00126 }
00127 
00128 }
00129 
00130 #endif
 

Webmaster: Tobias Kaupp (tobasco at users.sourceforge.net)


Generated for Orca Robotics by  doxygen 1.4.5