00001 #include "hector_costmap.h"
00002
00003
00004 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
00005
00006 #define OCCUPIED_CELL 100
00007 #define FREE_CELL 0
00008 #define UNKNOWN_CELL -1
00009
00010 #define USE_GRID_MAP_ONLY 0
00011 #define USE_ELEVATION_MAP_ONLY 1
00012 #define USE_GRID_AND_ELEVATION_MAP 2
00013 #define USE_GRID_AND_CLOUD_MAP 3
00014 #define USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP 4
00015
00016
00017 CostMapCalculation::CostMapCalculation() : nHandle(), pnHandle("~")
00018 {
00019 int width, height;
00020 double grid_res_xy;
00021
00022 pnHandle.param("elevation_resolution", grid_res_z, 0.01);
00023
00024 pnHandle.param("max_grid_size_x", width, 1024);
00025 cost_map.info.width = width;
00026 pnHandle.param("max_grid_size_y", height, 1024);
00027 cost_map.info.height = height;
00028 pnHandle.param("map_resolution", grid_res_xy, 0.05);
00029 cost_map.info.resolution = grid_res_xy;
00030 pnHandle.param("origin_x",cost_map.info.origin.position.x, -(double)cost_map.info.width*(double)cost_map.info.resolution/2.0);
00031 pnHandle.param("origin_y",cost_map.info.origin.position.y, -(double)cost_map.info.height*(double)cost_map.info.resolution/2.0);
00032 pnHandle.param("origin_z",cost_map.info.origin.position.z, 0.0);
00033 pnHandle.param("orientation_x",cost_map.info.origin.orientation.x, 0.0);
00034 pnHandle.param("orientation_y",cost_map.info.origin.orientation.y, 0.0);
00035 pnHandle.param("orientation_z",cost_map.info.origin.orientation.z, 0.0);
00036 pnHandle.param("orientation_w",cost_map.info.origin.orientation.w, 1.0);
00037
00038 pnHandle.param("initial_free_cells_radius", initial_free_cells_radius, 0.30);
00039 pnHandle.param("update_radius", update_radius_world, 4.0);
00040 pnHandle.param("max_delta_elevation", max_delta_elevation, 0.08);
00041
00042 pnHandle.param("map_frame_id", map_frame_id,std::string("map"));
00043 pnHandle.param("local_transform_frame_id", local_transform_frame_id,std::string("base_footprint"));
00044 pnHandle.param("cost_map_topic", cost_map_topic, std::string("cost_map"));
00045 pnHandle.param("elevation_map_topic", elevation_map_topic, std::string("elevation_map_local"));
00046 pnHandle.param("grid_map_topic", grid_map_topic, std::string("scanmatcher_map"));
00047
00048 pnHandle.param("use_elevation_map", use_elevation_map, true);
00049 pnHandle.param("use_grid_map", use_grid_map, true);
00050 pnHandle.param("use_cloud_map", use_cloud_map, false);
00051 pnHandle.param("allow_elevation_map_to_clear_occupied_cells", allow_elevation_map_to_clear_occupied_cells, true);
00052 pnHandle.param("max_clear_size", max_clear_size, 2);
00053
00054 pnHandle.param("point_cloud_topic", point_cloud_topic,std::string("openni/depth/points"));
00055 pnHandle.param("slize_min_height", slize_min_height, 0.30);
00056 pnHandle.param("slize_max_height", slize_min_height, 0.35);
00057
00058 pnHandle.param("costmap_pub_freq", costmap_pub_freq, 4.0);
00059
00060 pnHandle.param("sys_msg_topic", sys_msg_topic, std::string("syscommand"));
00061
00062 cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
00063 elevation_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
00064 cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
00065
00066 world_map_transform.setTransforms(cost_map.info);
00067
00068
00069 min_index(0) = cost_map.info.width/2-floor(initial_free_cells_radius/cost_map.info.resolution);
00070 min_index(1) = cost_map.info.height/2-floor(initial_free_cells_radius/cost_map.info.resolution);
00071 max_index(0) = cost_map.info.width/2+floor(initial_free_cells_radius/cost_map.info.resolution);
00072 max_index(1) = cost_map.info.height/2+floor(initial_free_cells_radius/cost_map.info.resolution);
00073 for (int v = min_index(1); v < max_index(1); ++v)
00074 for (int u = min_index(0); u < max_index(0); ++u)
00075 cost_map.data[MAP_IDX(cost_map.info.width, u, v)] = FREE_CELL;
00076
00077 pub_cost_map = nHandle.advertise<nav_msgs::OccupancyGrid>(cost_map_topic, 1, true);
00078
00079 received_elevation_map = false;
00080 received_grid_map = false;
00081 received_point_cloud = false;
00082
00083 sliced_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
00084
00085 if(use_grid_map)
00086 sub_elevation_map = nHandle.subscribe(elevation_map_topic,10,&CostMapCalculation::callbackElevationMap,this);
00087
00088 if(use_grid_map)
00089 sub_grid_map = nHandle.subscribe(grid_map_topic,10,&CostMapCalculation::callbackGridMap,this);
00090
00091 if(use_cloud_map)
00092 sub_point_cloud = nHandle.subscribe(point_cloud_topic,10,&CostMapCalculation::callbackPointCloud,this);
00093
00094 sub_map_info = nHandle.subscribe("map_metadata",1,&CostMapCalculation::updateMapParamsCallback,this);
00095
00096 sub_sysMessage = nHandle.subscribe(sys_msg_topic, 10, &CostMapCalculation::sysMessageCallback, this);
00097 dyn_rec_server_.setCallback(boost::bind(&CostMapCalculation::dynRecParamCallback, this, _1, _2));
00098
00099 timer = pnHandle.createTimer(ros::Duration(1.0/costmap_pub_freq), &CostMapCalculation::timerCallback,this);
00100
00101 ROS_INFO("HectorCM: is up and running.");
00102
00103 pub_cloud_slice = pnHandle.advertise<sensor_msgs::PointCloud2>("cloud_slice",1,true);
00104
00105 ros::spin();
00106 }
00107
00108
00109 CostMapCalculation::~CostMapCalculation()
00110 {
00111 ROS_INFO("HectorCM: Shutting down!");
00112 }
00113
00114
00115 void CostMapCalculation::sysMessageCallback(const std_msgs::String& string)
00116 {
00117 ROS_DEBUG("HectorCM: sysMsgCallback, msg contents: %s", string.data.c_str());
00118
00119 if (string.data == "reset")
00120 {
00121 ROS_INFO("HectorCM: reset");
00122
00123
00124 cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
00125 elevation_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
00126 cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
00127
00128
00129 min_index(0) = cost_map.info.width/2-floor(initial_free_cells_radius/cost_map.info.resolution);
00130 min_index(1) = cost_map.info.height/2-floor(initial_free_cells_radius/cost_map.info.resolution);
00131 max_index(0) = cost_map.info.width/2+floor(initial_free_cells_radius/cost_map.info.resolution);
00132 max_index(1) = cost_map.info.height/2+floor(initial_free_cells_radius/cost_map.info.resolution);
00133 for (int v = min_index(1); v < max_index(1); ++v)
00134 for (int u = min_index(0); u < max_index(0); ++u)
00135 cost_map.data[MAP_IDX(cost_map.info.width, u, v)] = FREE_CELL;
00136
00137 received_elevation_map = false;
00138 received_grid_map = false;
00139 received_point_cloud = false;
00140 }
00141 }
00142
00143
00144 void CostMapCalculation::dynRecParamCallback(hector_costmap::CostMapCalculationConfig &config, uint32_t level)
00145 {
00146 max_delta_elevation = config.max_delta_elevation;
00147 allow_elevation_map_to_clear_occupied_cells = config.allow_elevation_map_to_clear_occupied_cells;
00148 max_clear_size = config.max_clear_size;
00149 slize_min_height = config.slize_min_height;
00150 slize_max_height = config.slize_max_height;
00151 }
00152
00153
00154 void CostMapCalculation::updateMapParamsCallback(const nav_msgs::MapMetaData& map_meta_data)
00155 {
00156 ROS_DEBUG("HectorCM: received new map meta data -> overwrite old parameters");
00157
00158
00159 cost_map.info.width = map_meta_data.width;
00160 cost_map.info.height = map_meta_data.height;
00161 cost_map.info.resolution = map_meta_data.resolution;
00162
00163 cost_map.info.origin.position.x = map_meta_data.origin.position.x;
00164 cost_map.info.origin.position.y = map_meta_data.origin.position.y;
00165 cost_map.info.origin.position.z = map_meta_data.origin.position.z;
00166
00167 cost_map.info.origin.orientation.x = map_meta_data.origin.orientation.x;
00168 cost_map.info.origin.orientation.y = map_meta_data.origin.orientation.y;
00169 cost_map.info.origin.orientation.z = map_meta_data.origin.orientation.z;
00170 cost_map.info.origin.orientation.w = map_meta_data.origin.orientation.w;
00171
00172 world_map_transform.setTransforms(cost_map.info);
00173
00174
00175 cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
00176 elevation_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
00177 cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
00178
00179
00180 min_index(0) = cost_map.info.width/2-floor(initial_free_cells_radius/cost_map.info.resolution);
00181 min_index(1) = cost_map.info.height/2-floor(initial_free_cells_radius/cost_map.info.resolution);
00182 max_index(0) = cost_map.info.width/2+floor(initial_free_cells_radius/cost_map.info.resolution);
00183 max_index(1) = cost_map.info.height/2+floor(initial_free_cells_radius/cost_map.info.resolution);
00184 for (int v = min_index(1); v < max_index(1); ++v)
00185 for (int u = min_index(0); u < max_index(0); ++u)
00186 cost_map.data[MAP_IDX(cost_map.info.width, u, v)] = FREE_CELL;
00187
00188
00189 received_elevation_map = false;
00190 received_grid_map = false;
00191 use_cloud_map = false;
00192 }
00193
00194 void CostMapCalculation::timerCallback(const ros::TimerEvent& event)
00195 {
00196 ROS_DEBUG("HectorCM: published a new costmap");
00197
00198
00199 cost_map.header.stamp = ros::Time::now();
00200 cost_map.header.frame_id = map_frame_id;
00201
00202 pub_cost_map.publish(cost_map);
00203 }
00204
00205
00206 void CostMapCalculation::callbackElevationMap(const hector_elevation_msgs::ElevationGridConstPtr& elevation_map_msg)
00207 {
00208 ROS_DEBUG("HectorCM: received new elevation map");
00209
00210
00211 if((int)(1000*elevation_map_msg->info.resolution_xy) != (int)(1000*cost_map.info.resolution) &&
00212 elevation_map_msg->info.height != cost_map.info.height &&
00213 elevation_map_msg->info.width != cost_map.info.width)
00214 {
00215 ROS_ERROR("HectorCM: elevation map resolution and or size incorrect!");
00216 return;
00217 }
00218
00219
00220 elevation_map_ = cv::Mat(elevation_map_msg->info.height, elevation_map_msg->info.width, CV_16S, const_cast<int16_t*>(&elevation_map_msg->data[0]), 2*(size_t)elevation_map_msg->info.width);
00221
00222
00223 elevation_zero_level = elevation_map_msg->info.zero_elevation;
00224
00225
00226 if(!computeWindowIndices(elevation_map_msg->header.stamp, update_radius_world))
00227 return;
00228
00229
00230 int filtered_cell, filtered_cell_x, filtered_cell_y;
00231 for (int v = min_index(1); v < max_index(1); ++v)
00232 {
00233 for (int u = min_index(0); u < max_index(0); ++u)
00234 {
00235
00236 unsigned int cost_map_index = MAP_IDX(cost_map.info.width, u, v);
00237
00238
00239 if(elevation_map_.at<int16_t>(v+1,u) == (-elevation_zero_level) ||
00240 elevation_map_.at<int16_t>(v-1,u) == (-elevation_zero_level) ||
00241 elevation_map_.at<int16_t>(v,u+1) == (-elevation_zero_level) ||
00242 elevation_map_.at<int16_t>(v,u-1) == (-elevation_zero_level))
00243 continue;
00244
00245
00246 filtered_cell_y = abs(elevation_map_.at<int16_t>(v,u-1) - elevation_map_.at<int16_t>(v,u+1));
00247 filtered_cell_x = abs(elevation_map_.at<int16_t>(v-1,u) - elevation_map_.at<int16_t>(v+1,u));
00248
00249
00250 if(filtered_cell_x > filtered_cell_y)
00251 filtered_cell = filtered_cell_x;
00252 else
00253 filtered_cell = filtered_cell_y;
00254
00255
00256 if(filtered_cell > max_delta_elevation/grid_res_z)
00257 {
00258
00259 elevation_cost_map.data[cost_map_index] = OCCUPIED_CELL;
00260 }
00261 else
00262 {
00263
00264 elevation_cost_map.data[cost_map_index] = FREE_CELL;
00265 }
00266 }
00267 }
00268
00269
00270 received_elevation_map = true;
00271
00272
00273 if(received_grid_map)
00274 {
00275 if(received_point_cloud)
00276 {
00277 calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP);
00278 }
00279 else
00280 {
00281 calculateCostMap(USE_GRID_AND_ELEVATION_MAP);
00282 }
00283 }
00284 else
00285 {
00286 calculateCostMap(USE_ELEVATION_MAP_ONLY);
00287 }
00288 }
00289
00290 void CostMapCalculation::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
00291 {
00292 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sensor (new pcl::PointCloud<pcl::PointXYZ>);
00293 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_base_link (new pcl::PointCloud<pcl::PointXYZ>);
00294
00295 ROS_DEBUG("HectorCM: received new point cloud");
00296
00297
00298 pcl::fromROSMsg(*cloud_msg, *cloud_sensor);
00299
00300
00301 try
00302 {
00303 listener.waitForTransform("base_stabilized", cloud_msg->header.frame_id,cloud_sensor->header.stamp,ros::Duration(1.0));
00304 pcl_ros::transformPointCloud("base_stabilized",*cloud_sensor,*cloud_base_link,listener);
00305 }
00306 catch (tf::TransformException ex)
00307 {
00308 ROS_ERROR("%s",ex.what());
00309 ROS_DEBUG("HectorCM: pointcloud transform failed");
00310 return;
00311 }
00312
00313
00314 if(!computeWindowIndices(cloud_msg->header.stamp, update_radius_world))
00315 return;
00316
00317 Eigen::Vector2f world_coords;
00318
00319 Eigen::Vector4f minPoint;
00320 world_coords = world_map_transform.getC1Coords(min_index.cast<float>());
00321 minPoint[0]=world_coords(0);
00322 minPoint[1]=world_coords(1);
00323 minPoint[2]=slize_min_height;
00324
00325 Eigen::Vector4f maxPoint;
00326 world_coords = world_map_transform.getC1Coords(max_index.cast<float>());
00327 maxPoint[0]=world_coords(0);
00328 maxPoint[1]=world_coords(1);
00329 maxPoint[2]=slize_max_height;
00330
00331 pcl::CropBox<pcl::PointXYZ> cropFilter;
00332 cropFilter.setInputCloud (cloud_base_link);
00333 cropFilter.setMin(minPoint);
00334 cropFilter.setMax(maxPoint);
00335 cropFilter.filter (*sliced_cloud);
00336
00337 ROS_DEBUG("HectorCM: slice size: %d", (int)sliced_cloud->size());
00338 pub_cloud_slice.publish(sliced_cloud);
00339
00340 cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
00341
00342
00343 for (unsigned int k = 0; k < sliced_cloud->size(); ++k)
00344 {
00345 const pcl::PointXYZ& pt_cloud = sliced_cloud->points[k];
00346
00347
00348 Eigen::Vector2f index_world(pt_cloud.x, pt_cloud.y);
00349 Eigen::Vector2f index_map (world_map_transform.getC2Coords(index_world));
00350
00351 cloud_cost_map.data[MAP_IDX(cost_map.info.width, (int)round(index_map(0)), (int)round(index_map(1)))] = OCCUPIED_CELL;
00352 }
00353
00354
00355 received_point_cloud = true;
00356
00357
00358 if(received_grid_map)
00359 {
00360 if(received_elevation_map)
00361 {
00362 calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP);
00363 }
00364 else
00365 {
00366 calculateCostMap(USE_GRID_AND_CLOUD_MAP);
00367 }
00368 }
00369 }
00370
00371 void CostMapCalculation::callbackGridMap(const nav_msgs::OccupancyGridConstPtr& grid_map_msg)
00372 {
00373 ROS_DEBUG("HectorCM: received new grid map");
00374
00375
00376 if((int)(1000*grid_map_msg->info.resolution) != (int)(1000*cost_map.info.resolution) &&
00377 grid_map_msg->info.height != cost_map.info.height &&
00378 grid_map_msg->info.width != cost_map.info.width)
00379 {
00380 ROS_ERROR("HectorCM: grid map resolution and or size incorrect!");
00381 return;
00382 }
00383
00384 grid_map_msg_ = grid_map_msg;
00385
00386
00387 grid_map_ = cv::Mat(grid_map_msg_->info.height, grid_map_msg_->info.width, CV_8S, const_cast<int8_t*>(&grid_map_msg_->data[0]), (size_t)grid_map_msg_->info.width);
00388
00389
00390 received_grid_map = true;
00391
00392
00393 if(!computeWindowIndices(grid_map_msg->header.stamp, update_radius_world))
00394 return;
00395
00396
00397 if(received_elevation_map)
00398 {
00399 if(received_point_cloud)
00400 {
00401 calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP);
00402 }
00403 else
00404 {
00405 calculateCostMap(USE_GRID_AND_ELEVATION_MAP);
00406 }
00407 }
00408 else
00409 {
00410 if(received_point_cloud)
00411 {
00412 calculateCostMap(USE_GRID_AND_CLOUD_MAP);
00413 }
00414 else
00415 {
00416 calculateCostMap(USE_GRID_MAP_ONLY);
00417 }
00418 }
00419 }
00420
00421 bool CostMapCalculation::calculateCostMap(char map_level)
00422 {
00423 switch(map_level)
00424 {
00425 case USE_GRID_MAP_ONLY:
00426 {
00427
00428
00429 ROS_DEBUG("HectorCM: compute costmap based on grid map");
00430
00431
00432 for (int v = min_index(1); v < max_index(1); ++v)
00433 {
00434 for (int u = min_index(0); u < max_index(0); ++u)
00435 {
00436 unsigned int index = MAP_IDX(cost_map.info.width, u, v);
00437
00438
00439 if((char)grid_map_.data[index] != UNKNOWN_CELL)
00440 {
00441 if(grid_map_.data[index] == OCCUPIED_CELL)
00442 {
00443
00444 cost_map.data[index] = OCCUPIED_CELL;
00445 }
00446 else
00447 {
00448
00449 cost_map.data[index] = FREE_CELL;
00450 }
00451 }
00452 }
00453 }
00454
00455 break;
00456 }
00457 case USE_ELEVATION_MAP_ONLY:
00458 {
00459
00460
00461 ROS_DEBUG("HectorCM: compute costmap based on elevation map");
00462
00463
00464
00465 for (int v = min_index(1); v < max_index(1); ++v)
00466 {
00467 for (int u = min_index(0); u < max_index(0); ++u)
00468 {
00469 unsigned int index = MAP_IDX(cost_map.info.width, u, v);
00470
00471
00472 if(elevation_cost_map.data[index] != UNKNOWN_CELL)
00473 {
00474 if(elevation_cost_map.data[index] == OCCUPIED_CELL)
00475 {
00476
00477 cost_map.data[index] = OCCUPIED_CELL;
00478 }
00479 else
00480 {
00481
00482 cost_map.data[index] = FREE_CELL;
00483 }
00484 }
00485 }
00486 }
00487
00488 break;
00489 }
00490 case USE_GRID_AND_ELEVATION_MAP:
00491 {
00492
00493
00494 ROS_DEBUG("HectorCM: compute costmap based on grid and elevation map");
00495
00496 int checksum_grid_map;
00497
00498
00499 for (int v = min_index(1); v < max_index(1); ++v)
00500 {
00501 for (int u = min_index(0); u < max_index(0); ++u)
00502 {
00503 unsigned int index = MAP_IDX(cost_map.info.width, u, v);
00504
00505
00506 if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL)
00507 {
00508 if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || elevation_cost_map.data[index] == OCCUPIED_CELL)
00509 {
00510 checksum_grid_map = 0;
00511
00512 checksum_grid_map += grid_map_.at<int8_t>(v-1, u);
00513 checksum_grid_map += grid_map_.at<int8_t>(v+1, u);
00514 checksum_grid_map += grid_map_.at<int8_t>(v, u-1);
00515 checksum_grid_map += grid_map_.at<int8_t>(v, u+1);
00516 checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1);
00517 checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1);
00518 checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1);
00519 checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1);
00520
00521 if(elevation_cost_map.data[index] == FREE_CELL && allow_elevation_map_to_clear_occupied_cells)
00522 {
00523 if(checksum_grid_map <= max_clear_size*OCCUPIED_CELL)
00524 {
00525
00526 cost_map.data[index] = FREE_CELL;
00527 }
00528 else
00529 {
00530
00531 cost_map.data[index] = OCCUPIED_CELL;
00532 }
00533 }
00534 else
00535 {
00536
00537 cost_map.data[index] = OCCUPIED_CELL;
00538 }
00539 }
00540 else
00541 {
00542 cost_map.data[index] = FREE_CELL;
00543 }
00544 }
00545 }
00546 }
00547 break;
00548 }
00549 case USE_GRID_AND_CLOUD_MAP:
00550 {
00551
00552
00553 ROS_DEBUG("HectorCM: compute costmap based on grid and cloud map");
00554
00555
00556 for (int v = min_index(1); v < max_index(1); ++v)
00557 {
00558 for (int u = min_index(0); u < max_index(0); ++u)
00559 {
00560 unsigned int index = MAP_IDX(cost_map.info.width, u, v);
00561
00562
00563 if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL)
00564 {
00565 if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || cloud_cost_map.data[index] == OCCUPIED_CELL)
00566 {
00567
00568 cost_map.data[index] = OCCUPIED_CELL;
00569 }
00570 else
00571 {
00572 cost_map.data[index] = FREE_CELL;
00573 }
00574 }
00575 }
00576 }
00577 break;
00578 }
00579
00580 case USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP:
00581 {
00582
00583
00584 ROS_DEBUG("HectorCM: compute costmap based on grid, cloud and elevation map");
00585
00586 int checksum_grid_map;
00587
00588
00589 for (int v = min_index(1); v < max_index(1); ++v)
00590 {
00591 for (int u = min_index(0); u < max_index(0); ++u)
00592 {
00593 unsigned int index = MAP_IDX(cost_map.info.width, u, v);
00594
00595
00596 if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL)
00597 {
00598 if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || elevation_cost_map.data[index] == OCCUPIED_CELL)
00599 {
00600 checksum_grid_map = 0;
00601
00602 checksum_grid_map += grid_map_.at<int8_t>(v-1, u);
00603 checksum_grid_map += grid_map_.at<int8_t>(v+1, u);
00604 checksum_grid_map += grid_map_.at<int8_t>(v, u-1);
00605 checksum_grid_map += grid_map_.at<int8_t>(v, u+1);
00606 checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1);
00607 checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1);
00608 checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1);
00609 checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1);
00610
00611 if(elevation_cost_map.data[index] == FREE_CELL && allow_elevation_map_to_clear_occupied_cells)
00612 {
00613 if(checksum_grid_map <= max_clear_size*OCCUPIED_CELL)
00614 {
00615
00616 cost_map.data[index] = FREE_CELL;
00617 }
00618 else
00619 {
00620
00621 cost_map.data[index] = OCCUPIED_CELL;
00622 }
00623 }
00624 else
00625 {
00626
00627 cost_map.data[index] = OCCUPIED_CELL;
00628 }
00629 }
00630 else
00631 {
00632 if(cloud_cost_map.data[index] == OCCUPIED_CELL)
00633 {
00634
00635 cost_map.data[index] = OCCUPIED_CELL;
00636 }
00637 else
00638 {
00639
00640 cost_map.data[index] = FREE_CELL;
00641 }
00642
00643 }
00644 }
00645 }
00646 }
00647 break;
00648 }
00649 }
00650
00651 ROS_DEBUG("HectorCM: computed a new costmap");
00652
00653 return true;
00654 }
00655
00656 bool CostMapCalculation::computeWindowIndices(ros::Time time,double update_radius)
00657 {
00658 int update_radius_map;
00659 Eigen::Vector2f index_world, index_map;
00660
00661
00662
00663 tf::StampedTransform transform;
00664
00665
00666 try
00667 {
00668 listener.waitForTransform(map_frame_id, local_transform_frame_id, time, ros::Duration(5));
00669 listener.lookupTransform(map_frame_id, local_transform_frame_id, time, transform);
00670 }
00671 catch (tf::TransformException ex)
00672 {
00673 ROS_ERROR("%s",ex.what());
00674 return false;
00675 }
00676
00677 index_world(0) = transform.getOrigin().x();
00678 index_world(1) = transform.getOrigin().y();
00679 index_map = world_map_transform.getC2Coords(index_world);
00680 update_radius_map = floor(((double)update_radius/(double)cost_map.info.resolution));
00681
00682
00683
00684 if(index_map(1) < update_radius_map)
00685 min_index(1) = 0;
00686 else
00687 min_index(1) = index_map(1) - update_radius_map;
00688
00689 if(index_map(1) + update_radius_map > cost_map.info.height)
00690 max_index(1) = cost_map.info.height;
00691 else
00692 max_index(1) = index_map(1) + update_radius_map;
00693
00694 if(index_map(0) < update_radius_map)
00695 min_index(0) = 0;
00696 else
00697 min_index(0) = index_map(0) - update_radius_map;
00698
00699 if(index_map(0) + update_radius_map > cost_map.info.width)
00700 max_index(0) = cost_map.info.width;
00701 else
00702 max_index(0) = index_map(0) + update_radius_map;
00703
00704 return true;
00705 }
00706