voxel_layer.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, 2013, 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 Willow Garage, Inc. 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  *         David V. Lu!!
00037  *********************************************************************/
00038 #include <costmap_2d/voxel_layer.h>
00039 #include <pluginlib/class_list_macros.h>
00040 #include <pcl_conversions/pcl_conversions.h>
00041 
00042 #define VOXEL_BITS 16
00043 PLUGINLIB_EXPORT_CLASS(costmap_2d::VoxelLayer, costmap_2d::Layer)
00044 
00045 using costmap_2d::NO_INFORMATION;
00046 using costmap_2d::LETHAL_OBSTACLE;
00047 using costmap_2d::FREE_SPACE;
00048 
00049 using costmap_2d::ObservationBuffer;
00050 using costmap_2d::Observation;
00051 
00052 namespace costmap_2d
00053 {
00054 
00055 void VoxelLayer::onInitialize()
00056 {
00057   ObstacleLayer::onInitialize();
00058   ros::NodeHandle private_nh("~/" + name_);
00059 
00060   private_nh.param("publish_voxel_map", publish_voxel_, false);
00061   if (publish_voxel_)
00062     voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1);
00063 
00064   clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);
00065 }
00066 
00067 void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
00068 {
00069   voxel_dsrv_ = new dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>(nh);
00070   dynamic_reconfigure::Server<costmap_2d::VoxelPluginConfig>::CallbackType cb = boost::bind(
00071       &VoxelLayer::reconfigureCB, this, _1, _2);
00072   voxel_dsrv_->setCallback(cb);
00073 }
00074 
00075 VoxelLayer::~VoxelLayer()
00076 {
00077   if (voxel_dsrv_)
00078     delete voxel_dsrv_;
00079 }
00080 
00081 void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
00082 {
00083   enabled_ = config.enabled;
00084   footprint_clearing_enabled_ = config.footprint_clearing_enabled;
00085   max_obstacle_height_ = config.max_obstacle_height;
00086   size_z_ = config.z_voxels;
00087   origin_z_ = config.origin_z;
00088   z_resolution_ = config.z_resolution;
00089   unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_);
00090   mark_threshold_ = config.mark_threshold;
00091   combination_method_ = config.combination_method;
00092   matchSize();
00093 }
00094 
00095 void VoxelLayer::matchSize()
00096 {
00097   ObstacleLayer::matchSize();
00098   voxel_grid_.resize(size_x_, size_y_, size_z_);
00099   ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
00100 }
00101 
00102 void VoxelLayer::reset()
00103 {
00104   deactivate();
00105   resetMaps();
00106   voxel_grid_.reset();
00107   activate();
00108 }
00109 
00110 void VoxelLayer::resetMaps()
00111 {
00112   Costmap2D::resetMaps();
00113   voxel_grid_.reset();
00114 }
00115 
00116 void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
00117                                        double* min_y, double* max_x, double* max_y)
00118 {
00119   if (rolling_window_)
00120     updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
00121   if (!enabled_)
00122     return;
00123   useExtraBounds(min_x, min_y, max_x, max_y);
00124 
00125   bool current = true;
00126   std::vector<Observation> observations, clearing_observations;
00127 
00128   // get the marking observations
00129   current = getMarkingObservations(observations) && current;
00130 
00131   // get the clearing observations
00132   current = getClearingObservations(clearing_observations) && current;
00133 
00134   // update the global current status
00135   current_ = current;
00136 
00137   // raytrace freespace
00138   for (unsigned int i = 0; i < clearing_observations.size(); ++i)
00139   {
00140     raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
00141   }
00142 
00143   // place the new obstacles into a priority queue... each with a priority of zero to begin with
00144   for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
00145   {
00146     const Observation& obs = *it;
00147 
00148     const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
00149 
00150     double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
00151 
00152     for (unsigned int i = 0; i < cloud.points.size(); ++i)
00153     {
00154       // if the obstacle is too high or too far away from the robot we won't add it
00155       if (cloud.points[i].z > max_obstacle_height_)
00156         continue;
00157 
00158       // compute the squared distance from the hitpoint to the pointcloud's origin
00159       double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
00160           + (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y)
00161           + (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
00162 
00163       // if the point is far enough away... we won't consider it
00164       if (sq_dist >= sq_obstacle_range)
00165         continue;
00166 
00167       // now we need to compute the map coordinates for the observation
00168       unsigned int mx, my, mz;
00169       if (cloud.points[i].z < origin_z_)
00170       {
00171         if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, origin_z_, mx, my, mz))
00172           continue;
00173       }
00174       else if (!worldToMap3D(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z, mx, my, mz))
00175       {
00176         continue;
00177       }
00178 
00179       // mark the cell in the voxel grid and check if we should also mark it in the costmap
00180       if (voxel_grid_.markVoxelInMap(mx, my, mz, mark_threshold_))
00181       {
00182         unsigned int index = getIndex(mx, my);
00183 
00184         costmap_[index] = LETHAL_OBSTACLE;
00185         touch((double)cloud.points[i].x, (double)cloud.points[i].y, min_x, min_y, max_x, max_y);
00186       }
00187     }
00188   }
00189 
00190   if (publish_voxel_)
00191   {
00192     costmap_2d::VoxelGrid grid_msg;
00193     unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
00194     grid_msg.size_x = voxel_grid_.sizeX();
00195     grid_msg.size_y = voxel_grid_.sizeY();
00196     grid_msg.size_z = voxel_grid_.sizeZ();
00197     grid_msg.data.resize(size);
00198     memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int));
00199 
00200     grid_msg.origin.x = origin_x_;
00201     grid_msg.origin.y = origin_y_;
00202     grid_msg.origin.z = origin_z_;
00203 
00204     grid_msg.resolutions.x = resolution_;
00205     grid_msg.resolutions.y = resolution_;
00206     grid_msg.resolutions.z = z_resolution_;
00207     grid_msg.header.frame_id = global_frame_;
00208     grid_msg.header.stamp = ros::Time::now();
00209     voxel_pub_.publish(grid_msg);
00210   }
00211 
00212   updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
00213 }
00214 
00215 void VoxelLayer::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info)
00216 {
00217   // get the cell coordinates of the center point of the window
00218   unsigned int mx, my;
00219   if (!worldToMap(wx, wy, mx, my))
00220     return;
00221 
00222   // compute the bounds of the window
00223   double start_x = wx - w_size_x / 2;
00224   double start_y = wy - w_size_y / 2;
00225   double end_x = start_x + w_size_x;
00226   double end_y = start_y + w_size_y;
00227 
00228   // scale the window based on the bounds of the costmap
00229   start_x = std::max(origin_x_, start_x);
00230   start_y = std::max(origin_y_, start_y);
00231 
00232   end_x = std::min(origin_x_ + getSizeInMetersX(), end_x);
00233   end_y = std::min(origin_y_ + getSizeInMetersY(), end_y);
00234 
00235   // get the map coordinates of the bounds of the window
00236   unsigned int map_sx, map_sy, map_ex, map_ey;
00237 
00238   // check for legality just in case
00239   if (!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey))
00240     return;
00241 
00242   // we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation
00243   unsigned int index = getIndex(map_sx, map_sy);
00244   unsigned char* current = &costmap_[index];
00245   for (unsigned int j = map_sy; j <= map_ey; ++j)
00246   {
00247     for (unsigned int i = map_sx; i <= map_ex; ++i)
00248     {
00249       // if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it
00250       if (*current != LETHAL_OBSTACLE)
00251       {
00252         if (clear_no_info || *current != NO_INFORMATION)
00253         {
00254           *current = FREE_SPACE;
00255           voxel_grid_.clearVoxelColumn(index);
00256         }
00257       }
00258       current++;
00259       index++;
00260     }
00261     current += size_x_ - (map_ex - map_sx) - 1;
00262     index += size_x_ - (map_ex - map_sx) - 1;
00263   }
00264 }
00265 
00266 void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,
00267                                            double* max_x, double* max_y)
00268 {
00269   if (clearing_observation.cloud_->points.size() == 0)
00270     return;
00271 
00272   double sensor_x, sensor_y, sensor_z;
00273   double ox = clearing_observation.origin_.x;
00274   double oy = clearing_observation.origin_.y;
00275   double oz = clearing_observation.origin_.z;
00276 
00277   if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
00278   {
00279     ROS_WARN_THROTTLE(
00280         1.0,
00281         "The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
00282         ox, oy, oz);
00283     return;
00284   }
00285 
00286   bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);
00287   if (publish_clearing_points)
00288   {
00289     clearing_endpoints_.points.clear();
00290     clearing_endpoints_.points.reserve(clearing_observation.cloud_->points.size());
00291   }
00292 
00293   // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
00294   double map_end_x = origin_x_ + getSizeInMetersX();
00295   double map_end_y = origin_y_ + getSizeInMetersY();
00296 
00297   for (unsigned int i = 0; i < clearing_observation.cloud_->points.size(); ++i)
00298   {
00299     double wpx = clearing_observation.cloud_->points[i].x;
00300     double wpy = clearing_observation.cloud_->points[i].y;
00301     double wpz = clearing_observation.cloud_->points[i].z;
00302 
00303     double distance = dist(ox, oy, oz, wpx, wpy, wpz);
00304     double scaling_fact = 1.0;
00305     scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);
00306     wpx = scaling_fact * (wpx - ox) + ox;
00307     wpy = scaling_fact * (wpy - oy) + oy;
00308     wpz = scaling_fact * (wpz - oz) + oz;
00309 
00310     double a = wpx - ox;
00311     double b = wpy - oy;
00312     double c = wpz - oz;
00313     double t = 1.0;
00314 
00315     // we can only raytrace to a maximum z height
00316     if (wpz > max_obstacle_height_)
00317     {
00318       // we know we want the vector's z value to be max_z
00319       t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
00320     }
00321     // and we can only raytrace down to the floor
00322     else if (wpz < origin_z_)
00323     {
00324       // we know we want the vector's z value to be 0.0
00325       t = std::min(t, (origin_z_ - oz) / c);
00326     }
00327 
00328     // the minimum value to raytrace from is the origin
00329     if (wpx < origin_x_)
00330     {
00331       t = std::min(t, (origin_x_ - ox) / a);
00332     }
00333     if (wpy < origin_y_)
00334     {
00335       t = std::min(t, (origin_y_ - oy) / b);
00336     }
00337 
00338     // the maximum value to raytrace to is the end of the map
00339     if (wpx > map_end_x)
00340     {
00341       t = std::min(t, (map_end_x - ox) / a);
00342     }
00343     if (wpy > map_end_y)
00344     {
00345       t = std::min(t, (map_end_y - oy) / b);
00346     }
00347 
00348     wpx = ox + a * t;
00349     wpy = oy + b * t;
00350     wpz = oz + c * t;
00351 
00352     double point_x, point_y, point_z;
00353     if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z))
00354     {
00355       unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
00356 
00357       // voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
00358       voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,
00359                                       unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
00360                                       cell_raytrace_range);
00361 
00362       updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
00363 
00364       if (publish_clearing_points)
00365       {
00366         geometry_msgs::Point32 point;
00367         point.x = wpx;
00368         point.y = wpy;
00369         point.z = wpz;
00370         clearing_endpoints_.points.push_back(point);
00371       }
00372     }
00373   }
00374 
00375   if (publish_clearing_points)
00376   {
00377     clearing_endpoints_.header.frame_id = global_frame_;
00378     clearing_endpoints_.header.stamp = pcl_conversions::fromPCL(clearing_observation.cloud_->header).stamp;
00379     clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;
00380 
00381     clearing_endpoints_pub_.publish(clearing_endpoints_);
00382   }
00383 }
00384 
00385 void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
00386 {
00387   // project the new origin into the grid
00388   int cell_ox, cell_oy;
00389   cell_ox = int((new_origin_x - origin_x_) / resolution_);
00390   cell_oy = int((new_origin_y - origin_y_) / resolution_);
00391 
00392   // compute the associated world coordinates for the origin cell
00393   // beacuase we want to keep things grid-aligned
00394   double new_grid_ox, new_grid_oy;
00395   new_grid_ox = origin_x_ + cell_ox * resolution_;
00396   new_grid_oy = origin_y_ + cell_oy * resolution_;
00397 
00398   // To save casting from unsigned int to int a bunch of times
00399   int size_x = size_x_;
00400   int size_y = size_y_;
00401 
00402   // we need to compute the overlap of the new and existing windows
00403   int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
00404   lower_left_x = std::min(std::max(cell_ox, 0), size_x);
00405   lower_left_y = std::min(std::max(cell_oy, 0), size_y);
00406   upper_right_x = std::min(std::max(cell_ox + size_x, 0), size_x);
00407   upper_right_y = std::min(std::max(cell_oy + size_y, 0), size_y);
00408 
00409   unsigned int cell_size_x = upper_right_x - lower_left_x;
00410   unsigned int cell_size_y = upper_right_y - lower_left_y;
00411 
00412   // we need a map to store the obstacles in the window temporarily
00413   unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
00414   unsigned int* local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
00415   unsigned int* voxel_map = voxel_grid_.getData();
00416 
00417   // copy the local window in the costmap to the local map
00418   copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
00419   copyMapRegion(voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x, cell_size_x,
00420                 cell_size_y);
00421 
00422   // we'll reset our maps to unknown space if appropriate
00423   resetMaps();
00424 
00425   // update the origin with the appropriate world coordinates
00426   origin_x_ = new_grid_ox;
00427   origin_y_ = new_grid_oy;
00428 
00429   // compute the starting cell location for copying data back in
00430   int start_x = lower_left_x - cell_ox;
00431   int start_y = lower_left_y - cell_oy;
00432 
00433   // now we want to copy the overlapping information back into the map, but in its new location
00434   copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
00435   copyMapRegion(local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_, cell_size_x, cell_size_y);
00436 
00437   // make sure to clean up
00438   delete[] local_map;
00439   delete[] local_voxel_map;
00440 }
00441 
00442 }  // namespace costmap_2d


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:22