53 lines
1.1 KiB
C++
53 lines
1.1 KiB
C++
|
#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;
|
||
|
|
|
||
|
|
}
|