cloud_matcher_control.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # This utility allows to test /cloud_matcher_service with two CSV files.
00004 # This python only works with 3D files, while /cloud_matcher_service
00005 # supports both 2D and 3D
00006 
00007 import sys
00008 import re
00009 import roslib; roslib.load_manifest('modular_cloud_matcher')
00010 import rospy
00011 import point_cloud
00012 from std_msgs.msg import String, Header
00013 from sensor_msgs.msg import PointCloud
00014 from geometry_msgs.msg import *
00015 from modular_cloud_matcher.srv import *
00016 
00017 def createCloud(fileName):
00018         points = []
00019         for line in open(fileName):
00020                 splittedLine = re.split('(?:\s|[,]|\n)+', line.strip())
00021                 coord = map(float, splittedLine)
00022                 while len(coord) < 3:
00023                         coord.append(0)
00024                 points.append(coord)
00025         return point_cloud.create_cloud_xyz32(Header(),points)
00026 
00027 def icpcontrol(referenceFileName, readingsFileName):
00028         serviceName = '/match_clouds'
00029         rospy.init_node('cloud_matcher_control')
00030         rospy.wait_for_service(serviceName)
00031         # read reference
00032         reference = createCloud(referenceFileName)
00033         # read readings
00034         readings = createCloud(readingsFileName)
00035         # call matcher
00036         try:
00037                 matchClouds = rospy.ServiceProxy(serviceName, MatchClouds)
00038                 finalTransform = matchClouds(reference, readings)
00039                 print finalTransform
00040         except rospy.ServiceException, e:
00041                 print "Service call failed: %s"%e
00042         
00043 if __name__ == '__main__':
00044         if len(sys.argv) != 3:
00045                 print 'Usage: ' + sys.argv[0] + ' reference reading'
00046                 sys.exit()
00047         icpcontrol(sys.argv[1], sys.argv[2])


ethzasl_icp_mapper
Author(s): François Pomerleau and Stéphane Magnenat
autogenerated on Thu Jan 2 2014 11:16:21