VisualLanes.h
Go to the documentation of this file.
00001 // $Id: VisualLanes.h 78 2010-03-25 02:26:05Z jack.oquin $
00002 
00003 #ifndef __VISUAL_LANES_H__
00004 #define __VISUAL_LANES_H__
00005 
00006 #include <vector>
00007 #include <math.h>
00008 #include <errno.h>
00009 #include <stdint.h>
00010 
00011 #define OCCUPANCY_UNKNOWN 0.0
00012 #define MAX_OCCUPANCY 127
00013 #define MIN_OCCUPANCY -128
00014 #define OCCUPANCY_INCREMENT 10
00015 #define OCCUPANCY_DECREMENT 5
00016 #define LOGODDS_OCCUPANCY_INCREMENT 3.5
00017 #define LOGODDS_OCCUPANCY_DECREMENT 2.0
00018 #define LOGODDS_MAX_OCCUPANCY 20.0
00019 #define LOGODDS_MIN_OCCUPANCY -20.0
00020 #define EPSILON 0.5
00021 #define MAX_RANGE 160.0
00022 
00023 
00024 typedef double cell;
00025 
00026 /*struct poly{
00027                 double x1;
00028                 double x2;
00029                 double x3;
00030                 double x4;
00031                 double y1;
00032                 double y2;
00033                 double y3;
00034                 double y4;
00035         };*/
00036 
00037 class VisualLanes {
00038 public:
00039   VisualLanes(double physical_size, int resolution);
00040   ~VisualLanes();
00041   
00044   void gpsEnabled(bool onOff)
00045   { gpsOnOff = onOff; }
00046   
00053   void initialize(double x, double y, double theta);
00054   
00059   void clear();
00060   
00069   void setPosition(double x, double y, double theta);
00070   
00075   void addSickScan(std::vector<double> ranges);
00076   
00087   cell value(int x, int y);
00088   
00089   void setThreshold(int threshold);
00090   void setCellShift(int shift);
00091   
00096   void savePGM(const char *filename);
00097   
00110   std::pair<double,double>* isPathClear(double x , double y); 
00111   
00120   std::pair<double,double> nearestClearPath(std::pair<double,double> obstacle, 
00121                                             std::pair<double,double> original);
00122   std::pair<double,double>* laserScan(double x, double y);
00123   
00124   std::vector<float>* getPose();
00125   
00126   void setLaserRange(float range){ laser_range = range; };
00127   
00128   double getPhysicalSize(){
00129     return _physical_size;}
00130 
00131   //map lane fuction that takes waypoints
00132   void addMapLane(double w1lat, double w1long, double w2lat, double w2long,
00133       double w3lat, double w3long, double laneWidth);
00134 
00135   //map lane function that takes a sick scan
00136   void addMapLane(std::vector<double> ranges);
00137 
00138   //add polygon
00139   //void addPoly(poly &polygon);
00140   void addPoly(double x1, double x2, double x3, double x4, double y1, double y2,
00141       double y3, double y4, bool is_stop);
00142 
00143   void addTrace(double w1lat, double w1long, double w2lat, double w2long);
00144   
00145 private:
00146   bool valid(int x, int y);
00147   void lighten(int x, int y);
00148   cell* at(int x, int y);
00149   cell *atgoal(int x, int y);
00150   
00151   cell **_m;
00152   double _physical_size;
00153   int _resolution;
00154   double _x;
00155   double _y;
00156   double _theta;
00157   int _x_offset;
00158   int _y_offset;
00159   
00160   int _threshold;
00161   int _shift;
00162   
00163   bool scan_off_right_side;
00164   bool scan_off_left_side;
00165   bool scan_off_bottom_side;
00166   bool scan_off_top_side;
00167   
00168   int count;
00169   
00170   double rX;
00171   double rY;
00172   
00173   float laser_range;
00174   
00175   
00176   bool gpsOnOff;
00177   
00178   // implement later 
00179   // may not need to implement
00180   void padObstacles(); 
00181   
00182   void clearBottom();
00183   void clearTop();
00184   void clearRight();
00185   void clearLeft();
00186   
00187   // General Bresenham Function that traverses from (xo, y1) to (x1,
00188   // y1) and applies 'function' std::pair<int,int>*
00189   // (VisualLanes::*_fp)(int,int)
00190   std::pair<int,int>* line(int x0, int y0, int x1, int y1, 
00191                            std::pair<int,int>* (VisualLanes::*_fp)(int,int));
00192   
00193   // Follows the list of functions that should only be used as
00194   // parameters to line!!  In no way shape or form should there ever
00195   // be a way to pass a public function pointer to line nor an
00196   // external function pointer.
00197   std::pair<int,int>* cellLighten(int x, int y);
00198   std::pair<int,int>* cellOccupied(int x, int y);
00199   std::pair<int,int>* cellOccupiedRelative(int x, int y);
00200   std::pair<int,int>* drawPointB(int x, int y); //for map lanes
00201   std::pair<int,int>* drawPointW(int x, int y); //for map lanes
00202   
00203   // Debug functions, not yet fully implemented; expect them to be
00204   // explained in the next update.
00205   std::pair<int,int>* cellOccupiedDebug(int x, int y);
00206   std::pair<int,int>* cellLightenDebug(int x, int y);
00207 };
00208 
00209 #endif


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:34