Go to the documentation of this file.00001
00019 #include <ros/ros.h>
00020
00021 #include <ftc_local_planner/join_costmap.h>
00022
00023 namespace ftc_local_planner
00024 {
00025 JoinCostmap::JoinCostmap()
00026 {
00027 }
00028
00029 void JoinCostmap::initialize(costmap_2d::Costmap2DROS* local_costmap_ros, costmap_2d::Costmap2DROS* global_costmap_ros){
00030 local_costmap_ros_ = local_costmap_ros;
00031 global_costmap_ros_ = global_costmap_ros;
00032
00033 if(local_costmap_ros_->getCostmap()->getResolution() < global_costmap_ros->getCostmap()->getResolution()){
00034 ROS_ERROR("JoinCostmap: Resolution of local costmap is higher than global costmap resolution!");
00035 init = false;
00036 }
00037
00038 global.resize(global_costmap_ros_->getCostmap()->getSizeInCellsX());
00039 for (unsigned int i = 0; i < global_costmap_ros_->getCostmap()->getSizeInCellsX(); ++i){
00040 global[i].resize(global_costmap_ros_->getCostmap()->getSizeInCellsY());
00041 }
00042
00043
00044 for(unsigned int i = 0; i < global_costmap_ros_->getCostmap()->getSizeInCellsX(); i++){
00045 for(unsigned int j = 0; j < global_costmap_ros_->getCostmap()->getSizeInCellsY(); j++){
00046 global[i][j] = global_costmap_ros_->getCostmap()->getCost(i,j);
00047 }
00048 }
00049 ROS_INFO("JoinCostmap: Initalize.");
00050 init = true;
00051 }
00052
00053 void JoinCostmap::joinMaps(){
00054 if(!init){
00055 ROS_WARN("JoinCostmap: Dont join costmap, because init faild.");
00056 }else{
00057 for(unsigned int i = 0; i < global_costmap_ros_->getCostmap()->getSizeInCellsX(); i++){
00058 for(unsigned int j = 0; j < global_costmap_ros_->getCostmap()->getSizeInCellsY(); j++){
00059 double wx = 0;
00060 double wy = 0;
00061 global_costmap_ros_->getCostmap()->mapToWorld(i,j, wx, wy);
00062
00063 unsigned int mx = 0;
00064 unsigned int my = 0;
00065 local_costmap_ros_->getCostmap()->worldToMap(wx, wy, mx, my);
00066
00067
00068 if(global[i][j] < local_costmap_ros_->getCostmap()->getCost(mx, my)){
00069 global_costmap_ros_->getCostmap()->setCost(i,j,local_costmap_ros_->getCostmap()->getCost(mx, my));
00070 }else{
00071 global_costmap_ros_->getCostmap()->setCost(i,j,global[i][j]);
00072 }
00073 }
00074 }
00075 }
00076 }
00077
00078 }