Go to the documentation of this file.00001
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