mapmerger.cpp
Go to the documentation of this file.
00001 #include "mapmerger.h"
00002 #include "numeric"
00003 #include "unistd.h"
00004 #include <boost/filesystem.hpp>
00005 #include <fstream>
00006 #include <cerrno>
00007 #include "adhoc_communication/MmPoint.h"
00008 
00009 
00010 
00011 //#define DEA_OPT_MAP_CHANGED
00012 //#define DEA_OPT_PARTIAL_MERGE
00013 //#define DEBUG
00014 MapMerger::MapMerger()
00015 {    
00016     pos_seq_my = 0;
00017     pos_pub_other = new std::vector<ros::Publisher>();
00018     pos_array_other = new std::vector<visualization_msgs::MarkerArray>();
00019     pos_seq_other = new std::vector<int>();
00020 
00021     global_map = NULL;
00022     local_map = NULL;
00023     local_map_old = NULL;
00024     updateMan = new updateManager();
00025 
00026     positions = new adhoc_communication::MmListOfPoints();
00027     //since_last_trans_try = 0;
00028     map_data = new std::vector<nav_msgs::OccupancyGrid*>();
00029     robots = new std::vector<std::string>();
00030     transforms = new std::vector<cv::Mat>();
00031     new_data_maps = new std::vector<bool>();
00032     robots_in_transform = new std::vector<std::string>();
00033     robots_position_publisher = new std::vector<ros::Publisher>();
00034     local_map = new nav_msgs::OccupancyGrid();
00035     local_map_old = new nav_msgs::OccupancyGrid();
00036     update_list = new std::vector<UpdateEntry*>();
00037     //get the ros parameters, or set them to their default value
00038     //for testing here in codes
00039     laser_range = 10;
00040     force_recompute_all = false;
00041     local_map_new_data = false;
00042     global_map_ready = true;
00043     cur_position = new geometry_msgs::PoseStamped();
00044     map_width = -1;
00045     map_height = -1;
00046     update_seq = 0;
00047     //ros::nodeHandle nodeHandle ("~");
00048     nodeHandle = new ros::NodeHandle("~");
00049     nodeHandle->param<std::string>("robot_name",robot_name,"");
00050     if(robot_name.empty())
00051     {
00052         char hostname_c[1024];
00053         hostname_c[1023] = '\0';
00054         int status = gethostname(hostname_c, 1023);
00055         if(status == -1)
00056             ROS_ERROR("Cannot determine hostname. ERROR: %s", strerror(errno));
00057         else
00058             robot_name = std::string(hostname_c);
00059     }
00060     nodeHandle->param<bool>("splitted",splitted,true);;
00061     nodeHandle->param<bool>("debug",debug,true);
00062     nodeHandle->param<bool>("has_local_map",has_local_map,true);
00063     nodeHandle->param<bool>("exchange_position",exchange_position,false);
00064 
00065     nodeHandle->param<int>("split_size",size,32);
00066     nodeHandle->param<int>("seconds_meta_timer",seconds_meta_timer,5);
00067     nodeHandle->param<int>("seconds_pub_timer",seconds_publish_timer,3);
00068     nodeHandle->param<int>("seconds_send_timer",seconds_send_timer,5);
00069     nodeHandle->param<int>("seconds_recompute_transform",seconds_recompute_transform,5);
00070     nodeHandle->param<int>("seconds_send_position",seconds_send_position,2);
00071     nodeHandle->param<double>("max_trans_robots",max_trans_robot,-1);
00072     nodeHandle->param<double>("max_rotation_robots",max_rotation_robot,5);
00073 
00074     nodeHandle->param<std::string>("local_map_topic",local_map_topic,"/map");
00075     nodeHandle->param<std::string>("local_map_metadata_topic",local_map_metadata_topic,"/map_metadata");
00076     nodeHandle->param<std::string>("local_map_frame_id",local_map_frame_id,"map");
00077     //nodeHandle->param<std::string>("meta_topic_over_network",map_meta_topic,"/map_meta");
00078     nodeHandle->param<std::string>("map_topic_over_network",topic_over_network,"map_other");
00079     nodeHandle->param<std::string>("position_local_robot_topic",position_local_robot_topic,"slam_out_pose");
00080     nodeHandle->param<std::string>("position_other_robots_topic",position_other_robots_topic,"position_other_robots");
00081     nodeHandle->param<std::string>("control_topic",control_topic,"control");
00082     nodeHandle->param<std::string>("robot_prefix",robot_prefix,"");
00083     nodeHandle->param<std::string>("log_path",log_path,"");
00084 
00085     time_start = ros::Time::now();
00086    /* if(has_local_map == false)
00087         robot_name = "BASE";
00088     robot_hostname = robot_name;*/
00089     //DEBUG
00090 #ifdef DEBUG
00091 
00092     //robot_name="robot_1";
00093    // robot_prefix="/robot_1";
00094    // local_map_topic = "/robot_1/map";
00095    // local_map_metadata_topic="/robot_1/map_metadata";
00096     //map_meta_topic="map_meta";
00097    // local_map_frame_id="robot_1/map";
00098    // topic_over_network = "/robot_1/map_other";
00099    // position_other_robots_topic = "/robot_1/position_other_robots";
00100     splitted = true;
00101     size=2048;
00102     has_local_map = false;
00103     map_width = 2976;
00104     map_height = 2976;
00105     /*
00106     robot_name="robot_0";
00107     robot_prefix="/robot_0";
00108     local_map_topic = "/robot_0/map";
00109     local_map_metadata_topic="/robot_0/map_metadata";
00110     map_meta_topic="map_meta";
00111     local_map_frame_id="robot_0/map";
00112     topic_over_network = "/robot_0/map_other";
00113     position_other_robots_topic = "/robot_0/position_other_robots";
00114     splitted = true;
00115     size=688;*
00116     has_local_map = false;
00117     robot_name = "BASE";
00118     robot_prefix = "/robot_2";
00119     robot_hostname = "robot_2";*/
00120 #endif
00121     //END DBEUG
00122 
00123     ROS_INFO("Local map topic:%s",local_map_topic.c_str());
00124     ROS_INFO("Local map meta data:%s",local_map_metadata_topic.c_str());
00125     ROS_INFO("Width:%i",map_width);
00126     ROS_INFO("Height:%i",map_height);
00127     ROS_INFO("Splitted:%i",splitted);
00128     ROS_INFO("Has local map:%i",has_local_map);
00129     ROS_INFO("Split size:%i",size);
00130     ROS_INFO("Robot name:%s",robot_name.c_str());
00131     ROS_INFO("Prefix:%s",robot_prefix.c_str());
00132     ROS_INFO("Local Map Frame:%s",local_map_frame_id.c_str());
00133     robot_hostname = robot_name;
00134 
00135 }
00136 void MapMerger::waitForLocalMetaData()
00137 {
00138     //ros::NodeHandle nodeHandle;
00139     ROS_INFO("Wait for map_meta_data, on topic:[%s]",local_map_metadata_topic.c_str());
00140     ros::Subscriber  sub = nodeHandle->subscribe(local_map_metadata_topic,1000,&MapMerger::callback_map_meta_data_local,this);
00141     while(ros::ok())
00142     {
00143        ros::spinOnce();
00144        ROS_DEBUG("No map_meta information");
00145        ros::Duration(0.3).sleep();
00146        if(this->map_height != -1 && this->map_width != -1)
00147             break;
00148     }
00149     ROS_INFO("Got map_meta_data");
00150     if(!splitted)
00151     {
00152         size = map_height;
00153     }
00154 }
00155 
00156 void MapMerger::waitForRobotInformation()
00157 {
00158     ROS_INFO("Wait to get a map from other robot");
00159     ros::Subscriber sub =nodeHandle->subscribe(robot_prefix+"/adhoc_communication/new_robot",
00160                                                1000,&MapMerger::callback_got_robot_for_data,
00161 
00162                                                this);
00163 
00164 
00165     ros::ServiceClient getNeighborsClient = nodeHandle->serviceClient<adhoc_communication::GetNeighbors>
00166             (robot_prefix+"/adhoc_communication/get_neighbors");
00167     adhoc_communication::GetNeighbors getNeighbors;
00168     ros::Duration(0.1).sleep();
00169     ros::AsyncSpinner spinner(10);
00170     spinner.start();
00171     ros::Duration(3).sleep();
00172     if(getNeighborsClient.call(getNeighbors))
00173     {
00174         ROS_INFO("%lu robots are already in the network, adding them,my name:%s",getNeighbors.response.neigbors.size(),
00175                  robot_name.c_str());
00176         if(getNeighbors.response.neigbors.size() > 0)
00177         {
00178             for(int i = 0; i < getNeighbors.response.neigbors.size();i++)
00179             {
00180                 std::string newRobotName = getNeighbors.response.neigbors.at(i);
00181                 if(newRobotName == robot_name)
00182                 {
00183                     ROS_WARN("Got my own name as a neigbor!");
00184                     continue;
00185                 }
00186                 else
00187                 {
00188                     if(has_local_map == false)
00189                         {
00190                             robot_name = newRobotName;
00191                             ROS_INFO("now using %s as name",robot_name.c_str());
00192                             return;
00193                         }
00194                 }
00195             }
00196         }
00197         else
00198         {
00199             ROS_INFO("No robots are were in the network before me");
00200         }
00201     }
00202     while(ros::ok())
00203     {
00204         ROS_INFO("No robot information");
00205         ros::spinOnce();
00206         ros::Duration(0.3).sleep();
00207         if(robot_name != "BASE")
00208             break;
00209     }
00210 }
00211 
00212 void MapMerger::callback_got_robot_for_data(const std_msgs::StringConstPtr &msg)
00213 {
00214     ROS_INFO("Using %s as pseudo local map",msg.get()->data.c_str());
00215     this->robot_name = msg.get()->data;
00216 }
00217 void MapMerger::callback_new_robot(const std_msgs::StringConstPtr &msg)
00218 {
00219     std::string newRobotName = msg.get()->data;
00220 
00221     for(int i = 0; i < robots->size(); i++)
00222     {
00223         if(robots->at(i) == newRobotName)
00224         {
00225             ROS_DEBUG("Already have that robot,sending last update to him");
00226             int h = update_list->size() -1;
00227             if(h < 0)
00228                 h = 0;
00229             if(update_list->size() <= h)
00230                 return;
00231             UpdateEntry* lastUpdate = update_list->at(h);
00232 
00233             std::vector<int>* lUpdate = new std::vector<int>();
00234             lUpdate->push_back(update_seq);
00235             sendMapOverNetwork(newRobotName,lUpdate,lastUpdate->getMinX(),lastUpdate->getMaxX(),lastUpdate->getMinY(),lastUpdate->getMaxX());
00236             delete lUpdate;
00237             return;
00238         }
00239     }
00240     robots->push_back(newRobotName);
00241     ROS_INFO("New robot:%s",newRobotName.c_str());
00242     updateMan->addNewUpdateList();
00243     makeEmptyMapData(newRobotName,map_height,map_width,local_map->info.resolution);
00244     ROS_INFO("asking %s for map in new Robot",newRobotName.c_str());
00245     std::vector<int>* empty = new std::vector<int>();
00246     //twice to be sure and sleep to prevent problems
00247     //first message might not be pbulished todo
00248     ROS_INFO("Send Control Message in new Robot");
00249     sendControlMessage(empty,newRobotName);
00250     //todo: send maps of other robots
00251     /*
00252     for(int i = 0; i < map_data->size(); i++)
00253         sendMapOverNetwork(newRobotName,updateMan->getUpdateListOfrobot(0));*/
00254     delete empty;
00255 }
00256 
00257 void MapMerger::callback_remove_robot(const std_msgs::StringConstPtr &msg)
00258 {
00259 }
00260 
00261 void MapMerger::callback_control(const adhoc_communication::MmControlConstPtr &msg)
00262 {
00263     ROS_DEBUG("Got a control message");
00264     adhoc_communication::MmControl tmp = *msg.get();
00265     if(tmp.update_numbers.size() == 0)
00266     {
00267         int min_x  = 9999;
00268         int min_y = 9999;
00269         int max_x = 0;
00270         int max_y = 0;
00271         //+=2 because nearly no differnce to +=1 but lowers number
00272         //of iterations per 75%
00273         for(int row = 0; row < local_map->info.height;row+=2)
00274         {
00275             for(int collum = 0; collum < local_map->info.width;collum+=2)
00276             {
00277                 int index = row * local_map->info.width + collum;
00278                 if(local_map->data[index]!= -1)
00279                 {
00280                     if(min_x > row)
00281                         min_x = row;
00282                     if(min_y > collum)
00283                         min_y = collum;
00284                     if(max_x < row)
00285                         max_x = row;
00286                     if(max_y < collum)
00287                         max_y = collum;
00288                 }
00289             }
00290         }
00291         min_x -= 5;
00292         min_y -= 5;
00293         max_x += 5;
00294         max_y += 5;
00295         if(min_x < 0)
00296             min_x = 0;
00297         if(min_y < 0)
00298             min_y = 0;
00299         if(max_x > map_height)
00300             max_x = map_height;
00301         if(max_y > map_width)
00302             max_y = map_width;
00303         //todo check if valid min/max values
00304         ROS_DEBUG("Send map after control minx:%i \t maxx:%i \t miny:%i \t maxy:%i",min_x,max_x,min_y,max_y);
00305         ROS_DEBUG("MAXX:%i",max_x);
00306         ROS_DEBUG("MINY:%i",min_y);
00307         std::vector<int> * containedUpdates = new std::vector<int>();
00308         for(int i = 0; i <= update_seq;i++)
00309             containedUpdates->push_back(i);
00310         sendMapOverNetwork(tmp.src_robot,containedUpdates,min_x,min_y,max_x,max_y);
00311         //sendMapOverNetwork(tmp.src_robot,containedUpdates);
00312         ROS_INFO("Sended map after beeing asked to %s,containd %lu updates,update_seq:%i",tmp.src_robot.c_str(),containedUpdates->size(),update_seq);
00313         delete containedUpdates;
00314     }
00315     else
00316     {
00317         if(tmp.update_numbers.at(0) == -1)
00318             return;
00319         int min_x = map_width;
00320         int min_y = map_height;
00321         int max_x = 0;
00322         int max_y = 0;
00323 
00324         for(int i = 0; i < tmp.update_numbers.size();i++)
00325         {
00326             UpdateEntry *  cur = update_list->at(tmp.update_numbers.at(i));
00327             if(max_x < cur->getMaxX())
00328                 max_x = cur->getMaxX();
00329             if(max_y < cur->getMaxY())
00330                 max_y = cur->getMaxY();
00331             if(min_x > cur->getMinX())
00332                 min_x = cur->getMinX();
00333             if(min_y > cur->getMinY())
00334                 min_y = cur->getMinY();
00335         }
00336         ROS_INFO("Sending %lu updates back to %s\n\t\tminX:%i\tminY:%i\tmaxX:%i\tmaxY:%i",tmp.update_numbers.size(),tmp.src_robot.c_str(),min_x,min_y,max_x,max_y);
00337         for(int i = 0; i < tmp.update_numbers.size();i++)
00338             ROS_INFO("Send %i",tmp.update_numbers.at(i));
00339         sendMapOverNetwork(tmp.src_robot,&tmp.update_numbers,min_x,min_y,max_x,max_y);
00340     }
00341 }
00342 
00343 void MapMerger::callback_map_meta_data_local(const nav_msgs::MapMetaData::ConstPtr &msg)
00344 {
00345     ROS_DEBUG("Got local map meta data");
00346     map_width = msg.get()->width;
00347     map_height = msg.get()->height;
00348 
00349     g_start_x = msg.get()->origin.position.x;
00350     g_start_y = msg.get()->origin.position.y;
00351 
00352     if(!has_local_map)
00353     {
00354         local_map = new nav_msgs::OccupancyGrid();
00355         local_map->info.height = msg.get()->height;
00356         local_map->info.width = msg.get()->width;
00357         local_map->info.origin.orientation.w = msg.get()->origin.orientation.w;
00358         local_map->info.resolution = msg.get()->resolution;
00359 
00360         global_map = new nav_msgs::OccupancyGrid();
00361         global_map->info.height = msg.get()->height;
00362         global_map->info.width = msg.get()->width;
00363         global_map->info.origin.orientation.w = msg.get()->origin.orientation.w;
00364         global_map->info.resolution = msg.get()->resolution;
00365 
00366         for(int i = 0; i < map_height * map_width; i++)
00367         {
00368                         global_map->data.push_back(-1);
00369                         local_map->data.push_back(-1);
00370         }
00371     }
00372 }
00373 
00374 void MapMerger::callback_global_pub(const ros::TimerEvent &e)
00375 {
00376    // sendMetaData(5);
00377     if(robots->size() == 0)
00378     {
00379         ROS_INFO("No other robots, publishing local map");
00380         if(local_map != NULL && local_map->data.size() > 0)
00381             pub.publish(*local_map);
00382         return;
00383     }
00384     if(transforms->size() != map_data->size() && local_map != NULL )
00385     {
00386         for(int i = 0; i < robots->size(); i++)
00387         {
00388             if(findTransformIndex(i) == -1 && new_data_maps->at(i) == true)
00389             {
00390                 //here is something terrible wrong
00391                 //todo fix it.
00392                 ROS_DEBUG("Computing Transform, robotIndex:%i",i);
00393                 computeTransform(0);
00394                 //here draw if debug
00395             }
00396         }
00397     }
00398     if(map_data->size() > 0 && transforms->size() > 0)
00399     {
00400         ROS_DEBUG("Merging other maps into global map in callback_global_pub,sizeTransforms:%lu,sizeRobots:%lu",transforms->size(),robots->size());
00401         for(int i = 0; i < transforms->size(); i++)
00402         {
00403             if(new_data_maps->at(findRobotIndex(i)) == false)
00404             {
00405                 int robotIndex = findRobotIndex(i);
00406                 ROS_DEBUG("Skipping %s,no new Data",robots->at(robotIndex).c_str());
00407                 continue;
00408             }
00409             nav_msgs::OccupancyGrid * whole_map = map_data->at(i);
00410             std::string name = whole_map->header.frame_id;
00411             ROS_DEBUG("Merging [%s] map",name.c_str());
00412             nav_msgs::OccupancyGrid * map_to_merge = new nav_msgs::OccupancyGrid();
00413             map_to_merge->data = whole_map->data;
00414             map_to_merge->info = whole_map->info;
00415             map_to_merge->header = whole_map->header;
00416             ROS_DEBUG("In mergeMaps MapData:%i,Transform:%i|size MapData:%lu,size Tranform:%lu",
00417                       i,
00418                       findTransformIndex(i),
00419                       map_data->size(),
00420                       transforms->size());
00421             updateMap (map_to_merge,findTransformIndex(i));
00422             mergeMaps(map_to_merge);
00423 
00424             delete map_to_merge;
00425         }
00426     }
00427 
00428     //cv::imwrite(robot_prefix+"other.pgm",mapToMat(map_data->at(0)));
00429     ROS_DEBUG("Publishing global_map, contains data of local map and %lu other maps| i am having %lu other maps",
00430               transforms->size(),
00431               map_data->size());
00432     if(global_map != NULL && global_map_ready == true)
00433     {
00434         //todo: clear all near me!
00435         //cv::Point org_point(map_width / 2 +tmp.pose.position.x / 0.05, map_height / 2 +tmp.pose.position.y / 0.05);
00436         bool newData = false;
00437         for(int i = 0; i < new_data_maps->size(); i++)
00438         {
00439             newData = new_data_maps->at(i);
00440                     if(newData)
00441                         break;
00442         }
00443         if(local_map_new_data || newData)
00444         {
00445                 updateMapArea(-1,local_map);
00446         }
00447        // updateMapArea(-1,local_map,true);
00448         pub.publish(*global_map);
00449         ROS_DEBUG("!!!After publishing global map!!!");
00450     }
00451     else
00452     {
00453         ROS_ERROR("!!!Global map not ready!!!");
00454     }
00455     return;
00456 }
00457 
00458 void MapMerger::callback_send_position(const ros::TimerEvent &e)
00459 {
00460     ROS_DEBUG("Sending Position, x:%f|y:%f",cur_position->pose.position.x,cur_position->pose.position.y);
00461     processPosition(cur_position);
00462 
00463     ROS_DEBUG("Publish my position");
00464     visualization_msgs::Marker m;
00465     m.header.frame_id = local_map_frame_id;
00466     m.header.stamp = ros::Time::now();
00467     m.pose.position.x = cur_position->pose.position.x;
00468     m.pose.position.y = cur_position->pose.position.y;
00469     m.pose.position.z = 0;
00470 
00471     m.scale.x = 0.2;
00472     m.scale.y = 0.2;
00473     m.scale.z = 0.2;
00474 
00475 
00476     m.type = visualization_msgs::Marker::SPHERE;
00477     m.action = visualization_msgs::Marker::ADD;
00478     m.pose.orientation.x = 0.0;
00479     m.pose.orientation.y = 0.0;
00480     m.pose.orientation.z = 0.0;
00481     m.pose.orientation.w = 1.0;
00482     m.header.seq = pos_seq_my++;
00483     m.id = pos_seq_my++;
00484     pos_seq_my++;
00485     m.color.a = 1.0;
00486     m.color.r = 1.0;
00487 
00488     pos_array_my.markers.push_back(m);
00489     my_pos_pub.publish(pos_array_my);
00490 }
00491 
00492 void MapMerger::callback_recompute_transform(const ros::TimerEvent &e)
00493 {
00494     if(transforms->size() == 0)
00495     {
00496         ROS_DEBUG("No transforms to recompute");
00497         return;
00498     }
00499     ROS_INFO("Recompute transforms");
00500 
00501     int newTransforms = 0;
00502     for(int i = 0; i < map_data->size(); i++)
00503     {
00504         if(new_data_maps->at(i) || force_recompute_all == true)
00505         {
00506             ROS_DEBUG("New data at index:%i, recomputing Transform",i);
00507             bool success = recomputeTransform(i);
00508             if(success)
00509                 newTransforms++;
00510         }
00511     }
00512 
00513     if(newTransforms == 0)
00514         return;
00515     else
00516     {
00517         global_map_ready = false;
00518         global_map->data.clear();
00519         global_map->data.resize(local_map->data.size());
00520         std::copy(local_map->data.begin(),local_map->data.end(),global_map->data.begin());
00521         for(int i = 0; i < map_data->size(); i++)
00522         {
00523             nav_msgs::OccupancyGrid *whole_map = map_data->at(i);
00524             nav_msgs::OccupancyGrid * map_to_merge = new nav_msgs::OccupancyGrid();
00525             map_to_merge->data = whole_map->data;
00526             map_to_merge->info = whole_map->info;
00527             map_to_merge->header = whole_map->header;
00528             updateMap(map_to_merge,findTransformIndex(i));
00529             mergeMaps(map_to_merge);
00530             delete map_to_merge;
00531             new_data_maps->at(i) = false;
00532         }
00533         global_map_ready = true;
00534     }
00535 }
00536 
00537 void MapMerger::callback_send_map(const ros::TimerEvent &e)
00538 {
00539     if(local_map_new_data == false)
00540     {
00541         ROS_DEBUG("Do not send map,no new local map data");
00542         return;
00543     }
00544 
00545     if(robots->size() == 0)
00546     {
00547         ROS_WARN("Do not send map, no robots");
00548         return;
00549     }
00550 
00551     if(local_map->data.size() == 0 || local_map_old->data.size() == 0)
00552     {
00553         ROS_WARN("Do not send map, local map contains no data");
00554          return;
00555     }
00556     local_map_new_data = false;
00557     if(debug)
00558     {
00559         cv::imwrite("local.pgm",mapToMat(local_map));
00560         cv::imwrite("old.pgm",mapToMat(local_map_old));
00561     }
00562 #ifndef DEA_OPT_MAP_CHANGED
00563     int min_x = 9999;
00564     int min_y = 9999;
00565     int max_x = -1;
00566     int max_y = -1;
00567     //get map parts to send;
00568     int index;
00569     ROS_DEBUG("Check if map changed");
00570     //+=2 because nearly no differnce to +=1 but lowers number
00571     //of iterations per 75%
00572     for(int row = 0; row < local_map->info.height;row+=2)
00573     {
00574         for(int collum = 0; collum < local_map->info.width;collum+=2)
00575         {
00576             index = row*local_map->info.width + collum;
00577             if(local_map->data[index]!= local_map_old->data[index])
00578             {
00579                 if(min_x > row)
00580                     min_x = row;
00581                 if(min_y > collum)
00582                     min_y = collum;
00583                 if(max_x < row)
00584                     max_x = row;
00585                 if(max_y < collum)
00586                     max_y = collum;
00587             }
00588         }
00589     }
00590     if(min_x == 9999 || min_y == 9999 || max_x == -1 || max_y == -1)
00591     {
00592         ROS_DEBUG("map did not changed,skip sending");
00593         return;
00594     }
00595 #else
00596     min_x = 0;
00597     min_y = 0;
00598     max_x = map_width;
00599     max_y = map_height;
00600 #endif
00601     if(max_x < min_x || max_y < min_y)
00602         return;
00603     ROS_DEBUG("Found values\n min_x:%i|max_x:%i|min_y:%i|max_y:%i",min_x,max_x,min_y,max_y);
00604     ROS_DEBUG("Merge Local Map");
00605     mergeMaps(local_map);
00606     //because that is optimized for a map_part, not for other start values
00607     //mergeMaps(local_map,min_x -5 ,min_y -5 ,max_x +5, max_x +5 );
00608     std::vector<int> * containedUpdates = new std::vector<int>();
00609     containedUpdates->push_back(update_seq);
00610     //todo:mc_ROBOT
00611    /* for(int i = 0; i < robots->size(); i++)
00612         sendMapOverNetwork(robots->at(i),containedUpdates,min_x,min_y,max_x,max_y);
00613 */
00614     sendMapOverNetwork("mc_" + robot_name,containedUpdates,min_x,min_y,max_x,max_y);
00615     delete containedUpdates;
00616     //sendMapOverNetwork("",min_x,min_y,max_x,max_y);
00617     //send_map_over_network("");
00618     ROS_DEBUG("Sended local map over network,adding updateentry for update number:%i\n\t\t\tminx:%i\tmaxx:%i\tminy:%i\tmaxy:%i",local_map->header.seq,min_x,max_x,min_y,max_y);
00619     update_list->push_back(new UpdateEntry(update_seq,min_x,min_y,max_x,max_y));
00620     update_seq++;
00621     if(local_map->data.size() != local_map_old->data.size())
00622     {
00623         ROS_FATAL("Local map changed size, not implemented yet");
00624         //need to delete all and to remerge the maps
00625         ROS_INFO("Deleting global_map");
00626         delete global_map;
00627         global_map = new nav_msgs::OccupancyGrid();
00628         global_map->data.resize(local_map->data.size());
00629         ROS_INFO("Copying local map data into global map");
00630         std::copy(local_map->data.begin(),local_map->data.end(),global_map->data.begin());
00631         ROS_INFO("Clearing transforms");
00632         transforms->clear();
00633         robots_in_transform->clear();
00634         nav_msgs::OccupancyGrid * whole_map;
00635         ROS_INFO("Start recalcing the transformations");
00636         for(int i = 0; i < robots->size(); i++)
00637         {
00638             ROS_INFO("Merge %i in map_data",i);
00639             computeTransform(i);
00640             whole_map = map_data->at(i);
00641             nav_msgs::OccupancyGrid * map_to_merge = new nav_msgs::OccupancyGrid();
00642             map_to_merge->data = whole_map->data;
00643             map_to_merge->info = whole_map->info;
00644             map_to_merge->header = whole_map->header;
00645             updateMap(map_to_merge,findTransformIndex(i));
00646             mergeMaps(map_to_merge);
00647             delete map_to_merge;
00648             ROS_INFO("Merged %i in map_data",i);
00649         }
00650         //exit(-2);
00651         ROS_INFO("Managed map size change");
00652     }
00653     ROS_DEBUG("copy local_map->data to local_map_old->data");
00654     std::copy(local_map->data.begin(),local_map->data.end(),local_map_old->data.begin());
00655 }
00656 
00657 void MapMerger::callback_got_position_network(const adhoc_communication::MmRobotPosition::ConstPtr &msg)
00658 {
00659     //ros::nodeHandle nodeHandle ("~");
00660     geometry_msgs::PoseStamped tmp = msg.get()->position;
00661     std::string source_host = msg.get()->src_robot;
00662     //ROS_ERROR("Got position from:%s,robotssize:%i",source_host.c_str(),robots->size());
00663     for(int i = 0; i < robots->size(); i++)
00664     {
00665         //ROS_ERROR("i:%i",i);
00666         //ROS_ERROR("top of if, robot_name:%s",source_host.);
00667         std::string name = robots->at(i);
00668         //ROS_ERROR("got robot name %s",name.c_str());
00669         if(name == source_host)
00670        {
00671             //ROS_ERROR("enter if for %s",name.c_str());
00672             bool addNew = true;
00673             //ROS_ERROR("start for");//here is the problem, todo
00674             int siz = positions->positions.size();
00675             //ROS_ERROR("siz:%i",siz);
00676             for(int j = 0; j < positions->positions.size(); j++)
00677             {
00678                 //ROS_ERROR("Comparing %s to %s, size:",positions->positions.at(j).sourceRobotName.c_str(),
00679                  //         source_host.c_str()
00680                 //          );
00681                 if(positions->positions.at(j).src_robot == source_host)
00682                 {
00683                     ROS_DEBUG("Updating Position point for %s",name.c_str());
00684                     addNew = false;
00685                     adhoc_communication::MmPoint  newPosition = positions->positions.at(j);
00686                     int index_transform = findTransformIndex(i);
00687                     if(transforms->size() == 0)
00688                     {
00689                         ROS_WARN("no transformation for %s, position is not published",name.c_str());
00690                         return;
00691                     }
00692                     cv::Mat trans = transforms->at(index_transform);
00693                     cv::Point org_point(map_width / 2 +tmp.pose.position.x / 0.05,
00694                                         map_height / 2 +tmp.pose.position.y / 0.05);
00695                     cv::Point homogeneous;
00696                     std::vector<cv::Point> inPts,outPts;
00697                     inPts.push_back(org_point);
00698                     outPts.push_back(homogeneous);
00699                     cv::Size s;
00700                     s.height = map_height; //* 0.05;
00701                     s.width = map_width ;//* 0.05;
00702 
00703                     cv::transform(inPts,outPts,trans);
00704 
00705                     newPosition.x = (outPts.at(0).x - map_width / 2) * 0.05;
00706                     newPosition.y = (outPts.at(0).y - map_height / 2) * 0.05;
00707                     positions->positions.at(j) = newPosition;
00708 
00709                     ROS_DEBUG("Publish other position,j:%i",j);
00710                     visualization_msgs::Marker m;
00711                     m.header.frame_id = local_map_frame_id;
00712                     m.header.stamp = ros::Time::now();
00713                     m.pose.position.x = newPosition.x;
00714                     m.pose.position.y = newPosition.y;
00715                     m.pose.position.z = 0;
00716 
00717                     m.scale.x = 0.2;
00718                     m.scale.y = 0.2;
00719                     m.scale.z = 0.2;
00720 
00721 
00722                     m.type = visualization_msgs::Marker::SPHERE;
00723                     m.action = visualization_msgs::Marker::ADD;
00724                     m.pose.orientation.x = 0.0;
00725                     m.pose.orientation.y = 0.0;
00726                     m.pose.orientation.z = 0.0;
00727                     m.pose.orientation.w = 1.0;
00728                     m.header.seq = pos_seq_other->at(j);
00729                     m.id = pos_seq_other->at(j);
00730                     pos_seq_other->at(j)++;
00731                     m.color.a = 1.0;
00732                     if(i == 0)
00733                         m.color.b = 1.0;
00734                     if(i == 1)
00735                         m.color.g = 1.0;
00736                     if(i == 2)
00737                     {
00738                         m.color.g = 1;
00739                         m.color.b = 1;
00740                     }
00741 
00742                     pos_array_other->at(j).markers.push_back(m);
00743                    // ROS_ERROR("SIZ:%i",pos_array_other->at(j).markers.size());
00744                     pos_pub_other->at(j).publish<visualization_msgs::MarkerArray>(pos_array_other->at(j));
00745                 }
00746             }
00747             //ROS_ERROR("after for");
00748             if(addNew)
00749             {
00750                 ROS_DEBUG("Adding new Position point for %s",name.c_str());
00751                 adhoc_communication::MmPoint newPosition;// ();
00752                 newPosition.src_robot = source_host;
00753                 ROS_DEBUG("find trans");
00754                 int index_transform = findTransformIndex(i);
00755                 ROS_DEBUG("finished find trans");
00756                 if(transforms->size() == 0 );
00757                 {
00758                     ROS_WARN("no transformation for %s, position is not published,index:%u, size:%lu",name.c_str(),index_transform,transforms->size());
00759                 }
00760                 if( index_transform > transforms->size())
00761                 {
00762                     //ROS_WARN("Index:%u,size:%lu",index_transform,transforms->size());
00763                     return;
00764                 }
00765                 ROS_DEBUG("get transform");
00766                 cv::Mat trans = transforms->at(index_transform);
00767                 cv::Point org_point(map_width / 2 +tmp.pose.position.x / 0.05,
00768                                     map_height / 2 +tmp.pose.position.y / 0.05);
00769                 cv::Point homogeneous;
00770                 ROS_DEBUG("got transform");
00771                 std::vector<cv::Point> inPts,outPts;
00772                 inPts.push_back(org_point);
00773                 outPts.push_back(homogeneous);
00774                 cv::Size s;
00775                 s.height = map_height; //* 0.05;
00776                 s.width = map_width ;//* 0.05;
00777 
00778                 cv::transform(inPts,outPts,trans);
00779 
00780                 newPosition.x = (outPts.at(0).x - map_width / 2) * 0.05;
00781                 newPosition.y = (outPts.at(0).y - map_height / 2) * 0.05;
00782                 positions->positions.push_back(newPosition);
00783             }
00784             //ROS_ERROR("Publisch List");
00785             list_of_positions_publisher.publish(*positions);
00786             //ROS_ERROR("Size of positions:%i",positions->positions.size());
00787 /*
00788             if(robots_position_publisher->size() > 0 && transforms->size() > 0)
00789             {
00790 
00791                 //need transform
00792                 //ROS_ERROR("In IF");
00793                 int index_transform = 0;
00794                 //index_transform = findTransformIndex()
00795                 tmp.header.frame_id = local_map_frame_id;
00796                 cv::Mat trans = transforms->at(index_transform);
00797                 cv::Point org_point(map_width / 2 +tmp.pose.position.x / 0.05,
00798                                     map_height / 2 +tmp.pose.position.y / 0.05);
00799                 cv::Point homogeneous;
00800                 std::vector<cv::Point> inPts,outPts;
00801                 inPts.push_back(org_point);
00802                 outPts.push_back(homogeneous);
00803                 cv::Size s;
00804                 s.height = map_height; //* 0.05;
00805                 s.width = map_width ;//* 0.05;
00806                 cv::transform(inPts,outPts,trans);
00807                 tmp.pose.position.x = (outPts.at(0).x - map_width / 2) * 0.05;
00808                 tmp.pose.position.y = (outPts.at(0).y - map_height / 2) * 0.05;
00809                 //_position_other_robots[i] = tmp;
00810                 tmp.header.frame_id = local_map_frame_id;
00811                 tmp.pose.orientation.w -=  180./M_PI*atan2(trans.at<double>(0,1),trans.at<double>(1,1));
00812                 ros::Publisher pub_rob = robots_position_publisher->at(i);
00813                 pub_rob.publish(tmp);
00814                 return;
00815             }
00816             */
00817         }
00818 
00819     }
00820     //_position_other_robots.push_back(tmp);
00821    /* ros::Publisher pub_robot = nodeHandle->advertise<geometry_msgs::PoseStamped>(robot_prefix+"/position/"+source_host,3);
00822     tmp.header.frame_id = local_map_frame_id;
00823     robots_position_publisher->push_back(pub_robot);
00824     pub_robot.publish(tmp);*/
00825     return;
00826 }
00827 
00828 void MapMerger::callback_got_position(const nav_msgs::Odometry::ConstPtr &msg)
00829 {
00830     //ROS_WARN("Got local position");
00831     geometry_msgs::Pose  tmp = msg.get()->pose.pose;
00832     cur_position->pose = tmp;
00833     //ROS_ERROR("ROTATION:%f",tmp.orientation.z);
00834         //processPosition(*msg.get());
00835 }
00836 
00837 void MapMerger::callback_map_other(const adhoc_communication::MmMapUpdateConstPtr &msg)
00838 {
00839     nav_msgs::OccupancyGrid tmp =  msg.get()->map;
00840     nav_msgs::OccupancyGrid * toInsert = &tmp;
00841     int index_robots = -1;
00842     ROS_DEBUG("Got map from %s",msg.get()->src_robot.c_str());
00843     std::string src_robot = msg.get()->src_robot;
00844     if(has_local_map == false)
00845     {
00846         if(src_robot == robot_name)
00847         {
00848             //coping data to the local map;
00849             if(local_map->data.size() == 0)
00850             {
00851                 ROS_INFO("Local map is null");
00852                 local_map->header.frame_id = "/robot_0/map";
00853                 local_map->info.width = map_width;
00854                 local_map->info.height = map_height;
00855                 local_map->data.resize(local_map->info.height * local_map->info.width);
00856                 //here recalc Indexes!!!
00857                 for(int i = 0; i < local_map->data.size(); i++)
00858                     local_map->data[i] = -1;
00859                 updateMapArea(-2,toInsert);
00860                 global_map = new nav_msgs::OccupancyGrid();
00861                 global_map->data = local_map->data;
00862                 global_map->header = local_map->header;
00863                 global_map->info = local_map->info;
00864 
00865                 global_map->info.origin.position.x = g_start_x;
00866                 global_map->info.origin.position.y = g_start_y;
00867                 global_map->info.resolution = 0.05;
00868                 //this->map_width = toInsert->info.width;
00869                 //this->map_height = toInsert->info.height;
00870                 ROS_INFO("Local map is %ix%i",toInsert->info.height,toInsert->info.width);
00871             }
00872             else
00873             {
00874                 updateMapArea(-1,toInsert);
00875                 if(has_local_map == false)
00876                     updateMapArea(-2,toInsert);
00877             }
00878             return;
00879         }
00880     }
00881 
00882     for(int i = 0; i < robots->size();i++)
00883     {
00884         if(robots->at(i)==toInsert->header.frame_id)
00885         {
00886             index_robots = i;
00887             break;
00888         }
00889     }
00890     if(index_robots == -1)
00891     {
00892         ROS_WARN("Got map with a not valid frame_id:%s| index_robot == -1| size of data:%lu",toInsert->header.frame_id.c_str(),toInsert->data.size());
00893         return;
00894     }
00895     else
00896     {
00897         if(new_data_maps->size() < index_robots)
00898             return;
00899         new_data_maps->at(index_robots) = true;
00900         ROS_DEBUG("GOT NOT LOCAL MAP,Process it, data size:%lu",toInsert->data.size());
00901 
00902         if(map_data->size() < index_robots+1)
00903         {
00904             ROS_ERROR("No map_data for robot_index:%i",index_robots);
00905             return;
00906         }
00907         updateMapArea(index_robots,toInsert);
00908         ROS_DEBUG("Adding new list entry");
00909         updateMan->addToupdateList(index_robots,msg.get()->update_numbers);
00910         if(updateMan->isUpdatesMissing(index_robots))
00911         {
00912             ROS_DEBUG("Missed some updates for %s",robots->at(index_robots).c_str());
00913             std::vector<int>* listOfMissedUpdates = updateMan->getMissingUpdateOfRobot(index_robots);
00914             if(listOfMissedUpdates->size() == 0)
00915             {
00916                 ROS_WARN("Something is Wrong with the missing updates");
00917                 delete listOfMissedUpdates;
00918                 return;
00919             }
00920             ROS_DEBUG("%lu updates are missing,asking for them",listOfMissedUpdates->size());
00921             for(int i = 0; i < listOfMissedUpdates->size(); i++)
00922             {
00923                 ROS_WARN("Miss: %i",listOfMissedUpdates->at(i));
00924             }
00925             sendControlMessage(listOfMissedUpdates,robots->at(index_robots));
00926             delete listOfMissedUpdates;
00927         }
00928         return;
00929     }
00930 }
00931 
00932 void MapMerger::callback_map(const nav_msgs::OccupancyGrid::ConstPtr& msg)
00933 {
00934     ROS_DEBUG("Start callback_map");
00935     //get the message
00936     nav_msgs::OccupancyGrid tmp = *msg.get();
00937     nav_msgs::OccupancyGrid * toInsert = &tmp;
00938     //insert it into the list
00939     int index_robots = -1;
00940     //ROS_INFO("Got map, frame_id:%s|size of data:%i",toInsert->header.frame_id.c_str(),toInsert->data.size());
00941     if(toInsert->header.frame_id == local_map_frame_id)
00942     {
00943         processLocalMap(toInsert,index_robots);
00944         force_recompute_all = true;
00945         return;
00946     }
00947     else
00948     {
00949         for(int i = 0; i < robots->size();i++)
00950         {
00951             if(robots->at(i)==toInsert->header.frame_id)
00952             {
00953                 index_robots = i;
00954                 break;
00955             }
00956         }
00957     }
00958     if(index_robots == -1)
00959     {
00960         ROS_WARN("Got map with a not valid frame_id:%s| index_robot == -1| size of data:%lu",toInsert->header.frame_id.c_str(),toInsert->data.size());
00961         return;
00962     }
00963     else
00964     {
00965         new_data_maps->at(index_robots) = true;
00966         ROS_DEBUG("GOT NOT LOCAL MAP");
00967         processMap(toInsert,index_robots);
00968     }
00969     return;
00970 }
00971 
00972 void MapMerger::processLocalMap(nav_msgs::OccupancyGrid * toInsert,int index)
00973 {
00974     ROS_DEBUG("Process Local Map");
00975     if(!has_local_map)
00976         return;
00977     local_map_new_data = true;
00978     if(global_map == NULL)
00979     {
00980         local_map->data = toInsert->data;
00981         local_map->info = toInsert->info;
00982         local_map->header = toInsert->header;
00983         local_map->header.frame_id = local_map_frame_id;
00984         /*for(int i = 0; i < local_map->data.size(); i++)
00985             local_map_old->data.push_back(local_map->data[i]);*/
00986         //local_map_old->data.insert(local_map_old->data.begin(),local_map->data.begin(),local_map->data.end());
00987         local_map_old->data.resize(toInsert->data.size());
00988         local_map_old->header = local_map->header;
00989         std::copy(toInsert->data.begin(),toInsert->data.end(),local_map_old->data.begin());
00990         //local_map_old->data = toInsert->data;
00991         local_map_old->info = local_map->info;
00992         ROS_INFO("%p|%p",&local_map->data,&local_map_old->data);
00993         global_map = new nav_msgs::OccupancyGrid();
00994         global_map->data = local_map->data;
00995         global_map->header = local_map->header;
00996         global_map->info = local_map->info;
00997         global_map->info.origin.position.x = g_start_x;
00998         global_map->info.origin.position.y = g_start_y;
00999 
01000         //get SIGABRT if i call the ROS_WARN ...why ?
01001         //if(local_map->data == global_map->data)
01002         //    ROS_WARN("WTF");
01003 
01004         //global_map->data.resize(toInsert->data.size());
01005         //mergeMaps(toInsert);
01006     }
01007     else
01008     {
01009         local_map->data = toInsert->data;
01010         local_map->header.seq = toInsert->header.seq;
01011         //may not necessary
01012         local_map->header.frame_id = toInsert->header.frame_id;
01013         if(seconds_publish_timer < 2)
01014         {
01015             ROS_DEBUG("Updating is Expensive");
01016             updateMapArea(-1,toInsert);
01017         }
01018     }
01019     return;
01020 }
01021 
01022 void MapMerger::processMap(nav_msgs::OccupancyGrid *map,int index_in_mapdata)
01023 {
01024     //since_last_trans_try++;
01025     ROS_DEBUG("Process Map Splitted");
01026 
01027     if(map_data->size() < index_in_mapdata+1)
01028     {
01029         ROS_ERROR("Missing map_data for index:%i",index_in_mapdata);
01030         return;
01031     }
01032     updateMapArea(index_in_mapdata,map);
01033     return;
01034 }
01035 
01036 void MapMerger::processPosition(geometry_msgs::PoseStamped * pose)
01037 {
01038     //ros::nodeHandle nodeHandle ("~");
01039     //broadcast to all with map and set frame ID to mine
01040     ros::ServiceClient client = nodeHandle->serviceClient<adhoc_communication::SendMmRobotPosition>
01041             (robot_prefix + "/adhoc_communication/send_position");
01042     adhoc_communication::SendMmRobotPosition exchange;
01043     exchange.request.topic = position_other_robots_topic;
01044     exchange.request.dst_robot = "mc_"+robot_name;
01045     exchange.request.position.position.pose = pose->pose;
01046     exchange.request.position.src_robot = robot_name;
01047     //exchange. = robot_name;
01048     if(client.call(exchange))
01049         {
01050             if(exchange.response.status)
01051                 ROS_DEBUG("Could  send position");
01052             else
01053                 ROS_WARN("Problem sending position");
01054         }
01055         else
01056             ROS_DEBUG("Could not call service to send position");
01057 }
01058 
01059 void MapMerger::start()
01060 {
01061     //ros::NodeHandle nodeHandle;
01062     ROS_INFO("Started MapMerger");
01063     ROS_INFO("Subscribe map topic [%s]",local_map_topic.c_str());
01064 
01065 
01066     ros::Subscriber  sub = nodeHandle->subscribe(local_map_topic,1000,&MapMerger::callback_map,this);
01067     ros::Duration(0.1).sleep();
01068     ros::spinOnce();
01069 
01070     ros::Subscriber  sub_control = nodeHandle->subscribe(robot_prefix+"/"+control_topic,1000,&MapMerger::callback_control,this);
01071     ros::Duration(0.1).sleep();
01072     ros::spinOnce();
01073 
01074 
01075 
01076     ROS_ERROR("Init Sub_other_maps");
01077     ros::Subscriber  sub_other_maps = nodeHandle->subscribe(robot_prefix+"/"+topic_over_network,1000,&MapMerger::callback_map_other,this);
01078     ros::Duration(0.1).sleep();
01079     ros::spinOnce();
01080     //#ifndef DEBUG
01081 
01082     ros::Subscriber  sub_new_robot = nodeHandle->subscribe(robot_prefix+"/adhoc_communication/new_robot",1000,&MapMerger::callback_new_robot,this);
01083     ros::Duration(0.1).sleep();
01084     ros::spinOnce();
01085     //#else
01086   //  ros::Subscriber sub_new_robot =  nodeHandle->subscribe("/robot_1/new_robot",1000,&MapMerger::callback_new_robot,this);
01087 //#endif
01088     ros::Subscriber  sub_position_local,sub_position_network;
01089     if(exchange_position)
01090     {
01091         ROS_INFO("Creating position stuff");
01092         sub_position_local = nodeHandle->subscribe(robot_prefix+ "/" + position_local_robot_topic,1000,&MapMerger::callback_got_position,this);
01093         sub_position_network = nodeHandle->subscribe(robot_prefix + "/" + position_other_robots_topic,5,&MapMerger::callback_got_position_network,this);
01094         if(has_local_map)
01095             send_position = nodeHandle->createTimer(ros::Duration(seconds_send_position),&MapMerger::callback_send_position,this);
01096         list_of_positions_publisher = nodeHandle->advertise<adhoc_communication::MmListOfPoints>(robot_prefix+"/"+"all_positions",3);
01097     }
01098   
01099     ROS_INFO("Init Subscriber");
01100     pub = nodeHandle->advertise<nav_msgs::OccupancyGrid>("global_map",3);
01101     my_pos_pub = nodeHandle->advertise<visualization_msgs::MarkerArray>("position_"+robot_name,3);
01102     //because i hat to send them in a occupancy grid, i send them as int, so i lose the numbers
01103     //after the comma
01104    // ros::Publisher pub_position = nodeHandle->advertise<geometry_msgs::Pose>("position_over_network",3);
01105 
01106     if(seconds_recompute_transform != -1)
01107     {
01108         ROS_INFO("Create timer to recompute the transformations all %i seconds",seconds_recompute_transform);
01109         recompute_transform_timer = nodeHandle->createTimer(ros::Duration(seconds_recompute_transform),&MapMerger::callback_recompute_transform,this);
01110     }
01111     global_timer_pub = nodeHandle->createTimer(ros::Duration(seconds_publish_timer),&MapMerger::callback_global_pub,this);
01112     send_map = nodeHandle->createTimer(ros::Duration(seconds_send_timer),&MapMerger::callback_send_map,this);
01113     ros::ServiceServer transform_srv = nodeHandle->advertiseService("transformPoint",
01114                                                           &MapMerger::transformPointSRV,
01115                                                           this);
01116     ros::ServiceServer log_output_srv = nodeHandle->advertiseService("logOutput",
01117                                                                      &MapMerger::log_output_srv,
01118                                                                      this);
01119         //    ros::spin();
01120     while(local_map->data.size() == 0 && has_local_map)
01121     {
01122         ros::Duration(1).sleep();
01123         ros::spinOnce();
01124         ROS_INFO("Local_Map size = 0");
01125     }
01126     ros::Duration(0.1).sleep();
01127     ros::AsyncSpinner spinner(10);
01128     spinner.start();
01129     ros::Duration(1).sleep();
01130     ask_other_timer = nodeHandle->createTimer(ros::Duration(5),&MapMerger::callback_ask_other_robots,this);
01131 
01132     ros::waitForShutdown();
01133 }
01134 
01135 void MapMerger::mergeMaps(nav_msgs::OccupancyGrid *mapToMerge, int min_x, int min_y, int max_x, int max_y)
01136 {
01137     ROS_DEBUG("Start merging map: [%s]",mapToMerge->header.frame_id.c_str());
01138     if(global_map == NULL)
01139     {
01140         ROS_INFO("Stop merging maps, globl map is null");
01141         return;
01142     }
01143     int collums,rows;
01144     if(max_x == -1 || max_y == -1)
01145     {
01146         rows    =  mapToMerge->info.height;
01147         collums =  mapToMerge->info.width;
01148     }
01149     else
01150     {
01151         rows    =  max_x;
01152         collums =  max_y;
01153     }
01154     int start_x = min_x;
01155     int start_y = min_y;
01156     //loop to merge the maps, all of the maps are overlapping becouse they got transformed in the
01157     //calcOffsets Method. if the cur_map_to_merge knows something the local map do not know -> i
01158     // set the value
01159     int index_map_to_merge;
01160     int index_global_map;
01161     if(mapToMerge->header.frame_id != local_map_frame_id)
01162         if(min_x < 0 || min_y < 0)
01163         {
01164             ROS_WARN("Map starts in negative area");
01165             return;
01166         }
01167      for(int row = start_x; +row < rows ; row++)
01168      {
01169          for(int collum = start_y; collum < collums;collum++)
01170          {
01171             index_map_to_merge = (row - start_x) * (collums - start_y) + (collum - start_y);
01172             index_global_map = row * map_width + collum;
01173 
01174             if(mapToMerge->data[index_map_to_merge] != -1)
01175                 if(local_map->data[index_global_map] == -1)
01176                     global_map->data[index_global_map] = mapToMerge->data[index_map_to_merge];
01177          }
01178     }
01179      ROS_DEBUG("Merged maps succesfully");
01180 }
01181 
01182 cv::Mat MapMerger::mapToMat(const nav_msgs::OccupancyGrid *map)
01183 {
01184   uint8_t *data = (uint8_t*) map->data.data(),
01185            testpoint = data[0];
01186   bool mapHasPoints = true;
01187 
01188   Mat im(map->info.height, map->info.width, CV_8UC1);
01189   // transform the map in the same way the map_saver component does
01190   for (size_t i=0; i<map->data.size(); i++)
01191   {
01192     if (data[i] == 0)
01193     {
01194         im.data[i] = 254;
01195     }
01196     else
01197         if(data[i] == 100)
01198         {
01199             im.data[i] = 0;
01200         }
01201     else im.data[i] = 205;
01202     // just check if there is actually something in the map
01203     /*if (i!=0) mapHasPoints = mapHasPoints || (data[i] != testpoint);
01204     testpoint = data[i];*/
01205   }
01206   /*
01207   for(int i = 0; i < map->data.size();i++)
01208       im.data[i] = (unsigned int)map->data.at(i);*/
01209   // sanity check
01210   if (!mapHasPoints)
01211   {
01212       std::string name = map->header.frame_id;
01213       ROS_WARN("map [%s] is empty, ignoring update. ",name.c_str());
01214       ROS_ERROR("If appilcation die uncomment the mapHasPoints if");
01215   }
01216   return im;
01217 }
01218 
01219 nav_msgs::OccupancyGrid* MapMerger::matToMap(const Mat mat, nav_msgs::OccupancyGrid *forInfo)
01220 {
01221     nav_msgs::OccupancyGrid* toReturn = forInfo;
01222     for(size_t i=2; i<toReturn->info.height * toReturn->info.width;i++)
01223     {
01224         //toReturn->data.push_back(mat.data[i]);
01225        if(mat.data[i]== 254)//KNOWN
01226            toReturn->data[i]=0;
01227         // here it is <10 and not 0 (like in mapToMat), becouse otherwise we loose much
01228         //walls etc. but so we get a little of wrong information in the unkown area, what is
01229         // not so terrible.
01230         else if(mat.data[i] > -1 && mat.data[i] < 50) toReturn->data[i]=100; //WALL
01231        else toReturn->data[i] = -1; //UNKOWN
01232     }
01233     return toReturn;
01234 }
01235 
01236 cv::Mat MapMerger::transformImage(Mat img1,Mat trans)
01237 {
01238  // create storage for new image and get transformations
01239  Mat image(img1.size(), img1.type());
01240  cv::Size s;
01241  s.height = image.rows + 0;
01242  s.width = image.cols + 0;
01243 
01244  /*if(image.rows == 32)
01245  {
01246      s.height + 10;
01247      s.width + 10;
01248  }*/
01249 
01250  //should be faster than the other
01251  warpAffine(img1,image,trans,s,INTER_NEAREST,BORDER_TRANSPARENT);
01252  //warpAffine(img1,image,trans,s,INTER_AREA,BORDER_DEFAULT);
01253  //cv::imwrite("test.pgm",image);
01254  return image;
01255 }
01256 
01257 void MapMerger::updateMap(nav_msgs::OccupancyGrid *mapToUpdate,int index_of_transform)
01258 {
01259     ROS_DEBUG("Start updateMap");
01260     if(index_of_transform == -1)
01261     {
01262         ROS_WARN("No transformation available for:[%s]",mapToUpdate->header.frame_id.c_str());
01263         return;
01264     }
01265     //convert the map to a mat, then transform the image, and then convert the mat to a map
01266     //and merge them
01267     ROS_DEBUG("Update:[%s]",mapToUpdate->header.frame_id.c_str());
01268     cv::Mat img = mapToMat(mapToUpdate);
01269     img = transformImage(img,transforms->at(index_of_transform));
01270     /*if(debug)
01271         cv::imwrite("transed_img.pgm",img);*/
01272     mapToUpdate =  matToMap(img,mapToUpdate);
01273 }
01274 
01275 nav_msgs::OccupancyGrid* MapMerger::getMapPart(int map_index, int start_x, int start_y, int width, int height)
01276 {
01277     ROS_DEBUG("Start in getMapPart, creating map part, min_x:%i|max_x:%i|min_y:%i|max_y:%i|Index:%i",start_x,start_x+height,start_y,start_y+width,map_index);
01278 
01279     nav_msgs::OccupancyGrid * tmp = local_map;
01280     while(true)
01281     {
01282         nav_msgs::OccupancyGrid * tmp = local_map;
01283         if(tmp->data.size() > 0)
01284         {
01285             ROS_DEBUG("TMP larger than 0");
01286             break;
01287         }
01288         else
01289         {
01290             ROS_DEBUG("TMP 0");
01291             ros::Duration(2).sleep();
01292         }
01293     }
01294     if(map_index != -1)
01295     {
01296         tmp = map_data->at(map_index);
01297     }
01298     int sizePart = (start_x + height)*(start_y+ width);
01299      if(sizePart > tmp->data.size())
01300      {
01301          ROS_ERROR("MapWidth:%i\tMapHeight:%i",map_width,map_height);
01302          ROS_ERROR("TmpWidth:%i\tTmpHeight:%i\tstartx:%i\tstarty:%i",tmp->info.width,tmp->info.height,start_x,start_y);
01303          ROS_ERROR("Part is too large \n\t\t\t\tsizePart:%i\ttmp size:%lu\twidth:%i\theight:%i",
01304                    sizePart,tmp->data.size(),width,height);
01305          return NULL;
01306      }
01307      nav_msgs::OccupancyGrid* part = new nav_msgs::OccupancyGrid();
01308      for(int row = 0; row < height;row ++)
01309      {
01310          for(int collum = 0; collum < width;collum++)
01311          {
01312              part->data.push_back(tmp->data[(row+start_x)*tmp->info.width+(collum + start_y)]);
01313          }
01314      }
01315      part->header = tmp->header;
01316      part->info.origin.position.x = start_x;
01317      part->info.origin.position.y = start_y;
01318      part->info.height = height;
01319      part->info.width = width;
01320      return part;
01321 }
01322 
01323 void MapMerger::updateMapArea(int map_index, nav_msgs::OccupancyGrid *newData, bool clear)
01324 {
01325     nav_msgs::OccupancyGrid * tmp;
01326     int start_x = newData->info.origin.position.x;
01327     int start_y = newData->info.origin.position.y;
01328     if(map_index == -2)
01329         tmp = local_map;
01330     if(map_index == -1)
01331     {
01332             tmp = global_map;
01333             if(has_local_map)
01334             {
01335                 start_x = 0;
01336                 start_y = 0;
01337             }
01338     }
01339     if(map_index > -1)
01340          tmp = map_data->at(map_index);
01341 
01342     ROS_DEBUG("Updating map Area, new data from map:%s|x:%i|y:%i",newData->header.frame_id.c_str(),start_x,start_y);
01343     int width = newData->info.width;
01344     int height = newData->info.height;
01345     if(clear)
01346     {
01347         width = 10;
01348         height = 10;
01349     }
01350     for(int row = 0; row < height; row++)
01351     {
01352         for(int collum = 0; collum < width; collum++)
01353         {
01354             int i_data =row * width + collum ;
01355             if(newData->data[i_data] == -1 && clear == false)
01356                 continue;
01357             //may segfault, because every map has a other size
01358             int i_global = (row+start_x)*tmp->info.width+(collum + start_y);
01359             tmp->data[i_global] = newData->data[i_data];
01360         }
01361     }
01362     tmp->header.frame_id = newData->header.frame_id;
01363 }
01364 
01365 bool MapMerger::recomputeTransform(int mapDataIndex)
01366 {
01367     int sum_elements = std::accumulate(local_map->data.begin(),local_map->data.end(),0);
01368     if(local_map->data.size()== 0
01369             || sum_elements == (-1) * local_map->data.size())
01370     {
01371         ROS_WARN("Local Map is empty, break up transforming");
01372         return false;
01373     }
01374     nav_msgs::OccupancyGrid * map = map_data->at(mapDataIndex);
01375     std::string map_name = map->header.frame_id;
01376     ROS_DEBUG("Restransforming : [%s]",map_name.c_str());
01377     cv::Mat img_local = mapToMat(local_map);
01378     cv::Mat img_other = mapToMat(map);
01379 
01380     int transIndex = findTransformIndex(mapDataIndex);
01381     cv::Mat newTrans;
01382     bool worked;
01383     if(transIndex == -1)
01384     {
01385         StitchedMap mapStiched(img_local,img_other,max_trans_robot,max_rotation_robot,5,cv::Mat());
01386         worked = mapStiched.works;
01387         if(worked)
01388         {
01389             newTrans = mapStiched.H;
01390             lastTrans = mapStiched.cur_trans;
01391         }
01392     }
01393     else
01394     {
01395         StitchedMap mapStiched(img_local,img_other,max_trans_robot,max_rotation_robot,5,transforms->at(transIndex));
01396         worked = mapStiched.works;
01397         if(worked)
01398         {
01399             newTrans = mapStiched.H;
01400             lastTrans = mapStiched.cur_trans;
01401         }
01402     }
01403     //calculates the transformation between the local map and all the other maps, and
01404     //then updates the map data.
01405     if(worked)
01406     {
01407         ROS_DEBUG("Storing new Transform for %s in array",map->header.frame_id.c_str());
01408         if(findTransformIndex(mapDataIndex) == -1)
01409         {
01410             ROS_DEBUG("Appending transform for robot:%i",mapDataIndex);
01411             transforms->push_back(newTrans);
01412             robots_in_transform->push_back(robots->at(mapDataIndex));
01413         }
01414         else
01415         {
01416             ROS_DEBUG("Updateing transform for robot:%i",mapDataIndex);
01417             transforms->at(findTransformIndex(mapDataIndex)) = newTrans;
01418         }
01419     }
01420     return worked;
01421 }
01422 
01423 void MapMerger::computeTransform(int mapDataIndex)
01424 {
01425     ROS_DEBUG("Start computeTransform for mapDataIndex:%i",mapDataIndex);
01426     int sum_elements = std::accumulate(local_map->data.begin(),local_map->data.end(),0);
01427     if(local_map->data.size()== 0
01428             || sum_elements == (-1) * local_map->data.size())
01429     {
01430         ROS_WARN("Local Map is empty, break up transforming");
01431         return;
01432     }
01433     nav_msgs::OccupancyGrid * whole_map = map_data->at(mapDataIndex);
01434 
01435     //this check only here to test if g√ľnther node send all data
01436     sum_elements = std::accumulate(whole_map->data.begin(),whole_map->data.end(),0);
01437     if(whole_map->data.size() == 0
01438        || sum_elements == (-1) * whole_map->data.size() )
01439     {
01440             ROS_WARN("Other map is empty, break up transforming|%i",sum_elements);
01441             return;
01442     }
01443     //ROS_WARN("INFO sum_elements other map is %i",sum_elements);
01444     //end check
01445 
01446 
01447     new_data_maps->at(mapDataIndex) = false;
01448    std::string map_name = whole_map->header.frame_id;
01449     ROS_DEBUG("Transforming : [%s]",map_name.c_str());
01450     cv::Mat img_local = mapToMat(local_map);
01451     cv::Mat img_other = mapToMat(whole_map);
01452     StitchedMap mapStiched(img_local,img_other,max_trans_robot,max_rotation_robot,5,cv::Mat());
01453     //calculates the transformation between the local map and all the other maps, and
01454     //then updates the map data.
01455     if(!mapStiched.works)
01456     {
01457         return;
01458     }
01459     else
01460     {
01461         if(findTransformIndex(mapDataIndex) == -1)
01462         {
01463             transforms->push_back(mapStiched.H);
01464             robots_in_transform->push_back(robots->at(mapDataIndex));
01465             lastTrans = mapStiched.cur_trans;
01466         }
01467         else
01468         {
01469             ROS_DEBUG("Updateing transform for robot:%i",mapDataIndex);
01470             transforms->at(findTransformIndex(mapDataIndex)) = mapStiched.H;
01471             lastTrans = mapStiched.cur_trans;
01472         }
01473         //imwrite("img_local_map",mapToMat(local_map));
01474         //imwrite("img_other",mapToMat(map_data->at(index)));
01475 
01476         nav_msgs::OccupancyGrid * map_to_merge = new nav_msgs::OccupancyGrid();
01477         map_to_merge->data = whole_map->data;
01478         map_to_merge->info = whole_map->info;
01479         map_to_merge->header = whole_map->header;
01480         updateMap(map_to_merge,findTransformIndex(mapDataIndex));
01481         mergeMaps(map_to_merge);
01482         delete map_to_merge;
01483     }
01484     return;
01485 }
01486 
01487 void MapMerger::makeEmptyMapData(string robot_name,int height,int width,float resolution)
01488 {
01489     ROS_DEBUG("Creating emptyMapData for [%s]",robot_name.c_str());
01490     nav_msgs::OccupancyGrid * tmp = new nav_msgs::OccupancyGrid();
01491     tmp->info.height = height;
01492     tmp->info.width = width;
01493     tmp->info.origin.orientation.w = 1;
01494     tmp->header.frame_id = robot_name;
01495     tmp->info.resolution =resolution; //should not be hard coded
01496     ROS_DEBUG("Filling emptyMapData for [%s]",robot_name.c_str());
01497     for(int i = 0; i < tmp->info.height * tmp->info.width; i++)
01498     {
01499         tmp->data.push_back(-1);
01500     }
01501     ROS_DEBUG("Pushing  emptyMapData for [%s] onto array",robot_name.c_str());
01502     map_data->push_back(tmp);
01503     ROS_DEBUG("Creating bool for checking if new data");
01504     new_data_maps->push_back(false);
01505     if(exchange_position)
01506     {
01507         pos_seq_other->push_back(0);
01508         pos_array_other->push_back(visualization_msgs::MarkerArray());
01509         pos_pub_other->push_back(nodeHandle->advertise<visualization_msgs::MarkerArray>("position_"+robot_name,3));
01510     }
01511 }
01512 
01513 void MapMerger::callback_robot_status(const nav_msgs::MapMetaData::ConstPtr &msg)
01514 {
01515     for(int i = 0; i < robots->size(); i++)
01516     {
01517 
01518         /*if(robots->at(i) == robot_name)
01519         {
01520             ROS_INFO("Robot known");
01521         }
01522         else
01523         {
01524             ROS_INFO("New Robot:%s",robot_name.c_str());
01525             robots->push_back(robot_name);
01526         }*/
01527     }
01528 }
01529 
01530 void MapMerger::sendMapOverNetwork(string destination, std::vector<int>* containedUpdates, int start_row, int start_collum, int end_row, int end_collum)
01531 {
01532      //ros::nodeHandle nodeHandle ("~");
01533      ros::ServiceClient client;
01534      bool isWholeMap = false;
01535      client = nodeHandle->serviceClient<adhoc_communication::SendMmMapUpdate>
01536              (robot_prefix + "/adhoc_communication/send_map_update");
01537       std::string service = robot_prefix + "/adhoc_communication/send_map_update";
01538      if(end_row == -1 || end_collum == -1)
01539     {
01540         ROS_DEBUG("Send whole map");
01541         end_row = map_height;
01542         end_collum = map_width;
01543         start_row = 0;
01544         start_collum = 0;
01545         isWholeMap = true;
01546     }
01547         ROS_DEBUG("Send map using:%s \n\t\t\t\t the normal way min_x:%i|max_x:%i|min_y:%i|max_y:%i",service.c_str(),start_row,end_row,start_collum,end_collum);
01553         int height,width;
01554         height = size;
01555         width = size;
01556         for(int row = start_row; row < end_row;row+=size)
01557         {
01558             for(int collum = start_collum; collum < end_collum;collum+=size)
01559             {
01560                 if(row + height > end_row)
01561                     height = end_row - row;
01562                 if(collum + width > end_collum)
01563                     width = end_collum - collum;
01564                 ROS_DEBUG("Creating map part t");
01565                 nav_msgs::OccupancyGrid * t = this->getMapPart(-1,row,collum,width,height);
01566                 ROS_DEBUG("Checking data of  map part t,size:%lu",t->data.size());
01567 
01568                 /*int sum_elements = std::accumulate(t->data.begin(),t->data.end(),0);
01569                 if(sum_elements == (-1)* t->data.size())
01570                 {
01571                     ROS_DEBUG("Map is empty, do not send");
01572                     delete t;
01573                     continue;
01574                 }
01575                 ROS_WARN("INFO send map, sum_elements is:%i",sum_elements);
01576                 */
01577                 adhoc_communication::SendMmMapUpdate exchange;
01578                 exchange.request.topic =topic_over_network;
01579                 exchange.request.map_update.src_robot = robot_hostname;
01580                 exchange.request.map_update.map.info = t->info;
01581                 exchange.request.map_update.map.header = t->header;
01582                 exchange.request.map_update.map.header.seq = update_seq;
01583                 exchange.request.map_update.map.header.frame_id = robot_name.c_str();
01584                 exchange.request.map_update.map.data = t->data;
01585                 exchange.request.map_update.update_numbers = *containedUpdates;
01586                 std::string hostname = destination;
01587                 exchange.request.dst_robot = destination;
01588                 ROS_DEBUG("Send Map to %s(if nothing -> boradcast) ,frame id:%s| topic:[%s]|data<_size:%lu",
01589                           destination.c_str(),t->header.frame_id.c_str(),
01590                           topic_over_network.c_str(), exchange.request.map_update.map.data.size());
01591                 if(client.call(exchange))
01592                 {
01593                     if(exchange.response.status)
01594                         if(destination == "")
01595                             ROS_DEBUG("Sended Map to:all,topic:%s|r:%i;c:%i",exchange.request.topic.c_str(),row,collum);
01596                     else
01597                         ROS_DEBUG("Sended Map to:%s,topic:%s|r:%i;c:%i",exchange.request.dst_robot.c_str(),exchange.request.topic.c_str(),row,collum);
01598                     else
01599                         ROS_WARN("Destination host unreachable [%s](if nothing -> boradcast)",hostname.c_str());
01600                 }
01601                 else
01602                     ROS_DEBUG("Could not call service to send map");
01603             }
01604         }
01605 }
01606 
01607 void MapMerger::sendMetaData(float res)
01608 {
01609     //ros::nodeHandle nodeHandle ("~");
01610     ros::ServiceClient client;
01611     std::string service = robot_prefix + "/adhoc_communication/send_map";
01612     client = nodeHandle->serviceClient<adhoc_communication::SendOccupancyGrid>(service);
01613     ROS_DEBUG("Calling service[%s] to send meta",service.c_str());
01614     adhoc_communication::SendOccupancyGrid exchange;
01615     exchange.request.topic = map_meta_topic;
01616     exchange.request.map.info.height = map_height;
01617     exchange.request.map.info.width = map_width;
01618     exchange.request.map.info.resolution = res;
01619     exchange.request.map.info.origin.orientation.w = 1;
01620     exchange.request.map.header.frame_id = robot_name.c_str();
01621     exchange.request.dst_robot = "";
01622         if(client.call(exchange))
01623         {
01624             if(exchange.response.status)
01625                 ROS_DEBUG("Could  send meta");
01626             else
01627                 ROS_WARN("Problem sending meta_data");
01628         }
01629         else
01630             ROS_WARN("Could not call service to send meta");
01631 }
01632 
01633 int MapMerger::findTransformIndex(int robot_index)
01634 {
01635     //ROS_DEBUG("finding TransformIndex for robot_index:%i",robot_index);
01636     if(transforms->size() == 0)
01637     {
01638       //  ROS_DEBUG("no transforms, skipping findingTransformindex");
01639        /* if(robots_in_transform->size() == 0)
01640             ROS_DEBUG("no robots_in_transform values");*/
01641         return -1;
01642     }
01643     for(int r = 0; r < robots_in_transform->size(); r++)
01644     {
01645         if(robots_in_transform->at(r)==robots->at(robot_index))
01646         {
01647             //ROS_DEBUG("found TransformIndex for robot_index:%i",robot_index);
01648             return r;
01649         }
01650     }
01651    // ROS_DEBUG("Could not found TransformIndex for robot_index:%i",robot_index);
01652     return -1;
01653 }
01654     int MapMerger::findRobotIndex(int transform_index)
01655 {
01656     for(int i = 0; i < robots_in_transform->size(); i++)
01657     {
01658         if(robots_in_transform->at(transform_index) == robots->at(i))
01659             return i;
01660     }
01661 }
01662 
01663 
01664 
01665 bool MapMerger::createLogPath()
01666 {
01667     full_log_path = log_path + std::string("/map_merger/") + robot_name;
01668     ROS_INFO("Creating log path \"%s\".", full_log_path.c_str());
01669     boost::filesystem::path boost_log_path(full_log_path.c_str());
01670     if(!boost::filesystem::exists(boost_log_path))
01671         try{
01672             if(!boost::filesystem::create_directories(boost_log_path))
01673                 ROS_ERROR("Cannot create directory \"%s\".", full_log_path.c_str());
01674         }catch(const boost::filesystem::filesystem_error& e)
01675         {
01676             ROS_ERROR("Cannot create path \"%s\".", full_log_path.c_str());
01677             return false;
01678         }
01679     return true;
01680 }
01681 
01682 
01689 bool MapMerger::log_output_srv(map_merger::LogMaps::Request &req, map_merger::LogMaps::Response &res)
01690 {
01691     ROS_DEBUG("--> Top of service log_output_srv");
01692     if(!createLogPath())
01693         return false;
01694 
01695     unsigned int local_progress = 0;
01696     unsigned int global_progress = 0;
01697     std::string file;
01698     ros::Duration ros_time = ros::Time::now() - time_start;
01699     float creation_time = ros_time.toSec();
01700 
01701     if(req.log & LOG_GLOBAL_MAP)
01702     {
01703         if(!global_map)
01704             ROS_WARN("No global map. Skipping save global map.");
01705         else
01706         {
01707             // log the global map 
01708             ROS_DEBUG("Storing global file...");
01709             file = full_log_path + std::string("/global.pgm");
01710             cv::Mat glo = mapToMat(global_map);
01711             circle(glo,Point(glo.rows/2,glo.cols/2),5,Scalar(0,0,0),2);
01712             if(global_map_ready)
01713                 cv::imwrite(file.c_str(),glo);
01714             ROS_DEBUG("Successfully logged global map.");
01715         }
01716     }
01717     if(req.log & LOG_LOCAL_MAP)
01718     {
01719         if(!local_map)
01720             ROS_WARN("No global map. Skipping save local map.");
01721         else
01722         {
01723             // log the local map
01724             ROS_DEBUG("Storing local file...");
01725             file = full_log_path + std::string("/local.pgm");
01726 
01727             cv::Mat loc = mapToMat(local_map);
01728             circle(loc,Point(loc.rows/2,loc.cols/2),5,Scalar(0,0,0),2);
01729             cv::imwrite(file.c_str(),loc);
01730 
01731             ROS_DEBUG("Successfully logged local map.");
01732         }
01733     }
01734     if(req.log & LOG_LOCAL_MAP_PROGRESS)
01735     {
01736         if(!local_map)
01737             ROS_WARN("No local map. Skipping save local map progress.");
01738         else
01739         {
01740             // log the local map progress, i.e., the number of free spaces in the local map
01741             ROS_DEBUG("Logging local map progress");
01742             for(int i=0; i<local_map->data.size(); i++)
01743             {
01744                 if(local_map->data[i] == 0)
01745                     local_progress++;
01746             }
01747         }
01748     }
01749     if(req.log & LOG_GLOBAL_MAP_PROGRESS)
01750     {
01751         if(!global_map)
01752             ROS_WARN("No global map. Skipping save global map progress.");
01753         else
01754         {
01755             // log the global map progress, i.e., the number of free spaces in the local map
01756             ROS_DEBUG("Logging global map progress");
01757             unsigned int progress = 0;
01758             for(int i=0; i<global_map->data.size(); i++)
01759             {
01760                 if(global_map->data[i] == 0)
01761                     global_progress++;
01762             }
01763         }
01764     }
01765 
01766     
01767     if(req.log & LOG_GLOBAL_MAP_PROGRESS || req.log & LOG_LOCAL_MAP_PROGRESS)
01768     {
01769         // save progress to file
01770         file = full_log_path + std::string("/periodical.log");
01771         std::string file_updates = full_log_path + std::string("/updates_log");
01772         std::ofstream fs1;
01773         fs1.open(file_updates.c_str(),std::ios_base::app);
01774         if(!fs1)
01775         {
01776             ROS_ERROR("Cannot create file. Error: %s", strerror(errno));
01777         }
01778 
01779         for(int i = 0; i < robots->size(); i++)
01780         {
01781             std::string rName = robots->at(i);
01782             fs1 << rName << ": ";
01783             for(int j = 0 ; j < updateMan->getUpdateListOfrobot(0)->size(); j++)
01784             {
01785                 int v = updateMan->getUpdateListOfrobot(0)->at(j);
01786                 fs1 << v << "-";
01787             }
01788             fs1 << std::endl;
01789         }
01790         fs1.close();
01791         ROS_DEBUG("Logging to file \"%s\".", file.c_str());
01792         std::ofstream fs;
01793         fs.open(file.c_str(), std::ios_base::app);
01794         if(!fs)
01795         {
01796             ROS_ERROR("Cannot create file. Error: %s", strerror(errno));
01797         }
01798         //todo: write creation_time if needed
01799         //fs << creation_time << "," << global_progress << "," << local_progress << std::endl;
01800         fs << ros::Time::now() << "," << global_progress << "," << local_progress << "," << cur_position->pose.position.x << "," << cur_position->pose.position.y << "," << update_list->size() << "," << transforms->size() << std::endl;
01801 
01802         fs.close();
01803 
01804         ROS_DEBUG("Storing local file...");
01805         file = full_log_path + std::string("/local.pgm");
01806 
01807         cv::Mat loc = mapToMat(local_map);
01808         circle(loc,Point(loc.rows/2,loc.cols/2),5,Scalar(0,0,0),2);
01809         cv::imwrite(file.c_str(),loc);
01810 
01811         ROS_DEBUG("Successfully logged local map.");
01812 
01813         ROS_DEBUG("Storing global file...");
01814         file = full_log_path + std::string("/global.pgm");
01815         cv::Mat glo = mapToMat(global_map);
01816         circle(glo,Point(glo.rows/2,glo.cols/2),5,Scalar(0,0,0),2);
01817         if(global_map_ready)
01818             cv::imwrite(file.c_str(),glo);
01819         ROS_DEBUG("Successfully logged global map.");
01820     }
01821     ROS_DEBUG("<-- end log_output_srv");
01822     return true;
01823 }
01824 
01825 
01826 bool MapMerger::transformPointSRV(map_merger::TransformPoint::Request &req, map_merger::TransformPoint::Response &res)
01827 {
01828     //need transform
01829     int index_transform = -1;
01830     if(req.point.src_robot == robot_name)
01831         ROS_ERROR("Transform my own point!");
01832     for(int i = 0; i < robots->size();i++)
01833     {
01834         if(robots->at(i) == req.point.src_robot)
01835         {
01836             ROS_DEBUG("Found %s with %s at %i",
01837                       robots->at(i).c_str(),
01838                       req.point.src_robot.c_str(),
01839                       i);
01840             index_transform = findTransformIndex(i);
01841             break;
01842         }
01843     }
01844     if(index_transform == -1)
01845     {
01846         ROS_WARN("Could not transform point, no transform matrix found for %s",
01847                  req.point.src_robot.c_str());
01848         return false;
01849     }
01850     cv::Mat trans = transforms->at(index_transform);
01851     cv::Point org_point(map_width / 2 +req.point.x / 0.05,
01852                         map_height / 2 +req.point.y / 0.05);
01853     cv::Point homogeneous;
01854     std::vector<cv::Point> inPts,outPts;
01855     inPts.push_back(org_point);
01856     outPts.push_back(homogeneous);
01857     cv::Size s;
01858     s.height = map_height; //* 0.05;
01859     s.width = map_width ;//* 0.05;
01860     cv::transform(inPts,outPts,trans);
01861     res.point.x = (outPts.at(0).x - map_width / 2) * 0.05;
01862     res.point.y = (outPts.at(0).y - map_height / 2) * 0.05;
01863     res.point.src_robot = robot_hostname;
01864     return true;
01865 }
01866 
01867 void MapMerger::sendBackAskedMapData(string robotName, std::vector<int> missingUpdates)
01868 {
01869     int min_x = map_width;
01870     int max_x = 0;
01871     int min_y = map_height;
01872     int max_y = 0;
01873     for(int i = 0; i < missingUpdates.size()  ;i++)
01874     {
01875         UpdateEntry * tmp = update_list->at(missingUpdates.at(i));
01876         if(tmp->getMaxX() > max_x)
01877             max_x = tmp->getMaxX();
01878         if(tmp->getMaxY() > max_y)
01879             max_y = tmp->getMaxY();
01880         if(tmp->getMinX() > min_x)
01881             min_x = tmp->getMinX();
01882         if(tmp->getMinY() > max_y)
01883             min_y = tmp->getMinY();
01884     }
01885     ROS_INFO("Sending missed map updates to %s",robotName.c_str());
01886     std::vector<int>* containedUpdates = new std::vector<int>();
01887     //todo here all map updates which are included
01888     containedUpdates->push_back(update_seq);
01889     sendMapOverNetwork(robotName,containedUpdates,min_x,min_y,max_x,max_y);
01890     delete containedUpdates;
01891 }
01892 void MapMerger::sendControlMessage(std::vector<int>* updateNumbers,std::string dest)
01893 {
01894     ros::ServiceClient client = nodeHandle->serviceClient<adhoc_communication::SendMmControl>(robot_prefix +"/adhoc_communication/send_control_message");
01895     adhoc_communication::SendMmControl exchange;
01896     exchange.request.dst_robot= dest;
01897     exchange.request.topic = control_topic;
01898     exchange.request.msg.src_robot = robot_hostname;
01899     exchange.request.msg.update_numbers = *updateNumbers;
01900     if(client.call(exchange))
01901         {
01902             if(exchange.response.status)
01903                 ROS_DEBUG("Could  send control message");
01904             else
01905                 ROS_WARN("Problem sending control message to %s",dest.c_str());
01906         }
01907         else
01908             ROS_DEBUG("Could not call service to send control message");
01909 }
01910 bool MapMerger::getHasLocalMap()
01911 {
01912     return has_local_map;
01913 }
01914 
01915 void MapMerger::callback_ask_other_robots(const ros::TimerEvent &e)
01916 {
01917     ROS_INFO("Checking if robots are already in the network, service:%s/adhoc_communication/get_neighbors",robot_prefix.c_str());
01918 
01919 
01920     ros::ServiceClient getNeighborsClient = nodeHandle->serviceClient<adhoc_communication::GetNeighbors>
01921             (robot_prefix+"/adhoc_communication/get_neighbors");
01922     adhoc_communication::GetNeighbors getNeighbors;
01923 
01924     if(!has_local_map)
01925     {
01926         std::vector<int>* empty = new std::vector<int>();
01927         sendControlMessage(empty,robot_name);
01928         //sendControlMessage(empty,robot_name);
01929         delete empty;
01930     }
01931 
01932     if(getNeighborsClient.call(getNeighbors))
01933     {
01934         ROS_INFO("%lu robots are already in the network, adding them,my name:%s",getNeighbors.response.neigbors.size(),
01935                  robot_name.c_str());
01936         if(getNeighbors.response.neigbors.size() > 0)
01937         {
01938             for(int i = 0; i < getNeighbors.response.neigbors.size();i++)
01939             {
01940                 std::string newRobotName = getNeighbors.response.neigbors.at(i);
01941                 if(newRobotName == robot_name)
01942                 {
01943                     ROS_WARN("Got my own name as a neigbor!");
01944                     continue;
01945                 }
01946                 bool cont = false;
01947                 for(int h = 0; h < robots->size(); h++)
01948                 {
01949                     if(newRobotName == robots->at(h))
01950                         cont = true;
01951                 }
01952                 if(cont)
01953                     continue;
01954                 robots->push_back(newRobotName);
01955                 ROS_INFO("New robot:%s",newRobotName.c_str());
01956                 updateMan->addNewUpdateList();
01957                 makeEmptyMapData(newRobotName,map_height,map_width,local_map->info.resolution);
01958                 ROS_INFO("asking %s for map",newRobotName.c_str());
01959                 std::vector<int>* empty = new std::vector<int>();
01960                 std::vector<int>* neg = new std::vector<int>();
01961                 neg->push_back(-1);
01962                 //twice to be sure and sleep to prevent problems
01963                 //first message might not be pbulished
01964                 //ros::Duration(1).sleep();
01965                // sendControlMessage(neg,newRobotName);
01966                 ros::Duration(1).sleep();
01967                 sendControlMessage(empty,newRobotName);
01968                 delete empty;
01969                 delete neg;
01970             }
01971         }
01972         else
01973         {
01974             ROS_INFO("No robots are were in the network before me");
01975         }
01976     }
01977     else
01978     {
01979         ROS_WARN("Can't call service to geht neighbors");
01980     }
01981     ask_other_timer.stop();
01982 }


map_merger
Author(s): Peter Kohout , Torsten Andre
autogenerated on Thu Aug 27 2015 11:56:48