#include"ros/ros.h"
#include "nav_msgs/OccupancyGrid.h"
#include "std_msgs/String.h"
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <stdio.h>

 


void callback(const nav_msgs::OccupancyGrid::ConstPtr& mapmsg)
{
   int height,width,k=0;
   cv::Mat map(2000,2000,CV_8UC1);
   height = mapmsg->info.height;
   width = mapmsg->info.width;
   std::vector<int> compression_params;
   compression_params.push_back(cv::IMWRITE_PNG_COMPRESSION);
   compression_params.push_back(0);   

   for (int i = 0; i < 2000; i++)
   {
       for(int j = 0; j < 2000; j++)
       {
          map.at<uchar>(i,j) = mapmsg->data[k];
          k++;
       }
       
   }

   cv::imshow("map",map);
   if (cv::waitKey(1)==27)
   {
      cv::imwrite("map.png", map, compression_params);
      exit(0);
   }
   
}



int main(int argc, char *argv[])
{
    //init_keyboard();
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"listener_map");
    ros::NodeHandle nh;
    ros::Subscriber map_sub = nh.subscribe<nav_msgs::OccupancyGrid>("map",1,callback);
    ros::spin();
    return 0;

}