def cloudCallback(self, data):
        # print 'Time between cloud calls:', time.time() - self.cloudTime
        # startTime = time.time()

        self.pointCloud = data

        self.transposeGripperToCamera()

        # Determine location of spoon
        spoon3D = [0.22, -0.050, 0]
        spoon = np.dot(self.lGripperTransposeMatrix, np.array([spoon3D[0], spoon3D[1], spoon3D[2], 1.0]))[:3]
        self.spoonX, self.spoonY = self.pinholeCamera.project3dToPixel(spoon)

        lowX, highX, lowY, highY = self.boundingBox(0.05, 0.3, 0.05, 20, 100, 50)

        points2D = [[x, y] for y in xrange(lowY, highY) for x in xrange(lowX, highX)]
        try:
            points3D = pc2.read_points(self.pointCloud, field_names=('x', 'y', 'z'), skip_nans=True, uvs=points2D)
            gripperPoint = pc2.read_points(self.pointCloud, field_names=('x', 'y', 'z'), skip_nans=True, uvs=[[self.lGripX, self.lGripY]]).next()
        except:
            # print 'Unable to unpack from PointCloud2.', self.cameraWidth, self.cameraHeight, self.pointCloud.width, self.pointCloud.height
            return

        points3D = np.array([point for point in points3D])

        self.clusterPoints = points3D

        # # Perform dbscan clustering
        # X = StandardScaler().fit_transform(points3D)
        # labels = self.dbscan.fit_predict(X)
        # # unique_labels = set(labels)
        #
        # # Find the point closest to our gripper and it's corresponding label
        # index, closePoint = min(enumerate(np.linalg.norm(points3D - gripperPoint, axis=1)), key=operator.itemgetter(1))
        # closeLabel = labels[index]
        # while closeLabel == -1 and points3D.size > 0:
        #     np.delete(points3D, [index])
        #     np.delete(labels, [index])
        #     index, closePoint = min(enumerate(np.linalg.norm(points3D - gripperPoint, axis=1)), key=operator.itemgetter(1))
        #     closeLabel = labels[index]
        # if points3D.size <= 0:
        #     return
        # # print 'Label:', closeLabel
        #
        # # Find the cluster closest to our gripper
        # self.clusterPoints = points3D[labels==closeLabel]
        #
        # if self.visual:
        #     # Publish depth features for spoon features
        #     self.publishPoints('spoonPoints', self.clusterPoints, g=1.0)
        #
        #     # Publish depth features for non spoon features
        #     nonClusterPoints = points3D[labels!=closeLabel]
        #     self.publishPoints('nonSpoonPoints', nonClusterPoints, r=1.0)

        self.updateNumber += 1

    def process(self):
        if self.last_scan is None or self.last_image is None:
            return

        last_image = self.last_image
        last_scan = self.last_scan

        scan_pc = pc2.read_points(last_scan)
        image_pc = pc2.read_points(last_image)

        scan_obstacles = []
        tmp = Obstacle()

        # Enumerate the point cloud from the laser scan to group points that belongs
        # to the same obstacle.
        for x, y, _ in scan_pc:
            dist = x ** 2 + y ** 2

            # If it's in range, the point is considered to be part of the current obstacle
            if dist < self.max_range:
                tmp.add_point(Point(x, y, dist))
            else:
                if not tmp.is_empty():
                    scan_obstacles.append(tmp)
                    tmp = Obstacle()

        if not tmp.is_empty():
            scan_obstacles.append(tmp)

        if len(scan_obstacles) == 0:
            self.cloud_in.publish(last_image)
        else:
            image_points = []

            for x, y, z, _ in image_pc:
                point = Point(x, y)

                for obs in scan_obstacles:
                    if not obs.is_behind(point):
                        image_points.append((x, y, z))

            msg = pc2.create_cloud_xyz32(last_image.header, image_points)
            self.cloud_in.publish(msg)
            
        self.cloud_in.publish(last_scan)

        self.last_scan = None
        self.last_image = None

