$search
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/costmap_2d.h> 00038 #include <cstdio> 00039 00040 #include <costmap_2d/Costmap2DConfig.h> 00041 00042 using namespace std; 00043 00044 namespace costmap_2d{ 00045 Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, 00046 double resolution, double origin_x, double origin_y, double inscribed_radius, 00047 double circumscribed_radius, double inflation_radius, double max_obstacle_range, 00048 double max_obstacle_height, double max_raytrace_range, double weight, 00049 const std::vector<unsigned char>& static_data, unsigned char lethal_threshold, bool track_unknown_space, unsigned char unknown_cost_value) : size_x_(cells_size_x), 00050 size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y), static_map_(NULL), 00051 costmap_(NULL), markers_(NULL), max_obstacle_range_(max_obstacle_range), 00052 max_obstacle_height_(max_obstacle_height), max_raytrace_range_(max_raytrace_range), cached_costs_(NULL), cached_distances_(NULL), 00053 inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius), inflation_radius_(inflation_radius), 00054 weight_(weight), lethal_threshold_(lethal_threshold), track_unknown_space_(track_unknown_space), unknown_cost_value_(unknown_cost_value), inflation_queue_(){ 00055 //create the costmap, static_map, and markers 00056 costmap_ = new unsigned char[size_x_ * size_y_]; 00057 static_map_ = new unsigned char[size_x_ * size_y_]; 00058 markers_ = new unsigned char[size_x_ * size_y_]; 00059 memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char)); 00060 00061 //convert our inflations from world to cell distance 00062 cell_inscribed_radius_ = cellDistance(inscribed_radius); 00063 cell_circumscribed_radius_ = cellDistance(circumscribed_radius); 00064 cell_inflation_radius_ = cellDistance(inflation_radius); 00065 00066 //set the cost for the circumscribed radius of the robot 00067 circumscribed_cost_lb_ = computeCost(cell_circumscribed_radius_); 00068 00069 computeCaches(); 00070 00071 if(!static_data.empty()){ 00072 ROS_ASSERT_MSG(size_x_ * size_y_ == static_data.size(), "If you want to initialize a costmap with static data, their sizes must match."); 00073 00074 //make sure the inflation queue is empty at the beginning of the cycle (should always be true) 00075 ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation"); 00076 00077 unsigned int index = 0; 00078 unsigned char* costmap_index = costmap_; 00079 std::vector<unsigned char>::const_iterator static_data_index = static_data.begin(); 00080 00081 //initialize the costmap with static data 00082 for(unsigned int i = 0; i < size_y_; ++i){ 00083 for(unsigned int j = 0; j < size_x_; ++j){ 00084 //check if the static value is above the unknown or lethal thresholds 00085 if(track_unknown_space_ && unknown_cost_value_ > 0 && *static_data_index == unknown_cost_value_) 00086 *costmap_index = NO_INFORMATION; 00087 else if(*static_data_index >= lethal_threshold_) 00088 *costmap_index = LETHAL_OBSTACLE; 00089 else 00090 *costmap_index = FREE_SPACE; 00091 00092 if(*costmap_index == LETHAL_OBSTACLE){ 00093 unsigned int mx, my; 00094 indexToCells(index, mx, my); 00095 enqueue(index, mx, my, mx, my, inflation_queue_); 00096 } 00097 00098 ++costmap_index; 00099 ++static_data_index; 00100 ++index; 00101 } 00102 } 00103 00104 //now... let's inflate the obstacles 00105 inflateObstacles(inflation_queue_); 00106 00107 //we also want to keep a copy of the current costmap as the static map 00108 memcpy(static_map_, costmap_, size_x_ * size_y_ * sizeof(unsigned char)); 00109 } 00110 else{ 00111 //everything is unknown initially if we don't have a static map unless we aren't tracking unkown space in which case it is free 00112 resetMaps(); 00113 } 00114 } 00115 00116 void Costmap2D::reconfigure(Costmap2DConfig &config, const Costmap2DConfig &last_config) { 00117 boost::recursive_mutex::scoped_lock rel(configuration_mutex_); 00118 00119 max_obstacle_height_ = config.max_obstacle_height; 00120 max_obstacle_range_ = config.max_obstacle_range; 00121 max_raytrace_range_ = config.raytrace_range; 00122 00123 if(last_config.inflation_radius != config.inflation_radius) 00124 { 00125 inflation_radius_ = config.inflation_radius; 00126 cell_inflation_radius_ = cellDistance(inflation_radius_); 00127 computeCaches(); 00128 } 00129 00130 //only update the origin for the map if the 00131 if(!config.static_map && (last_config.origin_x != config.origin_x || last_config.origin_y != config.origin_y)) 00132 updateOrigin(config.origin_x, config.origin_y); 00133 00134 unknown_cost_value_ = config.unknown_cost_value; 00135 lethal_threshold_ = config.lethal_cost_threshold; 00136 00137 weight_ = config.cost_scaling_factor; 00138 00139 if((config.footprint == "" || config.footprint == "[]") && config.robot_radius > 0) { 00140 inscribed_radius_ = config.robot_radius; 00141 circumscribed_radius_ = inscribed_radius_; 00142 } 00143 00144 finishConfiguration(config); 00145 } 00146 00147 void Costmap2D::finishConfiguration(costmap_2d::Costmap2DConfig &config) { 00148 } 00149 00150 void Costmap2D::replaceFullMap(double win_origin_x, double win_origin_y, 00151 unsigned int data_size_x, unsigned int data_size_y, 00152 const std::vector<unsigned char>& static_data){ 00153 boost::recursive_mutex::scoped_lock rfml(configuration_mutex_); 00154 00155 //delete our old maps 00156 deleteMaps(); 00157 00158 //update the origin and size of the new map 00159 size_x_ = data_size_x; 00160 size_y_ = data_size_y; 00161 origin_x_ = win_origin_x; 00162 origin_y_ = win_origin_y; 00163 00164 //initialize our various maps 00165 initMaps(size_x_, size_y_); 00166 00167 //make sure the inflation queue is empty at the beginning of the cycle (should always be true) 00168 ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation"); 00169 00170 unsigned int index = 0; 00171 unsigned char* costmap_index = costmap_; 00172 std::vector<unsigned char>::const_iterator static_data_index = static_data.begin(); 00173 00174 //copy static data into the costmap 00175 for(unsigned int i = 0; i < size_y_; ++i){ 00176 for(unsigned int j = 0; j < size_x_; ++j){ 00177 //check if the static value is above the unknown or lethal thresholds 00178 if(track_unknown_space_ && unknown_cost_value_ > 0 && *static_data_index == unknown_cost_value_) 00179 *costmap_index = NO_INFORMATION; 00180 else if(*static_data_index >= lethal_threshold_) 00181 *costmap_index = LETHAL_OBSTACLE; 00182 else 00183 *costmap_index = FREE_SPACE; 00184 00185 if(*costmap_index == LETHAL_OBSTACLE){ 00186 unsigned int mx, my; 00187 indexToCells(index, mx, my); 00188 enqueue(index, mx, my, mx, my, inflation_queue_); 00189 } 00190 ++costmap_index; 00191 ++static_data_index; 00192 ++index; 00193 } 00194 } 00195 00196 //now... let's inflate the obstacles 00197 inflateObstacles(inflation_queue_); 00198 00199 //we also want to keep a copy of the current costmap as the static map 00200 memcpy(static_map_, costmap_, size_x_ * size_y_ * sizeof(unsigned char)); 00201 } 00202 00203 void Costmap2D::replaceStaticMapWindow(double win_origin_x, double win_origin_y, 00204 unsigned int data_size_x, unsigned int data_size_y, 00205 const std::vector<unsigned char>& static_data){ 00206 boost::recursive_mutex::scoped_lock stwl(configuration_mutex_); 00207 00208 unsigned int start_x, start_y; 00209 if(!worldToMap(win_origin_x, win_origin_y, start_x, start_y) || (start_x + data_size_x) > size_x_ || (start_y + data_size_y) > size_y_){ 00210 ROS_ERROR("You must call replaceStaticMapWindow with a window origin and size that is contained within the map"); 00211 return; 00212 } 00213 00214 00215 //we need to compute the region of the costmap that could change from inflation of new obstacles 00216 unsigned int max_inflation_change = 2 * cell_inflation_radius_; 00217 00218 00219 00220 //make sure that we don't go out of map bounds 00221 unsigned int copy_sx = std::min(std::max(0, (int)start_x - (int)max_inflation_change), (int)size_x_); 00222 unsigned int copy_sy = std::min(std::max(0, (int)start_y - (int)max_inflation_change), (int)size_x_); 00223 unsigned int copy_ex = std::max(std::min((int)size_x_, (int)start_x + (int)data_size_x + (int)max_inflation_change), 0); 00224 unsigned int copy_ey = std::max(std::min((int)size_y_, (int)start_y + (int)data_size_y + (int)max_inflation_change), 0); 00225 00226 unsigned int copy_size_x = copy_ex - copy_sx; 00227 unsigned int copy_size_y = copy_ey - copy_sy; 00228 00229 //now... we have to compute the window 00230 double ll_x, ll_y, ur_x, ur_y; 00231 mapToWorld(copy_sx, copy_sy, ll_x, ll_y); 00232 mapToWorld(copy_ex, copy_ey, ur_x, ur_y); 00233 double mid_x = (ll_x + ur_x) / 2; 00234 double mid_y = (ll_y + ur_y) / 2; 00235 double size_x = ur_x - ll_x; 00236 double size_y = ur_y - ll_y; 00237 00238 //finally... we'll clear all non-lethal costs in the area that could be affected by the map update 00239 //we'll reinflate after the map update is complete 00240 clearNonLethal(mid_x, mid_y, size_x, size_y); 00241 00242 //copy static data into the costmap 00243 unsigned int start_index = start_y * size_x_ + start_x; 00244 unsigned char* costmap_index = costmap_ + start_index; 00245 std::vector<unsigned char>::const_iterator static_data_index = static_data.begin(); 00246 for(unsigned int i = 0; i < data_size_y; ++i){ 00247 for(unsigned int j = 0; j < data_size_x; ++j){ 00248 //check if the static value is above the unknown or lethal thresholds 00249 if(track_unknown_space_ && unknown_cost_value_ > 0 && *static_data_index == unknown_cost_value_) 00250 *costmap_index = NO_INFORMATION; 00251 else if(*static_data_index >= lethal_threshold_) 00252 *costmap_index = LETHAL_OBSTACLE; 00253 else 00254 *costmap_index = FREE_SPACE; 00255 00256 ++costmap_index; 00257 ++static_data_index; 00258 } 00259 costmap_index += size_x_ - data_size_x; 00260 } 00261 00262 //now, we're ready to reinflate obstacles in the window that has been updated 00263 //we won't clear all non-lethal obstacles first because the static map update 00264 //may have included non-lethal costs 00265 reinflateWindow(mid_x, mid_y, size_x, size_y, false); 00266 00267 00268 //we also want to keep a copy of the current costmap as the static map... we'll only need to write the region that has changed 00269 copyMapRegion(costmap_, copy_sx, copy_sy, size_x_, static_map_, copy_sx, copy_sy, size_x_, copy_size_x, copy_size_y); 00270 00271 } 00272 00273 void Costmap2D::reshapeStaticMap(double win_origin_x, double win_origin_y, 00274 unsigned int data_size_x, unsigned int data_size_y, const std::vector<unsigned char>& static_data){ 00275 int m_ox, m_oy; 00276 worldToMapNoBounds(win_origin_x, win_origin_y, m_ox, m_oy); 00277 00278 //compute the bounds for the size of our new map 00279 int bl_x = std::min(m_ox, 0); 00280 int bl_y = std::min(m_oy, 0); 00281 int ur_x = std::max(m_ox + data_size_x, size_x_); 00282 int ur_y = std::max(m_oy + data_size_y, size_y_); 00283 00284 //create a temporary map to hold our static data and copy the old static map into it 00285 unsigned char* static_map_copy = new unsigned char[size_x_ * size_y_]; 00286 memcpy(static_map_copy, static_map_, size_x_ * size_y_ * sizeof(unsigned char)); 00287 00288 //delete our old maps... the user will lose any 00289 //cost information not stored in the static map when reshaping a map 00290 deleteMaps(); 00291 00292 //update the origin and sizes, and cache data we'll need 00293 double old_origin_x = origin_x_; 00294 double old_origin_y = origin_y_; 00295 unsigned int old_size_x = size_x_; 00296 unsigned int old_size_y = size_y_; 00297 00298 size_x_ = ur_x - bl_x; 00299 size_y_ = ur_y - bl_y; 00300 origin_x_ = std::min(origin_x_, win_origin_x); 00301 origin_y_ = std::min(origin_y_, win_origin_y); 00302 00303 //initialize our various maps 00304 initMaps(size_x_, size_y_); 00305 00306 //reset our maps to be full of unknown space if appropriate 00307 resetMaps(); 00308 00309 //now, copy the old static map into the new costmap 00310 unsigned int start_x, start_y; 00311 worldToMap(old_origin_x, old_origin_y, start_x, start_y); 00312 copyMapRegion(static_map_copy, 0, 0, old_size_x, costmap_, start_x, start_y, size_x_, old_size_x, old_size_y); 00313 00314 delete[] static_map_copy; 00315 00316 //now we want to update the map with the new static map data 00317 replaceStaticMapWindow(win_origin_x, win_origin_y, data_size_x, data_size_y, static_data); 00318 } 00319 00320 void Costmap2D::updateStaticMapWindow(double win_origin_x, double win_origin_y, 00321 unsigned int data_size_x, unsigned int data_size_y, 00322 const std::vector<unsigned char>& static_data){ 00323 00324 if(data_size_x * data_size_y != static_data.size()){ 00325 ROS_ERROR("The sizes passed in are incorrect for the size of the static data char array. Doing nothing."); 00326 return; 00327 } 00328 00329 //get the map coordinates of the origin of static map window 00330 int m_ox, m_oy; 00331 worldToMapNoBounds(win_origin_x, win_origin_y, m_ox, m_oy); 00332 00333 //if the static map contains the full costmap, then we'll just overwrite the costmap 00334 if(m_ox <= 0 && m_oy <= 0 && (m_ox + data_size_x) >= size_x_ && (m_oy + data_size_y) >= size_y_){ 00335 replaceFullMap(win_origin_x, win_origin_y, data_size_x, data_size_y, static_data); 00336 } 00337 //if the static map overlaps with the costmap, but not completely... we'll have to resize the costmap and maintain certain information 00338 else if(m_ox < 0 || m_oy < 0 || (m_ox + data_size_x) > size_x_ || (m_oy + data_size_y) > size_y_){ 00339 reshapeStaticMap(win_origin_x, win_origin_y, data_size_x, data_size_y, static_data); 00340 } 00341 //if the costmap fully contains the changes we'll make with the static map... then we can just overwrite a portion of the costmap 00342 else{ 00343 replaceStaticMapWindow(win_origin_x, win_origin_y, data_size_x, data_size_y, static_data); 00344 } 00345 } 00346 00347 void Costmap2D::deleteMaps(){ 00348 //clean up old data 00349 delete[] costmap_; 00350 delete[] static_map_; 00351 delete[] markers_; 00352 } 00353 00354 void Costmap2D::deleteKernels(){ 00355 if(cached_distances_ != NULL){ 00356 for(unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i){ 00357 delete[] cached_distances_[i]; 00358 } 00359 delete[] cached_distances_; 00360 } 00361 00362 if(cached_costs_ != NULL){ 00363 for(unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i){ 00364 delete[] cached_costs_[i]; 00365 } 00366 delete[] cached_costs_; 00367 } 00368 } 00369 00370 void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y){ 00371 costmap_ = new unsigned char[size_x * size_y]; 00372 static_map_ = new unsigned char[size_x * size_y]; 00373 markers_ = new unsigned char[size_x * size_y]; 00374 00375 //reset markers for inflation 00376 memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char)); 00377 } 00378 00379 void Costmap2D::resetMaps(){ 00380 //reset our maps to have no information 00381 if(track_unknown_space_){ 00382 memset(static_map_, NO_INFORMATION, size_x_ * size_y_ * sizeof(unsigned char)); 00383 memset(costmap_, NO_INFORMATION, size_x_ * size_y_ * sizeof(unsigned char)); 00384 } 00385 else{ 00386 memset(static_map_, FREE_SPACE, size_x_ * size_y_ * sizeof(unsigned char)); 00387 memset(costmap_, FREE_SPACE, size_x_ * size_y_ * sizeof(unsigned char)); 00388 } 00389 } 00390 00391 void Costmap2D::copyKernels(const Costmap2D& map, unsigned int cell_inflation_radius){ 00392 cached_costs_ = new unsigned char*[cell_inflation_radius + 2]; 00393 cached_distances_ = new double*[cell_inflation_radius + 2]; 00394 for(unsigned int i = 0; i <= cell_inflation_radius + 1; ++i){ 00395 cached_costs_[i] = new unsigned char[cell_inflation_radius + 2]; 00396 cached_distances_[i] = new double[cell_inflation_radius + 2]; 00397 for(unsigned int j = 0; j <= cell_inflation_radius + 1; ++j){ 00398 cached_distances_[i][j] = map.cached_distances_[i][j]; 00399 cached_costs_[i][j] = map.cached_costs_[i][j]; 00400 } 00401 } 00402 } 00403 00404 void Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y){ 00405 boost::recursive_mutex::scoped_lock cpl(configuration_mutex_); 00406 00407 //check for self windowing 00408 if(this == &map){ 00409 ROS_ERROR("Cannot convert this costmap into a window of itself"); 00410 return; 00411 } 00412 00413 //clean up old data 00414 deleteMaps(); 00415 deleteKernels(); 00416 00417 //compute the bounds of our new map 00418 unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y; 00419 if(!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y) 00420 || ! map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y)){ 00421 ROS_ERROR("Cannot window a map that the window bounds don't fit inside of"); 00422 return; 00423 } 00424 00425 size_x_ = upper_right_x - lower_left_x; 00426 size_y_ = upper_right_y - lower_left_y; 00427 resolution_ = map.resolution_; 00428 origin_x_ = win_origin_x; 00429 origin_y_ = win_origin_y; 00430 00431 ROS_DEBUG("ll(%d, %d), ur(%d, %d), size(%d, %d), origin(%.2f, %.2f)", 00432 lower_left_x, lower_left_y, upper_right_x, upper_right_y, size_x_, size_y_, origin_x_, origin_y_); 00433 00434 00435 //initialize our various maps and reset markers for inflation 00436 initMaps(size_x_, size_y_); 00437 00438 //copy the window of the static map and the costmap that we're taking 00439 copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_); 00440 copyMapRegion(map.static_map_, lower_left_x, lower_left_y, map.size_x_, static_map_, 0, 0, size_x_, size_x_, size_y_); 00441 00442 max_obstacle_range_ = map.max_obstacle_range_; 00443 max_obstacle_height_ = map.max_obstacle_height_; 00444 max_raytrace_range_ = map.max_raytrace_range_; 00445 00446 inscribed_radius_ = map.inscribed_radius_; 00447 circumscribed_radius_ = map.circumscribed_radius_; 00448 inflation_radius_ = map.inflation_radius_; 00449 00450 cell_inscribed_radius_ = map.cell_inscribed_radius_; 00451 cell_circumscribed_radius_ = map.cell_circumscribed_radius_; 00452 cell_inflation_radius_ = map.cell_inflation_radius_; 00453 00454 //set the cost for the circumscribed radius of the robot 00455 circumscribed_cost_lb_ = map.circumscribed_cost_lb_; 00456 00457 weight_ = map.weight_; 00458 00459 //copy the cost and distance kernels 00460 copyKernels(map, cell_inflation_radius_); 00461 } 00462 00463 Costmap2D& Costmap2D::operator=(const Costmap2D& map) { 00464 00465 //check for self assignement 00466 if(this == &map) 00467 return *this; 00468 00469 //clean up old data 00470 deleteMaps(); 00471 deleteKernels(); 00472 00473 size_x_ = map.size_x_; 00474 size_y_ = map.size_y_; 00475 resolution_ = map.resolution_; 00476 origin_x_ = map.origin_x_; 00477 origin_y_ = map.origin_y_; 00478 00479 //initialize our various maps 00480 initMaps(size_x_, size_y_); 00481 00482 //copy the static map 00483 memcpy(static_map_, map.static_map_, size_x_ * size_y_ * sizeof(unsigned char)); 00484 00485 //copy the cost map 00486 memcpy(costmap_, map.costmap_, size_x_ * size_y_ * sizeof(unsigned char)); 00487 00488 max_obstacle_range_ = map.max_obstacle_range_; 00489 max_obstacle_height_ = map.max_obstacle_height_; 00490 max_raytrace_range_ = map.max_raytrace_range_; 00491 00492 inscribed_radius_ = map.inscribed_radius_; 00493 circumscribed_radius_ = map.circumscribed_radius_; 00494 inflation_radius_ = map.inflation_radius_; 00495 00496 cell_inscribed_radius_ = map.cell_inscribed_radius_; 00497 cell_circumscribed_radius_ = map.cell_circumscribed_radius_; 00498 cell_inflation_radius_ = map.cell_inflation_radius_; 00499 00500 //set the cost for the circumscribed radius of the robot 00501 circumscribed_cost_lb_ = map.circumscribed_cost_lb_; 00502 00503 weight_ = map.weight_; 00504 00505 //copy the cost and distance kernels 00506 copyKernels(map, cell_inflation_radius_); 00507 00508 return *this; 00509 } 00510 00511 Costmap2D::Costmap2D(const Costmap2D& map) : static_map_(NULL), costmap_(NULL), markers_(NULL), cached_costs_(NULL), cached_distances_(NULL) { 00512 *this = map; 00513 } 00514 00515 //just initialize everything to NULL by default 00516 Costmap2D::Costmap2D() : size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), static_map_(NULL), 00517 costmap_(NULL), markers_(NULL), cached_costs_(NULL), cached_distances_(NULL) {} 00518 00519 Costmap2D::~Costmap2D(){ 00520 deleteMaps(); 00521 deleteKernels(); 00522 } 00523 00524 unsigned int Costmap2D::cellDistance(double world_dist){ 00525 double cells_dist = max(0.0, ceil(world_dist / resolution_)); 00526 return (unsigned int) cells_dist; 00527 } 00528 00529 const unsigned char* Costmap2D::getCharMap() const { 00530 return costmap_; 00531 } 00532 00533 unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const { 00534 ROS_ASSERT_MSG(mx < size_x_ && my < size_y_, "You cannot get the cost of a cell that is outside the bounds of the costmap"); 00535 return costmap_[getIndex(mx, my)]; 00536 } 00537 00538 void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) { 00539 ROS_ASSERT_MSG(mx < size_x_ && my < size_y_, "You cannot set the cost of a cell that is outside the bounds of the costmap"); 00540 costmap_[getIndex(mx, my)] = cost; 00541 } 00542 00543 void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const { 00544 wx = origin_x_ + (mx + 0.5) * resolution_; 00545 wy = origin_y_ + (my + 0.5) * resolution_; 00546 } 00547 00548 bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const { 00549 if(wx < origin_x_ || wy < origin_y_) 00550 return false; 00551 00552 mx = (int) ((wx - origin_x_) / resolution_); 00553 my = (int) ((wy - origin_y_) / resolution_); 00554 00555 if(mx < size_x_ && my < size_y_) 00556 return true; 00557 00558 return false; 00559 } 00560 00561 void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const { 00562 mx = (int) ((wx - origin_x_) / resolution_); 00563 my = (int) ((wy - origin_y_) / resolution_); 00564 } 00565 00566 void Costmap2D::resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y){ 00567 ROS_ASSERT_MSG(w_size_x >= 0 && w_size_y >= 0, "You cannot specify a negative size window"); 00568 00569 double start_point_x = wx - w_size_x / 2; 00570 double start_point_y = wy - w_size_y / 2; 00571 double end_point_x = start_point_x + w_size_x; 00572 double end_point_y = start_point_y + w_size_y; 00573 00574 //check start bounds 00575 start_point_x = max(origin_x_, start_point_x); 00576 start_point_y = max(origin_y_, start_point_y); 00577 00578 //check end bounds 00579 end_point_x = min(origin_x_ + getSizeInMetersX(), end_point_x); 00580 end_point_y = min(origin_y_ + getSizeInMetersY(), end_point_y); 00581 00582 unsigned int start_x, start_y, end_x, end_y; 00583 00584 //check for legality just in case 00585 if(!worldToMap(start_point_x, start_point_y, start_x, start_y) || !worldToMap(end_point_x, end_point_y, end_x, end_y)) 00586 return; 00587 00588 ROS_ASSERT(end_x >= start_x && end_y >= start_y); 00589 unsigned int cell_size_x = end_x - start_x; 00590 unsigned int cell_size_y = end_y - start_y; 00591 00592 //we need a map to store the obstacles in the window temporarily 00593 unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y]; 00594 00595 //copy the local window in the costmap to the local map 00596 copyMapRegion(costmap_, start_x, start_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y); 00597 00598 //now we'll reset the costmap to the static map 00599 memcpy(costmap_, static_map_, size_x_ * size_y_ * sizeof(unsigned char)); 00600 00601 //now we want to copy the local map back into the costmap 00602 copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); 00603 00604 //clean up 00605 delete[] local_map; 00606 } 00607 00608 void Costmap2D::updateWorld(double robot_x, double robot_y, 00609 const vector<Observation>& observations, const vector<Observation>& clearing_observations){ 00610 boost::recursive_mutex::scoped_lock uwl(configuration_mutex_); 00611 00612 //reset the markers for inflation 00613 memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char)); 00614 00615 //make sure the inflation queue is empty at the beginning of the cycle (should always be true) 00616 ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation"); 00617 00618 //raytrace freespace 00619 raytraceFreespace(clearing_observations); 00620 00621 //if we raytrace X meters out... we must re-inflate obstacles within the containing square of that circle 00622 double inflation_window_size = 2 * (max_raytrace_range_ + inflation_radius_); 00623 00624 //clear all non-lethal obstacles in preparation for re-inflation 00625 clearNonLethal(robot_x, robot_y, inflation_window_size, inflation_window_size); 00626 00627 //reset the inflation window 00628 resetInflationWindow(robot_x, robot_y, inflation_window_size + 2 * inflation_radius_, inflation_window_size + 2 * inflation_radius_, inflation_queue_, false); 00629 00630 //now we also want to add the new obstacles we've received to the cost map 00631 updateObstacles(observations, inflation_queue_); 00632 00633 inflateObstacles(inflation_queue_); 00634 } 00635 00636 void Costmap2D::reinflateWindow(double wx, double wy, double w_size_x, double w_size_y, bool clear){ 00637 boost::recursive_mutex::scoped_lock rwl(configuration_mutex_); 00638 //reset the markers for inflation 00639 memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char)); 00640 00641 //make sure the inflation queue is empty at the beginning of the cycle (should always be true) 00642 ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation"); 00643 00644 //reset the inflation window.. adds all lethal costs to the queue for re-propagation 00645 resetInflationWindow(wx, wy, w_size_x, w_size_y, inflation_queue_, clear); 00646 00647 //inflate the obstacles 00648 inflateObstacles(inflation_queue_); 00649 00650 } 00651 00652 void Costmap2D::updateObstacles(const vector<Observation>& observations, priority_queue<CellData>& inflation_queue){ 00653 //place the new obstacles into a priority queue... each with a priority of zero to begin with 00654 for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){ 00655 const Observation& obs = *it; 00656 00657 const pcl::PointCloud<pcl::PointXYZ>& cloud =obs.cloud_; 00658 00659 double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_; 00660 00661 00662 for(unsigned int i = 0; i < cloud.points.size(); ++i){ 00663 //if the obstacle is too high or too far away from the robot we won't add it 00664 if(cloud.points[i].z > max_obstacle_height_){ 00665 ROS_DEBUG("The point is too high"); 00666 continue; 00667 } 00668 00669 //compute the squared distance from the hitpoint to the pointcloud's origin 00670 double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x) 00671 + (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y) 00672 + (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z); 00673 00674 //if the point is far enough away... we won't consider it 00675 if(sq_dist >= sq_obstacle_range){ 00676 ROS_DEBUG("The point is too far away"); 00677 continue; 00678 } 00679 00680 //now we need to compute the map coordinates for the observation 00681 unsigned int mx, my; 00682 if(!worldToMap(cloud.points[i].x, cloud.points[i].y, mx, my)){ 00683 ROS_DEBUG("Computing map coords failed"); 00684 continue; 00685 } 00686 00687 unsigned int index = getIndex(mx, my); 00688 00689 //push the relevant cell index back onto the inflation queue 00690 enqueue(index, mx, my, mx, my, inflation_queue); 00691 } 00692 } 00693 } 00694 00695 void Costmap2D::inflateObstacles(priority_queue<CellData>& inflation_queue){ 00696 while(!inflation_queue.empty()){ 00697 //get the highest priority cell and pop it off the priority queue 00698 const CellData& current_cell = inflation_queue.top(); 00699 00700 unsigned int index = current_cell.index_; 00701 unsigned int mx = current_cell.x_; 00702 unsigned int my = current_cell.y_; 00703 unsigned int sx = current_cell.src_x_; 00704 unsigned int sy = current_cell.src_y_; 00705 00706 //attempt to put the neighbors of the current cell onto the queue 00707 if(mx > 0) 00708 enqueue(index - 1, mx - 1, my, sx, sy, inflation_queue); 00709 if(my > 0) 00710 enqueue(index - size_x_, mx, my - 1, sx, sy, inflation_queue); 00711 if(mx < size_x_ - 1) 00712 enqueue(index + 1, mx + 1, my, sx, sy, inflation_queue); 00713 if(my < size_y_ - 1) 00714 enqueue(index + size_x_, mx, my + 1, sx, sy, inflation_queue); 00715 00716 //remove the current cell from the priority queue 00717 inflation_queue.pop(); 00718 } 00719 } 00720 00721 00722 void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_observations){ 00723 for(unsigned int i = 0; i < clearing_observations.size(); ++i){ 00724 raytraceFreespace(clearing_observations[i]); 00725 } 00726 } 00727 00728 void Costmap2D::raytraceFreespace(const Observation& clearing_observation){ 00729 //create the functor that we'll use to clear cells from the costmap 00730 ClearCell clearer(costmap_); 00731 00732 double ox = clearing_observation.origin_.x; 00733 double oy = clearing_observation.origin_.y; 00734 pcl::PointCloud<pcl::PointXYZ> cloud = clearing_observation.cloud_; 00735 00736 //get the map coordinates of the origin of the sensor 00737 unsigned int x0, y0; 00738 if(!worldToMap(ox, oy, x0, y0)){ 00739 ROS_WARN("The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", ox, oy); 00740 return; 00741 } 00742 00743 //we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later 00744 double map_end_x = origin_x_ + getSizeInMetersX(); 00745 double map_end_y = origin_y_ + getSizeInMetersY(); 00746 00747 //for each point in the cloud, we want to trace a line from the origin and clear obstacles along it 00748 for(unsigned int i = 0; i < cloud.points.size(); ++i){ 00749 double wx = cloud.points[i].x; 00750 double wy = cloud.points[i].y; 00751 00752 //now we also need to make sure that the enpoint we're raytracing 00753 //to isn't off the costmap and scale if necessary 00754 double a = wx - ox; 00755 double b = wy - oy; 00756 00757 //the minimum value to raytrace from is the origin 00758 if(wx < origin_x_){ 00759 double t = (origin_x_ - ox) / a; 00760 wx = origin_x_; 00761 wy = oy + b * t; 00762 } 00763 if(wy < origin_y_){ 00764 double t = (origin_y_ - oy) / b; 00765 wx = ox + a * t; 00766 wy = origin_y_; 00767 } 00768 00769 //the maximum value to raytrace to is the end of the map 00770 if(wx > map_end_x){ 00771 double t = (map_end_x - ox) / a; 00772 wx = map_end_x; 00773 wy = oy + b * t; 00774 } 00775 if(wy > map_end_y){ 00776 double t = (map_end_y - oy) / b; 00777 wx = ox + a * t; 00778 wy = map_end_y; 00779 } 00780 00781 //now that the vector is scaled correctly... we'll get the map coordinates of its endpoint 00782 unsigned int x1, y1; 00783 00784 //check for legality just in case 00785 if(!worldToMap(wx, wy, x1, y1)) 00786 continue; 00787 00788 unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_); 00789 00790 //and finally... we can execute our trace to clear obstacles along that line 00791 raytraceLine(clearer, x0, y0, x1, y1, cell_raytrace_range); 00792 } 00793 } 00794 00795 void Costmap2D::clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info){ 00796 //get the cell coordinates of the center point of the window 00797 unsigned int mx, my; 00798 if(!worldToMap(wx, wy, mx, my)) 00799 return; 00800 00801 //compute the bounds of the window 00802 double start_x = wx - w_size_x / 2; 00803 double start_y = wy - w_size_y / 2; 00804 double end_x = start_x + w_size_x; 00805 double end_y = start_y + w_size_y; 00806 00807 //scale the window based on the bounds of the costmap 00808 start_x = max(origin_x_, start_x); 00809 start_y = max(origin_y_, start_y); 00810 00811 end_x = min(origin_x_ + getSizeInMetersX(), end_x); 00812 end_y = min(origin_y_ + getSizeInMetersY(), end_y); 00813 00814 //get the map coordinates of the bounds of the window 00815 unsigned int map_sx, map_sy, map_ex, map_ey; 00816 00817 //check for legality just in case 00818 if(!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey)) 00819 return; 00820 00821 //we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation 00822 unsigned int index = getIndex(map_sx, map_sy); 00823 unsigned char* current = &costmap_[index]; 00824 for(unsigned int j = map_sy; j <= map_ey; ++j){ 00825 for(unsigned int i = map_sx; i <= map_ex; ++i){ 00826 //if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it 00827 if(*current != LETHAL_OBSTACLE){ 00828 if(clear_no_info || *current != NO_INFORMATION) 00829 *current = FREE_SPACE; 00830 } 00831 current++; 00832 index++; 00833 } 00834 current += size_x_ - (map_ex - map_sx) - 1; 00835 index += size_x_ - (map_ex - map_sx) - 1; 00836 } 00837 } 00838 00839 void Costmap2D::resetInflationWindow(double wx, double wy, double w_size_x, double w_size_y, 00840 priority_queue<CellData>& inflation_queue, bool clear){ 00841 //get the cell coordinates of the center point of the window 00842 unsigned int mx, my; 00843 if(!worldToMap(wx, wy, mx, my)) 00844 return; 00845 00846 //compute the bounds of the window 00847 double start_x = wx - w_size_x / 2; 00848 double start_y = wy - w_size_y / 2; 00849 double end_x = start_x + w_size_x; 00850 double end_y = start_y + w_size_y; 00851 00852 //scale the window based on the bounds of the costmap 00853 start_x = max(origin_x_, start_x); 00854 start_y = max(origin_y_, start_y); 00855 00856 end_x = min(origin_x_ + getSizeInMetersX(), end_x); 00857 end_y = min(origin_y_ + getSizeInMetersY(), end_y); 00858 00859 //get the map coordinates of the bounds of the window 00860 unsigned int map_sx, map_sy, map_ex, map_ey; 00861 00862 //check for legality just in case 00863 if(!worldToMap(start_x, start_y, map_sx, map_sy) || !worldToMap(end_x, end_y, map_ex, map_ey)){ 00864 ROS_ERROR("Bounds not legal for reset window. Doing nothing."); 00865 return; 00866 } 00867 00868 //we know that we want to clear all non-lethal obstacles in this window to get it ready for inflation 00869 unsigned int index = getIndex(map_sx, map_sy); 00870 unsigned char* current = &costmap_[index]; 00871 for(unsigned int j = map_sy; j <= map_ey; ++j){ 00872 for(unsigned int i = map_sx; i <= map_ex; ++i){ 00873 //if the cell is a lethal obstacle... we'll keep it and queue it, otherwise... we'll clear it 00874 if(*current == LETHAL_OBSTACLE) 00875 enqueue(index, i, j, i, j, inflation_queue); 00876 else if(clear && *current != NO_INFORMATION) 00877 *current = FREE_SPACE; 00878 current++; 00879 index++; 00880 } 00881 current += size_x_ - (map_ex - map_sx) - 1; 00882 index += size_x_ - (map_ex - map_sx) - 1; 00883 } 00884 } 00885 00886 void Costmap2D::computeCaches() { 00887 //based on the inflation radius... compute distance and cost caches 00888 cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2]; 00889 cached_distances_ = new double*[cell_inflation_radius_ + 2]; 00890 for(unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i){ 00891 cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2]; 00892 cached_distances_[i] = new double[cell_inflation_radius_ + 2]; 00893 for(unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j){ 00894 cached_distances_[i][j] = sqrt(i*i + j*j); 00895 cached_costs_[i][j] = computeCost(cached_distances_[i][j]); 00896 } 00897 } 00898 } 00899 00900 void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y){ 00901 //project the new origin into the grid 00902 int cell_ox, cell_oy; 00903 cell_ox = int((new_origin_x - origin_x_) / resolution_); 00904 cell_oy = int((new_origin_y - origin_y_) / resolution_); 00905 00906 //compute the associated world coordinates for the origin cell 00907 //beacuase we want to keep things grid-aligned 00908 double new_grid_ox, new_grid_oy; 00909 new_grid_ox = origin_x_ + cell_ox * resolution_; 00910 new_grid_oy = origin_y_ + cell_oy * resolution_; 00911 00912 //To save casting from unsigned int to int a bunch of times 00913 int size_x = size_x_; 00914 int size_y = size_y_; 00915 00916 //we need to compute the overlap of the new and existing windows 00917 int lower_left_x, lower_left_y, upper_right_x, upper_right_y; 00918 lower_left_x = min(max(cell_ox, 0), size_x); 00919 lower_left_y = min(max(cell_oy, 0), size_y); 00920 upper_right_x = min(max(cell_ox + size_x, 0), size_x); 00921 upper_right_y = min(max(cell_oy + size_y, 0), size_y); 00922 00923 unsigned int cell_size_x = upper_right_x - lower_left_x; 00924 unsigned int cell_size_y = upper_right_y - lower_left_y; 00925 00926 //we need a map to store the obstacles in the window temporarily 00927 unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y]; 00928 00929 //copy the local window in the costmap to the local map 00930 copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y); 00931 00932 //now we'll set the costmap to be completely unknown if we track unknown space 00933 resetMaps(); 00934 00935 //update the origin with the appropriate world coordinates 00936 origin_x_ = new_grid_ox; 00937 origin_y_ = new_grid_oy; 00938 00939 //compute the starting cell location for copying data back in 00940 int start_x = lower_left_x - cell_ox; 00941 int start_y = lower_left_y - cell_oy; 00942 00943 //now we want to copy the overlapping information back into the map, but in its new location 00944 copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); 00945 00946 //make sure to clean up 00947 delete[] local_map; 00948 } 00949 00950 bool Costmap2D::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value) { 00951 //we assume the polygon is given in the global_frame... we need to transform it to map coordinates 00952 std::vector<MapLocation> map_polygon; 00953 for(unsigned int i = 0; i < polygon.size(); ++i){ 00954 MapLocation loc; 00955 if(!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)){ 00956 ROS_DEBUG("Polygon lies outside map bounds, so we can't fill it"); 00957 return false; 00958 } 00959 map_polygon.push_back(loc); 00960 } 00961 00962 std::vector<MapLocation> polygon_cells; 00963 00964 //get the cells that fill the polygon 00965 convexFillCells(map_polygon, polygon_cells); 00966 00967 //set the cost of those cells 00968 for(unsigned int i = 0; i < polygon_cells.size(); ++i){ 00969 unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y); 00970 costmap_[index] = cost_value; 00971 } 00972 return true; 00973 } 00974 00975 void Costmap2D::polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells){ 00976 PolygonOutlineCells cell_gatherer(*this, costmap_, polygon_cells); 00977 for(unsigned int i = 0; i < polygon.size() - 1; ++i){ 00978 raytraceLine(cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y); 00979 } 00980 if(!polygon.empty()){ 00981 unsigned int last_index = polygon.size() - 1; 00982 //we also need to close the polygon by going from the last point to the first 00983 raytraceLine(cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y); 00984 } 00985 } 00986 00987 void Costmap2D::convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells){ 00988 //we need a minimum polygon of a traingle 00989 if(polygon.size() < 3) 00990 return; 00991 00992 //first get the cells that make up the outline of the polygon 00993 polygonOutlineCells(polygon, polygon_cells); 00994 00995 //quick bubble sort to sort points by x 00996 MapLocation swap; 00997 unsigned int i = 0; 00998 while(i < polygon_cells.size() - 1){ 00999 if(polygon_cells[i].x > polygon_cells[i + 1].x){ 01000 swap = polygon_cells[i]; 01001 polygon_cells[i] = polygon_cells[i + 1]; 01002 polygon_cells[i + 1] = swap; 01003 01004 if(i > 0) 01005 --i; 01006 } 01007 else 01008 ++i; 01009 } 01010 01011 i = 0; 01012 MapLocation min_pt; 01013 MapLocation max_pt; 01014 unsigned int min_x = polygon_cells[0].x; 01015 unsigned int max_x = polygon_cells[polygon_cells.size() -1].x; 01016 01017 //walk through each column and mark cells inside the polygon 01018 for(unsigned int x = min_x; x <= max_x; ++x){ 01019 if(i >= polygon_cells.size() - 1) 01020 break; 01021 01022 if(polygon_cells[i].y < polygon_cells[i + 1].y){ 01023 min_pt = polygon_cells[i]; 01024 max_pt = polygon_cells[i + 1]; 01025 } 01026 else{ 01027 min_pt = polygon_cells[i + 1]; 01028 max_pt = polygon_cells[i]; 01029 } 01030 01031 i += 2; 01032 while(i < polygon_cells.size() && polygon_cells[i].x == x){ 01033 if(polygon_cells[i].y < min_pt.y) 01034 min_pt = polygon_cells[i]; 01035 else if(polygon_cells[i].y > max_pt.y) 01036 max_pt = polygon_cells[i]; 01037 ++i; 01038 } 01039 01040 MapLocation pt; 01041 //loop though cells in the column 01042 for(unsigned int y = min_pt.y; y < max_pt.y; ++y){ 01043 pt.x = x; 01044 pt.y = y; 01045 polygon_cells.push_back(pt); 01046 01047 } 01048 } 01049 } 01050 01051 unsigned int Costmap2D::getSizeInCellsX() const{ 01052 return size_x_; 01053 } 01054 01055 unsigned int Costmap2D::getSizeInCellsY() const{ 01056 return size_y_; 01057 } 01058 01059 double Costmap2D::getSizeInMetersX() const{ 01060 return (size_x_ - 1 + 0.5) * resolution_; 01061 } 01062 01063 double Costmap2D::getSizeInMetersY() const{ 01064 return (size_y_ - 1 + 0.5) * resolution_; 01065 } 01066 01067 double Costmap2D::getOriginX() const{ 01068 return origin_x_; 01069 } 01070 01071 double Costmap2D::getOriginY() const{ 01072 return origin_y_; 01073 } 01074 01075 double Costmap2D::getResolution() const{ 01076 return resolution_; 01077 } 01078 01079 bool Costmap2D::isCircumscribedCell(unsigned int x, unsigned int y) const { 01080 unsigned char cost = getCost(x, y); 01081 if(cost < INSCRIBED_INFLATED_OBSTACLE && cost >= circumscribed_cost_lb_) 01082 return true; 01083 return false; 01084 } 01085 01086 void Costmap2D::saveMap(std::string file_name){ 01087 FILE *fp = fopen(file_name.c_str(), "w"); 01088 01089 if(!fp){ 01090 ROS_WARN("Can't open file %s", file_name.c_str()); 01091 return; 01092 } 01093 01094 fprintf(fp, "P2\n%d\n%d\n%d\n", size_x_, size_y_, 0xff); 01095 for(unsigned int iy = 0; iy < size_y_; iy++) { 01096 for(unsigned int ix = 0; ix < size_x_; ix++) { 01097 unsigned char cost = getCost(ix,iy); 01098 if (cost == LETHAL_OBSTACLE) { 01099 fprintf(fp, "255 "); 01100 } 01101 else if (cost == NO_INFORMATION){ 01102 fprintf(fp, "180 "); 01103 } 01104 else if (cost == INSCRIBED_INFLATED_OBSTACLE ) { 01105 fprintf(fp, "128 "); 01106 } 01107 else if (cost > 0){ 01108 fprintf(fp, "50 "); 01109 } 01110 else { 01111 fprintf(fp, "0 "); 01112 } 01113 } 01114 fprintf(fp, "\n"); 01115 } 01116 fclose(fp); 01117 } 01118 01119 };