#!/usr/bin/python
import roslib; roslib.load_manifest('matcher')
from ethzasl_icp_mapper.srv import MatchClouds
import rospy
from random import random

from sensor_msgs.point_cloud2 import read_points, create_cloud
from sensor_msgs.msg import PointCloud2, PointField

rospy.init_node("matchit")
rospy.wait_for_service('/match_clouds')

match_em = rospy.ServiceProxy('/match_clouds', MatchClouds)

pc = PointCloud2()
pc.header.frame_id = '/map'
pc.fields.append( PointField('x', 0, 7, 1) )
pc.fields.append( PointField('y', 4, 7, 1) )
pc.fields.append( PointField('z', 8, 7, 1) )

new_points = [ (1,0,0) ]*10
combined = create_cloud(pc.header, pc.fields, new_points)

new_points2 = [ (2+random(),3+random(),0) for i in range(10)]
combined2 = create_cloud(pc.header, pc.fields, new_points2)

print match_em(combined, combined2)