navfn.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

//
// Navigation function computation
// Uses Dijkstra's method
// Modified for Euclidean-distance computation
//

#ifndef _NAVFN_H
#define _NAVFN_H

#include <math.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <vector>

// cost defs
#define COST_UNKNOWN_ROS 255            // 255 is unknown cost
#define COST_OBS 254            // 254 for forbidden regions
#define COST_OBS_ROS 253        // ROS values of 253 are obstacles
#define COST_NEUTRAL 50         // Set this to "open space" value
#define COST_FACTOR 3           // Used for translating costs in NavFn::setCostmap()

// Define the cost type in the case that it is not set. However, this allows
// clients to modify it without changing the file. Arguably, it is better to require it to
// be defined by a user explicitly
#ifndef COSTTYPE
#define COSTTYPE unsigned char  // Whatever is used...
#endif

// potential defs
#define POT_HIGH 1.0e10         // unassigned cell potential

// priority buffers
#define PRIORITYBUFSIZE 10000

//for some datatypes
#include <tf/transform_datatypes.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <Eigen/Core>
#include <iostream>
#include <math.h>

namespace iri_bspline_navfn {
  int create_nav_plan_astar(const COSTTYPE *costmap, int nx, int ny,
      int* goal, int* start,
      float *plan, int nplan);



  class NavFn
  {
    public:
      NavFn(int nx, int ny);    // size of map

      ~NavFn();

      void setNavArr(int nx, int ny); 
      int nx, ny, ns;           
      void setCostmap(const COSTTYPE *cmap, bool isROS=true, bool allow_unknown = true); 
      bool calcNavFnAstar();    
      bool calcNavFnDijkstra(bool atStart = false);     
/*******************************************************************************************************************************************/
      void setNavPoses(const geometry_msgs::PoseStamped& goal_, const geometry_msgs::PoseStamped& start_, const double resolution_, const double costmap_ox_, const double costmap_oy_);
      
      geometry_msgs::PoseStamped w_goal;
      geometry_msgs::PoseStamped w_start;
      double resolution;
      double costmap_ox;
      double costmap_oy;
      float theta1;
      float theta2;
      float theta3;
      float theta4;
      int m_goal[2];
      float p1[2];
      float p2[2];
      float p3[2];
      float p4[2];

/*******************************************************************************************************************************************/

      float *getPathX();                
      float *getPathY();                
      int   getPathLen();               
/************************************************************************************************************************/      
      void spline();
     
/************************************************************************************************************************/
      
      float getLastPathCost();      
      COSTTYPE *obsarr;         
      COSTTYPE *costarr;                
      float   *potarr;          
/***************************************************************************************************************************/
      int world_s;
      int S_map[303][2];
/***************************************************************************************************************************/
      bool    *pending;         
      int nobs;                 
      int *pb1, *pb2, *pb3;             
      int *curP, *nextP, *overP;        
      int curPe, nextPe, overPe; 
      float curT;                       
      float priInc;                     
      void setGoal(int *goal);  

      void setStart(int *start);        

      int goal[2];
      int start[2];
      void initCost(int k, float v); 
      void setObs();

      void updateCell(int n);   
      void updateCellAstar(int n);      
      void setupNavFn(bool keepit = false); 
      bool propNavFnDijkstra(int cycles, bool atStart = false); 
      bool propNavFnAstar(int cycles); 
      float *gradx, *grady;             
      float *pathx, *pathy;             
      int npath;                        
      int npathbuf;                     
      float last_path_cost_; 
      int calcPath(int n, int *st = NULL); 
      float gradCell(int n);    
      float pathStep;           
      void display(void fn(NavFn *nav), int n = 100); 
      int displayInt;           
      void (*displayFn)(NavFn *nav); 
      void savemap(const char *fname); 
  };
};


#endif  // NAVFN