Python msg.Image类代码示例

def CreateMonoBag(imgs,bagname):
    '''Creates a bag file with camera images'''
    bag =rosbag.Bag(bagname, 'w')

    try:
        for i in range(len(imgs)):
            print("Adding %s" % imgs[i])
            fp = open( imgs[i], "r" )
            p = ImageFile.Parser()

            while 1:
                s = fp.read(1024)
                if not s:
                    break
                p.feed(s)

            im = p.close()

            Stamp = rospy.rostime.Time.from_sec(time.time())
            Img = Image()
            Img.header.stamp = Stamp
            Img.width = im.size[0]
            Img.height = im.size[1]
            Img.encoding = "rgb8"
            Img.header.frame_id = "camera"
            Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
            Img.data = Img_data

            bag.write('camera/image_raw', Img, Stamp)
    finally:
        bag.close()       

def CreateMonoBag(imgs,bagname):
    '''Creates a bag file with camera images'''
    bag = rosbag.Bag(bagname, 'w')
    imgs = sorted(imgs)

    try:
        for i in range(len(imgs)):
            print("Adding %s" % imgs[i])
            fp = open( imgs[i], "r" )
            p = ImageFile.Parser()

            rgb_file = imgs[i]
            llim = rgb_file.rfind('/')
            rlim = rgb_file.rfind('.')
            rgb_ext = rgb_file[rlim:]
            msec = rgb_file[llim+1:rlim]
            sec = float(msec) / 1000 # msec to sec

            while 1:
                s = fp.read(1024)
                if not s:
                    break
                p.feed(s)

            im = p.close()

            # Stamp = rospy.rostime.Time.from_sec(time.time())
            Stamp = rospy.rostime.Time.from_sec(sec)
            Img = Image()
            Img.header.stamp = Stamp
            Img.width = im.size[0]
            Img.height = im.size[1]
            Img.encoding = "rgb8"
            Img.header.frame_id = "camera"
            Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
            Img.data = Img_data

            bag.write('camera/rgb/image_color', Img, Stamp)

            #####
            d_file = rgb_file.replace(rgb_ext, '.txt')
            print("Adding %s" % d_file)
            fid = open(d_file, 'r')
            raw = fid.readlines()
            fid.close()
            #depth = numpy.reshape(raw, (im.size[1], im.size[0]))

            Img_depth = Image()
            Img_depth.header.stamp = Stamp
            Img_depth.width = im.size[0]
            Img_depth.height = im.size[1]
            Img_depth.encoding = "rgb8"
            Img_depth.header.frame_id = "camera"
            #Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
            Img_depth.data = raw
            bag.write('camera/depth/image', Img, Stamp)
    finally:
        bag.close()       

 def appendMessages(self, stamp, messages):
     if not self._image is None:
         # Build the image message and push it on the message list
         msg = Image()
         msg.header.stamp = stamp
         msg.header.frame_id = self._frameId
         msg.width = self._image.shape[0]
         msg.height = self._image.shape[1]
         if (len(self._image.shape) == 2) or (len(self._image.shape) == 3 and self._image.shape[2] == 1):
             # A gray image
             msg.encoding = '8UC1'
             stepMult = 1
         elif len(self._image.shape) == 3 and self._image.shape[2] == 3:
             # A color image
             msg.encoding = 'rgb8'
             stepMult = 3
         elif len(self._image.shape) == 3 and self._image.shape[2] == 4:
             # A color image
             msg.encoding = 'rgba8'
             stepMult = 3
         else:
             raise RuntimeError("The parsing of images is very simple. " +\
                                "Only 3-channel rgb (rgb8), 4 channel rgba " +\
                                "(rgba8) and 1 channel mono (mono8) are " +\
                                "supported. Got an image with shape " +\
                                "{0}".format(self._image.shape))
         msg.is_bigendian = False
         msg.step = stepMult * msg.width
         msg.data = self._image.flatten().tolist()
         messages.append((self._topic, msg))

