articulation_cartcollector.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('articulation_structure')
00004 import rospy
00005 import sys
00006 from visualization_msgs.msg import Marker, MarkerArray
00007 from geometry_msgs.msg import Vector3, Point, Pose,PoseStamped, PoseArray
00008 from std_msgs.msg import ColorRGBA
00009 from sensor_msgs.msg import ChannelFloat32
00010 from articulation_msgs.msg import *
00011 from articulation_msgs.srv import *
00012 from articulation_models.track_utils import *
00013 from articulation_models.transform_datatypes import *
00014 import tf
00015 import logging
00016 import getopt
00017 import colorsys
00018 import numpy
00019 import copy
00020 
00021 class articulation_cartcollector:
00022   def __init__(self):
00023     try:
00024       rospy.wait_for_service('fit_models', 5)
00025       rospy.wait_for_service('get_spanning_tree', 5)
00026       rospy.wait_for_service('get_fast_graph', 5)
00027       rospy.wait_for_service('visualize_graph', 5)
00028       print "services OK"
00029     except rospy.ROSException:
00030       print "Services not found"
00031       rospy.signal_shutdown('Quit')
00032 
00033     self.listener = tf.TransformListener()
00034 
00035     self.pose_pub = rospy.Publisher('track', TrackMsg)
00036     self.object_pub = rospy.Publisher('object', ArticulatedObjectMsg)
00037     rospy.Subscriber("/r_cart/state/x", PoseStamped, self.callback,queue_size=1)
00038     self.object_parts = []
00039     self.fit_models = rospy.ServiceProxy('fit_models', ArticulatedObjectSrv)
00040     self.get_spanning_tree = rospy.ServiceProxy('get_spanning_tree', ArticulatedObjectSrv)
00041     self.get_fast_graph = rospy.ServiceProxy('get_fast_graph', ArticulatedObjectSrv)
00042     self.visualize_graph = rospy.ServiceProxy('visualize_graph', ArticulatedObjectSrv)
00043     
00044     self.sigma_position = rospy.get_param('~sigma_position',0.01)
00045     self.sigma_orientation = rospy.get_param('~sigma_orientation',0.3)
00046     self.reduce_dofs = rospy.get_param('~reduce_dofs',1)
00047     self.samples = rospy.get_param('~samples',50) #keep this number of samples
00048     self.downsample = rospy.get_param('~downsample',False)#downsample or latest obs?
00049     self.object_msg = ArticulatedObjectMsg()
00050     set_param(self.object_msg, "sigma_position", self.sigma_position, ParamMsg.PRIOR)
00051     set_param(self.object_msg, "sigma_orientation", self.sigma_orientation, ParamMsg.PRIOR)
00052     set_param(self.object_msg, "reduce_dofs", self.reduce_dofs, ParamMsg.PRIOR)
00053   
00054   def callback(self, pose):
00055     print "adding pose.."
00056 #    try:
00057 #      (trans,rot) = self.listener.lookupTransform('/torso_lift_link', '/wide_stereo_optical_frame', pose.header.stamp)
00058 #    except (tf.LookupException, tf.ConnectivityException):
00059 #      return
00060 #    print trans,rot
00061 #      
00062 #    transform = numpy.dot(translation_matrix(trans),quaternion_matrix(rot))
00063 #    transformedPose.header = pose.header
00064 #    transformedPose.header.frame_id = '/wide_stereo_optical_frame'
00065 #    transformedPose.pose = mat44_to_pose( numpy.dot(transform,pose_to_mat44( pose.pose) ) )
00066     transformedPose = pose 
00067       
00068     if(len(self.object_parts)==0):
00069       self.object_parts = [ 
00070           TrackMsg(
00071                    id=i,
00072 
00073           ) 
00074           for i in range(2)
00075           ]
00076       
00077     identity = Pose(Point(0,0,0),Quaternion(0,0,0,1))
00078     self.object_parts[0].pose.append(identity)
00079     self.object_parts[1].pose.append(transformedPose.pose)
00080       
00081     rospy.loginfo('sending tracks with '+('/'.join(["%d"%len(track.pose) for track in self.object_parts]))+' poses')
00082     for track in self.object_parts:
00083       track.header = transformedPose.header
00084       self.pose_pub.publish(track)
00085       
00086                 # downsample or cut-off?
00087 #    self.object_msg.parts = self.object_parts
00088     self.object_msg.header = transformedPose.header
00089     self.object_msg.parts = copy.deepcopy(self.object_parts)
00090     if self.downsample:
00091       for part in self.object_msg.parts:
00092         if len(part.pose)>self.samples:
00093           part.pose = [p for (i,p) in enumerate(part.pose) if i % (len(part.pose) / self.samples + 1) == 0 or i==len(part.pose)-1] 
00094     else:
00095       for part in self.object_msg.parts:
00096         if len(part.pose)>self.samples:
00097           part.pose = part.pose[len(part.pose) - self.samples:]
00098 
00099     self.object_msg.header = transformedPose.header
00100     
00101     request = ArticulatedObjectSrvRequest()
00102     request.object = self.object_msg
00103     
00104     parts = len(self.object_parts) 
00105     print "calling fit service"
00106     response = self.fit_models(request)
00107     print " fit service done"
00108     print '\n'.join(
00109        ['(%s:%d,%d) bic=%f pos_err=%f rot_err=%f sigma_pos=%f sigma_orient=%f'%(
00110             model.name[0:2],
00111             model.id/parts,
00112             model.id%parts,
00113             get_param(model,'bic'),
00114             get_param(model,'avg_error_position'),
00115             get_param(model,'avg_error_orientation'),
00116             get_param(model,'sigma_position'),
00117             get_param(model,'sigma_orientation')
00118         ) for model in response.object.models])
00119     request.object = copy.deepcopy(response.object)
00120 
00121     response = self.get_fast_graph(request)
00122     self.object_pub.publish(response.object)
00123     self.object_msg.models = response.object.models
00124       
00125     request.object = copy.deepcopy(response.object)
00126     response = self.visualize_graph(request)
00127     
00128     rospy.loginfo('received articulation model: '+(' '.join(
00129        ['(%s:%d,%d)'%(model.name[0:2],model.id/parts,model.id%parts) for model in response.object.models])))
00130   
00131     print '\n'.join(
00132        ['(%s:%d,%d) bic=%f pos_err=%f rot_err=%f'%(
00133             model.name[0:2],
00134             model.id/parts,
00135             model.id%parts,
00136             get_param(model,'bic'),
00137             get_param(model,'avg_error_position'),
00138             get_param(model,'avg_error_orientation')
00139         ) for model in response.object.models])
00140     print "dofs=%d, nominal dofs=%d, bic=%f pos_err=%f rot_err=%f"%(
00141             get_param(response.object,'dof'),
00142             get_param(response.object,'dof.nominal'),
00143             get_param(response.object,'bic'),
00144             get_param(response.object,'avg_error_position'),
00145             get_param(response.object,'avg_error_orientation')
00146             )      
00147     print "done evaluating new pose.."
00148 def main():
00149   try:
00150     rospy.init_node('articulation_cartcollector')
00151     articulation_cartcollector()
00152     rospy.spin()
00153   except rospy.ROSInterruptException: pass
00154     
00155 if __name__ == '__main__':
00156   main()
00157 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


articulation_structure
Author(s): sturm
autogenerated on Wed Dec 26 2012 15:37:59