model_selection_client.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('door_perception')
00004 import rospy
00005 import numpy
00006 
00007 from articulation_msgs.msg import *
00008 from articulation_msgs.srv import *
00009 from geometry_msgs.msg import Pose, Point, Quaternion
00010 from sensor_msgs.msg import ChannelFloat32
00011 
00012 
00013 PRISMATIC = 0
00014 ROTATIONAL = 1
00015 MODELS={PRISMATIC:'prismatic',ROTATIONAL:'rotational'}
00016 
00017 def sample_track(model = PRISMATIC, n = 100, sigma_position = 0.02):
00018         msg = TrackMsg()
00019         msg.header.stamp = rospy.get_rostime()
00020         msg.header.frame_id = "/"
00021         msg.id = model
00022         for i in range(0,n):
00023                 q = i / float(n)
00024                 if model == PRISMATIC:
00025                         pose = Pose(Point(q, 0, 0), Quaternion(0, 0, 0, 1))
00026                 elif model == ROTATIONAL:
00027                         pose = Pose(Point(numpy.sin(q), numpy.cos(q) - 1.0, 0), Quaternion(0, 0, 0, 1))
00028                 else:
00029                         raise NameError("unknown model, cannot generate trajectory!")
00030                 pose.position.x += numpy.random.rand()*sigma_position
00031                 pose.position.y += numpy.random.rand()*sigma_position
00032                 pose.position.z += numpy.random.rand()*sigma_position
00033                 msg.pose.append( pose )
00034         return msg
00035 
00036 def main():
00037         rospy.init_node('test_fitting')
00038 
00039         model_select = rospy.ServiceProxy('model_select', TrackModelSrv)
00040         model_pub = rospy.Publisher('model', ModelMsg)
00041         print
00042   
00043         while True:
00044                 for model_type,model_name in MODELS.items():
00045                         request = TrackModelSrvRequest()
00046                         print "generating track of type '%s'" % model_name
00047                         request.model.track = sample_track( model_type )
00048                  
00049                         try:            
00050                                 response = model_select(request)
00051                                 print "selected model: '%s' (n = %d, log LH = %f)" % (
00052                                         response.model.name,
00053                                         len(response.model.track.pose),
00054                                         [entry.value for entry in response.model.params if entry.name=='loglikelihood'][0]
00055                                 )
00056                                 model_pub.publish(response.model)
00057                         except rospy.ServiceException: 
00058                                 print "model selection failed"
00059                                 pass
00060                         if rospy.is_shutdown():
00061                                 exit(0)
00062                         print
00063                         rospy.sleep(0.5)
00064 
00065 
00066 
00067 if __name__ == '__main__':
00068   main()
00069 
00070 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


door_perception_old
Author(s): Felix Endres
autogenerated on Wed Dec 26 2012 15:56:10