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


hector_costmap
Author(s): Thorsten Graber
autogenerated on Mon Oct 6 2014 00:28:32