def _deserialize_numpy(self, str):
    """
    wrapper for factory-generated class that passes numpy module into deserialize    
    """
    self.deserialize_numpy(str, numpy)  # deserialize (with numpy wherever possible)

    # for Image msgs
    if self._type == 'sensor_msgs/Image':
        self.data = numpy.asarray(bridge.imgmsg_to_cv(self, desired_encoding=self.encoding))  # convert pixel data to numpy array

    # for PointCloud2 msgs
    if self._type == 'sensor_msgs/PointCloud2':
        print 'Cloud is being deserialized...'
        points = point_cloud2.read_points(self)
        points_arr = numpy.asarray(list(points))
        
        # Unpack RGB color info
        _float2rgb_vectorized = numpy.vectorize(_float2rgb)
        r, g, b = _float2rgb_vectorized(points_arr[:, 3])
        r = numpy.expand_dims(r, 1).astype('uint8')  # insert blank 3rd dimension (for concatenation)
        g = numpy.expand_dims(g, 1).astype('uint8')  
        b = numpy.expand_dims(b, 1).astype('uint8')  

        # Concatenate and Reshape
        pixels_rgb = numpy.concatenate((r, g, b), axis=1)
        image_rgb = pixels_rgb.reshape(self.height, self.width, 3)
        points_arr = points_arr[:, :3].reshape(self.height, self.width, 3).astype('float32')

        # Build record array to separate datatypes -- int16 for XYZ, uint8 for RGB
        image_xyzrgb = numpy.rec.fromarrays((points_arr, image_rgb), names=('xyz', 'rgb'))
        self.data = image_xyzrgb

    return self

    def pointcloud_callback(self, msg):

        pointcloud = pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=False, uvs=[])
        # While there are points in the cloud to read...

        if self.pc_count<self.pc_to_keep:
            self.pc_count=self.pc_count+1
        #else:self.store_pc.pop(0)
        
        while True:
            try:
                # new point
                point = next(pointcloud)
                #print point
                #convert point to a_star map coordinates
                self.xyz_point = point
                #add point to list of points
                self.store_points.append(list(self.xyz_point))
                #self.store_points.pop(0)

            # When the last point has been processed
            except StopIteration:
                break
        
        self.store_pc.append(list(self.store_points))

def callback(data):
	#print (data.data)
	print "-------------------------------------------"
	height = int(data.height/2)
	width = int(data.width)
	#print (data.header)
	data_out= pc2.read_points(data)
	#cloud = pc2.create_cloud_xyz32(data.header,data.data)
	int_data = next(data_out)	
	#print size(data_out)
	g=[]
	for i in data_out:
		k=i
		if i[0] is None or i[1] is None or i[2] is None:
			pass				
			#print "skipped"
		elif i[3]>0.00:		
		#elif i[0] < 5.00 or i[1]<5.00 or i[2]<5.00 or i[3]<5.00:
			#pass
			#print "skip
			g.append(i)	
	#print len(g),len(g[0])
	print g
	#	print i
	print "--------------------------------------------"

    def callback(self, ros_msg):
        """ 
        This method is invoked each time a new ROS message is generated.
        the message is of type CompressedImage, with data and format
        """
        self.msg = ros_msg
        
        # we don't need to be called again
        self.subscriber.unregister()
        
        if self.save_to_file:
            # create directories if necessary
            dest_dir = os.path.split(self.path)[0]    
            if not os.path.exists(dest_dir):
                os.makedirs(dest_dir)
            # write data to disk
	    if self.msg_type == CompressedImage:
                f = open(self.path, 'w')
		f.write(ros_msg.data)
		f.close()
	    elif self.msg_type == PointCloud2:
		rawpoints = numpy.array(list(point_cloud2.read_points(ros_msg, skip_nans=False)), dtype=numpy.float32)[:, :3]
		notnanindices = ~numpy.isnan(rawpoints[:, 0])
		f = open(self.path, 'wb')
		pickle.dump((rawpoints[notnanindices], notnanindices, len(rawpoints)), f)
		f.close()
	    else:
                f = open(self.path, 'wb')
		pickle.dump(ros_msg, f)
		f.close()
        
        self.done = True

    def cloud_callback(self, cloud):
        points = point_cloud2.read_points(cloud)
        points_list = np.asarray(list(points))
        points_arr = np.asarray(points_list)

        # Unpack RGB color info
        _float2rgb_vectorized = np.vectorize(_float2rgb)
        r, g, b = _float2rgb_vectorized(points_arr[:, 3])

        # Concatenate and Reshape
        r = np.expand_dims(r, 1)  # insert blank 3rd dimension (for concatenation)
        g = np.expand_dims(g, 1)  
        b = np.expand_dims(b, 1)  
        points_rgb = np.concatenate((points_arr[:, 0:3], r, g, b), axis=1)
        image_rgb = points_rgb.reshape(cloud.height, cloud.width, 6)
        z = copy.deepcopy(image_rgb[:, :, 2])  # get depth values (I think)
        image_np = copy.deepcopy(image_rgb[:, :, 3:].astype('uint8'))
        #code.interact(local=locals())
        
        # TWO-METER DISTANCE FILTER
        z[np.isnan(z)] = 0.0
        mask = np.logical_or(z > 2, z == 0)
        for i in range(image_np.shape[2]): 
            image_np[:, :, i][mask] = 0
        
        # Convert to Image msg
        image_cv = cv.fromarray(image_np)
        image_msg = self.bridge.cv_to_imgmsg(image_cv, encoding='bgr8')
        self.pub.publish(image_msg)

