00001
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)
00043 self.every = rospy.get_param('~every',1)
00044 self.downsample = rospy.get_param('~downsample',False)
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
00106
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()
