ROSのSLAMから、MAPを受信してOpenCV形式で保存する

目的

　ROSのSLAMパッケージで生成したMap topicを受信して、MapをOpenCV形式で保存する。
　画像を保存するには、map_servar Packageを使えばよいのだが、map_servar Packageは内部で2値化処理を入れていることと、様々な画像形式で出力できるようにするため、OpenCVのcv:mat形式で読み取り、保存する。

実装

Map Topicを受信し、OpenCV形式で保存する。

Main.cpp

#include <iostream>
using namespace std;
#include <ros/ros.h>
#include <nav_msgs/GetMap.h>
#include <tf/tf.h>
#include <opencv2/opencv.hpp>
using namespace cv;

void mapCallback(const nav_msgs::OccupancyGridConstPtr &map) {
    geometry_msgs::Quaternion orientation = map->info.origin.orientation;
    double yaw, pitch, roll;
    tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));
    mat.getEulerYPR(yaw, pitch, roll);
    double map_theta= yaw;

    ROS_INFO("Received a %d X %d map @ %.3f m/pix_%f_%f_%f",
    map->info.width,map->info.height,map->info.resolution,
    map->info.origin.position.x,map->info.origin.position.y,map_theta);

    Mat img_out = Mat::zeros(cv::Size(map->info.width,map->info.height), CV_8UC1);

    for (unsigned int y = 0; y < map->info.height; y++){
        for (unsigned int x = 0; x < map->info.width; x++){
            unsigned int i = x + (map->info.height - y - 1) * map->info.width;
            int intensity=205;
            if (map->data[i] >= 0 && map->data[i] <=100)
            intensity= round((float)(100.0-map->data[i])*2.55);
            img_out.at<unsigned char>(y, x)=intensity;
        }
    }

    imwrite("map.png",img_out);

    ros::shutdown();
}

int main(int argc, char* argv[]) {
    ros::init(argc, argv, "map_save");
    ros::NodeHandle n;
    ros::Subscriber map_sub_ = n.subscribe("map", 2, mapCallback);
    ros::Rate r(10.0);
    ros::spin();
    return 0;
}

参考

ROSのLidarSLAMまとめ

Ros Wiki OccupancyGrid
http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html

Navigation map_saver
https://github.com/ros-planning/navigation/blob/kinetic-devel/map_server/src/map_saver.cpp