00001
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
00027
00028
00029
00030
00031
00032
00033
00034
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
00132 void addMapLane(double w1lat, double w1long, double w2lat, double w2long,
00133 double w3lat, double w3long, double laneWidth);
00134
00135
00136 void addMapLane(std::vector<double> ranges);
00137
00138
00139
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
00179
00180 void padObstacles();
00181
00182 void clearBottom();
00183 void clearTop();
00184 void clearRight();
00185 void clearLeft();
00186
00187
00188
00189
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
00194
00195
00196
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);
00201 std::pair<int,int>* drawPointW(int x, int y);
00202
00203
00204
00205 std::pair<int,int>* cellOccupiedDebug(int x, int y);
00206 std::pair<int,int>* cellLightenDebug(int x, int y);
00207 };
00208
00209 #endif