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(np.uint8(0), msg.info.width):
                        i = np.uint8(x + (msg.info.height - y - 1) * msg.info.width)
                        if(msg.data[i] >= 0 and msg.data[i] <= 0):
                            #occ [0,0.1)
                            out.write(bytearray(str(254), "utf-8"))
                        elif(msg.data[i] <= 100 and msg.data[i] >= 100):
                            #occ (0.65,1]
                            out.write(bytearray(str(000), "utf-8"))
                        else:
                            #occ [0.1,0.65]
                            out.write(bytearray(str(205), "utf-8"))
                out.close()
        except IOError:
            rospy.loginfo("Couldn't save map file to %s", mapdatafile)

    def stop(self):
        """Turn off subscriber."""
        self.enable = False
        self.sub.unregister()


if __name__ == '__main__':
    # Initialize the node and name it.
    node_name = "MapListenerNode"
    rospy.init_node(node_name, anonymous=True)

    mapListener = MapListener()

    # Go to the main loop
    try:
        mapListener.start()
        # Wait for messages on topic, go to callback function when new messages arrive.
        # spin() simply keeps python from exiting until this node is stopped
        rospy.spin()
    # Stop with Ctrl + C
    except KeyboardInterrupt:
        mapListener.stop()

        nodes = os.popen("rosnode list").readlines()
        for i in range(len(nodes)):
            nodes[i] = nodes[i].replace("\n","")

        for node in nodes:
            os.system("rosnode kill " + node_name)


        print("Node stopped")

This could be run with

rosrun map_listener mapListener.py

after adding

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS scripts/mapListener.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

to CMakeLists.txt. As you can see I created a sub folder scripts for my Python script.

With Python my image is only gray. The value of msg.data[i] is always -1.

Thanks in advance

    def mapConvert(self, msg):
        header = msg.header

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

        buff=array.array('B')

        for i in range(0, msg.info.width*msg.info.height):
            if(msg.data[i] >= 0 and msg.data[i] <= 0):
                buff.append(254)
            elif(msg.data[i] <= 100 and msg.data[i] >= 100):
                buff.append(000)
            else:
                buff.append(205)

        try:
            out = open(mapdatafile, 'wb')
        except IOError:
            rospy.loginfo("Couldn't save map file to %s", mapdatafile)

        pgmHeader = 'P5' + '\n' + str(msg.info.width) + '  ' + str(msg.info.height) + '  ' + str(255) + '\n'
        out.write(bytearray(pgmHeader, "utf-8"))

        buff.tofile(out)
        out.close()