process_robotbag.py
Go to the documentation of this file.
00001 #!/usr/bin/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 *
00008 from std_msgs.msg import ColorRGBA
00009 from sensor_msgs.msg import ChannelFloat32,CameraInfo,CompressedImage
00010 from tf.msg import tfMessage 
00011 from articulation_msgs.msg import *
00012 from articulation_msgs.srv import *
00013 from articulation_models.track_utils import *
00014 from imagerender.srv import *
00015 import tf
00016 import process_bags
00017 import rosbag
00018 import time
00019 
00020 class ProcessRobotBag:
00021         def __init__(self):
00022                 rospy.wait_for_service('fit_models', 5)
00023                 rospy.wait_for_service('get_spanning_tree', 5)
00024                 rospy.wait_for_service('get_fast_graph', 5)
00025                 rospy.wait_for_service('visualize_graph', 5)
00026                 rospy.wait_for_service('render_image', 5)
00027                 self.tf = tf.TransformListener()
00028                 self.fit_models = rospy.ServiceProxy('fit_models', ArticulatedObjectSrv)
00029                 self.get_spanning_tree = rospy.ServiceProxy('get_spanning_tree', ArticulatedObjectSrv)
00030                 self.get_fast_graph = rospy.ServiceProxy('get_fast_graph', ArticulatedObjectSrv)
00031                 self.visualize_graph = rospy.ServiceProxy('visualize_graph', ArticulatedObjectSrv)
00032                 self.render_image = rospy.ServiceProxy('render_image', RenderImageSrv)
00033                 
00034                 self.infile = rospy.get_param('~infile','in.bag')
00035                 self.outfile = rospy.get_param('~outfile','out.bag')
00036 
00037                 self.start_t = rospy.get_param('~start',0.0)
00038                 self.stop_t = rospy.get_param('~stop',0.0)
00039                 
00040                 self.sigma_position = rospy.get_param('~sigma_position',0.01)
00041                 self.sigma_orientation = rospy.get_param('~sigma_orientation',0.3)
00042                 self.samples = rospy.get_param('~samples',50) #keep this number of samples
00043                 self.every = rospy.get_param('~every',1) #skip poses?
00044                 self.downsample = rospy.get_param('~downsample',False)#downsample or latest obs?
00045                 self.object_msg = ArticulatedObjectMsg()
00046                 set_param(self.object_msg, "sigma_position", self.sigma_position, ParamMsg.PRIOR)
00047                 set_param(self.object_msg, "sigma_orientation", self.sigma_orientation, ParamMsg.PRIOR)
00048                 
00049                 self.object_parts = [ 
00050                                 TrackMsg(
00051                                                                  id=i,
00052 
00053                                 ) 
00054                                 for i in range(2)
00055                         ]
00056                 
00057                 self.run()
00058 
00059         def run(self):
00060                 startn=0
00061                 outbag = rosbag.Bag(self.outfile, "w")
00062                 count = 0
00063                 out_publishers = {}
00064                 first_t = None
00065                 for topic, msg, t in rosbag.Bag(self.infile).read_messages():
00066                         if topic=="/wide_stereo/left/image_color/compressed" and count>=startn:
00067                           print "rendering image.."
00068                           self.render_image( RenderImageSrvRequest() )
00069                           print "done rendering image.."
00070                                         
00071                         if first_t==None:
00072                                 first_t = t
00073                         if topic not in out_publishers:
00074                                 print "adding publisher for topic '%s' with type '%s'"%(topic,msg.__class__.__name__)
00075                                 out_publishers[topic] = rospy.Publisher(topic, msg.__class__)
00076 
00077                         out_publishers[topic].publish(msg)
00078                         outbag.write(topic, msg, t)
00079                         if t<rospy.Duration(self.start_t) + first_t:
00080                                 continue
00081                         if self.stop_t>0 and t>rospy.Duration(self.stop_t) + first_t:
00082                                 continue
00083                         if topic=="/r_cart/state/x":
00084                                 identity = PoseStamped(msg.header,Pose(Point(0,0,0),Quaternion(0,0,0,1)))
00085                                 try:
00086                                   msgtime = msg.header.stamp
00087                                   msg.header.stamp = rospy.Time(0)
00088                                   msg = self.tf.transformPose('wide_stereo_optical_frame',msg)
00089                                   msg.header.stamp = msgtime
00090                                   identity.header.stamp = rospy.Time(0)
00091                                   identity = self.tf.transformPose('wide_stereo_optical_frame',identity)
00092                                   identity.header.stamp = msgtime
00093                                 except (tf.LookupException, tf.ConnectivityException,tf.ExtrapolationException) as e:
00094                                   print "transform failure",e,e.__class__
00095                                   continue
00096 
00097                                 count += 1
00098                                 if(count % self.every == 0):
00099                                         self.object_parts[0].pose.append(identity.pose)
00100                                         self.object_parts[1].pose.append(msg.pose)
00101                                         
00102                                         if(count <startn):
00103                                                 continue
00104                                         
00105 #                                       if len(self.object_parts[0].pose)<255:
00106 #                                               continue
00107                                         rospy.loginfo('sending tracks with '+('/'.join(["%d"%len(track.pose) for track in self.object_parts]))+' poses')
00108                                         for track in self.object_parts:
00109                                                 track.header = msg.header
00110                                                 outbag.write('/track', track, t)
00111                                                 
00112                                         self.object_msg.header = msg.header
00113                                         self.object_msg.parts = copy.deepcopy(self.object_parts)
00114                                         if self.downsample:
00115                                                 for part in self.object_msg.parts:
00116                                                         if len(part.pose)>self.samples:
00117                                                                 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] 
00118                                         else:
00119                                                 for part in self.object_msg.parts:
00120                                                         if len(part.pose)>self.samples:
00121                                                                 part.pose = part.pose[len(part.pose) - self.samples:]
00122                                         
00123                                         rospy.loginfo('fitting tracks with '+('/'.join(["%d"%len(track.pose) for track in self.object_msg.parts]))+' poses')
00124                                         
00125                                         request = ArticulatedObjectSrvRequest()
00126                                         request.object = self.object_msg
00127                                         parts = len(self.object_parts) 
00128                                         print "fitting models.."                
00129                                         response = self.fit_models(request)
00130                                         print '\n'.join(
00131                                                  ['(%s:%d,%d) bic=%f pos_err=%f rot_err=%f sigma_pos=%f sigma_orient=%f'%(
00132                                                                         model.name[0:2],
00133                                                                         model.id/parts,
00134                                                                         model.id%parts,
00135                                                                         get_param(model,'bic'),
00136                                                                         get_param(model,'avg_error_position'),
00137                                                                         get_param(model,'avg_error_orientation'),
00138                                                                         get_param(model,'sigma_position'),
00139                                                                         get_param(model,'sigma_orientation')
00140                                                         ) for model in response.object.models])
00141                                         request.object = copy.deepcopy(response.object)
00142         
00143                                         print "selecting models and structure.."                
00144                                         response = self.get_fast_graph(request)
00145                                         outbag.write('/object', response.object, t)
00146                                         self.object_msg.models = response.object.models
00147                                                 
00148                                         request.object = copy.deepcopy(response.object)
00149                                         print "visualizing models.."            
00150                                         response = self.visualize_graph(request)
00151                                         outbag.write('/structure_array', response.object.markers, t)
00152                                         
00153                                         rospy.loginfo('received articulation model: '+(' '.join(
00154                                                  ['(%s:%d,%d)'%(model.name[0:2],model.id/parts,model.id%parts) for model in response.object.models])))
00155                                 
00156                                         print '\n'.join(
00157                                                  ['(%s:%d,%d) bic=%f pos_err=%f rot_err=%f'%(
00158                                                                         model.name[0:2],
00159                                                                         model.id/parts,
00160                                                                         model.id%parts,
00161                                                                         get_param(model,'bic'),
00162                                                                         get_param(model,'avg_error_position'),
00163                                                                         get_param(model,'avg_error_orientation')
00164                                                         ) for model in response.object.models])
00165                                         print "dofs=%d, nominal dofs=%d, bic=%f pos_err=%f rot_err=%f"%(
00166                                                                         get_param(response.object,'dof'),
00167                                                                         get_param(response.object,'dof.nominal'),
00168                                                                         get_param(response.object,'bic'),
00169                                                                         get_param(response.object,'avg_error_position'),
00170                                                                         get_param(response.object,'avg_error_orientation')
00171                                                                         )                       
00172                                         print "done evaluating new pose.."
00173                                         
00174 
00175                 outbag.close()          
00176                  
00177 if __name__ == '__main__':
00178         rospy.init_node('process_robotbag')
00179 
00180         ProcessRobotBag()
 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