compilefix
[rock-planning:vfh_star.git] / src / VFH.cpp
1 #include "VFH.h"
2 #include <iomanip>
3
4 using namespace Eigen;
5 namespace vfh_star
6 {
7
8 VFH::VFH()
9 {
10     senseRadius = 0.9;
11     histogramSize = 90;
12     narrowThreshold = 10;
13     lowThreshold = 6;
14     highThreshold = 8;
15 }
16
17
18
19 void addDir(std::vector< std::pair<double, double> > &drivableDirections, double angularResoultion, int narrowThreshold, int start, int end, int histSize)
20 {
21 //     std::cout << "adding area " << start << " end " << end << " is narrow " << (abs(end-start) < narrowThreshold) <<  std::endl;
22     
23     int gapSize = end-start;
24     
25     if(gapSize < 0)
26         gapSize += histSize;
27         
28     if(gapSize < narrowThreshold) {
29         double middle = (start + gapSize / 2.0);
30         if(middle > histSize)
31             middle -= histSize;
32         
33         double dir = middle * angularResoultion;
34         drivableDirections.push_back(std::make_pair(dir, dir));
35     } else {
36         double left = start * angularResoultion;
37         double right = end * angularResoultion;
38         drivableDirections.push_back(std::make_pair(left, right));
39     }    
40 }
41
42 void VFH::setNewTraversabilityGrid(const envire::Grid< Traversability >* trGrid)
43 {
44     traversabillityGrid = trGrid;
45     
46     assert(traversabillityGrid->getScaleX() == traversabillityGrid->getScaleY());
47     
48     //precompute distances
49     lut.recompute(traversabillityGrid->getScaleX(), 5.0);
50     
51 }
52
53 std::vector< std::pair<double, double> > VFH::getNextPossibleDirections(
54         const base::Pose& curPose, double obstacleSafetyDist, double robotWidth,
55         vfh_star::VFHDebugData* dd) const
56 {
57     std::vector< std::pair<double, double> > drivableDirections;
58     std::vector<double> histogram;
59     std::vector<bool> bHistogram;
60
61     //4 degree steps
62     histogram.resize(histogramSize);
63
64     double angularResoultion = 2*M_PI / histogram.size();
65     
66     generateHistogram(histogram, curPose, senseRadius, obstacleSafetyDist, robotWidth / 2.0);
67
68     //we ignore one obstacle
69     getBinaryHistogram(histogram, bHistogram, lowThreshold, highThreshold);
70     
71     int start = -1;
72     int end = -1;
73     int firstEnd = bHistogram.size();
74     for(unsigned int i = 0; i < bHistogram.size(); i++) 
75     {
76         if(bHistogram[i] && start < 0)   
77         {           
78             start = i;
79         }
80         
81         if(!bHistogram[i] && start >= 0)
82         {
83             if(start == 0)
84             {
85                 firstEnd = i-1;
86                 start = -1;
87                 continue;
88             }
89             
90             end = i-1;
91             
92             addDir(drivableDirections, angularResoultion, narrowThreshold, start, end, histogramSize);
93             start = -1;
94         }   
95     }
96     
97     if(start >= 0)
98     {
99         addDir(drivableDirections, angularResoultion, narrowThreshold, start, firstEnd, histogramSize);
100     }
101     else 
102     {
103         if(firstEnd != static_cast<signed int>(bHistogram.size()))
104         {
105             addDir(drivableDirections, angularResoultion, narrowThreshold, 0, firstEnd, histogramSize);
106         }
107     }
108     
109     if(dd) 
110     {
111         dd->histogram.resize(bHistogram.size());
112         copy(bHistogram.begin(), bHistogram.end(), dd->histogram.begin());
113         dd->obstacleSafetyDist = obstacleSafetyDist;
114         dd->pose = curPose;
115         dd->robotWidth = robotWidth;
116         dd->senseRadius = senseRadius;
117     }
118     
119     return drivableDirections;
120 }
121
122 double normalize(double ang)
123 {
124     if(ang < 0)
125         return ang + 2*M_PI;
126     
127     if(ang > 2*M_PI)
128         return ang - 2*M_PI;
129     
130     return ang;
131 }
132
133
134
135 bool VFH::validPosition(const base::Pose& curPose) const
136 {
137     const envire::FrameNode *gridPos = traversabillityGrid->getFrameNode();
138     
139     double distanceToCenter = (gridPos->getTransform().translation() - curPose.position).norm();
140     double gridWidthHalf = traversabillityGrid->getWidth() / 2.0 * traversabillityGrid->getScaleX();
141     double gridHeightHalf = traversabillityGrid->getHeight() / 2.0 * traversabillityGrid->getScaleY();
142     
143     return !(gridHeightHalf  - (distanceToCenter + senseRadius) < 0) && !(gridWidthHalf - (distanceToCenter + senseRadius) < 0);    
144 }
145
146 vfh_star::Traversability VFH::getWorstTerrainInRadius(const base::Pose& curPose, double radius) const
147 {
148     vfh_star::Traversability worstTerrain = TRAVERSABLE;
149     const envire::FrameNode *gridPos = traversabillityGrid->getFrameNode();
150     const Vector3d toCorner(traversabillityGrid->getWidth() / 2.0 * traversabillityGrid->getScaleX(), traversabillityGrid->getHeight() / 2.0 * traversabillityGrid->getScaleY(), 0);   
151     const Vector3d cornerPos = gridPos->getTransform().translation() - toCorner;
152     const Vector3d robotPosInGrid = curPose.position - cornerPos;
153     const envire::Grid<Traversability>::ArrayType &gridData = traversabillityGrid->getGridData();
154     const int robotX = robotPosInGrid.x() / traversabillityGrid->getScaleX();
155     const int robotY = robotPosInGrid.y() / traversabillityGrid->getScaleY();
156
157     int localSenseSize = (radius) / traversabillityGrid->getScaleX();
158
159     for(int y = -localSenseSize; y <= localSenseSize; y++)
160     {
161         for(int x = -localSenseSize; x <= localSenseSize; x++)
162         {
163             double distToRobot = lut.getDistance(x, y);
164             
165             //check if outside circle
166             if(distToRobot > radius)
167                 continue;
168           
169             int rx = robotX + x;
170             int ry = robotY + y;
171
172             if(!traversabillityGrid->inGrid(rx, ry))
173             {
174                 std::cout << "not in Grid exit x:" << rx << " y:" << ry << std::endl;
175                 std::cout << "Grid size x:" << traversabillityGrid->getWidth() << " y:" << traversabillityGrid->getHeight() << std::endl;
176                 std::cout << "Sense size x:" << localSenseSize << " sense radius" << radius << std::endl;
177                 return OBSTACLE;
178 //              throw std::runtime_error("Accessed cell outside of grid");
179             }
180
181             switch(gridData[rx][ry]) {
182                 case OBSTACLE:
183                     worstTerrain = OBSTACLE;
184                     break;
185                 case UNCLASSIFIED:
186                     if(worstTerrain == TRAVERSABLE)
187                         worstTerrain = UNCLASSIFIED;                
188                     break;
189                 case TRAVERSABLE:
190                     //can't get worse ;-)
191                     break;
192                 case UNKNOWN_OBSTACLE:
193                     //worse than traversable and unclassified, but still
194                     //better than an known obstacle
195                     if(worstTerrain != OBSTACLE)
196                         worstTerrain = UNKNOWN_OBSTACLE;
197                     break;
198             }       
199         }
200     }
201     
202     return worstTerrain;
203 }
204
205 std::pair< TerrainStatistic, TerrainStatistic > VFH::getTerrainStatisticsForRadius(const base::Pose& curPose, double innerRadius, double outerRadius) const
206 {
207     TerrainStatistic innerStats;
208     TerrainStatistic outerStats;
209
210     const envire::FrameNode *gridPos = traversabillityGrid->getFrameNode();
211     const Vector3d toCorner(traversabillityGrid->getWidth() / 2.0 * traversabillityGrid->getScaleX(), traversabillityGrid->getHeight() / 2.0 * traversabillityGrid->getScaleY(), 0);   
212     const Vector3d cornerPos = gridPos->getTransform().translation() - toCorner;
213     const Vector3d robotPosInGrid = curPose.position - cornerPos;
214     const envire::Grid<Traversability>::ArrayType &gridData = traversabillityGrid->getGridData();
215     const int robotX = robotPosInGrid.x() / traversabillityGrid->getScaleX();
216     const int robotY = robotPosInGrid.y() / traversabillityGrid->getScaleY();
217
218     int localSenseSize = (innerRadius + outerRadius) / traversabillityGrid->getScaleX();
219
220     for(int y = -localSenseSize; y <= localSenseSize; y++)
221     {
222         for(int x = -localSenseSize; x <= localSenseSize; x++)
223         {
224             double distToRobot = lut.getDistance(x, y);
225             
226             //check if outside circle
227             if(distToRobot > innerRadius + outerRadius)
228                 continue;
229           
230             int rx = robotX + x;
231             int ry = robotY + y;
232             
233             Traversability terrainType =  OBSTACLE;
234             
235             if(traversabillityGrid->inGrid(rx, ry))
236                 terrainType = gridData[rx][ry];
237             
238 /*          if(!traversabillityGrid->inGrid(rx, ry))
239             {
240                 std::cout << "not in Grid exit x:" << rx << " y:" << ry << std::endl;
241                 std::cout << "Grid size x:" << traversabillityGrid->getWidth() << " y:" << traversabillityGrid->getHeight() << std::endl;
242                 std::cout << "Sense size x:" << localSenseSize << " sense radius" << innerRadius + outerRadius << std::endl;
243                 
244                 throw std::runtime_error("Accessed cell outside of grid");
245             }*/
246             
247             TerrainStatistic *curStats;
248             if(distToRobot > innerRadius)
249                 curStats = &outerStats;
250             else
251                 curStats = &innerStats;
252
253             curStats->minDistance[terrainType] = std::min(curStats->minDistance[terrainType], distToRobot);
254
255             curStats->statistic[terrainType]++;
256             curStats->count++;
257         }
258     }
259     return std::make_pair(innerStats, outerStats);
260 }
261
262 void VFH::generateHistogram(std::vector< double >& histogram, const base::Pose& curPose, double senseRadius, double obstacleSafetyDist, double robotRadius) const
263 {
264     const envire::FrameNode *gridPos = traversabillityGrid->getFrameNode();
265     
266     int nrDirs = histogram.size();
267     
268     double dirResolution = 2*M_PI / nrDirs;
269     
270     const double a = 2.0;
271     const double b = 1.0/ (senseRadius * senseRadius);
272
273     
274     std::vector<double> &dirs(histogram);    
275     
276     //calculate robot pos in grid coordinates
277     const Vector3d toCorner(traversabillityGrid->getWidth() / 2.0 * traversabillityGrid->getScaleX(), traversabillityGrid->getHeight() / 2.0 * traversabillityGrid->getScaleY(), 0);   
278     const Vector3d cornerPos = gridPos->getTransform().translation() - toCorner;
279     const Vector3d robotPosInGrid = curPose.position - cornerPos;
280     const envire::Grid<Traversability>::ArrayType &gridData = traversabillityGrid->getGridData();
281     const int robotX = robotPosInGrid.x() / traversabillityGrid->getScaleX();
282     const int robotY = robotPosInGrid.y() / traversabillityGrid->getScaleY();
283     
284 /*    std::cout << "Grid pos " << gridPos->getTransform().translation().transpose() << std::endl;
285     std::cout << "to Corener " << toCorner.transpose() << std::endl;
286     std::cout << "robotPos " << curPose.position.transpose() << std::endl;
287     std::cout << "robotPos in Grid" << robotPosInGrid.transpose() << std::endl;
288     std::cout << "RobotX " << robotX << " Y " << robotY << std::endl;*/
289     
290     //TODO what happens if scale x an scale y differ...
291     int senseSize = senseRadius / traversabillityGrid->getScaleX();
292     
293 //     std::cout << "senseSize " << senseSize << std::endl;
294     
295     /*//Debug code for printing local obstacle map  
296
297     std::cout <<  std::endl;
298
299     for(int y = senseSize; y >= -senseSize; y--)
300     {
301         std::cout << std::setw(4) << y;
302         for(int x = -senseSize; x <= senseSize; x++)
303         {
304             int rx = robotX + x;
305             int ry = robotY + y;
306             
307             if(!traversabillityGrid->inGrid(rx, ry))
308                 continue;
309
310             //go safe, if we do not know anything about something, it is an obstacle
311             if(gridData[rx][ry] == OBSTACLE)
312                 std::cout <<  "O";              
313             else
314                 std::cout <<  "T";
315                 
316         }
317         std::cout <<  std::endl;
318     }*/
319     
320     //walk over area of grid within of circle with radius senseRadius around the robot
321     for(int y = -senseSize; y <= senseSize; y++)
322     {
323         for(int x = -senseSize; x <= senseSize; x++)
324         {
325             double distToRobot = lut.getDistance(x, y);
326             
327             //check if outside circle
328             if(distToRobot > senseRadius)
329                 continue;
330           
331             int rx = robotX + x;
332             int ry = robotY + y;
333             
334 //          std::cout << "rx " << rx << " ry " << ry << std::endl;
335
336             //go safe, if we do not know anything about something, it is an obstacle
337             if(!traversabillityGrid->inGrid(rx, ry) || (gridData[rx][ry] == OBSTACLE))
338             {
339                 if(!traversabillityGrid->inGrid(rx, ry))
340                 {
341                     std::cout << "not in Grid exit x:" << rx << " y:" << ry << std::endl;
342                     std::cout << "Grid size x:" << traversabillityGrid->getWidth() << " y:" << traversabillityGrid->getHeight() << std::endl;
343                     std::cout << "Sense size x:" << senseSize << " sense radius" << senseRadius << std::endl;
344                  
345                     throw std::runtime_error("Accessed cell outside of grid");
346                 }
347                 
348                 double angleToObstace = lut.getAngle(x, y); // atan2(y, x);
349                 
350                 //convert to ENU
351                 angleToObstace -= M_PI / 2.0;
352                 
353                 //move to range 0 to 2*M_PI
354                 angleToObstace = normalize(angleToObstace);
355                 
356                 //calculate magnitude m = c*(a-b*d²)
357                 //ci is 1
358                 double magnitude = a - b * distToRobot*distToRobot;
359
360 //              std::cout << "Magnitude is " << magnitude << std::endl;
361                 
362                 //in case we are allready hit the obstacle, we set distToRobot
363                 //to (robotWidth + obstacleSafetyDist) which results in a 90 degree masking
364                 //of the area where the collided obstable is
365                 if((robotRadius + obstacleSafetyDist) > distToRobot)
366                     distToRobot = (robotRadius + obstacleSafetyDist);
367                 
368                 //boundary of obstacle including robot width and safety distance
369                 double y_t = asin((robotRadius + obstacleSafetyDist) / distToRobot);
370                 
371                 //add to histogramm
372                 int s = (angleToObstace - y_t) / dirResolution;
373                 int e = (angleToObstace + y_t) / dirResolution;
374                 for(int a = s; a <= e; a++) {
375                     int ac = a;
376                     if(ac < 0)
377                         ac += nrDirs;
378                     
379                     if(ac >= nrDirs)
380                         ac -=nrDirs;
381                     
382                     dirs[ac] += magnitude;
383                 }
384             }
385         }
386     }
387 }
388
389
390 void VFH::getBinaryHistogram(const std::vector< double >& histogram, std::vector< bool >& binHistogram, double lowThreshold, double highThreshold) const
391 {
392     binHistogram.clear();
393     binHistogram.reserve(histogram.size());
394     for(std::vector<double>::const_iterator it = histogram.begin(); it != histogram.end(); it++) {
395         bool drivable = false;
396         
397         if(*it <= lowThreshold)
398             drivable = true;
399         
400         //FIXME we are missing out the last binary histogramm
401         
402         binHistogram.push_back(drivable);
403     }
404 }
405 }