def callback(self, ros_data):
    #### direct conversion to CV2 ####
    np_arr = np.fromstring(ros_data.data, np.uint8)
    image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
    
    #(rows,cols,channels) = image_np.shape
    #image_np = image_np[0:64, 0:480] 

    #equ = cv2.equalizeHist(image_np)
    #res = np.hstack((image_np,equ))

    #### Feature detectors using CV2 #### 
    # "","Grid","Pyramid" + 
    # "FAST","GFTT","HARRIS","MSER","ORB","SIFT","STAR","SURF", "GridFAST"
    #method = "GridFAST"
    #feat_det = cv2.FeatureDetector_create(method)
    #time1 = time.time()

    # convert np image to grayscale
    #featPoints = feat_det.detect( cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY))
    #time2 = time.time()
    #if VERBOSE :
    #  print '%s detector found: %s points in: %s sec.'%(method, len(featPoints),time2-time1)

    #for featpoint in featPoints:
    #  x,y = featpoint.pt
    #  cv2.circle(image_np,(int(x),int(y)), 3, (0,0,255), -1)

    #### Create CompressedIamge ####
    msg = CompressedImage()
    msg.header.stamp = rospy.Time.now()
    msg.format = "jpeg"
    msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
    # Publish new image
    self.image_pub.publish(msg)

