Example #1
0
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()       
Example #2
0
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()
Example #3
0
 def publish_image(self):
     # get the image from the Nao
     img = self.nao_cam.getImageRemote(self.proxy_name)
     # copy the data into the ROS Image
     ros_img = Image()
     ros_img.width = img[0]
     ros_img.height = img[1]
     ros_img.step = img[2] * img[0]
     ros_img.header.stamp.secs = img[5]
     ros_img.data = img[6]
     ros_img.is_bigendian = False
     ros_img.encoding = "rgb8"
     ros_img.data = img[6]
     # publish the image
     self.nao_cam_pub.publish(ros_img)
Example #4
0
    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)
Example #5
0
def post_image(self, component_instance):
    """ Publish the data of the Camera as a ROS-Image message.

    """
    image_local = component_instance.local_data['image']
    if not image_local or image_local == '' or not image_local.image or not component_instance.capturing:
        return # press [Space] key to enable capturing

    parent_name = component_instance.robot_parent.blender_obj.name

    image = Image()
    image.header.stamp = rospy.Time.now()
    image.header.seq = self._seq
    # http://www.ros.org/wiki/geometry/CoordinateFrameConventions#Multi_Robot_Support
    image.header.frame_id = ('/' + parent_name + '/base_image')
    image.height = component_instance.image_height
    image.width = component_instance.image_width
    image.encoding = 'rgba8'
    image.step = image.width * 4
    # NOTE: Blender returns the image as a binary string encoded as RGBA
    # sensor_msgs.msg.Image.image need to be len() friendly
    # TODO patch ros-py3/common_msgs/sensor_msgs/src/sensor_msgs/msg/_Image.py
    # to be C-PyBuffer "aware" ? http://docs.python.org/c-api/buffer.html
    image.data = bytes(image_local.image)
    # RGBA8 -> RGB8 ? (remove alpha channel, save h*w bytes, CPUvore ?)
    # http://wiki.blender.org/index.php/Dev:Source/GameEngine/2.49/VideoTexture
    # http://www.blender.org/documentation/blender_python_api_2_57_release/bge.types.html#bge.types.KX_Camera.useViewport

    for topic in self._topics:
        # publish the message on the correct topic
        if str(topic.name) == str("/" + parent_name + "/" + component_instance.blender_obj.name):
            topic.publish(image)

    self._seq = self._seq + 1
Example #6
0
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 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)
Example #8
0
 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))
Example #9
0
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()       
Example #10
0
    def main_loop(self):
        img = Image()
        r = rospy.Rate(self.fps)
        while not rospy.is_shutdown():
            image = self.camProxy.getImageRemote(self.nameId)
            stampAL = image[4] + image[5]*1e-6
            #print image[5],  stampAL, "%lf"%(stampAL)
            img.header.stamp = rospy.Time(stampAL)
            img.header.frame_id = self.frame_id
            img.height = image[1]
            img.width = image[0]
            nbLayers = image[2]
            if image[3] == kYUVColorSpace:
                encoding = "mono8"
            elif image[3] == kRGBColorSpace:
                encoding = "rgb8"
            elif image[3] == kBGRColorSpace:
                encoding = "bgr8"
            elif image[3] == kYUV422ColorSpace:
                encoding = "yuv422" # this works only in ROS groovy and later
            else:
                rospy.logerror("Received unknown encoding: {0}".format(image[3]))

            img.encoding = encoding
            img.step = img.width * nbLayers
            img.data = image[6]
            infomsg = self.cim.getCameraInfo()
            infomsg.header = img.header
            self.pub_info_.publish(infomsg)
            self.pub_img_.publish(img)
            r.sleep()


        self.camProxy.unsubscribe(self.nameId)
Example #11
0
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
Example #12
0
    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()
Example #13
0
    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 [],[]
Example #14
0
 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 );
Example #15
0
    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)
