orca-robotics INTRODUCTION Overview Download and Install Documentation REPOSITORY Interfaces Drivers Libraries Utilities Software Map DEVELOPER Dashboard PEOPLE Contributors Users Project Download Mailing lists

sparseskel.h /* * Orca-Robotics Project: Components for robotics * http://orca-robotics.sf.net/ * Copyright (c) 2004-2008 Alex Brooks, Alexei Makarenko, Tobias Kaupp * * This copy of Orca is licensed to you under the terms described in * the LICENSE file included in this distribution. * */ #ifndef HYDROPATHPLAN_SPARSE_SKEL_H #define HYDROPATHPLAN_SPARSE_SKEL_H #include <hydropathplan/util.h> #include <hydroogmap/hydroogmap.h> namespace hydropathplan { namespace sparseskel { class SparseSkelArc; class ContiguousSparseSkel; class SparseSkelNode; class SparseSkel { public: SparseSkel( const hydroogmap::OgMap &ogMap, double traversabilityThreshhold, const Cell2DVector &skel, const FloatMap &costMap, bool addExtraNodes, double extraNodeResolution ); ~SparseSkel(); // access const std::vector<ContiguousSparseSkel*> &contiguousSkels() const { return contiguousSkels_; } const hydroogmap::OgMap &ogMap() const { return ogMap_; } const hydroogmap::OgMap &extraNodesOgMap() const { return extraNodesOgMap_; } int numNodes() const; double traversabilityThreshhold() const { return traversabilityThreshhold_; } private: // // 'wps' is the set of waypoints: end-points or junctions of the skel. // void build( Cell2DList &skel, Cell2DList &wps, const FloatMap &costMap, bool addExtraNodes, double extraNodeResolution ); // If the map has disjoint sections, we may require more than one // separate ContiguousSparseSkel. std::vector<ContiguousSparseSkel*> contiguousSkels_; // Use this OG map generally. It should have been pre-grown const hydroogmap::OgMap &ogMap_; // Use this OG map for adding extra nodes: only add if LOS is very clear. hydroogmap::OgMap extraNodesOgMap_; const double traversabilityThreshhold_; }; // // If the map has disjoint sections, we may require more than one // separate ContiguousSparseSkel. // // Not intended to be instantiated by anything other than SparseSkel. // class ContiguousSparseSkel { public: // // This constructor removes the stuff it uses from the parameters wps and skel. // ContiguousSparseSkel( SparseSkel &parent, Cell2DList &wps, Cell2DList &skel, const FloatMap &costMap, bool addExtraNodes, double extraNodeResolution ); ~ContiguousSparseSkel(); // access const std::vector<SparseSkelNode*> &nodes() const { return nodes_; } const hydroogmap::OgMap &ogMap() const { return parent_.ogMap(); } double traversabilityThreshhold() const { return parent_.traversabilityThreshhold(); } bool isSane() const; private: // This function grows outwards along the skel from 'fromNode', // finding and adding all directly-connected neighbours. It will // modify its arguments, adding to 'ungrownCells', // and removing from and 'skel' void growNode( SparseSkelNode *fromNode, Cell2DList &ungrownCells, Cell2DList &wps, Cell2DList &skel ); // Search through the skel to find the next node in this // direction. findNeighbourNode deletes all visited // members of skel. Visited members of skel get put in // cellsEnRoute. bool findNeighbourNode( const SparseSkelNode *fromNode, const Cell2D &startCell, Cell2D &neighbourPos, Cell2DVector &cellsEnRoute, Cell2DList &wps, Cell2DList &skel ); SparseSkelNode* findNode( const Cell2D &pos ); void mergeAdjacentNodes(); // prior to this, all costs are initialised to -1 void setArcCosts( const FloatMap &costMap ); // return true if it's ok to merge. bool canMerge( SparseSkelNode *slave, SparseSkelNode *master ); // replace all references to slave with refs to master. void merge( SparseSkelNode *slave, SparseSkelNode *master ); // Add extra nodes to cover the space a bit better // (if this isn't done, the skeleton has trouble accurately estimating the cost // in large open areas where the skeleton just goes through the middle) void addExtraNodes( const hydroogmap::OgMap &ogMap, int cellStep ); void tryAddExtraNode( const Cell2D &cell, double minDistForLinkSq, double maxDistForLinkSq, const hydroogmap::OgMap &ogMap ); // Returns the node that it arced to (possibly after creating it) // Note that cellsEnRoute doesn't include either end-point. SparseSkelNode *createArc( SparseSkelNode *fromNode, Cell2D &neighbourNodePos, Cell2DVector &cellsEnRoute ); std::vector<SparseSkelNode*> nodes_; SparseSkel &parent_; }; // // Not intended to be instantiated by anything other than SparseSkel. // class SparseSkelNode { public: SparseSkelNode( const Cell2D &nodePos ) : pos(nodePos) {} ~SparseSkelNode(); Cell2D pos; std::vector<SparseSkelArc*> arcs; // Temporary used for searching mutable float nodeCost; }; // // Not intended to be instantiated by anything other than SparseSkel. // class SparseSkelArc { public: SparseSkelArc( SparseSkelNode *initToNode, float initCost ) : toNode(initToNode), cost(initCost) {} SparseSkelNode *toNode; float cost; }; inline float distance( const Cell2D &a, const Cell2D &b ) { #ifdef __QNX__ return std::hypot( a.x()-b.x(), a.y()-b.y() ); #else return hypot( a.x()-b.x(), a.y()-b.y() ); #endif } inline float distanceSq( const Cell2D &a, const Cell2D &b ) { return (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()); } // // helper function for debugging. // void printCellList( const Cell2DList &cellList ); } } #endif

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

---