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, int max_j)
00173 {
00174 boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
00175 if (!enabled_ || (cell_inflation_radius_ == 0))
00176 return;
00177
00178
00179 ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");
00180
00181 unsigned char* master_array = master_grid.getCharMap();
00182 unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
00183
00184 if (seen_ == NULL) {
00185 ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
00186 seen_size_ = size_x * size_y;
00187 seen_ = new bool[seen_size_];
00188 }
00189 else if (seen_size_ != size_x * size_y)
00190 {
00191 ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");
00192 delete[] seen_;
00193 seen_size_ = size_x * size_y;
00194 seen_ = new bool[seen_size_];
00195 }
00196 memset(seen_, false, size_x * size_y * sizeof(bool));
00197
00198
00199
00200
00201
00202 min_i -= cell_inflation_radius_;
00203 min_j -= cell_inflation_radius_;
00204 max_i += cell_inflation_radius_;
00205 max_j += cell_inflation_radius_;
00206
00207 min_i = std::max(0, min_i);
00208 min_j = std::max(0, min_j);
00209 max_i = std::min(int(size_x), max_i);
00210 max_j = std::min(int(size_y), max_j);
00211
00212
00213
00214
00215
00216 std::vector<CellData>& obs_bin = inflation_cells_[0.0];
00217 for (int j = min_j; j < max_j; j++)
00218 {
00219 for (int i = min_i; i < max_i; i++)
00220 {
00221 int index = master_grid.getIndex(i, j);
00222 unsigned char cost = master_array[index];
00223 if (cost == LETHAL_OBSTACLE)
00224 {
00225 obs_bin.push_back(CellData(index, i, j, i, j));
00226 }
00227 }
00228 }
00229
00230
00231
00232 std::map<double, std::vector<CellData> >::iterator bin;
00233 for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
00234 {
00235 for (int i = 0; i < bin->second.size(); ++i)
00236 {
00237
00238 const CellData& cell = bin->second[i];
00239
00240 unsigned int index = cell.index_;
00241
00242
00243 if (seen_[index])
00244 {
00245 continue;
00246 }
00247
00248 seen_[index] = true;
00249
00250 unsigned int mx = cell.x_;
00251 unsigned int my = cell.y_;
00252 unsigned int sx = cell.src_x_;
00253 unsigned int sy = cell.src_y_;
00254
00255
00256 unsigned char cost = costLookup(mx, my, sx, sy);
00257 unsigned char old_cost = master_array[index];
00258 if (old_cost == NO_INFORMATION && cost >= INSCRIBED_INFLATED_OBSTACLE)
00259 master_array[index] = cost;
00260 else
00261 master_array[index] = std::max(old_cost, cost);
00262
00263
00264 if (mx > 0)
00265 enqueue(index - 1, mx - 1, my, sx, sy);
00266 if (my > 0)
00267 enqueue(index - size_x, mx, my - 1, sx, sy);
00268 if (mx < size_x - 1)
00269 enqueue(index + 1, mx + 1, my, sx, sy);
00270 if (my < size_y - 1)
00271 enqueue(index + size_x, mx, my + 1, sx, sy);
00272 }
00273 }
00274
00275 inflation_cells_.clear();
00276 }
00277
00287 inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
00288 unsigned int src_x, unsigned int src_y)
00289 {
00290 if (!seen_[index])
00291 {
00292
00293 double distance = distanceLookup(mx, my, src_x, src_y);
00294
00295
00296 if (distance > cell_inflation_radius_)
00297 return;
00298
00299
00300 inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
00301 }
00302 }
00303
00304 void InflationLayer::computeCaches()
00305 {
00306 if (cell_inflation_radius_ == 0)
00307 return;
00308
00309
00310 if (cell_inflation_radius_ != cached_cell_inflation_radius_)
00311 {
00312 deleteKernels();
00313
00314 cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
00315 cached_distances_ = new double*[cell_inflation_radius_ + 2];
00316
00317 for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
00318 {
00319 cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
00320 cached_distances_[i] = new double[cell_inflation_radius_ + 2];
00321 for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
00322 {
00323 cached_distances_[i][j] = hypot(i, j);
00324 }
00325 }
00326
00327 cached_cell_inflation_radius_ = cell_inflation_radius_;
00328 }
00329
00330 for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
00331 {
00332 for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
00333 {
00334 cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
00335 }
00336 }
00337 }
00338
00339 void InflationLayer::deleteKernels()
00340 {
00341 if (cached_distances_ != NULL)
00342 {
00343 for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
00344 {
00345 if (cached_distances_[i])
00346 delete[] cached_distances_[i];
00347 }
00348 if (cached_distances_)
00349 delete[] cached_distances_;
00350 cached_distances_ = NULL;
00351 }
00352
00353 if (cached_costs_ != NULL)
00354 {
00355 for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i)
00356 {
00357 if (cached_costs_[i])
00358 delete[] cached_costs_[i];
00359 }
00360 delete[] cached_costs_;
00361 cached_costs_ = NULL;
00362 }
00363 }
00364
00365 void InflationLayer::setInflationParameters(double inflation_radius, double cost_scaling_factor)
00366 {
00367 if (weight_ != cost_scaling_factor || inflation_radius_ != inflation_radius)
00368 {
00369
00370
00371 boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
00372
00373 inflation_radius_ = inflation_radius;
00374 cell_inflation_radius_ = cellDistance(inflation_radius_);
00375 weight_ = cost_scaling_factor;
00376 need_reinflation_ = true;
00377 computeCaches();
00378 }
00379 }
00380
00381 }