00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <algorithm>
00039 #include <costmap_2d/inflation_layer.h>
00040 #include <costmap_2d/costmap_math.h>
00041 #include <costmap_2d/footprint.h>
00042 #include <boost/thread.hpp>
00043 #include <pluginlib/class_list_macros.h>
00044
00045 PLUGINLIB_EXPORT_CLASS(costmap_2d::InflationLayer, costmap_2d::Layer)
00046
00047 using costmap_2d::LETHAL_OBSTACLE;
00048 using costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
00049 using costmap_2d::NO_INFORMATION;
00050
00051 namespace costmap_2d
00052 {
00053
00054 InflationLayer::InflationLayer()
00055 : inflation_radius_(0)
00056 , weight_(0)
00057 , cell_inflation_radius_(0)
00058 , cached_cell_inflation_radius_(0)
00059 , dsrv_(NULL)
00060 , seen_(NULL)
00061 , cached_costs_(NULL)
00062 , cached_distances_(NULL)
00063 , last_min_x_(-std::numeric_limits<float>::max())
00064 , last_min_y_(-std::numeric_limits<float>::max())
00065 , last_max_x_(std::numeric_limits<float>::max())
00066 , last_max_y_(std::numeric_limits<float>::max())
00067 {
00068 inflation_access_ = new boost::recursive_mutex();
00069 }
00070
00071 void InflationLayer::onInitialize()
00072 {
00073 {
00074 boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
00075 ros::NodeHandle nh("~/" + name_), g_nh;
00076 current_ = true;
00077 if (seen_)
00078 delete[] seen_;
00079 seen_ = NULL;
00080 seen_size_ = 0;
00081 need_reinflation_ = false;
00082
00083 dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>::CallbackType cb = boost::bind(
00084 &InflationLayer::reconfigureCB, this, _1, _2);
00085
00086 if (dsrv_ != NULL){
00087 dsrv_->clearCallback();
00088 dsrv_->setCallback(cb);
00089 }
00090 else
00091 {
00092 dsrv_ = new dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>(ros::NodeHandle("~/" + name_));
00093 dsrv_->setCallback(cb);
00094 }
00095 }
00096
00097 matchSize();
00098 }
00099
00100 void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level)
00101 {
00102 setInflationParameters(config.inflation_radius, config.cost_scaling_factor);
00103
00104 if (enabled_ != config.enabled) {
00105 enabled_ = config.enabled;
00106 need_reinflation_ = true;
00107 }
00108 }
00109
00110 void InflationLayer::matchSize()
00111 {
00112 boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
00113 costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
00114 resolution_ = costmap->getResolution();
00115 cell_inflation_radius_ = cellDistance(inflation_radius_);
00116 computeCaches();
00117
00118 unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
00119 if (seen_)
00120 delete[] seen_;
00121 seen_size_ = size_x * size_y;
00122 seen_ = new bool[seen_size_];
00123 }
00124
00125 void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
00126 double* min_y, double* max_x, double* max_y)
00127 {
00128 if (need_reinflation_)
00129 {
00130 last_min_x_ = *min_x;
00131 last_min_y_ = *min_y;
00132 last_max_x_ = *max_x;
00133 last_max_y_ = *max_y;
00134
00135
00136
00137 *min_x = -std::numeric_limits<float>::max();
00138 *min_y = -std::numeric_limits<float>::max();
00139 *max_x = std::numeric_limits<float>::max();
00140 *max_y = std::numeric_limits<float>::max();
00141 need_reinflation_ = false;
00142 }
00143 else
00144 {
00145 double tmp_min_x = last_min_x_;
00146 double tmp_min_y = last_min_y_;
00147 double tmp_max_x = last_max_x_;
00148 double tmp_max_y = last_max_y_;
00149 last_min_x_ = *min_x;
00150 last_min_y_ = *min_y;
00151 last_max_x_ = *max_x;
00152 last_max_y_ = *max_y;
00153 *min_x = std::min(tmp_min_x, *min_x) - inflation_radius_;
00154 *min_y = std::min(tmp_min_y, *min_y) - inflation_radius_;
00155 *max_x = std::max(tmp_max_x, *max_x) + inflation_radius_;
00156 *max_y = std::max(tmp_max_y, *max_y) + inflation_radius_;
00157 }
00158 }
00159
00160 void InflationLayer::onFootprintChanged()
00161 {
00162 inscribed_radius_ = layered_costmap_->getInscribedRadius();
00163 cell_inflation_radius_ = cellDistance(inflation_radius_);
00164 computeCaches();
00165 need_reinflation_ = true;
00166
00167 ROS_DEBUG("InflationLayer::onFootprintChanged(): num footprint points: %lu,"
00168 " inscribed_radius_ = %.3f, inflation_radius_ = %.3f",
00169 layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
00170 }
00171
00172 void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i,
00173 int max_j)
00174 {
00175 boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
00176 if (!enabled_)
00177 return;
00178
00179
00180 ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
00181
00182 unsigned char* master_array = master_grid.getCharMap();
00183 unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
00184
00185 if (seen_ == NULL) {
00186 ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
00187 seen_size_ = size_x * size_y;
00188 seen_ = new bool[seen_size_];
00189 }
00190 else if (seen_size_ != size_x * size_y)
00191 {
00192 ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");
00193 delete[] seen_;
00194 seen_size_ = size_x * size_y;
00195 seen_ = new bool[seen_size_];
00196 }
00197 memset(seen_, false, size_x * size_y * sizeof(bool));
00198
00199
00200
00201
00202
00203 min_i -= cell_inflation_radius_;
00204 min_j -= cell_inflation_radius_;
00205 max_i += cell_inflation_radius_;
00206 max_j += cell_inflation_radius_;
00207
00208 min_i = std::max(0, min_i);
00209 min_j = std::max(0, min_j);
00210 max_i = std::min(int(size_x), max_i);
00211 max_j = std::min(int(size_y), max_j);
00212
00213 for (int j = min_j; j < max_j; j++)
00214 {
00215 for (int i = min_i; i < max_i; i++)
00216 {
00217 int index = master_grid.getIndex(i, j);
00218 unsigned char cost = master_array[index];
00219 if (cost == LETHAL_OBSTACLE)
00220 {
00221 enqueue(index, i, j, i, j);
00222 }
00223 }
00224 }
00225
00226 while (!inflation_queue_.empty())
00227 {
00228
00229 const CellData& current_cell = inflation_queue_.top();
00230
00231 unsigned int index = current_cell.index_;
00232 unsigned int mx = current_cell.x_;
00233 unsigned int my = current_cell.y_;
00234 unsigned int sx = current_cell.src_x_;
00235 unsigned int sy = current_cell.src_y_;
00236
00237
00238 inflation_queue_.pop();
00239
00240
00241 if (seen_[index])
00242 {
00243 continue;
00244 }
00245
00246 seen_[index] = true;
00247
00248
00249 unsigned char cost = costLookup(mx, my, sx, sy);
00250 unsigned char old_cost = master_array[index];
00251 if (old_cost == NO_INFORMATION && cost >= INSCRIBED_INFLATED_OBSTACLE)
00252 master_array[index] = cost;
00253 else
00254 master_array[index] = std::max(old_cost, cost);
00255
00256
00257 if (mx > 0)
00258 enqueue(index - 1, mx - 1, my, sx, sy);
00259 if (my > 0)
00260 enqueue(index - size_x, mx, my - 1, sx, sy);
00261 if (mx < size_x - 1)
00262 enqueue(index + 1, mx + 1, my, sx, sy);
00263 if (my < size_y - 1)
00264 enqueue(index + size_x, mx, my + 1, sx, sy);
00265 }
00266 }
00267
00277 inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
00278 unsigned int src_x, unsigned int src_y)
00279 {
00280 if (!seen_[index])
00281 {
00282
00283 double distance = distanceLookup(mx, my, src_x, src_y);
00284
00285
00286 if (distance > cell_inflation_radius_)
00287 return;
00288
00289
00290 CellData data(distance, index, mx, my, src_x, src_y);
00291 inflation_queue_.push(data);
00292 }
00293 }
00294
00295 void InflationLayer::computeCaches()
00296 {
00297 if (cell_inflation_radius_ == 0)
00298 return;
00299
00300
00301 if (cell_inflation_radius_ != cached_cell_inflation_radius_)
00302 {
00303 deleteKernels();
00304
00305 cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
00306 cached_distances_ = new double*[cell_inflation_radius_ + 2];
00307
00308 for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
00309 {
00310 cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
00311 cached_distances_[i] = new double[cell_inflation_radius_ + 2];
00312 for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
00313 {
00314 cached_distances_[i][j] = hypot(i, j);
00315 }
00316 }
00317
00318 cached_cell_inflation_radius_ = cell_inflation_radius_;
00319 }
00320
00321 for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
00322 {
00323 for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
00324 {
00325 cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
00326 }
00327 }
00328 }
00329
00330 void InflationLayer::deleteKernels()
00331 {
00332 if (cached_distances_ != NULL)
00333 {
00334 for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
00335 {
00336 if (cached_distances_[i])
00337 delete[] cached_distances_[i];
00338 }
00339 if (cached_distances_)
00340 delete[] cached_distances_;
00341 cached_distances_ = NULL;
00342 }
00343
00344 if (cached_costs_ != NULL)
00345 {
00346 for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
00347 {
00348 if (cached_costs_[i])
00349 delete[] cached_costs_[i];
00350 }
00351 delete[] cached_costs_;
00352 cached_costs_ = NULL;
00353 }
00354 }
00355
00356 void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor)
00357 {
00358 if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius)
00359 {
00360
00361
00362 boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
00363
00364 inflation_radius_ = inflation_radius;
00365 cell_inflation_radius_ = cellDistance(inflation_radius_);
00366 weight_ = cost_scaling_factor;
00367 need_reinflation_ = true;
00368 computeCaches();
00369 }
00370 }
00371
00372 }