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 SPARSESKELETONPATHPLANNER_H
00011 #define SPARSESKELETONPATHPLANNER_H
00012 
00013 #include <hydropathplan/ipathplanner2d.h>
00014 #include <hydropathplan/util.h>
00015 #include <hydroogmap/hydroogmap.h>
00016 
00017 namespace hydropathplan {
00018 
00019 // fwd decl
00020 namespace sparseskel {
00021     class SparseSkel;
00022     class ContiguousSparseSkel;
00023     class SparseSkelNode;
00024 }
00025 
00037 class SparseSkeletonPathPlanner : public IPathPlanner2d
00038 {
00039 
00040 public: 
00041 
00045     SparseSkeletonPathPlanner( const hydroogmap::OgMap &ogMap,
00046                                double robotDiameterMetres,
00047                                double traversabilityThreshhold,
00048                                bool   doPathOptimization,
00049                                bool   addExtraNodes,
00050                                double extraNodeResolution,
00051                                const CostEvaluator &costEvaluator=DefaultCostEvaluator() );
00052 
00053     ~SparseSkeletonPathPlanner();
00054 
00061     void computePath( int           startX,
00062                       int           startY,
00063                       int           endX,
00064                       int           endY,
00065                       Cell2DVector &path ) const;
00066 
00067     // Allow access for debugging/displaying
00068     const Cell2DVector &denseSkel() const  { return skel_; }
00069     const sparseskel::SparseSkel   &sparseSkel() const { return *sparseSkel_; }
00070 
00071 private: 
00072 
00073     void checkInputs( int           startX,
00074                       int           startY,
00075                       int           endX,
00076                       int           endY ) const;
00077 
00078     void findClosestNode( const Cell2D                                    &cell,
00079                           sparseskel::ContiguousSparseSkel               *&cSkel,
00080                           sparseskel::SparseSkelNode                     *&closestNode,
00081                           const std::vector<sparseskel::SparseSkelNode*>  &nodesToExclude ) const;
00082 
00083     void connectCell2SparseSkel( const Cell2D                      &cellOnDenseSkel,
00084                                  sparseskel::ContiguousSparseSkel *&contiguousSparseSkel,
00085                                  sparseskel::SparseSkelNode       *&node ) const;
00086 
00087     void planAlongSparseSkel( const sparseskel::ContiguousSparseSkel         &contiguousSparseSkel,
00088                               const sparseskel::SparseSkelNode               &startNode, 
00089                               const sparseskel::SparseSkelNode               &goalNode,
00090                               std::vector<const sparseskel::SparseSkelNode*> &nodePath ) const;
00091 
00092     //
00093     // Takes: a set of start/goal cells off the sparse skel, and a set of nodes,
00094     //        and makes a single complete path.
00095     //
00096     void convertToCellVector( const std::vector<Cell2D>                      &startCells,
00097                               const std::vector<Cell2D>                      &goalCells,
00098                               std::vector<const sparseskel::SparseSkelNode*> &nodePath,
00099                               Cell2DVector                                   &path ) const;
00100 
00101     void optimisePath( Cell2DVector &path ) const;
00102 
00103     const hydroogmap::OgMap &ogMap_;
00104 
00105     // OgMap with obstacles grown
00106     hydroogmap::OgMap        grownOgMap_;
00107 
00108     // (a pointer:) The map that the sparse planner uses
00109     const hydroogmap::OgMap *planOgMap_;
00110 
00111     Cell2DVector            skel_;
00112     sparseskel::SparseSkel *sparseSkel_;
00113     FloatMap                costMap_;
00114 
00115     const double robotDiameterMetres_;
00116     const double traversabilityThreshhold_;
00117     const bool   doPathOptimization_;
00118 };
00119 
00120 }
00121 
00122 #endif
sparseskeletonpathplanner.h