Example #16
0
    def main_loop(self):
        img = Image()
        while not rospy.is_shutdown():
            #print "getting image..",
            images = self.camProxy.getImagesRemote  (self.nameId)
            #print "ok"
            # TODO: better time
            for i in [0,1]:
		#print len(images[i])
                image = images[i]
                img.header.stamp = rospy.Time.now()
                if image[7] == 0:
                    img.header.frame_id = "/CameraTop_frame"
                elif image[7] == 1:
                    img.header.frame_id = "/CameraBottom_frame"
                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
		if len(images) >= 2:
                	img.data = images[2][len(images[2])/2 * i:len(images[2])/2 *(i+1) + 1]
		else:
			img.data = []
			print "image with no data"
                self.info_[i].width = img.width
                self.info_[i].height = img.height
                self.info_[i].header = img.header
                self.pub_img_[i].publish(img)
                self.pub_info_[i].publish(self.info_[i])

	    self.camProxy.releaseImages(self.nameId)

        self.camProxy.unsubscribe(self.nameId)
	def publishCombined(self):
		#Enter Main Loop
		while not rospy.is_shutdown():
			
			#Convert to Numpy  Arrays
			map = []
			for i in range(0, self.numRobots):
				map.append(numpy.array(self.searchedData[i].data))
				
			combined2 = map[0]
			if self.numRobots > 1:
				#Find Minimum of all maps
				for i in range(1, self.numRobots):
					combined2 = numpy.minimum(combined2,map[i])
					
			#Pack Occupancy Grid Message
			mapMsg=OccupancyGrid()
			mapMsg.header.stamp=rospy.Time.now()
			mapMsg.header.frame_id=self.mapData.header.frame_id
			mapMsg.info.resolution=self.mapData.info.resolution
			mapMsg.info.width=self.mapData.info.width
			mapMsg.info.height=self.mapData.info.height
			mapMsg.info.origin=self.mapData.info.origin
			mapMsg.data=combined2.tolist()
			
			#Convert combined Occupancy grid values to grayscal image values
			combined2[combined2 == -1] = 150			#Unknown -1->150 		(gray)
			combined2[combined2 == 100] = 255			#Not_Searched 100->255	(white)
														#Searched=0				(black)
														
			#Calculate percentage of open area searched
			numNotSearched = combined2[combined2==255].size
			numSearched = combined2[combined2==0].size
			percentSearched = 100*float(numSearched)/(numNotSearched+numSearched)
			percentSearchedMsg = Float32()
			percentSearchedMsg.data = percentSearched
			self.percentPub.publish(percentSearchedMsg)
			
			#Pack Image Message
			imageMsg=Image()
			imageMsg.header.stamp = rospy.Time.now()
			imageMsg.header.frame_id = self.mapData.header.frame_id
			imageMsg.height = self.mapData.info.height
			imageMsg.width = self.mapData.info.width
			imageMsg.encoding = 'mono8'
			imageMsg.is_bigendian = 0
			imageMsg.step = self.mapData.info.width
			imageMsg.data = combined2.tolist()
			
			#Publish Combined Occupancy Grid and Image
			self.searchedCombinePub.publish(mapMsg)
			self.imagePub.publish(imageMsg)
			
			#Update Every 0.5 seconds
			rospy.sleep(1.0)
Example #18
0
 def __data_to_image(self, data):
     """
     Transforms input state format to Image msg, displayable in rviz.
     :param data: input state
     """
     msg = Image()
     msg.header.frame_id = "/base_footprint"
     msg.height = data.shape[1]
     msg.width = data.shape[0]
     msg.encoding = "mono8"
     msg.data = np.uint8(np.ndarray.flatten(data, order='F'))[::-1].tolist()
     return msg
Example #19
0
 def shot(self, data=None):
     img_to_send = Image()
     image = self.vision.getImageRemote(self.nameId)
     img_to_send.header.stamp = rospy.Time.now()
     img_to_send.height = image[1]
     img_to_send.width = image[0]
     layers = image[2]  #3 for RGB
     img_to_send.encoding = "8UC3"  #8 unsigned bit 3 channel
     img_to_send.step = img_to_send.width * layers
     img_to_send.data = image[6]
     rospy.loginfo('Sending image...')
     return ShotResponse(img_to_send)
def cv2_to_imgmsg(cv_image):

    img_msg = Image()
    img_msg.height = cv_image.shape[0]
    img_msg.width = cv_image.shape[1]
    img_msg.encoding = "bgr8"
    img_msg.is_bigendian = 0
    img_msg.data = cv_image.tostring()
    img_msg.step = len(
        img_msg.data
    ) // img_msg.height  # That double line is actually integer division, not a comment
    return img_msg
Example #21
0
 def write(self, data):
     
     # Publish raw image
     data_y = data[:RES[0]*RES[1]]
     msg = Image()
     msg.header.stamp = rospy.Time.now()
     msg.width = RES[0]
     msg.height = RES[1]
     msg.encoding = "mono8"
     msg.step = len(data_y) // RES[1]
     msg.data = data_y
     self.pub_img.publish(msg)
    def _publish_img(self, obs):
        # Hardcoded Implementation of ros_numpy's ImageConverter
        img_msg = Image(encoding='uint8')
        img_msg.height, img_msg.width, _ = obs.shape
        contig = np.ascontiguousarray(obs)
        img_msg.data = contig.tostring()
        img_msg.step = contig.strides[0]
        img_msg.is_bigendian = (obs.dtype.byteorder == '>'
                                or obs.dtype.byteorder == '='
                                and sys.byteorder == 'big')

        self.cam_pub.publish(img_msg)
Example #23
0
 def parse_seg(self, seg):
     # seg.convert(cc.CityScapesPalette)
     array = np.frombuffer(seg.raw_data, dtype=np.dtype("uint8")).copy()
     not_road = array != 7
     array[not_road] = 0
     array[~not_road] = 255
     img_to_publish = Image()
     img_to_publish.data = array.tolist()
     img_to_publish.encoding = 'bgra8'
     img_to_publish.width = seg.width
     img_to_publish.height = seg.height
     self.seg_pub.publish(img_to_publish)
Example #24
0
    def decode_image(self, frame, orig_msg):
        msg = Image()
        msg.data = frame.to_rgb().planes[0].to_bytes()
        msg.width = frame.width
        msg.height = frame.height
        msg.step = frame.width * 3
        msg.is_bigendian = 0
        msg.encoding = 'rgb8'
        msg.header = Header()
        msg.header = orig_msg.header

        self.pub.publish(msg)