def CreateStereoBag(left_imgs, right_imgs, bagname):
    '''Creates a bag file containing stereo image pairs'''
    bag =rosbag.Bag(bagname, 'w')

    try:
        for i in range(len(left_imgs)):
            print("Adding %s" % left_imgs[i])
            fp_left = open( left_imgs[i], "r" )
            p_left = ImageFile.Parser()

            while 1:
                s = fp_left.read(1024)
                if not s:
                    break
                p_left.feed(s)

            im_left = p_left.close()

            fp_right = open( right_imgs[i], "r" )
            print("Adding %s" % right_imgs[i])
            p_right = ImageFile.Parser()

            while 1:
                s = fp_right.read(1024)
                if not s:
                    break
                p_right.feed(s)

            im_right = p_right.close()

            Stamp = roslib.rostime.Time.from_sec(time.time())

            Img_left = Image()
            Img_left.header.stamp = Stamp
            Img_left.width = im_left.size[0]
            Img_left.height = im_left.size[1]
            Img_left.encoding = "rgb8"
            Img_left.header.frame_id = "camera/left"
            Img_left_data = [pix for pixdata in im_left.getdata() for pix in pixdata]
            Img_left.data = Img_left_data
            Img_right = Image()
            Img_right.header.stamp = Stamp
            Img_right.width = im_right.size[0]
            Img_right.height = im_right.size[1]
            Img_right.encoding = "rgb8"
            Img_right.header.frame_id = "camera/right"
            Img_right_data = [pix for pixdata in im_right.getdata() for pix in pixdata]
            Img_right.data = Img_right_data

            bag.write('camera/left/image_raw', Img_left, Stamp)
            bag.write('camera/right/image_raw', Img_right, Stamp)
    finally:
        bag.close()

def cv_to_imgmsg(cvim, encoding = "passthrough"):
    try:
        return bridge.cv_to_imgmsg(cvim, encoding)
    except:
        img_msg = Image()
        (img_msg.width, img_msg.height) = cv.GetSize(cvim)
        if encoding == "passthrough":
            img_msg.encoding = bridge.cvtype_to_name[cv.GetElemType(cvim)]
        else:
            img_msg.encoding = encoding
            if encoding_as_cvtype(encoding) != cv.GetElemType(cvim):
                raise CvBridgeError, "invalid encoding"
        img_msg.data = cvim.tostring()
        img_msg.step = len(img_msg.data) / img_msg.height
        return img_msg

def convert(data):
	width=data.info.width
	height=data.info.height
	pixelList = [0] *width*height
	
	imageMsg=Image()
	imageMsg.header.stamp = rospy.Time.now()
	imageMsg.header.frame_id = '1'
	imageMsg.height = height
	imageMsg.width = width
	imageMsg.encoding = 'mono8'
	imageMsg.is_bigendian = 0
	imageMsg.step = width
	
	for h in range(height):
		for w in range(width):
			if data.data[h*width+w]==-1:
				pixelList[h*width+w] = 150
			elif data.data[h*width+w]==0:
				pixelList[h*width+w] = 0
			elif data.data[h*width+w]==100:
				pixelList[h*width+w] = 255	
			else:
				pixelList[h*width+w]=data.data[h*width+w]
				print 'ERROR'
				
	imageMsg.data = pixelList
	imagePub.publish(imageMsg)

