#!/usr/bin/env python


import rospy
from geometry_msgs.msg import Twist
from kobuki_msgs.msg import BumperEvent
from random import randint
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2


# states: HIT1, ROTATING, DEFAULT

def finder():
	rospy.init_node('finder', anonymous=True)
	rospy.Subscriber("/camera/depth_registered/points", PointCloud2, callback);
	
	rospy.spin()


def callback(data):
	data_out = pc2.read_points(data, field_names=None, skip_nans=False, uvs=[pc2.width, pc2.height])
    	int_data = next(data_out)
    	rospy.loginfo("int_data " + str(int_data))

	rospy.loginfo(rospy.get_caller_id() + "%s\n\n#\n\n", data)
        
if __name__ == '__main__':
	try:
		finder()
	except rospy.ROSInterruptException:
		pass

