VisualLanes.h

Go to the documentation of this file.

// $Id: VisualLanes.h 78 2010-03-25 02:26:05Z jack.oquin $

#ifndef __VISUAL_LANES_H__
#define __VISUAL_LANES_H__

#include <vector>
#include <math.h>
#include <errno.h>
#include <stdint.h>

#define OCCUPANCY_UNKNOWN 0.0
#define MAX_OCCUPANCY 127
#define MIN_OCCUPANCY -128
#define OCCUPANCY_INCREMENT 10
#define OCCUPANCY_DECREMENT 5
#define LOGODDS_OCCUPANCY_INCREMENT 3.5
#define LOGODDS_OCCUPANCY_DECREMENT 2.0
#define LOGODDS_MAX_OCCUPANCY 20.0
#define LOGODDS_MIN_OCCUPANCY -20.0
#define EPSILON 0.5
#define MAX_RANGE 160.0


typedef double cell;

/*struct poly{
                double x1;
                double x2;
                double x3;
                double x4;
                double y1;
                double y2;
                double y3;
                double y4;
        };*/

class VisualLanes {
public:
  VisualLanes(double physical_size, int resolution);
  ~VisualLanes();
  
  void gpsEnabled(bool onOff)
  { gpsOnOff = onOff; }
  
  void initialize(double x, double y, double theta);
  
  void clear();
  
  void setPosition(double x, double y, double theta);
  
  void addSickScan(std::vector<double> ranges);
  
  cell value(int x, int y);
  
  void setThreshold(int threshold);
  void setCellShift(int shift);
  
  void savePGM(const char *filename);
  
  std::pair<double,double>* isPathClear(double x , double y); 
  
  std::pair<double,double> nearestClearPath(std::pair<double,double> obstacle, 
                                            std::pair<double,double> original);
  std::pair<double,double>* laserScan(double x, double y);
  
  std::vector<float>* getPose();
  
  void setLaserRange(float range){ laser_range = range; };
  
  double getPhysicalSize(){
    return _physical_size;}

  //map lane fuction that takes waypoints
  void addMapLane(double w1lat, double w1long, double w2lat, double w2long,
      double w3lat, double w3long, double laneWidth);

  //map lane function that takes a sick scan
  void addMapLane(std::vector<double> ranges);

  //add polygon
  //void addPoly(poly &polygon);
  void addPoly(double x1, double x2, double x3, double x4, double y1, double y2,
      double y3, double y4, bool is_stop);

  void addTrace(double w1lat, double w1long, double w2lat, double w2long);
  
private:
  bool valid(int x, int y);
  void lighten(int x, int y);
  cell* at(int x, int y);
  cell *atgoal(int x, int y);
  
  cell **_m;
  double _physical_size;
  int _resolution;
  double _x;
  double _y;
  double _theta;
  int _x_offset;
  int _y_offset;
  
  int _threshold;
  int _shift;
  
  bool scan_off_right_side;
  bool scan_off_left_side;
  bool scan_off_bottom_side;
  bool scan_off_top_side;
  
  int count;
  
  double rX;
  double rY;
  
  float laser_range;
  
  
  bool gpsOnOff;
  
  // implement later 
  // may not need to implement
  void padObstacles(); 
  
  void clearBottom();
  void clearTop();
  void clearRight();
  void clearLeft();
  
  // General Bresenham Function that traverses from (xo, y1) to (x1,
  // y1) and applies 'function' std::pair<int,int>*
  // (VisualLanes::*_fp)(int,int)
  std::pair<int,int>* line(int x0, int y0, int x1, int y1, 
                           std::pair<int,int>* (VisualLanes::*_fp)(int,int));
  
  // Follows the list of functions that should only be used as
  // parameters to line!!  In no way shape or form should there ever
  // be a way to pass a public function pointer to line nor an
  // external function pointer.
  std::pair<int,int>* cellLighten(int x, int y);
  std::pair<int,int>* cellOccupied(int x, int y);
  std::pair<int,int>* cellOccupiedRelative(int x, int y);
  std::pair<int,int>* drawPointB(int x, int y); //for map lanes
  std::pair<int,int>* drawPointW(int x, int y); //for map lanes
  
  // Debug functions, not yet fully implemented; expect them to be
  // explained in the next update.
  std::pair<int,int>* cellOccupiedDebug(int x, int y);
  std::pair<int,int>* cellLightenDebug(int x, int y);
};

#endif