voxel_grid_model.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 #include <base_local_planner/voxel_grid_model.h>
00038 
00039 using namespace std;
00040 using namespace costmap_2d;
00041 
00042 namespace base_local_planner {
00043   VoxelGridModel::VoxelGridModel(double size_x, double size_y, double size_z, double xy_resolution, double z_resolution,
00044           double origin_x, double origin_y, double origin_z, double max_z, double obstacle_range) :
00045     obstacle_grid_(size_x, size_y, size_z), xy_resolution_(xy_resolution), z_resolution_(z_resolution), 
00046     origin_x_(origin_x), origin_y_(origin_y), origin_z_(origin_z),
00047     max_z_(max_z), sq_obstacle_range_(obstacle_range * obstacle_range) {}
00048 
00049   double VoxelGridModel::footprintCost(const geometry_msgs::Point& position, const vector<geometry_msgs::Point>& footprint, 
00050       double inscribed_radius, double circumscribed_radius){
00051     if(footprint.size() < 3)
00052       return -1.0;
00053 
00054     //now we really have to lay down the footprint in the costmap grid
00055     unsigned int x0, x1, y0, y1;
00056     double line_cost = 0.0;
00057 
00058     //we need to rasterize each line in the footprint
00059     for(unsigned int i = 0; i < footprint.size() - 1; ++i){
00060       //get the cell coord of the first point
00061       if(!worldToMap2D(footprint[i].x, footprint[i].y, x0, y0))
00062         return -1.0;
00063 
00064       //get the cell coord of the second point
00065       if(!worldToMap2D(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
00066         return -1.0;
00067 
00068       line_cost = lineCost(x0, x1, y0, y1);
00069 
00070       //if there is an obstacle that hits the line... we know that we can return false right away 
00071       if(line_cost < 0)
00072         return -1.0;
00073     }
00074 
00075     //we also need to connect the first point in the footprint to the last point
00076     //get the cell coord of the last point
00077     if(!worldToMap2D(footprint.back().x, footprint.back().y, x0, y0))
00078       return -1.0;
00079 
00080     //get the cell coord of the first point
00081     if(!worldToMap2D(footprint.front().x, footprint.front().y, x1, y1))
00082       return -1.0;
00083 
00084     line_cost = lineCost(x0, x1, y0, y1);
00085 
00086     if(line_cost < 0)
00087       return -1.0;
00088 
00089     //if all line costs are legal... then we can return that the footprint is legal
00090     return 0.0;
00091   }
00092 
00093   //calculate the cost of a ray-traced line
00094   double VoxelGridModel::lineCost(int x0, int x1, 
00095       int y0, int y1){
00096     //Bresenham Ray-Tracing
00097     int deltax = abs(x1 - x0);        // The difference between the x's
00098     int deltay = abs(y1 - y0);        // The difference between the y's
00099     int x = x0;                       // Start x off at the first pixel
00100     int y = y0;                       // Start y off at the first pixel
00101 
00102     int xinc1, xinc2, yinc1, yinc2;
00103     int den, num, numadd, numpixels;
00104 
00105     double line_cost = 0.0;
00106     double point_cost = -1.0;
00107 
00108     if (x1 >= x0)                 // The x-values are increasing
00109     {
00110       xinc1 = 1;
00111       xinc2 = 1;
00112     }
00113     else                          // The x-values are decreasing
00114     {
00115       xinc1 = -1;
00116       xinc2 = -1;
00117     }
00118 
00119     if (y1 >= y0)                 // The y-values are increasing
00120     {
00121       yinc1 = 1;
00122       yinc2 = 1;
00123     }
00124     else                          // The y-values are decreasing
00125     {
00126       yinc1 = -1;
00127       yinc2 = -1;
00128     }
00129 
00130     if (deltax >= deltay)         // There is at least one x-value for every y-value
00131     {
00132       xinc1 = 0;                  // Don't change the x when numerator >= denominator
00133       yinc2 = 0;                  // Don't change the y for every iteration
00134       den = deltax;
00135       num = deltax / 2;
00136       numadd = deltay;
00137       numpixels = deltax;         // There are more x-values than y-values
00138     }
00139     else                          // There is at least one y-value for every x-value
00140     {
00141       xinc2 = 0;                  // Don't change the x for every iteration
00142       yinc1 = 0;                  // Don't change the y when numerator >= denominator
00143       den = deltay;
00144       num = deltay / 2;
00145       numadd = deltax;
00146       numpixels = deltay;         // There are more y-values than x-values
00147     }
00148 
00149     for (int curpixel = 0; curpixel <= numpixels; curpixel++)
00150     {
00151       point_cost = pointCost(x, y); //Score the current point
00152 
00153       if(point_cost < 0)
00154         return -1;
00155 
00156       if(line_cost < point_cost)
00157         line_cost = point_cost;
00158 
00159       num += numadd;              // Increase the numerator by the top of the fraction
00160       if (num >= den)             // Check if numerator >= denominator
00161       {
00162         num -= den;               // Calculate the new numerator value
00163         x += xinc1;               // Change the x as appropriate
00164         y += yinc1;               // Change the y as appropriate
00165       }
00166       x += xinc2;                 // Change the x as appropriate
00167       y += yinc2;                 // Change the y as appropriate
00168     }
00169 
00170     return line_cost;
00171   }
00172 
00173   double VoxelGridModel::pointCost(int x, int y){
00174     //if the cell is in an obstacle the path is invalid
00175     if(obstacle_grid_.getVoxelColumn(x, y)){
00176       return -1;
00177     }
00178 
00179     return 1;
00180   }
00181 
00182   void VoxelGridModel::updateWorld(const vector<geometry_msgs::Point>& footprint, 
00183       const vector<Observation>& observations, const vector<PlanarLaserScan>& laser_scans){
00184 
00185     //remove points in the laser scan boundry
00186     for(unsigned int i = 0; i < laser_scans.size(); ++i)
00187       removePointsInScanBoundry(laser_scans[i], 10.0);
00188 
00189     //iterate through all observations and update the grid
00190     for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
00191       const Observation& obs = *it;
00192       const pcl::PointCloud<pcl::PointXYZ>& cloud = obs.cloud_;
00193       for(unsigned int i = 0; i < cloud.points.size(); ++i){
00194         //filter out points that are too high
00195         if(cloud.points[i].z > max_z_)
00196           continue;
00197 
00198         //compute the squared distance from the hitpoint to the pointcloud's origin
00199         double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
00200           + (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y) 
00201           + (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
00202 
00203         if(sq_dist >= sq_obstacle_range_)
00204           continue;
00205 
00206         //insert the point
00207         insert(cloud.points[i]);
00208       }
00209     }
00210 
00211     //remove the points that are in the footprint of the robot
00212     //removePointsInPolygon(footprint);
00213   }
00214 
00215   void VoxelGridModel::removePointsInScanBoundry(const PlanarLaserScan& laser_scan, double raytrace_range){
00216     if(laser_scan.cloud.points.size() == 0)
00217       return;
00218 
00219     unsigned int sensor_x, sensor_y, sensor_z;
00220     double ox = laser_scan.origin.x;
00221     double oy = laser_scan.origin.y;
00222     double oz = laser_scan.origin.z;
00223     
00224     if(!worldToMap3D(ox, oy, oz, sensor_x, sensor_y, sensor_z))
00225       return;
00226 
00227     for(unsigned int i = 0; i < laser_scan.cloud.points.size(); ++i){
00228       double wpx = laser_scan.cloud.points[i].x;
00229       double wpy = laser_scan.cloud.points[i].y;
00230       double wpz = laser_scan.cloud.points[i].z;
00231 
00232       double distance = dist(ox, oy, oz, wpx, wpy, wpz);
00233       double scaling_fact = raytrace_range / distance;
00234       scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
00235       wpx = scaling_fact * (wpx - ox) + ox;
00236       wpy = scaling_fact * (wpy - oy) + oy;
00237       wpz = scaling_fact * (wpz - oz) + oz;
00238 
00239       //we can only raytrace to a maximum z height
00240       if(wpz >= max_z_){
00241         //we know we want the vector's z value to be max_z
00242         double a = wpx - ox;
00243         double b = wpy - oy;
00244         double c = wpz - oz;
00245         double t = (max_z_ - .01 - oz) / c;
00246         wpx = ox + a * t;
00247         wpy = oy + b * t;
00248         wpz = oz + c * t;
00249       }
00250       //and we can only raytrace down to the floor
00251       else if(wpz < 0.0){
00252         //we know we want the vector's z value to be 0.0
00253         double a = wpx - ox;
00254         double b = wpy - oy;
00255         double c = wpz - oz;
00256         double t = (0.0 - oz) / c;
00257         wpx = ox + a * t;
00258         wpy = oy + b * t;
00259         wpz = oz + c * t;
00260       }
00261 
00262       unsigned int point_x, point_y, point_z;
00263       if(worldToMap3D(wpx, wpy, wpz, point_x, point_y, point_z)){
00264         obstacle_grid_.clearVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
00265       }
00266     }
00267   }
00268 
00269   void VoxelGridModel::getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud){
00270     for(unsigned int i = 0; i < obstacle_grid_.sizeX(); ++i){
00271       for(unsigned int j = 0; j < obstacle_grid_.sizeY(); ++j){
00272         for(unsigned int k = 0; k < obstacle_grid_.sizeZ(); ++k){
00273           if(obstacle_grid_.getVoxel(i, j, k)){
00274             double wx, wy, wz;
00275             mapToWorld3D(i, j, k, wx, wy, wz);
00276             pcl::PointXYZ pt;
00277             pt.x = wx;
00278             pt.y = wy;
00279             pt.z = wz;
00280             cloud.points.push_back(pt);
00281           }
00282         }
00283       }
00284     }
00285   }
00286 
00287 };


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko
autogenerated on Mon Oct 6 2014 02:45:34