Example #25
0
    def image_callback(self, img_msg):

        a = datetime.now()

        n_channels = 3
        dtype = 'uint8'
        img_buf = np.asarray(img_msg.data, dtype=dtype)

        image_np = np.ndarray(shape=(img_msg.height, img_msg.width,
                                     n_channels),
                              dtype=dtype,
                              buffer=img_buf)

        # Expand dimensions since the model expects images to have shape: [1, None, None, 3]
        image_np_expanded = np.expand_dims(image_np, axis=0)

        # Actual detection.
        (boxes, scores, classes, num) = self.session.run(
            [
                self.detection_boxes, self.detection_scores,
                self.detection_classes, self.num_detections
            ],
            feed_dict={self.image_tensor: image_np_expanded})
        # Visualization of the results of a detection.
        vis_util.visualize_boxes_and_labels_on_image_array(
            image_np,
            np.squeeze(boxes),
            np.squeeze(classes).astype(np.int32),
            np.squeeze(scores),
            category_index,
            use_normalized_coordinates=True,
            line_thickness=8)

        # remove additional dimension
        classes = classes[0]
        scores = scores[0]
        num = num[0]

        b = datetime.now()
        c = b - a

        self.get_logger().info("handle_classify_image_srv took: %r" % c)

        img_msg = ImageMsg()

        img_msg.height = image_np.shape[0]
        img_msg.width = image_np.shape[1]
        img_msg.encoding = "bgr8"
        img_msg.data = image_np.tostring()
        img_msg.step = len(img_msg.data) // img_msg.height
        img_msg.header.frame_id = "world"

        self.pub.publish(img_msg)
