test.py

Go to the documentation of this file.

#!/usr/bin/env python

import roslib; roslib.load_manifest('door_perception')
import rospy
import numpy
import tf

from articulation_msgs.msg import *
from articulation_msgs.srv import *
from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
from sensor_msgs.msg import ChannelFloat32

#Storage
global_track_msg = TrackMsg()
global_track_msg.header.stamp = rospy.get_rostime()
global_track_msg.header.frame_id = "/"
global_track_msg.id = 1

def pose_callback(pose_stamped):
    rospy.loginfo(rospy.get_name()+" heard a pose in frame %s",pose_stamped.header.frame_id)
    request = TrackModelSrvRequest()
    global_track_msg.pose.append( pose_stamped.pose )
    global_track_msg.pose_headers.append( pose_stamped.header )
    request.model.track = global_track_msg

    try:      
        response = model_select(request)
        print "selected model: '%s' (n = %d, log LH = %f)" % (
            response.model.name,
            len(response.model.track.pose),
            [entry.value for entry in response.model.params if entry.name=='loglikelihood'][0]
        )
        model_pub.publish(response.model)
    except rospy.ServiceException: 
        print "model selection failed"
        pass

def main():
    rospy.init_node('door_fitting')
    listener = tf.TransformListener()
    rospy.Subscriber("/camera/pose0", PoseStamped, pose_callback)

    rospy.wait_for_service('model_select')
    model_select = rospy.ServiceProxy('model_select', TrackModelSrv)
    model_pub = rospy.Publisher('model', ModelMsg)
 
    rospy.spin()


if __name__ == '__main__':
  main()