Orca Robotics

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_UTIL_H
00011 #define HYDRO_PATHPLAN_UTIL_H
00012 
00013 #include <vector>
00014 #include <hydroogmap/hydroogmap.h>
00015 #include "cell2d.h"
00016 #include "typemap.h"
00017 
00018 namespace hydropathplan
00019 {
00020 
00022     class CostEvaluator {
00023     public:
00024         virtual ~CostEvaluator() {}
00025         
00026         // returns the cost of moving through a cell whose 4-distance to the nearest obstacle is distInMetres.
00027         virtual double costAtDistFromObstacle( double distInMetres ) const=0;
00028     };
00029 
00031     class DefaultCostEvaluator : public CostEvaluator {
00032     public:
00033         double costAtDistFromObstacle( double distInMetres ) const
00034             { if ( distInMetres > 0 ) return 1; else return NAN; }
00035     };
00036 
00037 // The 'util' namespace contains stuff that's not intended for use outside of this library.
00038 namespace util {
00039 
00043     bool isTraversable( const hydroogmap::OgMap &ogMap,
00044                         const int indX,
00045                         const int indY,
00046                         const float traversabilityThreshhold);
00047 
00049     inline bool isTraversable( const hydroogmap::OgMap &ogMap,
00050                                const Cell2D            &cell,
00051                                const float              traversabilityThreshhold)
00052     { return isTraversable(ogMap,cell.x(),cell.y(),traversabilityThreshhold); }
00053 
00055     bool isTraversableNeighbors( const hydroogmap::OgMap &ogMap,
00056                                  const int indX,
00057                                  const int indY,
00058                                  const float traversabilityThreshhold );
00059 
00061     bool losExists( const hydroogmap::OgMap &ogMap,
00062                     double traversabilityThreshhold,
00063                     const Cell2D &c1,
00064                     const Cell2D &c2 );
00065 
00068     int robotDiameterInCells( const hydroogmap::OgMap &ogMap, const double robotDiameterMetres );
00069 
00072     //  TODO: AlexB: Why grow by the diameter rather than the radius??
00073     void growObstaclesOgMap( hydroogmap::OgMap &ogMap,
00074                              const double       traversabilityThreshhold,
00075                              const int          robotDiameterCells );
00076 
00084     void optimizePath( const hydroogmap::OgMap &ogMap,
00085                        double                   traversabilityThreshhold,
00086                        const Cell2DVector      &origPath,
00087                        Cell2DVector            &optimisedPath );
00088 
00090     float straightLineCost( const Cell2D   &fromCell,
00091                             const Cell2D   &toCell,
00092                             const FloatMap &costMap );
00093     
00094 }
00095 
00096 }
00097 
00098 #endif
00099
util.h