Example #26
0
def image_process(image, params):
    #bridge = CvBridge()
    lower = params['lowerY']
    upper = params['upperY']
    debug_info = params['debug_info']
    global error
    try:
        #cv_image = bridge.imgmsg_to_cv2(image)
        raw_data = np.fromstring(image.data, np.uint8)
        cv_image = cv2.imdecode(raw_data, cv2.IMREAD_COLOR)
        gray = cv2.imdecode(raw_data, cv2.IMREAD_GRAYSCALE)
        _, gray = cv2.threshold(gray, 250, 255, cv2.THRESH_BINARY)
        y_len, x_len = gray.shape
        lower, upper = max(0, lower), min(upper, y_len)
        if debug_info: print(lower, upper, 'image crop in Y')
        gray = gray[lower:upper, :]
        y_len, x_len = gray.shape
        total_error = []
        for y in range(10, y_len, 90):
            weighted_mass = 0
            mass = 0
            for x in range(0, x_len):
                if gray[y, x] == 255:
                    mass += 1
                    weighted_mass += x
            if mass > 0:
                final_x = int(weighted_mass / mass)
                total_error.append(
                    float(final_x - x_len // 2) / float(x_len // 2))
                if params['display_processed_image']:
                    cv2.rectangle(gray, (final_x - 10, y),
                                  (final_x + 10, y + 35), 120, 2)
        #cv2.imshow('wooho', gray)

        if debug_info: print(total_error)
        if len(total_error) > 2:
            error = sum(total_error) / len(total_error)

        if params['display_image']: cv2.imshow('raw', cv_image)
        if params['display_processed_image']: cv2.imshow('processed', gray)
        if params['publish_processed_image']:
            msg = Image()
            msg.header.stamp = rospy.Time.now()
            msg.format = "jpeg"
            msg.data = np.array(cv2.imencode('.jpg', gray)[1]).tostring()
            params['img_pub'].publish(msg)

        cv2.waitKey(1)

    except Exception as e:
        if debug_info:
            print(e)
    def read_cam(self):
        cap = cv2.VideoCapture(
            "nvarguscamerasrc ! video/x-raw(memory:NVMM), width=(int)640, height=(int)360,format=(string)NV12, framerate=(fraction)7/1 ! nvvidconv ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR ! appsink"
        )
        #print(cap.isOpened())
        if cap.isOpened():
            #cv2.namedWindow("demo", cv2.WINDOW_AUTOSIZE)
            while not rospy.is_shutdown():
                ret_val, cv_image = cap.read()
                cv_image = cv2.flip(cv_image, -1)
                #print('Shape',cv_image.shape)
                # cv2.imshow('demo',cv_image)
                #print("cv_image", cv_image)

                try:
                    #bridge = CvBridge().cv2_to_imgmsg(cv_image, "bgr8")
                    #self.image_pub.publish(bridge)

                    #### Create CompressedIamge ####
                    msg = CompressedImage()
                    msg.header.stamp = rospy.Time.now()
                    msg.format = "jpeg"
                    msg.data = np.array(cv2.imencode('.jpg',
                                                     cv_image)[1]).tostring()

                    #### Create RawImage ####
                    msg_raw = Image()
                    msg_raw.header.stamp = rospy.Time.now()
                    msg_raw.encoding = "bgr8"
                    msg_raw.height = 360
                    msg_raw.width = 640
                    msg_raw.data = np.array(cv_image).tostring()
                    msg_raw.step = 640 * 3

                    # Publish new image
                    self.image_pub.publish(msg)
                    self.image_pub_raw.publish(msg_raw)

                except CvBridgeError as e:
                    print(e)

                keypressed = cv2.waitKey(1) % 256
                #print("key ",keypressed)

                if keypressed == 27:
                    print("escape key pressed")
                    cv2.destroyAllWindows()
                    break

        else:
            print("camera open failed, retrying")
            self.read_cam()
 def publish_image(self, image):
     img = Image()
     img.encoding = 'rgb8'
     img.width = 416
     img.height = 320
     img.step = img.width * 3
     img.data = image
     img.header.frame_id = 'raspicam'
     img.header.stamp = self._get_stamp(time())
     self._img_pub.publish(img)
     camera_info = self._camera_info_manager.getCameraInfo()
     camera_info.header = img.header
     self._camera_info_pub.publish(camera_info)
Example #29
0
def CreateDepthMessage(img_depth, img_time, sequence):
    msg_d = Image()
    bridge_d = CvBridge()
    msg_d.header.stamp = img_time
    msg_d.header.seq = sequence
    msg_d.header.frame_id = "world"
    msg_d.encoding = "32FC1"
    msg_d.height = img_depth.shape[0]
    msg_d.width = img_depth.shape[1]
    msg_d.data = bridge_d.cv2_to_imgmsg(img_depth, "32FC1").data
    msg_d.is_bigendian = 0
    msg_d.step = msg_d.width * 4
    return msg_d
Example #30
0
def publish_image(imgdata):
    image_temp = Image()
    header = Header(stamp=rospy.Time.now())
    header.frame_id = 'map'
    image_temp.height = IMAGE_HEIGHT
    image_temp.width = IMAGE_WIDTH
    image_temp.encoding = 'rgb8'
    image_temp.data = np.array(imgdata).tostring()
    #print(imgdata)
    #image_temp.is_bigendian=True
    image_temp.header = header
    image_temp.step = 1241 * 3
    image_pub.publish(image_temp)
Example #31
0
def publish_image(imgdata):
    image_temp = Image()
    header = Header(stamp=rospy.Time.now())
    header.frame_id = 'map'
    image_temp.height = np.shape(imgdata)[0]
    image_temp.width = np.shape(imgdata)[1]
    image_temp.encoding = 'rgb8'
    image_temp.data = np.array(imgdata).tostring()
    #print(imgdata)
    #image_temp.is_bigendian=True
    image_temp.header = header
    image_temp.step = np.shape(imgdata)[1] * 3
    img_pub.publish(image_temp)
Example #32
0
def write_img(ts_ns, img_array, camera, bag):
    ros_timestamp = nanoseconds_to_ros_timestamp(ts_ns)

    rosimage = Image()
    rosimage.data = img_array.tostring()
    rosimage.step = img_array.shape[
        1]  #only with mono8! (step = width * byteperpixel * numChannels)
    rosimage.encoding = "mono8"
    rosimage.height = img_array.shape[0]
    rosimage.width = img_array.shape[1]
    rosimage.header.stamp = ros_timestamp

    bag.write('/' + camera + '/image_raw', rosimage, t=ros_timestamp)
Example #33
0
def pubImage(publisher, nd_image):

    out_msg = Image()
    out_msg.height = nd_image.shape[0]
    out_msg.width = nd_image.shape[1]
    # print(out_vis_alpha_msg.height, out_vis_alpha_msg.width)
    out_msg.step = nd_image.strides[0]
    out_msg.encoding = 'bgr8'
    out_msg.header.frame_id = 'map'
    out_msg.header.stamp = rospy.Time.now()
    out_msg.data = nd_image.flatten().tolist()

    publisher.publish(out_msg)
Example #34
0
def CreateRGBMessage(img_rgb, img_time, sequence):
    msg_rgb = Image()
    bridge_rgb = CvBridge()
    msg_rgb.header.stamp = img_time
    msg_rgb.header.seq = sequence
    msg_rgb.header.frame_id = "world"
    msg_rgb.encoding = "bgr8"
    msg_rgb.height = img_rgb.shape[0]
    msg_rgb.width = img_rgb.shape[1]
    msg_rgb.data = bridge_rgb.cv2_to_imgmsg(img_rgb, "bgr8").data
    msg_rgb.is_bigendian = 0
    msg_rgb.step = msg_rgb.width * 3
    return msg_rgb
    def _create_image_and_info_messages(self, image):
        image_msg = Image()
        image_msg.height = image.shape[0]
        image_msg.width = image.shape[1]
        image_msg.encoding = 'bgr8'
        image_msg.step = image_msg.width * 3
        image_msg.data = array.array('B', image.tobytes())

        camera_info_msg = CameraInfo()
        camera_info_msg.height = image.shape[0]
        camera_info_msg.width = image.shape[1]

        return (image_msg, camera_info_msg)
    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
Example #37
0
def CreateMonoBag(imgs, bagname, timestamps):
    
    '''read time stamps'''
    file = open(timestamps, 'r')
    timestampslines = file.readlines()
    file.close()
    '''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()

            '''read image size'''
            imgpil = ImagePIL.open(imgs[0])
            width, height = imgpil.size
            # print "size:",width,height

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

            im = p.close()

            Stamp = rospy.rostime.Time.from_sec(float(timestampslines[i]))

            '''set image information '''
            Img = Image()
            Img.header.stamp = Stamp
            Img.height = height
            Img.width = width
            Img.header.frame_id = "camera"

            '''for rgb8'''
            # Img.encoding = "rgb8"
            # Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
            # Img.step = Img.width * 3

            '''for mono8'''
            Img.encoding = "mono8"
            Img_data = [pix for pixdata in [im.getdata()] for pix in pixdata]
            Img.step = Img.width

            Img.data = Img_data
            bag.write('camera/image_raw', Img, Stamp)
    finally:
        bag.close()       
Example #38
0
 def _publish_image(self):
     camera_image = self._cozmo.world.latest_image
     if camera_image is not None:
         img = camera_image.raw_image
         ros_img = Image()
         ros_img.encoding = 'rgb8'
         ros_img.width = img.size[0]
         ros_img.height = img.size[1]
         ros_img.step = 3 * ros_img.width
         ros_img.data = img.tobytes()
         ros_img.header.frame_id = 'cozmo_camera'
         cozmo_time = camera_image.image_recv_time
         ros_img.header.stamp = rospy.Time.from_sec(cozmo_time)
         self._image_pub.publish(ros_img)
Example #39
0
    def write(self, buf):
        img_msg = Image()

        img_msg.height = self.height
        img_msg.width = self.width
        img_msg.encoding = "rgb8"  # "bgr8"?
        img_msg.is_bigendian = False  # not sure about this either

        img_msg.data = buf
        img_msg.step = 3 * self.width

        #print("publishing image message")

        self.publisher.publish(img_msg)
Example #40
0
def make_rgb_msg(rgb, rgb_time_sec, rgb_time_nsec, test_image):
    rgb_msg = Image()

    rgb_msg.header.seq = test_image
    rgb_msg.header.frame_id = "/openni_rgb_optical_frame"
    rgb_msg.header.stamp.secs = rgb_time_sec
    rgb_msg.header.stamp.nsecs = rgb_time_nsec

    rgb_msg.step = 1920
    rgb_msg.is_bigendian = 0
    rgb_msg.encoding = 'rgb8'
    rgb_msg.data = rgb

    return rgb_msg
Example #41
0
    def toROS(img):
        """
        Convert a PIL/pygame image to a ROS compatible message (sensor_msgs.Image).
        """
        if img.mode == 'P':  # P -> 8-bit pixels, mapped to any other mode using a color palette
            img = img.convert('RGB')  # RGB -> 3x8-bit pixels, true color

        rosimage = ImageMsg()
        rosimage.encoding = ImageConverter.ENCODINGMAP_PY_TO_ROS[img.mode]
        (rosimage.width, rosimage.height) = img.size
        rosimage.step = (ImageConverter.PIL_MODE_CHANNELS[img.mode] *
                         rosimage.width)
        rosimage.data = img.tobytes()
        return rosimage
Example #42
0
def cv2_to_imgmsg(cv2img, encoding='bgr8'):
    """
    Converts an OpenCV image to a ROS image without using the cv_bridge package,
    for compatibility purposes.
    """

    msg = Image()
    msg.width = cv2img.shape[0]
    msg.height = cv2img.shape[1]
    msg.encoding = encoding
    msg.step = BPP[encoding] * cv2img.shape[0]
    msg.data = numpy.ascontiguousarray(cv2img).tobytes()

    return msg
def _array_to_imgmsg(img_array, encoding):
    assert len(img_array.shape) == 3
    img_msg = Image()
    img_msg.height = img_array.shape[0]
    img_msg.width = img_array.shape[1]
    if encoding == 'passthrough':
        img_msg.encoding = '8UC3'
    else:
        img_msg.encoding = encoding
    if img_array.dtype.byteorder == '>':
        img_msg.is_bigendian = True
    img_msg.data = img_array.tostring()
    img_msg.step = len(img_msg.data) // img_msg.height
    return img_msg
Example #44
0
def CreateMonoMessage(img_rgb, img_time, sequence):
    msg_mono = Image()
    bridge_mono = CvBridge()
    img_mono = cv2.cvtColor(img_rgb, cv2.COLOR_BGR2GRAY)
    msg_mono.header.stamp = img_time
    msg_mono.header.seq = sequence
    msg_mono.header.frame_id = "world"
    msg_mono.encoding = "mono8"
    msg_mono.height = img_rgb.shape[0]
    msg_mono.width = img_rgb.shape[1]
    msg_mono.data = bridge_mono.cv2_to_imgmsg(img_mono, "mono8").data
    msg_mono.is_bigendian = 0
    msg_mono.step = msg_mono.width
    return msg_mono
Example #45
0
def color2graymsg(img):
    img = img.astype(numpy.float32)
    im = (0.299 * img[:, :, 0] + 0.587 * img[:, :, 1] +
          0.114 * img[:, :, 2]).astype(numpy.uint8)
    msg = Image()
    msg.height = im.shape[0]
    msg.width = im.shape[1]
    msg.encoding = '8UC1'
    msg.data = im.tostring()
    msg.step = len(msg.data) // msg.height
    if im.dtype.byteorder == '>':
        msg.is_bigendian = True

    return msg
def map_image(values):
    """
    Map the values generated with the hypothesis-ros image strategy to a rospy Image type.
    """
    if not isinstance(values, _Image):
        raise TypeError('Wrong type. Use appropriate hypothesis-ros type.')
    ros_image = Image()
    ros_image.header = values.header
    ros_image.height = values.height
    ros_image.width = values.width
    ros_image.encoding = values.encoding
    ros_image.is_bigendian = values.is_bigendian
    ros_image.data = values.data
    return ros_image
Example #47
0
    def fictitious_pub(self):
        image_np = np.array((np.random.randint(255,
                    size=(self.args.height, self.args.width))), dtype=np.uint8)
        image_cv = cv2.imdecode(image_np, 1)

        if self.args.show:
            cv2.imshow('cv_img', image_np)
            cv.waitKey(2)

        fictitious_msg = Image()
        fictitious_msg.header.stamp = rospy.Time.now()
        fictitious_msg.data = np.array(cv2.imencode('.jpg', image_cv)).tostring()

        self.image_pub.publish(fictitious_msg)
Example #48
0
def main():
    global raw_image_msg

    rospy.init_node('object_detection')
    rospy.Subscriber("/raw_image", Image, object_detection_callback)

    p = pr.Predictor()
    image_pub = rospy.Publisher("/image", Image, queue_size=10)

    while raw_image_msg is None:
        continue

    azure_addr = "137.135.81.74"

    while not rospy.is_shutdown():
        image_array = list_to_array(raw_image_msg.data, raw_image_msg.height,
                                    raw_image_msg.width)

        try:
            cv2.imwrite("./input.png", image_array)
            # upload to azure
            os.system("scp -r ./input.png azureuser@{}:/home/azureuser".format(
                azure_addr))
            # download from azure
            os.system("scp azureuser@{}:/home/azureuser/output.png ./".format(
                azure_addr))
            output = cv2.imread("output.png")
        except Exception as e:
            print(e)
            print(
                "Something went wrong when communicating with azure, trying again"
            )
            continue

        # print(image_array)
        # output = p.transform(image_array)
        # print(numpy.all(output == 0))
        rgb_list, h, w = array_to_list(output)
        # print(rgb_list)
        image_msg = Image()
        image_msg.header.stamp = rospy.Time.now()
        image_msg.header.frame_id = 'a'
        image_msg.height = h
        image_msg.width = w
        image_msg.encoding = 'bgr8'
        image_msg.is_bigendian = 1
        image_msg.step = 3 * w
        image_msg.data = rgb_list

        image_pub.publish(image_msg)
Example #49
0
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 publish_state_image(state_from_env1, current_state_image_pub):
	current_state_image_msg = Image()
        current_state_image_msg.encoding = "mono8"
        current_state_image_msg.header.stamp = rospy.Time.now()
        current_state_image_msg.height = 68
        current_state_image_msg.width = 34
        current_state_image_msg.step = 68
        x = np.reshape(state_from_env1, (2,2312))
        idx_x = np.argwhere(x[0] == np.amax(x[0]))
        lx = idx_x.flatten().tolist()
        x[1][lx[0]] = 255
        x[1][lx[1]] = 255
        x[1][lx[2]] = 255
        y = x[1].tolist()
        current_state_image_msg.data = y
	current_state_image_pub.publish(current_state_image_msg)
Example #51
0
    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.height = self.component_instance.image_height
        image.width = self.component_instance.image_width
        image.encoding = self.encoding
        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.D = [0]
        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]

        if self.pub_tf:
            self.publish_with_robot_transform(image)
        else:
            self.publish(image)
        self.topic_camera_info.publish(camera_info)
Example #52
0
def main():
  if len(sys.argv) < 2:
    print("Usage: {} dataset_name".format(sys.argv[0]))
    exit(1)

  file_name = sys.argv[1]
  log_file = h5py.File('../dataset/log/{}.h5'.format(file_name))
  camera_file = h5py.File('../dataset/camera/{}.h5'.format(file_name))  

  zipped_log = izip(
    log_file['times'],
    log_file['fiber_accel'],
    log_file['fiber_gyro'])

  with rosbag.Bag('{}.bag'.format(file_name), 'w') as bag:
    bar = Bar('Camera', max=len(camera_file['X']))
    for i, img_data in enumerate(camera_file['X']):
      m_img = Image()
      m_img.header.stamp = rospy.Time.from_sec(0.01 * i)
      m_img.height = img_data.shape[1]
      m_img.width = img_data.shape[2]
      m_img.step = 3 * img_data.shape[2]
      m_img.encoding = 'rgb8'
      m_img.data = np.transpose(img_data, (1, 2, 0)).flatten().tolist()
      
      bag.write('/camera/image_raw', m_img, m_img.header.stamp)
      bar.next()
      
    bar.finish()

    bar = Bar('IMU', max=len(log_file['times']))
    for time, v_accel, v_gyro in zipped_log:
      m_imu = Imu()
      m_imu.header.stamp = rospy.Time.from_sec(time)
      [setattr(m_imu.linear_acceleration, c, v_accel[i]) for i, c in enumerate('xyz')]
      [setattr(m_imu.angular_velocity, c, v_gyro[i]) for i, c in enumerate('xyz')]

      bag.write('/fiber_imu', m_imu, m_imu.header.stamp)
      bar.next()

    bar.finish()
def array_to_image(array):
    """Takes a NxMx3 array and converts it into a ROS image message.

    """
    # Sanity check the input array shape
    if len(array.shape) != 3 or array.shape[2] != 3:
        raise ValueError('Array must have shape MxNx3')

    # Ravel the array into a single buffer
    image_data = (array.astype(np.uint8)).tostring(order='C')

    # Create the image message
    image_msg = Image()
    image_msg.height = array.shape[0]
    image_msg.width = array.shape[1]
    image_msg.encoding = 'rgb8'
    image_msg.is_bigendian = 0
    image_msg.step = array.shape[1] * 3
    image_msg.data = image_data

    return image_msg
def main():
    pub = rospy.Publisher('image_maker', Image)
    rospy.init_node('image_maker')
    #rospy.wait_for_service('tabletop_segmentation')
    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)]

    while not rospy.is_shutdown():
        try:
            pub.publish(im)
            sleep(.5)

        except Exception, e:
            print e
Example #55
0
def numpy_to_image(arr, encoding):
	if not encoding in name_to_dtypes:
		raise TypeError('Unrecognized encoding {}'.format(encoding))

	im = Image(encoding=encoding)

	# extract width, height, and channels
	dtype_class, exp_channels = name_to_dtypes[encoding]
	dtype = np.dtype(dtype_class)
	if len(arr.shape) == 2:
		im.height, im.width, channels = arr.shape + (1,)
	elif len(arr.shape) == 3:
		im.height, im.width, channels = arr.shape
	else:
		raise TypeError("Array must be two or three dimensional")

	# check type and channels
	if exp_channels != channels:
		raise TypeError("Array has {} channels, {} requires {}".format(
			channels, encoding, exp_channels
		))
	if dtype_class != arr.dtype.type:
		raise TypeError("Array is {}, {} requires {}".format(
			arr.dtype.type, encoding, dtype_class
		))

	# make the array contiguous in memory, as mostly required by the format
	contig = np.ascontiguousarray(arr)
	im.data = contig.tostring()
	im.step = contig.strides[0]
	im.is_bigendian = (
		arr.dtype.byteorder == '>' or 
		arr.dtype.byteorder == '=' and sys.byteorder == 'big'
	)

	return im
Example #56
0
def GetImageFromFile(im_path):
	fp = open( im_path, "r" )
	p = ImageFile.Parser()
	while 1:
		s = fp.read(307218) # trying to read a full file in one shot ...
		if not s:
		    break
		p.feed(s)
	im = p.close() # we should now have an image object
	im_stamp = os.path.basename(im_path).split(".")[0] #image timestamp is directly encoded in file name
	im_stamp =  float(im_stamp)/1000000.0
	Stamp = rospy.rostime.Time.from_sec(im_stamp)
	Img = Image()
	Img.header.stamp = Stamp
	Img.width = im.size[0]
	Img.height = im.size[1]
	Img.encoding = "mono8" #needs to be changed to rgb8 for rgb images
	Img.step=Img.width #some nodes may complains ...
	Img.header.frame_id = "camera"
	Img_data = list(im.getdata()) #works for mono channels images (grayscale)
	#Img_data = [pix for pix in im.getdata()]
	#  Img_data = [pix for pixdata in im.getdata() for pix in pixdata]
	Img.data = Img_data
	return (im_stamp, Stamp, Img)
Example #57
0
    def run(self):
        img = Image()
        r = rospy.Rate(self.config['frame_rate'])
        while self.is_looping():
            if self.pub_img_.get_num_connections() == 0:
                if self.nameId:
                    rospy.loginfo('Unsubscribing from camera as nobody listens to the topics.')
                    self.camProxy.unsubscribe(self.nameId)
                    self.nameId = None
                r.sleep()
                continue
            if self.nameId is None:
                self.reconfigure(self.config, 0)
                r.sleep()
                continue
            image = self.camProxy.getImageRemote(self.nameId)
            if image is None:
                continue
            # Deal with the image
            if self.config['use_ros_time']:
                img.header.stamp = rospy.Time.now()
            else:
                img.header.stamp = rospy.Time(image[4] + image[5]*1e-6)
            img.header.frame_id = self.frame_id
            img.height = image[1]
            img.width = image[0]
            nbLayers = image[2]
            if image[3] == kYUVColorSpace:
                encoding = "mono8"
            elif image[3] == kRGBColorSpace:
                encoding = "rgb8"
            elif image[3] == kBGRColorSpace:
                encoding = "bgr8"
            elif image[3] == kYUV422ColorSpace:
                encoding = "yuv422" # this works only in ROS groovy and later
            elif image[3] == kDepthColorSpace:
                encoding = "mono16"
            else:
                rospy.logerr("Received unknown encoding: {0}".format(image[3]))
            img.encoding = encoding
            img.step = img.width * nbLayers
            img.data = image[6]

            self.pub_img_.publish(img)

            # deal with the camera info
            if self.config['source'] == kDepthCamera and image[3] == kDepthColorSpace:
                infomsg = CameraInfo()
                # yes, this is only for an XTion / Kinect but that's the only thing supported by NAO
                ratio_x = float(640)/float(img.width)
                ratio_y = float(480)/float(img.height)
                infomsg.width = img.width
                infomsg.height = img.height
                # [ 525., 0., 3.1950000000000000e+02, 0., 525., 2.3950000000000000e+02, 0., 0., 1. ]
                infomsg.K = [ 525, 0, 3.1950000000000000e+02,
                              0, 525, 2.3950000000000000e+02,
                              0, 0, 1 ]
                infomsg.P = [ 525, 0, 3.1950000000000000e+02, 0,
                              0, 525, 2.3950000000000000e+02, 0,
                              0, 0, 1, 0 ]
                for i in range(3):
                    infomsg.K[i] = infomsg.K[i] / ratio_x
                    infomsg.K[3+i] = infomsg.K[3+i] / ratio_y
                    infomsg.P[i] = infomsg.P[i] / ratio_x
                    infomsg.P[4+i] = infomsg.P[4+i] / ratio_y

                infomsg.D = []
                infomsg.binning_x = 0
                infomsg.binning_y = 0
                infomsg.distortion_model = ""

                infomsg.header = img.header
                self.pub_info_.publish(infomsg)
            elif self.config['camera_info_url'] in self.camera_infos:
                infomsg = self.camera_infos[self.config['camera_info_url']]

                infomsg.header = img.header
                self.pub_info_.publish(infomsg)

            r.sleep()

        if (self.nameId):
            rospy.loginfo("unsubscribing from camera ")
            self.camProxy.unsubscribe(self.nameId)
Example #58
0
import numpy as np
import rospy
import sys
from sensor_msgs.msg import Image

if __name__ == '__main__': 
    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)
    rate = rospy.Rate(hz)

    pub = rospy.Publisher('image', Image, queue_size=1)

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

    while not rospy.is_shutdown():
        pub.publish(msg)
        rate.sleep()
Example #59
0
    def main_loop(self):
        img = Image()
        cimg = Image()
        r = rospy.Rate(15)
        while not rospy.is_shutdown():
            if self.pub_img_.get_num_connections() == 0:
                r.sleep()
                continue

            image = self.camProxy.getImageRemote(self.nameId)
            if image is None:
                continue
            # Deal with the image
            '''
            #Images received from NAO have
            if self.config['use_ros_time']:
                img.header.stamp = rospy.Time.now()
            else:
                img.header.stamp = rospy.Time(image[4] + image[5]*1e-6)
            '''
            img.header.stamp = rospy.Time.now()
            img.header.frame_id = self.frame_id
            img.height = image[1]
            img.width = image[0]
            nbLayers = image[2]
            if image[3] == kDepthColorSpace:
                encoding = "mono16"
            else:
                rospy.logerr("Received unknown encoding: {0}".format(image[3]))
            img.encoding = encoding
            img.step = img.width * nbLayers
            img.data = image[6]

            self.pub_img_.publish(img)
            
            # deal with the camera info
            infomsg = CameraInfo()
            infomsg.header = img.header
            # yes, this is only for an XTion / Kinect but that's the only thing supported by NAO
            ratio_x = float(640)/float(img.width)
            ratio_y = float(480)/float(img.height)
            infomsg.width = img.width
            infomsg.height = img.height
            # [ 525., 0., 3.1950000000000000e+02, 0., 525., 2.3950000000000000e+02, 0., 0., 1. ]
            infomsg.K = [ 525, 0, 3.1950000000000000e+02,
                         0, 525, 2.3950000000000000e+02,
                         0, 0, 1 ]
            infomsg.P = [ 525, 0, 3.1950000000000000e+02, 0,
                         0, 525, 2.3950000000000000e+02, 0,
                         0, 0, 1, 0 ]

            for i in range(3):
                infomsg.K[i] = infomsg.K[i] / ratio_x
                infomsg.K[3+i] = infomsg.K[3+i] / ratio_y
                infomsg.P[i] = infomsg.P[i] / ratio_x
                infomsg.P[4+i] = infomsg.P[4+i] / ratio_y

            infomsg.D = []
            infomsg.binning_x = 0
            infomsg.binning_y = 0
            infomsg.distortion_model = ""
            self.pub_info_.publish(infomsg)

	    #Currently we only get depth image from the 3D camera of NAO, so we make up a fake color image (a black image)
            #and publish it under image_color topic.
            #This should be update when the color image from 3D camera becomes available.
            colorimg = np.zeros((img.height,img.width,3), np.uint8)
            try:
              cimg = self.bridge.cv2_to_imgmsg(colorimg, "bgr8")
              cimg.header.stamp = img.header.stamp
              cimg.header.frame_id = img.header.frame_id
              self.pub_cimg_.publish(cimg)
            except CvBridgeError, e:
              print e

            r.sleep()
Example #60
-1
    def default(self, ci='unused'):
        """ Publish the data of the Camera as a ROS Image message. """
        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)

        # sensor_msgs/CameraInfo [ http://ros.org/wiki/rviz/DisplayTypes/Camera ]
        # 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)