How to convert OccupancyGridMap to .pgm with Python? [closed]

edit

Hi

I want to convert a OccupancyGridMap to a .pgm image file with Python. I have written a code in C++ and now I tried to translate this code to Python. Sadly I have an error. I have not found the error yet.

I tried it with the Turtlebot 3 Simulation:

roslaunch turtlebot3_gazebo turtlebot3_house.launch # for starting the gazebo simulation
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch # for controlling the robot
roslaunch turtlebot3_slam turtlebot3_slam.launch # for creating the OccupancyGridMap

Then I run my C++ code without any errors:

rosrun map_listener mapListener.cpp

This is the code:

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>
#include "nav_msgs/OccupancyGrid.h"

#include "std_msgs/Header.h"
#include "nav_msgs/MapMetaData.h"
#include "quadtree.h"
#include "geometry_msgs/Pose.h"


void mapConvert(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
    std_msgs::Header header = msg->header;

    // ros map saver
    std::string mapdatafile = "testmap.pgm";
    ROS_INFO("Writing map occupancy data to %s", mapdatafile.c_str());
    FILE* out = fopen(mapdatafile.c_str(), "w");
    if (!out) {
        ROS_ERROR("Couldn't save map file to %s", mapdatafile.c_str());
        return;
    }

    fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
    msg->info.resolution, msg->info.width, msg->info.height);
    for(unsigned int y = 0; y < msg->info.height; y++) {
        for(unsigned int x = 0; x < msg->info.width; x++) {
            unsigned int i = x + (msg->info.height - y - 1) * msg->info.width;
            if (msg->data[i] >= 0 && msg->data[i] <= 0) {
                //occ [0,0.1)
                fputc(254, out);
            } else if (msg->data[i] <= 100 && msg->data[i] >= 100) {
                //occ (0.65,1]
                fputc(000, out);
            } else {
                //occ [0.1,0.65]
                fputc(205, out);
            }
        }
    }
    fclose(out);
    // Ende map saver
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "map_converter");
    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe("map", 1000, mapConvert);

    ros::spin();

    return 0;
}

The repository you can find here: https://github.com/Michdo93/map_listener

Then I tried the equivalent code for Python:

#!/usr/bin/python
import sys
import os
import rospy
from std_msgs.msg import String
from std_msgs.msg import Header
from nav_msgs.msg import OccupancyGrid
from nav_msgs.msg import MapMetaData
from geometry_msgs.msg import Pose
import numpy as np

# OpenCV
import cv2

VERBOSE = False

class MapListener(object):

    def __init__(self):
        """Configure subscriber."""
        # Create a subscriber with appropriate topic, custom message and name of
        # callback function.
        self.sub = rospy.Subscriber("map", OccupancyGrid, self.mapConvert)

        # Initialize message variables.
        self.enable = False

        if self.enable:
            self.start()
        else:
            self.stop()

    def start(self):
        self.enable = True
        self.sub = rospy.Subscriber("map", OccupancyGrid, self.mapConvert)

    def mapConvert(self, msg):
        """Handle subscriber data."""
        header = msg.header

        # ros map saver
        mapdatafile = "testmap.pgm"
        rospy.loginfo("Writing map occupancy data to %s", mapdatafile)

        try:
            with open(mapdatafile, 'wb') as out:
                #out.write("P5\n# CREATOR: map_saver.cpp %s m/pix\n%s %s\n255\n" % (msg.info.resolution, msg.info.width, msg.info.height))
                out.write(bytearray('P5' + ' ' + str(msg.info.width) + ' ' + str(msg.info.height) + ' ' + str(255) +  '\n', "utf-8"))
                for y in range(np.uint8(0), msg.info.height):
                    for x in range ...