mapsplitter.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <nav_msgs/OccupancyGrid.h>
00003 #include "numeric"
00004 using namespace std;
00005 
00006 void callback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
00007 nav_msgs::OccupancyGrid* getMapPart(nav_msgs::OccupancyGrid *tmp, int start_x, int start_y, int width, int height);
00008 int main(int argc, char **argv)
00009 {
00010     ROS_INFO("Start map_splitter");
00011     ros::init(argc,argv,"map_splitter");
00012     string topic;
00013     ros::NodeHandle n;
00014     n.param<string>("/map_splitter/map_topic",topic,"map");
00015     ROS_INFO("Subscribed Topic: %s",topic.c_str());
00016 
00017     ros::Subscriber  sub = n.subscribe(topic,1000,&callback);
00018     ROS_DEBUG("Init Subscriber");
00019     ROS_INFO("Splitter Waits");
00020     ros::Duration(6).sleep();
00021     ros::spin();
00022 }
00023 
00024 void callback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
00025 {
00026     int size;
00027     ros::NodeHandle n;
00028     string frame,pub_frame,pub_topic_map,pub_topic_meta;
00029 
00030     n.param<int>("/map_splitter/size",size,32);
00031     n.param<string>("/map_splitter/map_frame",frame,"map");
00032     n.param<string>("/map_splitter/pub_topic_map",pub_topic_map,"map");
00033     n.param<string>("/map_splitter/pub_topic_meta",pub_topic_meta,"map_meta");
00034     //ROS_INFO("Wanted mapframe: %s",frame.c_str());
00035     n.param<string>("/map_splitter/pub_frame",pub_frame,"map");
00036     //ROS_INFO("Published frame: %s",pub_frame.c_str());
00037     ros::Publisher pub_map = n.advertise<nav_msgs::OccupancyGrid>(pub_topic_map,3);
00038     ros::Publisher pub_meta = n.advertise<nav_msgs::OccupancyGrid>(pub_topic_meta,3);
00039     //get the message
00040     nav_msgs::OccupancyGrid   local_map = *msg.get();
00041     //check if it is the map i want
00042     string frame_id = local_map.header.frame_id;
00043     if(local_map.header.frame_id != frame)
00044     {
00045         if(frame_id != pub_frame)
00046             ROS_WARN("Got Map with wrong frame_id[%s]",frame_id.c_str());
00047         return;
00048     }
00049     //prepare it for the publish
00050     ROS_INFO("Got Map to split");
00051     ROS_INFO("Publish meta_data_packet");
00052     nav_msgs::OccupancyGrid * meta_data = new nav_msgs::OccupancyGrid();
00053     meta_data->header.frame_id = pub_frame;
00054     meta_data->info.height = local_map.info.height;
00055     meta_data->info.width = local_map.info.width;
00056     meta_data->info.resolution = local_map.info.resolution;
00057 
00058     ros::Duration(3).sleep();
00059     for(int row = 0; row < local_map.info.height-size;row+=size)
00060     {
00061         for(int collum = 0; collum < local_map.info.width-size;collum+=size)
00062         {
00063             //ROS_INFO("row:%icollum:%i:",row,collum);
00064             nav_msgs::OccupancyGrid * t = getMapPart(&local_map,row,collum,size,size);
00065             int sum_elements = std::accumulate(t->data.begin(),t->data.end(),0);
00066             if(sum_elements == (-1)* t->data.size())
00067             {
00068                 ROS_DEBUG("Map is empty, do not publish");
00069                 continue;
00070             }
00071             ROS_INFO("Published Mapframe:%s||r:%i;c:%i",pub_frame.c_str(),row,collum);
00072             t->header.frame_id = pub_frame;
00073             pub_map.publish(*t);
00074             pub_meta.publish(*meta_data);
00075 
00076             delete t;
00077             ros::Duration(0.2).sleep();
00078         }
00079 
00080     }
00081 }
00082 
00083 nav_msgs::OccupancyGrid* getMapPart(nav_msgs::OccupancyGrid *tmp, int start_x, int start_y, int width, int height)
00084 {
00085     nav_msgs::OccupancyGrid* part = new nav_msgs::OccupancyGrid();
00086     for(int row = 0; row < height;row ++)
00087     {
00088         for(int collum = 0; collum < width;collum++)
00089         {
00090            part->data.push_back(tmp->data.at((row+start_x)*tmp->info.width+(collum + start_y)));
00091         }
00092     }
00093     part->header = tmp->header;
00094     part->info.origin.position.x = start_x;
00095     part->info.origin.position.y = start_y;
00096     part->info.height = height;
00097     part->info.width = width;
00098     return part;
00099 }


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