def airpub():
    pub = rospy.Publisher("airsim/image_raw", Image, queue_size=1)
    rospy.init_node('image_raw', anonymous=True)
    rate = rospy.Rate(10) # 10hz

    # connect to the AirSim simulator 
    client = airsim.MultirotorClient()
    client.confirmConnection()

    while not rospy.is_shutdown():
         # get camera images from the car
        responses = client.simGetImages([
            airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)])  #scene vision image in uncompressed RGBA array

        for response in responses:
            img_rgba_string = response.image_data_uint8

        # Populate image message
        msg=Image() 
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = "frameId"
        msg.encoding = "rgba8"
        msg.height = 360  # resolution should match values in settings.json 
        msg.width = 640
        msg.data = img_rgba_string
        msg.is_bigendian = 0
        msg.step = msg.width * 4

        # log time and size of published image
        rospy.loginfo(len(response.image_data_uint8))
        # publish image message
        pub.publish(msg)
        # sleep until next cycle
        rate.sleep()

 def publish( self ):
     # Get the image.
     image = self.__videoDeviceProxy.getImageRemote( self.__videoDeviceProxyName );
         
     # Create Image message.
     ImageMessage = Image();
     ImageMessage.header.stamp.secs = image[ 5 ];
     ImageMessage.width = image[ 0 ];
     ImageMessage.height = image[ 1 ];
     ImageMessage.step = image[ 2 ] * image[ 0 ];
     ImageMessage.is_bigendian = False;
     ImageMessage.encoding = 'bgr8';
     ImageMessage.data = image[ 6 ];
     
     self.__imagePublisher.publish( ImageMessage );
     
     # Create CameraInfo message.
     # Data from the calibration phase is hard coded for now.
     CameraInfoMessage = CameraInfo();
     CameraInfoMessage.header.stamp.secs = image[ 5 ];
     CameraInfoMessage.width = image[ 0 ];
     CameraInfoMessage.height = image[ 1 ];
     CameraInfoMessage.D = [ -0.0769218451517258, 0.16183180613612602, 0.0011626049774280595, 0.0018733894100460534, 0.0 ];
     CameraInfoMessage.K = [ 581.090096189648, 0.0, 341.0926325830606, 0.0, 583.0323248080421, 241.02441593704128, 0.0, 0.0, 1.0 ];
     CameraInfoMessage.R = [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ];
     CameraInfoMessage.P = [ 580.5918359588198, 0.0, 340.76398441334106, 0.0, 0.0, 582.4042541534321, 241.04182225157172, 0.0, 0.0, 0.0, 1.0, 0.0] ;
     CameraInfoMessage.distortion_model = 'plumb_bob';
     
     #CameraInfoMessage.roi.x_offset = self.__ROIXOffset;
     #CameraInfoMessage.roi.y_offset = self.__ROIYOffset;
     #CameraInfoMessage.roi.width = self.__ROIWidth;
     #CameraInfoMessage.roi.height = self.__ROIHeight;
     #CameraInfoMessage.roi.do_rectify = self.__ROIDoRectify;
     
     self.__cameraInfoPublisher.publish( CameraInfoMessage );

    def default(self, ci="unused"):
        if not self.component_instance.capturing:
            return  # press [Space] key to enable capturing

        image_local = self.data["image"]

        image = Image()
        image.header = self.get_ros_header()
        image.header.frame_id += "/base_image"
        image.height = self.component_instance.image_height
        image.width = self.component_instance.image_width
        image.encoding = "rgba8"
        image.step = image.width * 4

        # VideoTexture.ImageRender implements the buffer interface
        image.data = bytes(image_local)

        # fill this 3 parameters to get correcty image with stereo camera
        Tx = 0
        Ty = 0
        R = [1, 0, 0, 0, 1, 0, 0, 0, 1]

        intrinsic = self.data["intrinsic_matrix"]

        camera_info = CameraInfo()
        camera_info.header = image.header
        camera_info.height = image.height
        camera_info.width = image.width
        camera_info.distortion_model = "plumb_bob"
        camera_info.K = [
            intrinsic[0][0],
            intrinsic[0][1],
            intrinsic[0][2],
            intrinsic[1][0],
            intrinsic[1][1],
            intrinsic[1][2],
            intrinsic[2][0],
            intrinsic[2][1],
            intrinsic[2][2],
        ]
        camera_info.R = R
        camera_info.P = [
            intrinsic[0][0],
            intrinsic[0][1],
            intrinsic[0][2],
            Tx,
            intrinsic[1][0],
            intrinsic[1][1],
            intrinsic[1][2],
            Ty,
            intrinsic[2][0],
            intrinsic[2][1],
            intrinsic[2][2],
            0,
        ]

        self.publish(image)
        self.topic_camera_info.publish(camera_info)

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 == np.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 = np.array(image.flat, dtype=np.float32).tostring()
    if stamp is not None:
        rosimage.header.stamp = stamp
    return rosimage