def callbackScan(laserScan):
	global iniX
	global distX
	global espacoVazio
	global estadoCont

	point_cloud = laser_projector.projectLaser(laserScan)
	minY = 0 
	maxY = 0 

	# calcula os pontos mínimo e máximo de detecção do laser
	for p in pc2.read_points(point_cloud, field_names = ("x", "y", "z"), skip_nans=True):
		if p[1] < minY:
			minY = p[1] 
		if p[1] > maxY:
			maxY = p[1] 

	# Se o laser não detectar obstáculo, inicia a contagem do espaço vazio
	if (minY+maxY)/2 < -0.1 and estadoCont == 0:
		estadoCont = 1	
		iniX = posX
	# Se o laser detectar obstáculo, finaliza a contagem do espaço vazio
	elif (minY+maxY)/2 > 0.1 and estadoCont == 1:
		estadoCont = 0
		espacoVazio = distX
	# Se o estadoCont == 1, há um espaço vazio sendo mensurado
	elif estadoCont == 1:
		distX = posX - iniX
		
	rospy.loginfo("oriZ: %.1f	dist: %.1f	espaco: %.1f" %(oriZ, distX, espacoVazio))

    def _cloud_cb(self, cloud):
        points = np.array(list(read_points(cloud)))
        if points.shape[0] == 0:
            return
        
        pos = points[:,0:3]
        cor = np.reshape(points[:,-1], (points.shape[0], 1))

        # Get 4x4 matrix which transforms point cloud co-ords to odometry frame
        try:
            points_to_map = self.tf.asMatrix('/lasths', cloud.header)
        except tf.ExtrapolationException:
            return

        transformed_points = points_to_map.dot(np.vstack((pos.T, np.ones((1, pos.shape[0])))))
        transformed_points = transformed_points[:3,:].T

        self.seq += 1
        header = Header()
        header.seq = self.seq
        header.stamp = rospy.Time.now()
        header.frame_id = '/lasths'

        self.cloud = np.vstack((self.cloud, np.hstack((transformed_points, cor))))
        if self.seq % 30 == 0:
            print "plup!"
            self.cloud = np.zeros((0, 4))

        output_cloud = create_cloud(header, cloud.fields, self.cloud)
        self.cloud_pub.publish(output_cloud)

