point_grid.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 
00038 #include <base_local_planner/point_grid.h>
00039 #include <ros/console.h>
00040 #include <sys/time.h>
00041 #include <math.h>
00042 #include <cstdio>
00043 
00044 using namespace std;
00045 using namespace costmap_2d;
00046 
00047 void printPoint(pcl::PointXYZ pt){
00048   printf("(%.2f, %.2f, %.2f)", pt.x, pt.y, pt.z);
00049 }
00050 
00051 void printPSHeader(){
00052   printf("%%!PS\n");
00053   printf("%%%%Creator: Eitan Marder-Eppstein (Willow Garage)\n");
00054   printf("%%%%EndComments\n");
00055 }
00056 
00057 void printPSFooter(){
00058   printf("showpage\n%%%%EOF\n");
00059 }
00060 
00061 void printPolygonPS(const vector<geometry_msgs::Point>& poly, double line_width){
00062   if(poly.size() < 2)
00063     return;
00064 
00065   printf("%.2f setlinewidth\n", line_width);
00066   printf("newpath\n");
00067   printf("%.4f\t%.4f\tmoveto\n", poly[0].x * 10, poly[0].y * 10);
00068   for(unsigned int i = 1; i < poly.size(); ++i)
00069     printf("%.4f\t%.4f\tlineto\n", poly[i].x * 10, poly[i].y * 10);
00070   printf("%.4f\t%.4f\tlineto\n", poly[0].x * 10, poly[0].y * 10);
00071   printf("closepath stroke\n");
00072 
00073 }
00074 
00075 namespace base_local_planner {
00076 
00077 PointGrid::PointGrid(double size_x, double size_y, double resolution, geometry_msgs::Point origin, double max_z, double obstacle_range, double min_seperation) :
00078   resolution_(resolution), origin_(origin), max_z_(max_z), sq_obstacle_range_(obstacle_range * obstacle_range), sq_min_separation_(min_seperation * min_seperation)
00079   {
00080     width_ = (int) (size_x / resolution_);
00081     height_ = (int) (size_y / resolution_);
00082     cells_.resize(width_ * height_);
00083   }
00084 
00085   double PointGrid::footprintCost(const geometry_msgs::Point& position, const vector<geometry_msgs::Point>& footprint, 
00086       double inscribed_radius, double circumscribed_radius){
00087     //the half-width of the circumscribed sqaure of the robot is equal to the circumscribed radius
00088     double outer_square_radius = circumscribed_radius;
00089 
00090     //get all the points inside the circumscribed square of the robot footprint
00091     geometry_msgs::Point c_lower_left, c_upper_right;
00092     c_lower_left.x = position.x - outer_square_radius;
00093     c_lower_left.y = position.y - outer_square_radius;
00094 
00095     c_upper_right.x = position.x + outer_square_radius;
00096     c_upper_right.y = position.y + outer_square_radius;
00097 
00098     //This may return points that are still outside of the cirumscribed square because it returns the cells
00099     //contained by the range
00100     getPointsInRange(c_lower_left, c_upper_right, points_);
00101 
00102     //if there are no points in the circumscribed square... we don't have to check against the footprint
00103     if(points_.empty())
00104       return 1.0;
00105 
00106     //compute the half-width of the inner square from the inscribed radius of the robot
00107     double inner_square_radius = sqrt((inscribed_radius * inscribed_radius) / 2.0);
00108 
00109     //we'll also check against the inscribed square
00110     geometry_msgs::Point i_lower_left, i_upper_right;
00111     i_lower_left.x = position.x - inner_square_radius;
00112     i_lower_left.y = position.y - inner_square_radius;
00113 
00114     i_upper_right.x = position.x + inner_square_radius;
00115     i_upper_right.y = position.y + inner_square_radius;
00116 
00117     //if there are points, we have to do a more expensive check
00118     for(unsigned int i = 0; i < points_.size(); ++i){
00119       list<pcl::PointXYZ>* cell_points = points_[i];
00120       if(cell_points != NULL){
00121         for(list<pcl::PointXYZ>::iterator it = cell_points->begin(); it != cell_points->end(); ++it){
00122           const pcl::PointXYZ& pt = *it;
00123           //first, we'll check to make sure we're in the outer square
00124           //printf("(%.2f, %.2f) ... l(%.2f, %.2f) ... u(%.2f, %.2f)\n", pt.x, pt.y, c_lower_left.x, c_lower_left.y, c_upper_right.x, c_upper_right.y);
00125           if(pt.x > c_lower_left.x && pt.x < c_upper_right.x && pt.y > c_lower_left.y && pt.y < c_upper_right.y){
00126             //do a quick check to see if the point lies in the inner square of the robot
00127             if(pt.x > i_lower_left.x && pt.x < i_upper_right.x && pt.y > i_lower_left.y && pt.y < i_upper_right.y)
00128               return -1.0;
00129 
00130             //now we really have to do a full footprint check on the point
00131             if(ptInPolygon(pt, footprint))
00132               return -1.0;
00133           }
00134         }
00135       }
00136     }
00137 
00138     //if we get through all the points and none of them are in the footprint it's legal
00139     return 1.0;
00140   }
00141 
00142   bool PointGrid::ptInPolygon(const pcl::PointXYZ& pt, const vector<geometry_msgs::Point>& poly){
00143     if(poly.size() < 3)
00144       return false;
00145 
00146     //a point is in a polygon iff the orientation of the point
00147     //with respect to sides of the polygon is the same for every
00148     //side of the polygon
00149     bool all_left = false;
00150     bool all_right = false;
00151     for(unsigned int i = 0; i < poly.size() - 1; ++i){
00152       //if pt left of a->b
00153       if(orient(poly[i], poly[i + 1], pt) > 0){
00154         if(all_right)
00155           return false;
00156         all_left = true;
00157       }
00158       //if pt right of a->b
00159       else{
00160         if(all_left)
00161           return false;
00162         all_right = true;
00163       }
00164     }
00165     //also need to check the last point with the first point
00166     if(orient(poly[poly.size() - 1], poly[0], pt) > 0){
00167       if(all_right)
00168         return false;
00169     }
00170     else{
00171       if(all_left)
00172         return false;
00173     }
00174 
00175     return true;
00176   }
00177 
00178   void PointGrid::getPointsInRange(const geometry_msgs::Point& lower_left, const geometry_msgs::Point& upper_right, vector< list<pcl::PointXYZ>* >& points){
00179     points.clear();
00180 
00181     //compute the other corners of the box so we can get cells indicies for them
00182     geometry_msgs::Point upper_left, lower_right;
00183     upper_left.x = lower_left.x;
00184     upper_left.y = upper_right.y;
00185     lower_right.x = upper_right.x;
00186     lower_right.y = lower_left.y;
00187 
00188     //get the grid coordinates of the cells matching the corners of the range
00189     unsigned int gx, gy;
00190 
00191     //if the grid coordinates are outside the bounds of the grid... return
00192     if(!gridCoords(lower_left, gx, gy))
00193       return;
00194     //get the associated index
00195     unsigned int lower_left_index = gridIndex(gx, gy);
00196 
00197     if(!gridCoords(lower_right, gx, gy))
00198       return;
00199     unsigned int lower_right_index = gridIndex(gx, gy);
00200 
00201     if(!gridCoords(upper_left, gx, gy))
00202       return;
00203     unsigned int upper_left_index = gridIndex(gx, gy);
00204 
00205     //compute x_steps and y_steps
00206     unsigned int x_steps = lower_right_index - lower_left_index + 1;
00207     unsigned int y_steps = (upper_left_index - lower_left_index) / width_ + 1;
00208     /*
00209      * (0, 0) ---------------------- (width, 0)
00210      *  |                               |
00211      *  |                               |
00212      *  |                               |
00213      *  |                               |
00214      *  |                               |
00215      * (0, height) ----------------- (width, height)
00216      */
00217     //get an iterator
00218     vector< list<pcl::PointXYZ> >::iterator cell_iterator = cells_.begin() + lower_left_index;
00219     //printf("Index: %d, Width: %d, x_steps: %d, y_steps: %d\n", lower_left_index, width_, x_steps, y_steps);
00220     for(unsigned int i = 0; i < y_steps; ++i){
00221       for(unsigned int j = 0; j < x_steps; ++j){
00222         list<pcl::PointXYZ>& cell = *cell_iterator;
00223         //if the cell contains any points... we need to push them back to our list
00224         if(!cell.empty()){
00225           points.push_back(&cell);
00226         }
00227         if(j < x_steps - 1)
00228           cell_iterator++; //move a cell in the x direction
00229       }
00230       cell_iterator += width_ - (x_steps - 1); //move down a row
00231     }
00232   }
00233 
00234   void PointGrid::insert(pcl::PointXYZ pt){
00235     //get the grid coordinates of the point
00236     unsigned int gx, gy;
00237 
00238     //if the grid coordinates are outside the bounds of the grid... return
00239     if(!gridCoords(pt, gx, gy))
00240       return;
00241 
00242     //if the point is too close to its nearest neighbor... return
00243     if(nearestNeighborDistance(pt) < sq_min_separation_)
00244       return;
00245 
00246     //get the associated index
00247     unsigned int pt_index = gridIndex(gx, gy);
00248 
00249     //insert the point into the grid at the correct location
00250     cells_[pt_index].push_back(pt);
00251     //printf("Index: %d, size: %d\n", pt_index, cells_[pt_index].size());
00252   }
00253 
00254   double PointGrid::getNearestInCell(pcl::PointXYZ& pt, unsigned int gx, unsigned int gy){
00255     unsigned int index = gridIndex(gx, gy);
00256     double min_sq_dist = DBL_MAX;
00257     //loop through the points in the cell and find the minimum distance to the passed point
00258     for(list<pcl::PointXYZ>::iterator it = cells_[index].begin(); it != cells_[index].end(); ++it){
00259       min_sq_dist = min(min_sq_dist, sq_distance(pt, *it));
00260     }
00261     return min_sq_dist;
00262   }
00263 
00264 
00265   double PointGrid::nearestNeighborDistance(pcl::PointXYZ& pt){
00266     //get the grid coordinates of the point
00267     unsigned int gx, gy;
00268 
00269     gridCoords(pt, gx, gy);
00270 
00271     //get the bounds of the grid cell in world coords
00272     geometry_msgs::Point lower_left, upper_right;
00273     getCellBounds(gx, gy, lower_left, upper_right);
00274 
00275     //now we need to check what cells could contain the nearest neighbor
00276     pcl::PointXYZ check_point;
00277     double sq_dist = DBL_MAX;
00278     double neighbor_sq_dist = DBL_MAX;
00279     
00280     //left
00281     if(gx > 0){
00282       check_point.x = lower_left.x;
00283       check_point.y = pt.y;
00284       sq_dist = sq_distance(pt, check_point);
00285       if(sq_dist < sq_min_separation_)
00286         neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx - 1, gy));
00287     }
00288 
00289     //upper left
00290     if(gx > 0 && gy < height_ - 1){
00291       check_point.x = lower_left.x;
00292       check_point.y = upper_right.y;
00293       sq_dist = sq_distance(pt, check_point);
00294       if(sq_dist < sq_min_separation_)
00295         neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx - 1, gy + 1));
00296     }
00297 
00298     //top
00299     if(gy < height_ - 1){
00300       check_point.x = pt.x;
00301       check_point.y = upper_right.y;
00302       sq_dist = sq_distance(pt, check_point);
00303       if(sq_dist < sq_min_separation_)
00304         neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx, gy + 1));
00305     }
00306 
00307     //upper right
00308     if(gx < width_ - 1 && gy < height_ - 1){
00309       check_point.x = upper_right.x;
00310       check_point.y = upper_right.y;
00311       sq_dist = sq_distance(pt, check_point);
00312       if(sq_dist < sq_min_separation_)
00313         neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx + 1, gy + 1));
00314     }
00315 
00316     //right
00317     if(gx < width_ - 1){
00318       check_point.x = upper_right.x;
00319       check_point.y = pt.y;
00320       sq_dist = sq_distance(pt, check_point);
00321       if(sq_dist < sq_min_separation_)
00322         neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx + 1, gy));
00323     }
00324 
00325     //lower right
00326     if(gx < width_ - 1 && gy > 0){
00327       check_point.x = upper_right.x;
00328       check_point.y = lower_left.y;
00329       sq_dist = sq_distance(pt, check_point);
00330       if(sq_dist < sq_min_separation_)
00331         neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx + 1, gy - 1));
00332     }
00333 
00334     //bottom
00335     if(gy > 0){
00336       check_point.x = pt.x;
00337       check_point.y = lower_left.y;
00338       sq_dist = sq_distance(pt, check_point);
00339       if(sq_dist < sq_min_separation_)
00340         neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx, gy - 1));
00341     }
00342 
00343     //lower left
00344     if(gx > 0 && gy > 0){
00345       check_point.x = lower_left.x;
00346       check_point.y = lower_left.y;
00347       sq_dist = sq_distance(pt, check_point);
00348       if(sq_dist < sq_min_separation_)
00349         neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx - 1, gy - 1));
00350     }
00351 
00352     //we must also check within the cell we're in for a nearest neighbor
00353     neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx, gy));
00354 
00355     return neighbor_sq_dist;
00356   }
00357 
00358   void PointGrid::updateWorld(const vector<geometry_msgs::Point>& footprint, 
00359       const vector<Observation>& observations, const vector<PlanarLaserScan>& laser_scans){
00360     //for our 2D point grid we only remove freespace based on the first laser scan
00361     if(laser_scans.empty())
00362       return;
00363 
00364     removePointsInScanBoundry(laser_scans[0]);
00365 
00366     //iterate through all observations and update the grid
00367     for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
00368       const Observation& obs = *it;
00369       const pcl::PointCloud<pcl::PointXYZ>& cloud = (obs.cloud_);
00370       for(unsigned int i = 0; i < cloud.points.size(); ++i){
00371         //filter out points that are too high
00372         if(cloud.points[i].z > max_z_)
00373           continue;
00374 
00375         //compute the squared distance from the hitpoint to the pointcloud's origin
00376         double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
00377           + (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y) 
00378           + (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
00379 
00380         if(sq_dist >= sq_obstacle_range_)
00381           continue;
00382 
00383         //insert the point
00384         insert(cloud.points[i]);
00385       }
00386     }
00387 
00388     //remove the points that are in the footprint of the robot
00389     removePointsInPolygon(footprint);
00390   }
00391 
00392   void PointGrid::removePointsInScanBoundry(const PlanarLaserScan& laser_scan){
00393     if(laser_scan.cloud.points.size() == 0)
00394       return;
00395 
00396     //compute the containing square of the scan
00397     geometry_msgs::Point lower_left, upper_right;
00398     lower_left.x = laser_scan.origin.x;
00399     lower_left.y = laser_scan.origin.y;
00400     upper_right.x = laser_scan.origin.x;
00401     upper_right.y = laser_scan.origin.y;
00402 
00403     for(unsigned int i = 0; i < laser_scan.cloud.points.size(); ++i){
00404       lower_left.x = min(lower_left.x, (double)laser_scan.cloud.points[i].x);
00405       lower_left.y = min(lower_left.y, (double)laser_scan.cloud.points[i].y);
00406       upper_right.x = max(upper_right.x, (double)laser_scan.cloud.points[i].x);
00407       upper_right.y = max(upper_right.y, (double)laser_scan.cloud.points[i].y);
00408     }
00409 
00410     getPointsInRange(lower_left, upper_right, points_);
00411 
00412     //if there are no points in the containing square... we don't have to do anything
00413     if(points_.empty())
00414       return;
00415 
00416     //if there are points, we have to check them against the scan explicitly to remove them
00417     for(unsigned int i = 0; i < points_.size(); ++i){
00418       list<pcl::PointXYZ>* cell_points = points_[i];
00419       if(cell_points != NULL){
00420         list<pcl::PointXYZ>::iterator it = cell_points->begin();
00421         while(it != cell_points->end()){
00422           const pcl::PointXYZ& pt = *it;
00423 
00424           //check if the point is in the polygon and if it is, erase it from the grid
00425           if(ptInScan(pt, laser_scan)){
00426             it = cell_points->erase(it);
00427           }
00428           else
00429             it++;
00430         }
00431       }
00432     }
00433   }
00434 
00435   bool PointGrid::ptInScan(const pcl::PointXYZ& pt, const PlanarLaserScan& laser_scan){
00436     if(!laser_scan.cloud.points.empty()){
00437       //compute the angle of the point relative to that of the scan
00438       double v1_x = laser_scan.cloud.points[0].x - laser_scan.origin.x;
00439       double v1_y = laser_scan.cloud.points[0].y - laser_scan.origin.y;
00440       double v2_x = pt.x - laser_scan.origin.x;
00441       double v2_y = pt.y - laser_scan.origin.y;
00442 
00443       double perp_dot = v1_x * v2_y - v1_y * v2_x;
00444       double dot = v1_x * v2_x + v1_y * v2_y;
00445 
00446       //get the signed angle
00447       double vector_angle = atan2(perp_dot, dot);
00448 
00449       //we want all angles to be between 0 and 2PI
00450       if(vector_angle < 0)
00451         vector_angle = 2 * M_PI + vector_angle;
00452 
00453       double total_rads = laser_scan.angle_max - laser_scan.angle_min; 
00454 
00455       //if this point lies outside of the scan field of view... it is not in the scan
00456       if(vector_angle < 0 || vector_angle >= total_rads)
00457         return false;
00458 
00459       //compute the index of the point in the scan
00460       unsigned int index = (unsigned int) (vector_angle / laser_scan.angle_increment);
00461 
00462       //make sure we have a legal index... we always should at this point, but just in case
00463       if(index >= laser_scan.cloud.points.size() - 1){
00464         return false;
00465       }
00466 
00467       //if the point lies to the left of the line between the two scan points bounding it, it is within the scan
00468       if(orient(laser_scan.cloud.points[index], laser_scan.cloud.points[index + 1], pt) > 0){
00469         return true;
00470       }
00471 
00472       //otherwise it is not
00473       return false;
00474     }
00475     else
00476       return false;
00477   }
00478 
00479   void PointGrid::getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud){
00480     for(unsigned int i = 0; i < cells_.size(); ++i){
00481       for(list<pcl::PointXYZ>::iterator it = cells_[i].begin(); it != cells_[i].end(); ++it){
00482         cloud.points.push_back(*it);
00483       }
00484     }
00485   }
00486 
00487   void PointGrid::removePointsInPolygon(const vector<geometry_msgs::Point> poly){
00488     if(poly.size() == 0)
00489       return;
00490 
00491     geometry_msgs::Point lower_left, upper_right;
00492     lower_left.x = poly[0].x;
00493     lower_left.y = poly[0].y;
00494     upper_right.x = poly[0].x;
00495     upper_right.y = poly[0].y;
00496 
00497     //compute the containing square of the polygon
00498     for(unsigned int i = 1; i < poly.size(); ++i){
00499       lower_left.x = min(lower_left.x, poly[i].x);
00500       lower_left.y = min(lower_left.y, poly[i].y);
00501       upper_right.x = max(upper_right.x, poly[i].x);
00502       upper_right.y = max(upper_right.y, poly[i].y);
00503     }
00504 
00505     ROS_DEBUG("Lower: (%.2f, %.2f), Upper: (%.2f, %.2f)\n", lower_left.x, lower_left.y, upper_right.x, upper_right.y);
00506     getPointsInRange(lower_left, upper_right, points_);
00507 
00508     //if there are no points in the containing square... we don't have to do anything
00509     if(points_.empty())
00510       return;
00511 
00512     //if there are points, we have to check them against the polygon explicitly to remove them
00513     for(unsigned int i = 0; i < points_.size(); ++i){
00514       list<pcl::PointXYZ>* cell_points = points_[i];
00515       if(cell_points != NULL){
00516         list<pcl::PointXYZ>::iterator it = cell_points->begin();
00517         while(it != cell_points->end()){
00518           const pcl::PointXYZ& pt = *it;
00519 
00520           //check if the point is in the polygon and if it is, erase it from the grid
00521           if(ptInPolygon(pt, poly)){
00522             it = cell_points->erase(it);
00523           }
00524           else
00525             it++;
00526         }
00527       }
00528     }
00529   }
00530 
00531   void PointGrid::intersectionPoint(const geometry_msgs::Point& v1, const geometry_msgs::Point& v2, 
00532       const geometry_msgs::Point& u1, const geometry_msgs::Point& u2, geometry_msgs::Point& result){
00533     //generate the equation for line 1
00534     double a1 = v2.y - v1.y;
00535     double b1 = v1.x - v2.x;
00536     double c1 = a1 * v1.x + b1 * v1.y;
00537 
00538     //generate the equation for line 2
00539     double a2 = u2.y - u1.y;
00540     double b2 = u1.x - u2.x;
00541     double c2 = a2 * u1.x + b2 * u1.y;
00542 
00543     double det = a1 * b2 - a2 * b1;
00544 
00545     //the lines are parallel
00546     if(det == 0)
00547       return;
00548 
00549     result.x = (b2 * c1 - b1 * c2) / det;
00550     result.y = (a1 * c2 - a2 * c1) / det;
00551   }
00552 
00553 };
00554 
00555 
00556 using namespace base_local_planner;
00557 
00558 int main(int argc, char** argv){
00559   geometry_msgs::Point origin;
00560   origin.x = 0.0;
00561   origin.y = 0.0;
00562   PointGrid pg(50.0, 50.0, 0.2, origin, 2.0, 3.0, 0.0);
00563   /*
00564      double x = 10.0;
00565      double y = 10.0;
00566      for(int i = 0; i < 100; ++i){
00567      for(int j = 0; j < 100; ++j){
00568      pcl::PointXYZ pt;
00569      pt.x = x;
00570      pt.y = y;
00571      pt.z = 1.0;
00572      pg.insert(pt);
00573      x += .03;
00574      }
00575      y += .03;
00576      x = 10.0;
00577      }
00578      */
00579   vector<geometry_msgs::Point> footprint;
00580   geometry_msgs::Point pt;
00581 
00582   pt.x = 1.0;
00583   pt.y = 1.0;
00584   footprint.push_back(pt);
00585 
00586   pt.x = 1.0;
00587   pt.y = 1.65;
00588   footprint.push_back(pt);
00589 
00590   pt.x = 1.325;
00591   pt.y = 1.75;
00592   footprint.push_back(pt);
00593 
00594   pt.x = 1.65;
00595   pt.y = 1.65;
00596   footprint.push_back(pt);
00597 
00598   pt.x = 1.65;
00599   pt.y = 1.0;
00600   footprint.push_back(pt);
00601 
00602   vector<geometry_msgs::Point> footprint2;
00603 
00604   pt.x = 1.325;
00605   pt.y = 1.00;
00606   footprint2.push_back(pt);
00607 
00608   pt.x = 1.325;
00609   pt.y = 1.75;
00610   footprint2.push_back(pt);
00611 
00612   pt.x = 1.65;
00613   pt.y = 1.75;
00614   footprint2.push_back(pt);
00615 
00616   pt.x = 1.65;
00617   pt.y = 1.00;
00618   footprint2.push_back(pt);
00619 
00620   vector<geometry_msgs::Point> footprint3;
00621 
00622   pt.x = 0.99;
00623   pt.y = 0.99;
00624   footprint3.push_back(pt);
00625 
00626   pt.x = 0.99;
00627   pt.y = 1.66;
00628   footprint3.push_back(pt);
00629 
00630   pt.x = 1.3255;
00631   pt.y = 1.85;
00632   footprint3.push_back(pt);
00633 
00634   pt.x = 1.66;
00635   pt.y = 1.66;
00636   footprint3.push_back(pt);
00637 
00638   pt.x = 1.66;
00639   pt.y = 0.99;
00640   footprint3.push_back(pt);
00641 
00642   pt.x = 1.325;
00643   pt.y = 1.325;
00644 
00645   pcl::PointXYZ point;
00646   point.x = 1.2;
00647   point.y = 1.2;
00648   point.z = 1.0;
00649 
00650   struct timeval start, end;
00651   double start_t, end_t, t_diff;
00652 
00653   printPSHeader();
00654 
00655   gettimeofday(&start, NULL);
00656   for(unsigned int i = 0; i < 2000; ++i){
00657     pg.insert(point);
00658   }
00659   gettimeofday(&end, NULL);
00660   start_t = start.tv_sec + double(start.tv_usec) / 1e6;
00661   end_t = end.tv_sec + double(end.tv_usec) / 1e6;
00662   t_diff = end_t - start_t;
00663   printf("%%Insertion Time: %.9f \n", t_diff);
00664 
00665   vector<Observation> obs;
00666   vector<PlanarLaserScan> scan;
00667 
00668   gettimeofday(&start, NULL);
00669   pg.updateWorld(footprint, obs, scan);
00670   double legal = pg.footprintCost(pt, footprint, 0.0, .95);
00671   pg.updateWorld(footprint, obs, scan);
00672   double legal2 = pg.footprintCost(pt, footprint, 0.0, .95);
00673   gettimeofday(&end, NULL);
00674   start_t = start.tv_sec + double(start.tv_usec) / 1e6;
00675   end_t = end.tv_sec + double(end.tv_usec) / 1e6;
00676   t_diff = end_t - start_t;
00677 
00678   printf("%%Footprint calc: %.9f \n", t_diff);
00679 
00680   if(legal >= 0.0)
00681     printf("%%Legal footprint %.4f, %.4f\n", legal, legal2);
00682   else
00683     printf("%%Illegal footprint\n");
00684 
00685   printPSFooter();
00686 
00687   return 0;
00688 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Defines


bipedRobinBase_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko
autogenerated on Wed Oct 9 2013 10:07:14