voxel_costmap_2d.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 <costmap_2d/voxel_costmap_2d.h>
00038 
00039 #include <sensor_msgs/PointCloud.h>
00040 
00041 #define VOXEL_BITS 16
00042 
00043 using namespace std;
00044 
00045 namespace costmap_2d{
00046   VoxelCostmap2D::VoxelCostmap2D(unsigned int cells_size_x, unsigned int cells_size_y, unsigned int cells_size_z,
00047       double xy_resolution, double z_resolution, double origin_x, double origin_y, double origin_z, double inscribed_radius,
00048       double circumscribed_radius, double inflation_radius, double obstacle_range,
00049       double raytrace_range, double weight,
00050       const std::vector<unsigned char>& static_data, unsigned char lethal_threshold, unsigned int unknown_threshold, unsigned int mark_threshold, unsigned char unknown_cost_value)
00051     : Costmap2D(cells_size_x, cells_size_y, xy_resolution, origin_x, origin_y, inscribed_radius, circumscribed_radius,
00052         inflation_radius, obstacle_range, cells_size_z * z_resolution + origin_z, raytrace_range, weight, static_data, lethal_threshold, unknown_threshold < cells_size_z, unknown_cost_value),
00053     voxel_grid_(cells_size_x, cells_size_y, cells_size_z), xy_resolution_(xy_resolution), z_resolution_(z_resolution),
00054     origin_z_(origin_z), unknown_threshold_(unknown_threshold + (VOXEL_BITS - cells_size_z)), mark_threshold_(mark_threshold), size_z_(cells_size_z)
00055   {
00056   }
00057 
00058   VoxelCostmap2D::VoxelCostmap2D(costmap_2d::Costmap2D& costmap, 
00059       double z_resolution, unsigned int cells_size_z, double origin_z, 
00060       unsigned int mark_threshold, unsigned int unknown_threshold)
00061     : Costmap2D(costmap), voxel_grid_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY(), cells_size_z), 
00062         xy_resolution_(costmap.getResolution()), z_resolution_(z_resolution), origin_z_(origin_z), 
00063         unknown_threshold_(unknown_threshold + (VOXEL_BITS - cells_size_z)), mark_threshold_(mark_threshold), size_z_(cells_size_z)
00064   {
00065   }
00066 
00067   VoxelCostmap2D::~VoxelCostmap2D(){}
00068 
00069   void VoxelCostmap2D::initMaps(unsigned int size_x, unsigned int size_y){
00070     Costmap2D::initMaps(size_x, size_y);
00071     voxel_grid_.resize(size_x, size_y, size_z_);
00072     ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
00073   }
00074 
00075   void VoxelCostmap2D::resetMaps(){
00076     Costmap2D::resetMaps();
00077     voxel_grid_.reset();
00078   }
00079 
00080   void VoxelCostmap2D::resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y){
00081     ROS_ASSERT_MSG(w_size_x >= 0 && w_size_y >= 0, "You cannot specify a negative size window");
00082 
00083     double start_point_x = wx - w_size_x / 2;
00084     double start_point_y = wy - w_size_y / 2;
00085     double end_point_x = start_point_x + w_size_x;
00086     double end_point_y = start_point_y + w_size_y;
00087 
00088     //check start bounds
00089     start_point_x = max(origin_x_, start_point_x);
00090     start_point_y = max(origin_y_, start_point_y);
00091 
00092     //check end bounds
00093     end_point_x = min(origin_x_ + getSizeInMetersX(), end_point_x);
00094     end_point_y = min(origin_y_ + getSizeInMetersY(), end_point_y);
00095 
00096     unsigned int start_x, start_y, end_x, end_y;
00097 
00098     //check for legality just in case
00099     if(!worldToMap(start_point_x, start_point_y, start_x, start_y) || !worldToMap(end_point_x, end_point_y, end_x, end_y))
00100       return;
00101 
00102     ROS_ASSERT(end_x > start_x && end_y > start_y);
00103     unsigned int cell_size_x = end_x - start_x;
00104     unsigned int cell_size_y = end_y - start_y;
00105 
00106     //we need a map to store the obstacles in the window temporarily
00107     unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
00108     unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
00109     unsigned int* voxel_map = voxel_grid_.getData();
00110 
00111     //copy the local window in the costmap to the local map
00112     copyMapRegion(costmap_, start_x, start_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
00113     copyMapRegion(voxel_map, start_x, start_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
00114 
00115     //now we'll reset the costmap to the static map
00116     memcpy(costmap_, static_map_, size_x_ * size_y_ * sizeof(unsigned char));
00117 
00118     //the voxel grid will just go back to being unknown
00119     voxel_grid_.reset();
00120 
00121     //now we want to copy the local map back into the costmap
00122     copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
00123     copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
00124 
00125     //clean up
00126     delete[] local_map;
00127     delete[] local_voxel_map;
00128   }
00129 
00130   void VoxelCostmap2D::updateObstacles(const vector<Observation>& observations, priority_queue<CellData>& inflation_queue){
00131     //place the new obstacles into a priority queue... each with a priority of zero to begin with
00132     for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
00133       const Observation& obs = *it;
00134 
00135       const pcl::PointCloud<pcl::PointXYZ>& cloud =obs.cloud_;
00136 
00137       double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
00138 
00139       for(unsigned int i = 0; i < cloud.points.size(); ++i){
00140         //if the obstacle is too high or too far away from the robot we won't add it
00141         if(cloud.points[i].z > max_obstacle_height_)
00142           continue;
00143 
00144         //compute the squared distance from the hitpoint to the pointcloud's origin
00145         double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
00146           + (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y)
00147           + (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
00148 
00149         //if the point is far enough away... we won't consider it
00150         if(sq_dist >= sq_obstacle_range)
00151           continue;
00152 
00153         //now we need to compute the map coordinates for the observation
00154         unsigned int mx, my, mz;
00155         if(cloud.points[i].z < origin_z_){
00156           if(!worldToMap3D(cloud.points[i].x, cloud.points[i].y, origin_z_, mx, my, mz))
00157             continue;
00158         }
00159         else if(!worldToMap3D(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z, mx, my, mz)){
00160           continue;
00161         }
00162 
00163         //mark the cell in the voxel grid and check if we should also mark it in the costmap
00164         if(voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_)){
00165           unsigned int index = getIndex(mx, my);
00166 
00167           //push the relevant cell index back onto the inflation queue
00168           enqueue(index, mx, my, mx, my, inflation_queue);
00169         }
00170       }
00171     }
00172   }
00173 
00174   void VoxelCostmap2D::raytraceFreespace(const Observation& clearing_observation){
00175     if(clearing_observation.cloud_.points.size() == 0)
00176       return;
00177 
00178 
00179     double sensor_x, sensor_y, sensor_z;
00180     double ox = clearing_observation.origin_.x;
00181     double oy = clearing_observation.origin_.y;
00182     double oz = clearing_observation.origin_.z;
00183 
00184     if(!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)){
00185       ROS_WARN_THROTTLE(1.0, "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", ox, oy, oz);
00186       return;
00187     }
00188 
00189     //we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
00190     double map_end_x = origin_x_ + getSizeInMetersX();
00191     double map_end_y = origin_y_ + getSizeInMetersY();
00192 
00193     for(unsigned int i = 0; i < clearing_observation.cloud_.points.size(); ++i){
00194       double wpx = clearing_observation.cloud_.points[i].x;
00195       double wpy = clearing_observation.cloud_.points[i].y;
00196       double wpz = clearing_observation.cloud_.points[i].z;
00197 
00198       double distance = dist(ox, oy, oz, wpx, wpy, wpz);
00199       double scaling_fact = 1.0;
00200       scaling_fact = std::max(std::min(scaling_fact, (distance -  2 * xy_resolution_) / distance), 0.0);
00201       wpx = scaling_fact * (wpx - ox) + ox;
00202       wpy = scaling_fact * (wpy - oy) + oy;
00203       wpz = scaling_fact * (wpz - oz) + oz;
00204 
00205       double a = wpx - ox;
00206       double b = wpy - oy;
00207       double c = wpz - oz;
00208       double t = 1.0;
00209 
00210       //we can only raytrace to a maximum z height
00211       if(wpz > max_obstacle_height_){
00212         //we know we want the vector's z value to be max_z
00213         t = std::min(t, (max_obstacle_height_ - 0.01 - oz) / c);
00214       }
00215       //and we can only raytrace down to the floor
00216       else if(wpz < origin_z_){
00217         //we know we want the vector's z value to be 0.0
00218         t = std::min(t, (origin_z_ - oz) / c);
00219       }
00220 
00221       //the minimum value to raytrace from is the origin
00222       if(wpx < origin_x_){
00223         t = std::min(t, (origin_x_ - ox) / a);
00224       }
00225       if(wpy < origin_y_){
00226         t = std::min(t, (origin_y_ - oy) / b);
00227       }
00228 
00229       //the maximum value to raytrace to is the end of the map
00230       if(wpx > map_end_x){
00231         t = std::min(t, (map_end_x - ox) / a);
00232       }
00233       if(wpy > map_end_y){
00234         t = std::min(t, (map_end_y - oy) / b);
00235       }
00236 
00237       wpx = ox + a * t;
00238       wpy = oy + b * t;
00239       wpz = oz + c * t;
00240 
00241       double point_x, point_y, point_z;
00242       if(worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z)){
00243         unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
00244         
00245         //voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
00246         voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_, 
00247             unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION, cell_raytrace_range);
00248       }
00249     }
00250   }
00251 
00252 
00253   void VoxelCostmap2D::updateOrigin(double new_origin_x, double new_origin_y){
00254     //project the new origin into the grid
00255     int cell_ox, cell_oy;
00256     cell_ox = int((new_origin_x - origin_x_) / resolution_);
00257     cell_oy = int((new_origin_y - origin_y_) / resolution_);
00258 
00259     //compute the associated world coordinates for the origin cell
00260     //beacuase we want to keep things grid-aligned
00261     double new_grid_ox, new_grid_oy;
00262     new_grid_ox = origin_x_ + cell_ox * resolution_;
00263     new_grid_oy = origin_y_ + cell_oy * resolution_;
00264 
00265     //To save casting from unsigned int to int a bunch of times
00266     int size_x = size_x_;
00267     int size_y = size_y_;
00268 
00269     //we need to compute the overlap of the new and existing windows
00270     int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
00271     lower_left_x = min(max(cell_ox, 0), size_x);
00272     lower_left_y = min(max(cell_oy, 0), size_y);
00273     upper_right_x = min(max(cell_ox + size_x, 0), size_x);
00274     upper_right_y = min(max(cell_oy + size_y, 0), size_y);
00275 
00276     unsigned int cell_size_x = upper_right_x - lower_left_x;
00277     unsigned int cell_size_y = upper_right_y - lower_left_y;
00278 
00279     //we need a map to store the obstacles in the window temporarily
00280     unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
00281     unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
00282     unsigned int* voxel_map = voxel_grid_.getData(); 
00283 
00284     //copy the local window in the costmap to the local map
00285     copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
00286     copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
00287 
00288     //we'll reset our maps to unknown space if appropriate
00289     resetMaps();
00290 
00291     //update the origin with the appropriate world coordinates
00292     origin_x_ = new_grid_ox;
00293     origin_y_ = new_grid_oy;
00294 
00295     //compute the starting cell location for copying data back in
00296     int start_x = lower_left_x - cell_ox;
00297     int start_y = lower_left_y - cell_oy;
00298 
00299     //now we want to copy the overlapping information back into the map, but in its new location
00300     copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
00301     copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
00302 
00303     //make sure to clean up
00304     delete[] local_map;
00305     delete[] local_voxel_map;
00306 
00307   }
00308 
00309   void VoxelCostmap2D::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info){
00310     //get the cell coordinates of the center point of the window
00311     unsigned int mx, my;
00312     if(!worldToMap(wx, wy, mx, my))
00313       return;
00314 
00315     //compute the bounds of the window
00316     double start_x = wx - w_size_x / 2;
00317     double start_y = wy - w_size_y / 2;
00318     double end_x = start_x + w_size_x;
00319     double end_y = start_y + w_size_y;
00320 
00321     //scale the window based on the bounds of the costmap
00322     start_x = max(origin_x_, start_x);
00323     start_y = max(origin_y_, start_y);
00324 
00325     end_x = min(origin_x_ + getSizeInMetersX(), end_x);
00326     end_y = min(origin_y_ + getSizeInMetersY(), end_y);
00327 
00328     //get the map coordinates of the bounds of the window
00329     unsigned int map_sx, map_sy, map_ex, map_ey;
00330 
00331     //check for legality just in case
00332     if(!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
00333       return;
00334 
00335     //we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
00336     unsigned int index = getIndex(map_sx, map_sy);
00337     unsigned char* current = &costmap_[index];
00338     for(unsigned int j = map_sy; j <= map_ey; ++j){
00339       for(unsigned int i = map_sx; i <= map_ex; ++i){
00340         //if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
00341         if(*current != LETHAL_OBSTACLE){
00342           if(clear_no_info || *current != NO_INFORMATION){
00343             *current = FREE_SPACE;
00344             voxel_grid_.clearVoxelColumn(index);
00345           }
00346         }
00347         current++;
00348         index++;
00349       }
00350       current += size_x_ - (map_ex - map_sx) - 1;
00351       index += size_x_ - (map_ex - map_sx) - 1;
00352     }
00353   }
00354 
00355   void VoxelCostmap2D::getVoxelGridMessage(VoxelGrid& grid){
00356     unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
00357     grid.size_x = voxel_grid_.sizeX();
00358     grid.size_y = voxel_grid_.sizeY();
00359     grid.size_z = voxel_grid_.sizeZ();
00360     grid.data.resize(size);
00361     memcpy(&grid.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
00362 
00363     grid.origin.x = origin_x_;
00364     grid.origin.y = origin_y_;
00365     grid.origin.z = origin_z_;
00366 
00367     grid.resolutions.x = xy_resolution_;
00368     grid.resolutions.y = xy_resolution_;
00369     grid.resolutions.z = z_resolution_;
00370   }
00371 
00372   void VoxelCostmap2D::getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud){
00373     for(unsigned int i = 0; i < voxel_grid_.sizeX(); ++i){
00374       for(unsigned int j = 0; j < voxel_grid_.sizeY(); ++j){
00375         for(unsigned int k = 0; k < voxel_grid_.sizeZ(); ++k){
00376           if(voxel_grid_.getVoxel(i, j, k) == voxel_grid::MARKED){
00377             double wx, wy, wz;
00378             mapToWorld3D(i, j, k, wx, wy, wz);
00379             pcl::PointXYZ pt;
00380             pt.x = wx;
00381             pt.y = wy;
00382             pt.z = wz;
00383             cloud.points.push_back(pt);
00384           }
00385         }
00386       }
00387     }
00388   }
00389 
00390   void VoxelCostmap2D::finishConfiguration(costmap_2d::Costmap2DConfig &config) {
00391     unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_);
00392     mark_threshold_ = config.mark_threshold;
00393   }
00394 
00395 };


costmap_2d
Author(s): Eitan Marder-Eppstein
autogenerated on Mon Oct 6 2014 02:44:46