def extractLabeledCloud(pointCloud_):
	npts = 0
	points = {}
	normals = {}

	xIdx = 0
	yIdx = 1
	zIdx = 2
	nxIdx = 3
	nyIdx = 4
	nzIdx = 5
	labelIdx = 6

	for p in pc.read_points(pointCloud_, skip_nans=True, field_names=('x', 'y', 'z', 'normal_x', 'normal_y', 'normal_z', 'label')):
		label = p[labelIdx]

		if not label in points:
			points[label] = []
			normals[label] = []

		points[label].append([p[xIdx], p[yIdx], p[zIdx]])
		normals[label].append([p[nxIdx], p[nyIdx], p[nzIdx]])

		npts = npts + 1

	return [points, normals, npts]

    def find_centroid(self, request):
        '''Computes the average point in a point cloud. '''
        points = pc2.read_points(
            request.cluster.pointcloud,
            field_names=['x', 'y', 'z'],
            skip_nans=True,
        )

        num_points = 0
        avg_x = 0
        avg_y = 0
        avg_z = 0
        for x, y, z in points:
            num_points += 1
            avg_x += x
            avg_y += y
            avg_z += z
        if num_points > 0:
            avg_x /= num_points
            avg_y /= num_points
            avg_z /= num_points

        rospy.loginfo('Centroid: ({}, {}, {})'.format(avg_x, avg_y, avg_z))
        centroid = PointStamped(
            point=Point(x=avg_x, y=avg_y, z=avg_z),
            header=Header(
                frame_id=request.cluster.header.frame_id,
                stamp=rospy.Time.now(),
            )
        )
        return FindCentroidResponse(centroid=centroid)

    def cloudCallback(self, data):
        # print 'Time between cloud calls:', time.time() - self.cloudTime
        # startTime = time.time()

        self.pointCloud = data

        self.transposeBowlToCamera()
        self.transposeGripperToCamera()

        # Determine location of spoon
        spoon3D = [0.22, -0.050, 0]
        self.spoon = np.dot(self.lGripperTransposeMatrix, np.array([spoon3D[0], spoon3D[1], spoon3D[2], 1.0]))[:3]

        lowX, highX, lowY, highY = self.boundingBox()

        points2D = [[x, y] for y in xrange(lowY, highY) for x in xrange(lowX, highX)]
        try:
            points3D = pc2.read_points(self.pointCloud, field_names=('x', 'y', 'z'), skip_nans=True, uvs=points2D)
        except:
            print 'Unable to unpack from PointCloud2!', self.cameraWidth, self.cameraHeight, self.pointCloud.width, self.pointCloud.height
            return

        self.points3D = np.array([point for point in points3D])

        # self.publishImageFeatures()

        self.updateNumber += 1

 def callback(self, data):
     if self.show:
         print data.fields
         xs = []
         ys = []
         zs = []
         # For some reason, returns x in [0], y in  [1], z in [2], rgb in [3]
         for point in pc2.read_points(data,skip_nans=False,field_names=("rgb","x","y","z")):
             xs.append(point[0])
             ys.append(point[1])
             zs.append(point[2])
         print "Length: " + str(len(xs))
         #add start
         cloud = np.array([xs,ys,zs])
         nans = np.isnan(cloud)
         cloud[nans] = 0
         cloud = cloud.transpose()
         print cloud[0]
         # cloud, _ = perception.calPointCloud(cloud)
         #add end
         #cloud = cloud.transpose()
         cloud = cloud[::10]
         print cloud
         xs = cloud[:,0]
         ys = cloud[:,1]
         zs = cloud[:,2]
         #segmentation(cloud)
         # print xs
         # print ys
         print "length after down sample: "+str(len(zs))
         if len(xs) > 0:
             self.show = False
             print "Saving to file: " + SAVE_LOCATION
             np.savez(SAVE_LOCATION, xs, ys, zs)
             print 'done saving, you can exit now with CTRL-C'

    def k_means(self, msg):
        ## Perform K Means on the data
        #
        # @param msg A sensor_msgs/PointCloud2 message with num_means clusters
        starttime = time.clock()
        points = pc2.read_points(msg, field_names=["x","y","z","rgb"], skip_nans=True)
        data = []

        for point in points:
            data += [point[0:3]]

        data = numpy.array(data)
        kmeans = KM(n_clusters = self.num_means)
        kmeans.fit(data)
        print kmeans.cluster_centers_
        centers = sorted(kmeans.cluster_centers_, key=lambda p: p[0])

        markers = MarkerArray()
        for id_num, center in enumerate(centers):
            centerMarker = self.makeMarker(msg.header, id_num, center)
            markers.markers.append(centerMarker)

        self.pub.publish(markers)
        
        endtime = time.clock()
        if self.debug:
            print >> sys.stderr, "KMeans time: " + str(endtime - starttime)

    def pc_cb(self, data):
        # point_cloud2 library's read_points function returns a generator
        # where each entry is a list of the fields specified
        gen = pc2.read_points(data, field_names=("x", "y", "z"))

        # Filepath is just /tmp right now
        file_to_open = self.filepath + '/etu_points_raw.xyz'
        f = open(file_to_open, 'w')

        for xyz in gen:

            # This creates the message to be added to the group
            # and eventually published
            point = XYZ()
            point.x = xyz[0]
            point.y = xyz[1]
            point.z = xyz[2]
            self.group.append(point)

            # This is for writing directly to an XYZ file
            to_write = '%(1)f %(2)f %(3)f\n' % {'1':xyz[0], '2':xyz[1], '3':xyz[2]}
            f.write(to_write)

        f.close()

        # This takes the first published point cloud and sends it to
        # the xyz_to_mesh.py script
        if not self.printed:
            self.printed = True
            rospack = rospkg.RosPack()
            package_path = rospack.get_path('uv_decontamination')
            full_path = package_path + '/scripts/xyz_to_mesh.py'
            call(["python", full_path])

    def get_nearest_cloud(self, msg):
        points = list()
        
        # Get all the points in the visible cloud (may be prefiltered by other nodes)
        for point in point_cloud2.read_points(msg, skip_nans=True):
            points.append(point[:3])

        # Convert to a numpy array            
        points_arr = np.float32([p for p in points]).reshape(-1, 1, 3) 
        
        # Compute the COG 
        cog = np.mean(points_arr, 0)

        # Abort if we get an NaN in any component
        if np.isnan(np.sum(cog)):
            return
        
        # Store the COG in a ROS Point object
        cog_point = Point()
        cog_point.x = cog[0][0]
        cog_point.y = cog[0][1]
        cog_point.z = cog[0][2]
        
        # Give the COG a unit orientation and store as a PoseStamped message
        target = PoseStamped()
        target.header.stamp = rospy.Time.now()
        target.header.frame_id = msg.header.frame_id
        target.pose.position = cog_point
        target.pose.orientation.w = 1.0
        
        # Publish the PoseStamped message on the /target_pose topic
        self.target_pub.publish(target)

    def cloudCallback(self, data):
        print 'Time between cloud calls:', time.time() - self.cloudTime
        startTime = time.time()

        self.pointCloud = data

        self.transposeGripperToCamera()

        # Determine location of spoon
        spoon3D = [0.22, -0.050, 0]
        self.spoon = np.dot(self.lGripperTransposeMatrix, np.array([spoon3D[0], spoon3D[1], spoon3D[2], 1.0]))[:3]
        self.spoonX, self.spoonY = self.pinholeCamera.project3dToPixel(self.spoon)

        lowX, highX, lowY, highY = self.boundingBox()
        self.spoonImageData = self.imageData[lowY:highY, lowX:highX, :]

        points2D = [[x, y] for y in xrange(lowY, highY) for x in xrange(lowX, highX)]
        try:
            points3D = pc2.read_points(self.pointCloud, field_names=('x', 'y', 'z'), skip_nans=True, uvs=points2D)
        except:
            print 'Unable to unpack from PointCloud2!', self.cameraWidth, self.cameraHeight, self.pointCloud.width, self.pointCloud.height
            return

        self.points3D = np.array([point for point in points3D])

        self.updateNumber += 1
        print 'Cloud computation time:', time.time() - startTime
        self.cloudTime = time.time()

