observer.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2010 UT-Austin & Austin Robot Technology, Michael Quinlan
00003  *  License: Modified BSD Software License 
00004  */
00005 
00014 #include <art_observers/observer.h>
00015 #include <art_observers/QuadrilateralOps.h>
00016 
00017 namespace observers
00018 {
00019 
00020 Observer::~Observer() {}
00021 
00022 // \brief returns all obstacles located in wanted lane
00023 art_msgs::ArtLanes 
00024   Observer::getObstaclesInLane(art_msgs::ArtLanes obstacles,
00025                                art_msgs::ArtLanes lane_quads) 
00026 {
00027   int counter = 0;
00028   art_msgs::ArtLanes obstaclesInLane;
00029   obstaclesInLane.polygons.resize(obstacles.polygons.size());
00030   for(unsigned i = 0; i < obstacles.polygons.size(); i++) {
00031     // implementation of pointsInLane from lane_observations here on out.
00032     float x = obstacles.polygons[i].midpoint.x;
00033     float y = obstacles.polygons[i].midpoint.y;
00034     
00035     // Want to do the following in pointInLane, but for some reason can't
00036     size_t num_polys = lane_quads.polygons.size();
00037 
00038     bool inside = false;
00039   
00040     for (size_t j=0; j<num_polys; j++) {
00041       art_msgs::ArtQuadrilateral *p= &(lane_quads.polygons[j]);
00042 
00043       float dist= ((p->midpoint.x-x)*(p->midpoint.x-x)
00044                    + (p->midpoint.y-y)*(p->midpoint.y-y));
00045 
00046       if (dist > 16)         // quick check: are we near the polygon?
00047         continue;
00048 
00049       inside = quad_ops::quickPointInPolyRatio(x,y,*p,0.6);
00050 
00051       if(inside)
00052         break;
00053    }
00054 
00055     if(inside) {
00056       obstaclesInLane.polygons[counter] = obstacles.polygons[i];
00057       counter++;
00058     }
00059   }
00060   obstaclesInLane.polygons.resize(counter);
00061   return obstaclesInLane;
00062 }
00063 
00064 #if 0
00065 bool pointInLane(float x, float y, art_msgs::ArtLanes lane) {
00066   
00067   size_t num_polys = lane.polygons.size();
00068   
00069   bool inside = false;
00070   
00071   for (size_t i=0; i<num_polys; i++)
00072     {
00073       art_msgs::ArtQuadrilateral *p= &(lane.polygons[i]);
00074       float dist= ((p->midpoint.x-x)*(p->midpoint.x-x)
00075                    + (p->midpoint.y-y)*(p->midpoint.y-y));
00076 
00077       if (dist > 16)         // quick check: are we near the polygon?
00078         continue;
00079 
00080       inside = quad_ops::quickPointInPolyRatio(x,y,*p,0.6);
00081 
00082       if(inside)
00083         break;
00084     }
00085 
00086   return inside;
00087 }
00088 #endif
00089 
00090 }; // namespace observers


art_observers
Author(s): Michael Quinlan, Jack O'Quin, Corbyn Salisbury
autogenerated on Fri Jan 3 2014 11:09:22