createBitMap.py

Go to the documentation of this file.

#!/usr/bin/env python

import roslib; roslib.load_manifest('ar_kinect')
import rospy
import cv
import cv2
import numpy as np
from nav_msgs.msg import OccupancyGrid, MapMetaData

def callback(data):
    #Darstellung Array in Bitmap-Datei
    # cv.CV_32UC1
    #        || |
    #        || |____ Anzahl der Kanaele, entspricht Anzahl der parallel uebergebenen Matritzen. 
    #        ||
    #        ||______ U .. Unsigned, F ... Float, S ... Signed - Datentyp der Elemente der Matrix.     
    #        |_______ Anzahl der Pixel mit welchem ein Element der Matrix dargestellt wird (8, 16, 32, oder 64).
    #
    mat = cv.CreateMat(data.info.height, data.info.width, cv.CV_64FC1)
    for y in range(data.info.height):
        for x in range(data.info.width):
            if data.data[y*data.info.width+x] == 0:
                mat[y,x] = 255
            else:
                mat[y,x] = data.data[y*data.info.width+x]
          
    a = np.asarray(mat)
    params = list()
    params.append(cv2.cv.CV_IMWRITE_PXM_BINARY)
    params.append(0)
    cv2.imwrite("/home/janina/ros_workspace/Testbild.pbm", a, params)

    
if __name__ == '__main__':
    rospy.init_node('createBitMap', anonymous=True)
    rospy.Subscriber('occ_grid_map', OccupancyGrid, callback)
    rospy.spin()