def pointCloudCallback(msg: PointCloud2):
    ''' Process point cloud data (generated from the laser) '''

    # Store robot position in map grid coordinates
    robot_map_coord = np.array(
        [round((MAP_WIDTH/2+robot_pose.x)/MAP_RESOLUTION),  # Col
         round((MAP_HEIGHT/2-robot_pose.y)/MAP_RESOLUTION)],  # Row
        dtype=int)

    for p in point_cloud2.read_points(msg, field_names=("x", "y", "z"),
                                      skip_nans=True):

        # Get obtained value in the map grid (columm and row) - must be uint8.
        obstacle_map = np.array(
            [round((MAP_WIDTH/2+p[0])/MAP_RESOLUTION),  # Col / x
             round((MAP_HEIGHT/2-p[1])/MAP_RESOLUTION)],  # Row / y
            dtype=np.int)

        # Update map using the line iterator for free space
        it = createLineIterator(robot_map_coord,  # Start point
                                obstacle_map,  # End point
                                occ_map)
        for pt in it[:-1]:
            occ_map.itemset((pt[1], pt[0]), clipValue(pt[2]+CELL_DELTA_UP,
                                                      MIN_CELL_VALUE,
                                                      MAX_CELL_VALUE))

        # Update map using the line iterator for occupied space, if
        # applicable
        occ_map.itemset((it[-1][1], it[-1][0]),
                        clipValue(it[-1][2]-CELL_DELTA_DOWN,
                                  MIN_CELL_VALUE,
                                  MAX_CELL_VALUE))

    def callback(self, data):
        """Computes the average point in a point cloud and saves timing info.
        """
        points = pc2.read_points(data, field_names=['x', 'y', 'z'],
            skip_nans=True)
    
        num_points = 0
        avg_x = 0
        avg_y = 0
        avg_z = 0
        for x, y, z in points:
            num_points += 1
            avg_x += x
            avg_y += y
            avg_z += z
        if num_points > 0:
            avg_x /= num_points
            avg_y /= num_points
            avg_z /= num_points

        current_time = rospy.Time.now()
        self._time_taken += current_time - self._prev_time
        self._prev_time = current_time
        self._num_calls += 1

        rospy.loginfo('x: {}, y: {}, z: {}, time/point: {}s'.format(
            avg_x, avg_y, avg_z, self._time_taken.to_sec() / self._num_calls))

    def get_nearest_cloud(self, msg):
        points = list()
        points_xy = list()
        
        # Get all the points in the visible cloud (may be prefiltered by other nodes)
        for point in point_cloud2.read_points(msg, skip_nans=True):
            points.append(point[:3])
            points_xy.append(point[:2])

        # Convert to a numpy array            
        points_arr = np.float32([p for p in points]).reshape(-1, 1, 3) 
        
        # Compute the COG 
        cog = np.mean(points_arr, 0)
        
        # Convert to a Point
        cog_point = Point()
        cog_point.x = cog[0][0]
        cog_point.y = cog[0][1]
        cog_point.z = cog[0][2]
        #cog_point.z = 0.35
        
        # Abort if we get an NaN in any component
        if np.isnan(np.sum(cog)):
            return
        
        # If we have enough points, find the best fit ellipse around them
        try:
            if len(points_xy) > 6:
                points_xy_arr = np.float32([p for p in points_xy]).reshape(-1, 1, 2)  
                track_box = cv2.fitEllipse(points_xy_arr)
            else:
                # Otherwise, find the best fitting rectangle
                track_box = cv2.boundingRect(points_xy_arr)
            
            angle = pi - radians(track_box[2])
        except:
            return
        
        #print angle
        
        # Convert the rotation angle to a quaternion
        q_angle = quaternion_from_euler(0, angle, 0, axes='sxyz')
        q = Quaternion(*q_angle)
        
        q.x = 0.707
        q.y = 0
        q.z = 0.707
        q.w = 0

        # Publish the COG and orientation
        target = PoseStamped()
        target.header.stamp = rospy.Time.now()
        target.header.frame_id = msg.header.frame_id
        target.pose.position = cog_point
        target.pose.orientation = q
        
        # Publish the movement command
        self.target_pub.publish(target)