def copy_Image_empty_data(image_old):
    image_new = Image()
    image_new.header = copy(image_old.header)
    image_new.height = copy(image_old.height)
    image_new.width = copy(image_old.width)
    image_new.encoding = copy(image_old.encoding)
    image_new.is_bigendian = copy(image_old.is_bigendian)
    image_new.step = copy(image_old.step)
    return image_new

    def callback(self, data):
        #print "got some shit coming in"
        #if data is not None:
            #plane= data.pose.position
            #nrm= norm([plane.x, plane.y, plane.z])

            #normal= np.array([plane.x, plane.y, plane.z])/nrm

            #print "got here"

            ##replace with numpy array
            #plane= np.array([plane.x, plane.y, plane.z])

            ##get the rotation matrix
            #rmatrix= rotationMatrix(normal)

            ##print rmatrix

            ##for point in data.points:
                ##print point
                ##p= np.array([point.x, point.y, point.z])
                ##flattened_point= project(p, plane, normal)
                ##print flattened_point
                ##print np.dot(rmatrix,flattened_point)

        try:
            resp= self.seperation()
            print resp.result
            #self.pub.publish(resp.clusters[0])
            im= Image()
            im.header.seq= 72
            im.header.stamp.secs= 1365037570
            im.header.stamp.nsecs= 34077284
            im.header.frame_id= '/camera_rgb_optical_frame'
            im.height= 480
            im.width= 640
            im.encoding= '16UC1'
            im.is_bigendian= 0
            im.step= 1280
            im.data= [100 for n in xrange(1280*480)]

            for point in resp.clusters[0].points:
                x= point.x * 640
                y= point.y * 480
                im.data[y*1280 + x] = 10

            pub_image.publish(im)

        except Exception, e:
            print "service call failed"
            print e

    def process_frame(self,cam_id,buf,buf_offset,timestamp,framenumber):
        if have_ROS:
            msg = Image()
            msg.header.seq=framenumber
            msg.header.stamp=rospy.Time.from_sec(timestamp)
            msg.header.frame_id = "0"

            npbuf = np.array(buf)
            (height,width) = npbuf.shape

            msg.height = height
            msg.width = width
            msg.encoding = self.encoding
            msg.step = width
            msg.data = npbuf.tostring() # let numpy convert to string

            with self.publisher_lock:
                cam_info = self.camera_info
                cam_info.header.stamp = msg.header.stamp
                cam_info.header.seq = msg.header.seq
                cam_info.header.frame_id = msg.header.frame_id
                cam_info.width = width
                cam_info.height = height

                self.publisher.publish(msg)
                self.publisher_cam_info.publish(cam_info)
        return [],[]

    def __init__(self):
        rospy.init_node('image_publish')
        name = sys.argv[1]
        image = cv2.imread(name)
        #cv2.imshow("im", image)
        #cv2.waitKey(5)

        hz = rospy.get_param("~rate", 1)
        frame_id = rospy.get_param("~frame_id", "map")
        use_image = rospy.get_param("~use_image", True)
        rate = rospy.Rate(hz)

        self.ci_in = None
        ci_sub = rospy.Subscriber('camera_info_in', CameraInfo,
                                  self.camera_info_callback, queue_size=1)

        if use_image:
            pub = rospy.Publisher('image', Image, queue_size=1)
        ci_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=1)

        msg = Image()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = frame_id
        msg.encoding = 'bgr8'
        msg.height = image.shape[0]
        msg.width = image.shape[1]
        msg.step = image.shape[1] * 3
        msg.data = image.tostring()
        if use_image:
            pub.publish(msg)

        ci = CameraInfo()
        ci.header = msg.header
        ci.height = msg.height
        ci.width = msg.width
        ci.distortion_model ="plumb_bob"
        # TODO(lucasw) need a way to set these values- have this node
        # subscribe to an input CameraInfo?
        ci.D = [0.0, 0.0, 0.0, 0, 0]
        ci.K = [500.0, 0.0, msg.width/2, 0.0, 500.0, msg.height/2, 0.0, 0.0, 1.0]
        ci.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
        ci.P = [500.0, 0.0, msg.width/2, 0.0, 0.0, 500.0, msg.height/2, 0.0,  0.0, 0.0, 1.0, 0.0]
        # ci_pub.publish(ci)

        # TODO(lwalter) only run this is hz is positive,
        # otherwise wait for input trigger message to publish an image
        while not rospy.is_shutdown():
            if self.ci_in is not None:
                ci = self.ci_in

            msg.header.stamp = rospy.Time.now()
            ci.header = msg.header
            if use_image:
                pub.publish(msg)
            ci_pub.publish(ci)

            if hz <= 0:
                rospy.sleep()
            else:
                rate.sleep()

    def main_loop(self):
        img = Image()
        while not rospy.is_shutdown():
            #print "getting image..",
            image = self.camProxy.getImageRemote(self.nameId)
            #print "ok"
            # TODO: better time
            img.header.stamp = rospy.Time.now()
            img.header.frame_id = self.frame_id
            img.height = image[1]
            img.width = image[0]
            nbLayers = image[2]
            #colorspace = image[3]
            if image[3] == kYUVColorSpace:
                encoding = "mono8"
            elif image[3] == kRGBColorSpace:
                encoding = "rgb8"
            elif image[3] == kBGRColorSpace:
                encoding = "bgr8"
            else:
                rospy.logerror("Received unknown encoding: {0}".format(image[3]))

            img.encoding = encoding
            img.step = img.width * nbLayers
            img.data = image[6]
            self.info_.width = img.width
            self.info_.height = img.height
            self.info_.header = img.header
            self.pub_img_.publish(img)
            self.pub_info_.publish(self.info_)
            rospy.sleep(0.0001)# TODO: is this necessary?


        self.camProxy.unsubscribe(self.nameId)