def grabFrame(event):
   msg = CompressedImage()
   msg.header.frame_id = frame_id
   msg.format = "jpeg"
   resp, msg.data = http.request("http://" + host + request)
   msg.header.stamp = rospy.Time.now()
   pub.publish(msg)

  def stream(self):
    url = 'http://%s:%[email protected]%s/mjpg/video.mjpg' % (self.axis.username, self.axis.password, self.axis.hostname)
    #url = url + "?resolution=%dx%d" % (self.axis.width, self.axis.height)
    fp = urllib.urlopen(url)

    while True:
      boundary = fp.readline()

      header = {}
      while 1:
        line = fp.readline()
        if line == "\r\n": break
        line = line.strip()

        parts = line.split(": ", 1)
        header[parts[0]] = parts[1]

      content_length = int(header['Content-Length'])

      img = fp.read(content_length)
      line = fp.readline()
      
      msg = CompressedImage()
      msg.header.stamp = rospy.Time.now()
      msg.header.frame_id = self.axis_frame_id
      msg.format = "jpeg"
      msg.data = img

      self.axis.pub.publish(msg)

      """

 def publish(self, raw_img):
     msg = CompressedImage()
     msg.header.stamp = rospy.Time.now()
     msg.format = "jpeg"
     msg.data = np.array(cv2.imencode(".jpg", raw_img)[1]).tostring()
     # TODO compile sensor_msg error , use String instead
     self.image_pub.publish(msg.data)

    def __init__(self):
        # Get the ~private namespace parameters from command line or launch file.
        self.UDP_IP = rospy.get_param('~UDP_IP', '192.168.50.37') #get_ip.get_local()
        #if self.UDP_IP != 'localhost':
        #    self.UDP_IP = int(self.UDP_IP)
        self.UDP_PORT = int(rospy.get_param('~UDP_PORT', '49155'))

        # Create a dynamic reconfigure server.
        #self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
        
        # create ros::Publisher to send Odometry messages
        cam_pub = rospy.Publisher('image_raw/compressed', CompressedImage)

        #crappy test
        t = True
        while not rospy.is_shutdown():
            #crappy test continued
            if t:
                print "Camera Running"
            t = False

            udp_string = self.receive_packet()
            jpg_string = self.parse_string(udp_string)

            cam_msg = CompressedImage()
            cam_msg.header.stamp = rospy.Time.now() #YOYOYOYO - should be from robot time
            cam_msg.format = "jpeg"
            cam_msg.data = jpg_string

            cam_pub.publish(cam_msg)

    def grabAndPublish(self,stream,publisher):
        # Grab image from stream
        stream.seek(0)
        img_data = stream.getvalue()

        if self.uncompress:
            # Publish raw image
            data = np.fromstring(img_data, dtype=np.uint8)
            image = cv2.imdecode(data, 1)
            image_msg = self.bridge.cv2_to_imgmsg(image)
        else:
            # Publish compressed image only
            image_msg = CompressedImage()
            image_msg.data = img_data
            image_msg.format = "jpeg"
            
        image_msg.header.stamp = rospy.Time.now()
        # Publish 
        publisher.publish(image_msg)
        # Clear stream
        stream.seek(0)
        stream.truncate()

        if not self.has_published:
            rospy.loginfo("[%s] Published the first image." %(self.node_name))
            self.has_published = True

def ros_compressed_from_numpygray(np_arr):
    msg = CompressedImage()
    msg.header.stamp = rospy.Time.now()
    msg.format = "jpeg"
    np_arr = 255*(1.0*np_arr/np.max(np_arr))
    msg.data = np.array(cv2.imencode('.jpg', np.array(np_arr, dtype = np.uint8))[1]).tostring()
    return msg

    def grabAndPublish(self,stream,publisher):
        while not self.update_framerate and not self.is_shutdown and not rospy.is_shutdown(): 
            yield stream
            # Construct image_msg
            # Grab image from stream
            stamp = rospy.Time.now()
            stream.seek(0)
            stream_data = stream.getvalue()
            # Generate compressed image
            image_msg = CompressedImage()
            image_msg.format = "jpeg"
            image_msg.data = stream_data

            image_msg.header.stamp = stamp
            image_msg.header.frame_id = self.frame_id
            publisher.publish(image_msg)
                        
            # Clear stream
            stream.seek(0)
            stream.truncate()
            
            if not self.has_published:
                rospy.loginfo("[%s] Published the first image." %(self.node_name))
                self.has_published = True

            rospy.sleep(rospy.Duration.from_sec(0.001))

def handle_pkt(pkt=None):
    ts_begin_loc = pkt.find(begin_timestamp_marker)
    ts_end_loc = pkt.find(end_timestamp_marker)

    if ts_begin_loc == -1 or ts_end_loc == -1:
        #something's fishy, discard jpeg
        print "JPEG discarded, malformed data"
        return 
        
    ts = float(pkt[ts_begin_loc+len(begin_timestamp_marker):ts_end_loc])
	
#    if ts > last_ts:
#	last_ts = ts
#    else:
#	return

    pkt = pkt[ts_end_loc+len(end_timestamp_marker):]

    print "{} bytes of JPEG recvd".format(len(pkt))
    msg = CompressedImage()
    msg.header.stamp  = rospy.Time(tango_clock_offset + float(ts))
    msg.header.frame_id = camera_name
    msg.data = pkt
    msg.format = 'jpeg'
    pub_camera.publish(msg)

 def sendTestImage(self):
     img_name = '0'+str(self.sent_image_count)+'_orig.'+self.test_image_format
     test_img = cv2.imread(self.test_image_path+img_name)
     msg = CompressedImage()
     msg.header.stamp = rospy.Time.now()
     msg.format = self.test_image_format
     msg.data = np.array(cv2.imencode('.'+self.test_image_format, test_img)[1]).tostring()
     self.pub_test_image.publish(msg)

    def pubOriginal(self, msg):
        compressed_img_msg = CompressedImage()
        compressed_img_msg.format = "png"
        compressed_img_msg.data = np.array(cv2.imencode(".png", self.testImageOrig)[1]).tostring()

        # compressed_img_msg.header.stamp = msg.header.stamp
        # compressed_img_msg.header.frame_id = msg.header.frame_id
        self.pub_raw.publish(compressed_img_msg)

    def processImage(self, image_msg):
        image_cv = self.bridge.imgmsg_to_cv2(image_msg)

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

	def stream(self):
		"""
			Reads and process the streams from the camera	
		"""		 
		try:
			#If flag self.enable_auth is 'True' then use the user/password to access the camera. Otherwise use only self.url
			if self.enable_auth:
				req = urllib2.Request(self.url, None, {"Authorization": self.auth })
			else:
				req = urllib2.Request(self.url)
				
			fp = urllib2.urlopen(req, timeout = self.timeout)
			#fp = urllib2.urlopen(req)

			while self.run_camera and not rospy.is_shutdown():
				
				boundary = fp.readline()
				
				header = {}
				
				while not rospy.is_shutdown():
					line = fp.readline()
					#print 'read line %s'%line
					if line == "\r\n": break
					line = line.strip()
					#print line
					parts = line.split(": ", 1)
					header[parts[0]] = parts[1]

				content_length = int(header['Content-Length'])
				#print 'Length = %d'%content_length
				img = fp.read(content_length)
				line = fp.readline()
				
				msg = CompressedImage()
				msg.header.stamp = rospy.Time.now()
				msg.header.frame_id = self.axis_frame_id
				msg.format = "jpeg"
				msg.data = img
				# publish image
				self.compressed_image_publisher.publish(msg)
				
				cimsg = self.cinfo.getCameraInfo()
				cimsg.header.stamp = msg.header.stamp
				cimsg.header.frame_id = self.axis_frame_id
				# publish camera info
				self.caminfo_publisher.publish(cimsg)
				
				#print self.real_fps
				self.last_update = datetime.datetime.now()
				self.error_reading = False
				self.image_pub_freq.tick()
						
		except urllib2.URLError,e:
			self.error_reading = True
			self.error_reading_msg = e
			rospy.logerr('Axis:stream: Camera %s (%s:%d). Error: %s'%(self.camera_id, self.hostname, self.camera_number, e))

 def cv2_to_compressed_ros(self, img):
     try:
         msg = CompressedImage()
         msg.header.stamp = rospy.Time.now()
         msg.format = "jpeg"
         msg.data = np.array(cv2.imencode('.jpg', img)[1]).tostring()
         return msg
     except CvBridgeError as e:
         rospy.logerr(e)

 def pubImage(self):
     filename = '0'+str(self.num_sent)+'_orig.png'
     image = cv2.imread(self.im_dir+filename)
     msg = CompressedImage()
     msg.header.stamp = rospy.Time.now()
     msg.format = 'png'
     msg.data = np.array(cv2.imencode('.png', image)[1]).tostring()
     self.pub_fake_images.publish(msg)
     rospy.loginfo("[%s] Image %s published" %(self.node_name,filename))

	def pubOrig(self, args=None):
		image_msg_out = CompressedImage()
		image_msg_out.header.stamp = rospy.Time.now()
		image_msg_out.format = "png"
		image_msg_out.data = np.array(cv2.imencode('.png',
				self.original_image)[1]).tostring()

		self.pub_image.publish(image_msg_out)
		rospy.loginfo("Publishing original image")

def publish_image(pub, image_handle):
    image = CompressedImage()
    image.data = image_handle.read()
    image.format = 'jpeg'
    image.header.seq = 0
    image.header.stamp.secs = math.floor(time.time())
    image.header.stamp.nsecs = 0
    image.header.frame_id = "0"
    pub.publish(image)

  def stream(self):
    url = 'http://%s/mjpg/video.mjpg' % self.axis.hostname
    url = url + "?fps=0&resolultion=%dx%d" % (self.axis.width, self.axis.height)

    rospy.logdebug('opening ' + str(self.axis))

    # only try to authenticate if user/pass configured
    if self.axis.password != '' and self.axis.username != '':
      # create a password manager
      password_mgr = urllib2.HTTPPasswordMgrWithDefaultRealm()

      # Add the username and password, use default realm.
      top_level_url = "http://" + self.axis.hostname
      password_mgr.add_password(None, top_level_url,
                                self.axis.username,
                                self.axis.password)
      handler = urllib2.HTTPBasicAuthHandler(password_mgr)

      # create "opener" (OpenerDirector instance)
      opener = urllib2.build_opener(handler)

      # ...and install it globally so it can be used with urlopen.
      urllib2.install_opener(opener)

    fp = urllib2.urlopen(url)

    while True:
      boundary = fp.readline()

      header = {}
      while 1:
        line = fp.readline()
        if line == "\r\n": break
        line = line.strip()

        parts = line.split(": ", 1)
        header[parts[0]] = parts[1]

      content_length = int(header['Content-Length'])

      img = fp.read(content_length)
      line = fp.readline()
      
      msg = CompressedImage()
      msg.header.stamp = rospy.Time.now()
      msg.header.frame_id = self.axis.frame_id
      msg.format = "jpeg"
      msg.data = img

      self.axis.pub.publish(msg)

      cimsg = self.axis.cinfo.getCameraInfo()
      cimsg.header.stamp = msg.header.stamp
      cimsg.header.frame_id = self.axis.frame_id
      cimsg.width = self.axis.width
      cimsg.height = self.axis.height
      self.axis.caminfo_pub.publish(cimsg)

	def flipImageNP(self,msg):
		np_array = np.fromstring(msg.data, np.uint8)
		image_np = cv2.imdecode(np_array, cv2.CV_LOAD_IMAGE_COLOR)		
		#flip_arr = np.fliplr(np_arr)
		imageflip_np=cv2.flip(image_np,1)
		flip_im = CompressedImage()
		flip_im.data = np.array(cv2.imencode('.jpg', imageflip_np)[1]).tostring()
		#flip_im.data = flip_arr.tostring()
		flip_im.header.stamp = msg.header.stamp
		self.pub_image.publish(flip_im)

  def callback(self,ros_data):
    np_arr = np.fromstring(ros_data.data, np.uint8)
    image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
    image_np = cv2.flip(image_np, self.flip_code)

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