def numpy_to_imgmsg(image, stamp=None):
from sensor_msgs.msg import Image
rosimage = Image()
rosimage.height = image.shape[0]
rosimage.width = image.shape[1]
if image.dtype == numpy.uint8:
rosimage.encoding = '8UC%d' % image.shape[2]
rosimage.step = image.shape[2] * rosimage.width
rosimage.data = image.ravel().tolist()
else:
rosimage.encoding = '32FC%d' % image.shape[2]
rosimage.step = image.shape[2] * rosimage.width * 4
rosimage.data = numpy.array(image.flat, dtype=numpy.float32).tostring()
if stamp is not None:
rosimage.header.stamp = stamp
return rosimage
def _publish_image(self):
"""
Publish latest camera image as Image with CameraInfo.
"""
# only publish if we have a subscriber
if self._image_pub.get_num_connections() == 0:
return
# get latest image from vectors's camera
camera_image = self._vector.camera.latest_image
if camera_image is not None:
# convert image to gray scale
img = camera_image.convert('RGB')
# 640,360 image size?
# img = camera_image
ros_img = Image()
ros_img.encoding = 'rgb8'
ros_img.width = img.size[0]
ros_img.height = img.size[1]
ros_img.step = ros_img.width
ros_img.data = img.tobytes()
ros_img.header.frame_id = 'vector_camera'
# vector_time = camera_image.image_recv_time
# ros_img.header.stamp = rospy.Time.from_sec(vector_time)
ros_img.header.stamp = rospy.Time.now()
# publish images and camera info
self._image_pub.publish(ros_img)
def applyDistortionMap(self, map, inplace=False, order=0):
# for some reason, isinstance(map, fisheye.DistortionMap)
# or isinstance(map, DistortionMap) does not work?!
# this solves it...
if not map.__class__.__name__ == "DistortionMap":
raise TypeError("map has to be a DistortionMap")
if not np.shape(self.data)[:2] == map.src_shape:
logger.warning("Map source shape is not defined or does not match!")
if inplace: image = self # operate on this image
else: image = Image( self ) # copy over this image
# apply map
logger.debug("applying distortion map...")
# This is NOT very elegant...
# I don't know a better possibility to loop over the last index
# than this. The problem is, that a grayscale image has no third index.
# ...
if len(np.shape(image.data)) == 3:
for layer in range(np.shape(image.data)[2]):
layerout = scipy.ndimage.interpolation.map_coordinates(
input = self.data[:,:,layer],
coordinates = map.T, # map has to be transposed somehow
order = order
)
try: out = np.dstack( (out, layerout) ) # add to stack
except: out = layerout # first addition
image.data = out # set image data
#image.data = scipy.ndimage.filters.median_filter(image.data,(5,5,1))
else: # only 2 dim...
image.data = scipy.ndimage.interpolation.map_coordinates(
input = image.data,
coordinates = map.T, # map has to be transposed somehow
order = order
)
#image.data = scipy.ndimage.filters.median_filter(image.data,(5,5))
# set coordinates from DistortionMap
image.coordinates = map.out_coord
logger.debug("done applying distortion map.")
if not inplace: # return new image if not inplace
return image
def pil_to_imgmsg(image, encodingmap={'L': 'mono8', 'RGB': 'rgb8', 'RGBA': 'rgba8', 'YCbCr': 'yuv422'},
PILmode_channels={'L': 1, 'RGB': 3, 'RGBA': 4, 'YCbCr': 3}):
# import roslib # @UnresolvedImport @UnusedImport
# import rospy # @UnresolvedImport @UnusedImport
from sensor_msgs.msg import Image
# from sensor_msgs.msg import CompressedImage # @UnusedImport @UnresolvedImport
rosimage = Image()
# adam print 'Channels image.mode: ',PILmode_channels[image.mode]
rosimage.encoding = encodingmap[image.mode]
(rosimage.width, rosimage.height) = image.size
rosimage.step = PILmode_channels[image.mode] * rosimage.width
rosimage.data = image.tostring()
return rosimage
def getTextureImagePair(imageName):
"""
Generate a single texture ID and image pair.
"""
textureImage = Image()
textureID = glGenTextures(1)
try:
imageFile = open(imageName)
textureImage.sizeX = imageFile.size[0]
textureImage.sizeY = imageFile.size[1]
textureImage.data = imageFile.tobytes("raw", "RGBX", 0, -1)
except:
print(IMAGE_NOT_FOUND)
sys.exit()
return (textureID, textureImage)
def callback(self, data):
#points.append(171,224)
data.data
points_msg = Image()
bridge = CvBridge()
IMAGE_HEIGHT = 171
IMAGE_WIDTH = 224
points_msg.header.stamp = rospy.Time.now()
points_msg.header.frame_id = "camera_depth_optical_frame"
points_msg.encoding = "32FC1"
points_msg.height = IMAGE_HEIGHT
points_msg.width = IMAGE_WIDTH
points_msg.data = bridge.cv2_to_imgmsg(points_img, '32FC1').data
points_msg.is_bigendian = 0
points_msg.step = points_msg.width * 4
cvImage = bridge.cv2_to_imgmsg(points_img, encoding='passthrough')
cvImPub = rospy.Publisher('/cvImage_array', Image, queue_size=1)
cvImPub.publish(points_msg)
def from_file(file_path):
""" Creates an Image instance from a png file.
Parameters:
-----------
file_path : str
The path to the png file.
Returns:
--------
image : Image
The image instance
palette : agb.palette
The palette of the image
"""
with open(file_path, 'rb') as f:
reader = png.Reader(f)
width, height, data, attributes = reader.read()
image = Image(None, width, height, depth=attributes['bitdepth'])
image.data = np.array([*data]).T
colors = attributes['palette']
_palette = palette.Palette(colors)
return image, _palette
def open(filepath):
pim = PIL.Image.open(filepath)
im = Image(*pim.size)
im.data = multiprocessing.RawArray('B', [v for rgb in pim.getdata() for val in rgb])
return im