orca-robotics
INTRODUCTION
Overview
Download and Install
Documentation
REPOSITORY
Interfaces
Drivers
Libraries
Utilities
Software Map
DEVELOPER
Dashboard
PEOPLE
Contributors
Users
Project
Download
Mailing lists
latombeutil.h
00001 /* 00002 * Orca-Robotics Project: Components for robotics 00003 * http://orca-robotics.sf.net/ 00004 * Copyright (c) 2004-2008 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 #ifndef HYDRO_PATHPLAN_LATOMBE_UTIL_H 00011 #define HYDRO_PATHPLAN_LATOMBE_UTIL_H 00012 00013 #include <vector> 00014 #include <hydroogmap/hydroogmap.h> 00015 #include <hydropathplan/util.h> 00016 00017 namespace hydropathplan 00018 { 00019 00020 namespace latombeutil { 00021 00022 // Distance to a 4-adjacent cell 00023 const float AC = 1.0; 00024 // Distance to a diagonal 8-adjacent cell 00025 const float DC = 1.4142; 00026 00027 // ================= CHECKING FUNCTIONS ================================ 00029 bool areAllNans( const FloatMap &floatMap ); 00030 00033 inline bool containsNan( const FloatMap & navMap, const int x, const int y) 00034 #ifdef __QNX__ 00035 { return ( isnan(element(navMap, x, y )) ); }; 00036 #else 00037 { return ( std::isnan(element(navMap, x, y )) ); }; 00038 #endif 00039 00040 inline bool containsNan( const FloatMap & navMap, const Cell2D & c ) 00041 { return containsNan(navMap,c.x(),c.y()); }; 00042 00043 // Checks whether cell is in a free space region. 00044 // If 'checkForNan' is set, 'nan' means non-free. Else '0' means non-free. 00045 // If not, attempts to move it into one of the numCells surrounding cells. 00046 // If successful return TRUE, otherwise FALSE. 00047 bool validateCell( const FloatMap & theMap, Cell2D & c, int numCells, bool checkForNan ); 00048 00049 // ====================================================================== 00050 00051 // ================= CONVERSION FUNCTIONS ================================ 00053 int sub2ind( const int &indX, 00054 const int &indY, 00055 const unsigned int &sizeX, 00056 const unsigned int &sizeY ); 00057 00059 int sub2ind( const Cell2D &cell, 00060 const unsigned int &sizeX, 00061 const unsigned int &sizeY ); 00062 00064 Cell2D ind2sub( const int &index, 00065 const unsigned int &sizeX, 00066 const unsigned int &sizeY ); 00067 00068 // ========================================================================= 00069 00070 // ============== SKELETON NAVIGATION FUNCTIONS ============================= 00071 00072 // 00073 // Notes on parameters: 00074 // 00075 // - distGrid: 00076 // - for every OG cell, stores the 4-distance to the nearest obstacle cell 00077 // - zero means 'obstacle in this cell'. 00078 // - used in generating the skeleton 00079 // - useful for connecting free space cells to the skeleton (just follow the gradient of distGrid until you hit the skeleton) 00080 // 00081 // - costMap: 00082 // - stores the cost of moving through each cell 00083 // - 'NAN' represents infinite cost. 00084 // 00085 // - navMap: 00086 // - contains the potential at each OG cell 00087 // - potential = cost_to_goal, NAN means obstacle or close to obstacle 00088 // - following the gradient leads to the goal 00089 // 00090 // - skel: 00091 // - A vector of cells which lie on the skeleton 00092 // 00093 00098 bool computeSkeleton( const hydroogmap::OgMap &ogMap, 00099 Cell2DVector &skel, 00100 FloatMap &distGrid, 00101 double traversabilityThreshhold, 00102 double robotDiameterMetres ); 00103 00106 bool findClosestPointOnSkeleton( const Cell2DVector &skel, 00107 const FloatMap &distGrid, 00108 const Cell2D &fromCell, 00109 Cell2D &closestCell ); 00110 00113 bool connectCell2Skeleton( Cell2DVector &skel, 00114 Cell2D &cell, 00115 const FloatMap &distGrid, 00116 int robotRadiusCells ); 00117 00119 bool computePotentialSkeleton( const hydroogmap::OgMap &ogMap, 00120 const FloatMap &costMap, 00121 FloatMap &navMap, 00122 const Cell2DVector &skel, 00123 const Cell2D &startCell ); 00124 00126 void computePotentialFreeSpace( const hydroogmap::OgMap &ogMap, 00127 const FloatMap &costMap, 00128 FloatMap &navMap, 00129 const Cell2DVector &skel, 00130 double traversabilityThreshhold ); 00131 00135 bool calcSkeletonNavigation( const hydroogmap::OgMap &ogMap, 00136 FloatMap &navMap, 00137 Cell2D &startCell, 00138 double traversabilityThreshhold, 00139 double robotDiameterMetres); 00140 00142 void computeCostsFromDistGrid( const FloatMap &distGrid, 00143 FloatMap &costMap, 00144 double metresPerCell, 00145 const CostEvaluator &costEvaluator=DefaultCostEvaluator() ); 00146 00147 void computeUniformCosts( const hydroogmap::OgMap &ogMap, 00148 FloatMap &costMap, 00149 double traversabilityThreshhold ); 00150 00151 // ========================================================================= 00152 00153 // ================= GENERAL PATHPLANNING FUNCTIONS ==================== 00154 00156 enum Result 00157 { 00158 PathOk, 00159 PathStartNotValid, 00160 PathDestinationNotValid, 00161 PathDestinationUnreachable, 00162 OtherError 00163 }; 00164 00166 Result calcPath( const hydroogmap::OgMap &ogMap, 00167 const FloatMap &navMap, 00168 Cell2D &goalCell, 00169 Cell2DVector &path, 00170 double robotDiameterMetres ); 00171 00172 // ===================================================================== 00173 00174 } 00175 00176 } 00177 00178 #endif 00179
