latombeutil.h

/*
 * Orca-Robotics Project: Components for robotics 
 *               http://orca-robotics.sf.net/
 * Copyright (c) 2004-2009 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 HYDRO_PATHPLAN_LATOMBE_UTIL_H
#define HYDRO_PATHPLAN_LATOMBE_UTIL_H

#include <vector>
#include <hydroogmap/hydroogmap.h>
#include <hydropathplan/util.h>

namespace hydropathplan
{

namespace latombeutil {

    // Distance to a 4-adjacent cell
    const float AC = 1.0;
    // Distance to a diagonal 8-adjacent cell
    const float DC = 1.4142;

// ================= CHECKING FUNCTIONS ================================
    bool areAllNans( const FloatMap &floatMap );
    
    inline bool containsNan( const FloatMap & navMap, const int x, const int y) 
#ifdef __QNX__
    { return ( isnan(element(navMap, x, y )) ); };
#else
    { return ( std::isnan(element(navMap, x, y )) ); };
#endif
    inline bool containsNan( const FloatMap & navMap, const Cell2D & c ) 
    { return containsNan(navMap,c.x(),c.y()); };

    // Checks whether cell is in a free space region.
    // If 'checkForNan' is set, 'nan' means non-free.  Else '0' means non-free.
    // If not, attempts to move it into one of the numCells surrounding cells. 
    // If successful return TRUE, otherwise FALSE.
    bool validateCell( const FloatMap & theMap, Cell2D & c, int numCells, bool checkForNan );

// ======================================================================
    
// ================= CONVERSION FUNCTIONS ================================ 
    int sub2ind( const int          &indX,
                 const int          &indY,
                 const unsigned int &sizeX,
                 const unsigned int &sizeY );
            
    int sub2ind( const Cell2D       &cell,
                 const unsigned int &sizeX,
                 const unsigned int &sizeY );
    
    Cell2D ind2sub( const int          &index,
                    const unsigned int &sizeX,
                    const unsigned int &sizeY );

// =========================================================================

// ============== SKELETON NAVIGATION FUNCTIONS =============================

    //
    // Notes on parameters:
    //
    // - distGrid:
    //   - for every OG cell, stores the 4-distance to the nearest obstacle cell
    //   - zero means 'obstacle in this cell'.
    //     - used in generating the skeleton
    //     - useful for connecting free space cells to the skeleton (just follow the gradient of distGrid until you hit the skeleton)
    //
    // - costMap: 
    //   - stores the cost of moving through each cell
    //   - 'NAN' represents infinite cost.
    //
    // - navMap: 
    //   - contains the potential at each OG cell
    //     - potential = cost_to_goal, NAN means obstacle or close to obstacle
    //     - following the gradient leads to the goal
    //
    // - skel:
    //   - A vector of cells which lie on the skeleton
    //

    bool computeSkeleton( const hydroogmap::OgMap &ogMap,
                          Cell2DVector           &skel,
                          FloatMap               &distGrid,
                          double                  traversabilityThreshhold,
                          double                  robotDiameterMetres );

    bool findClosestPointOnSkeleton( const Cell2DVector   &skel,
                                     const FloatMap       &distGrid,
                                     const Cell2D         &fromCell,
                                     Cell2D               &closestCell );

    bool connectCell2Skeleton( Cell2DVector   &skel,
                               Cell2D         &cell,
                               const FloatMap &distGrid,
                               int             robotRadiusCells );

    bool computePotentialSkeleton( const hydroogmap::OgMap &ogMap,
                                   const FloatMap         &costMap,
                                   FloatMap               &navMap,
                                   const Cell2DVector     &skel,
                                   const Cell2D           &startCell );

    void computePotentialFreeSpace( const hydroogmap::OgMap &ogMap,
                                    const FloatMap         &costMap,
                                    FloatMap               &navMap,
                                    const Cell2DVector     &skel,
                                    double                  traversabilityThreshhold );

    bool calcSkeletonNavigation( const hydroogmap::OgMap &ogMap,
                                 FloatMap               &navMap,
                                 Cell2D                 &startCell,
                                 double                  traversabilityThreshhold,
                                 double                  robotDiameterMetres);

    void computeCostsFromDistGrid( const FloatMap      &distGrid,
                                   FloatMap            &costMap,
                                   double               metresPerCell,
                                   const CostEvaluator &costEvaluator=DefaultCostEvaluator() );

    void computeUniformCosts( const hydroogmap::OgMap &ogMap,
                              FloatMap               &costMap,
                              double                  traversabilityThreshhold );

// =========================================================================

// ================= GENERAL PATHPLANNING FUNCTIONS ====================

    enum Result 
    { 
        PathOk, 
        PathStartNotValid, 
        PathDestinationNotValid, 
        PathDestinationUnreachable,
        OtherError
    };

    Result calcPath( const hydroogmap::OgMap &ogMap,
                     const FloatMap         &navMap,
                     Cell2D                 &goalCell,
                     Cell2DVector           &path,
                     double                  robotDiameterMetres );

// =====================================================================

}

}

#endif