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

 

         

latombeutil.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 #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
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 
 

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


Generated for Orca Robotics by  doxygen 1.4.5