00001
00002
00003 import roslib; roslib.load_manifest('experiments_tro')
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
00036 self.start_t = rospy.get_param('~start',0.0)
00037 self.stop_t = rospy.get_param('~stop',0.0)
00038 self.save_frame = rospy.get_param('~save_frame',200)
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 count = 0
00062 out_publishers = {}
00063 first_t = None
00064 for topic, msg, t in rosbag.Bag(self.infile).read_messages():
00065 if first_t==None:
00066 first_t = t
00067 if topic not in out_publishers:
00068 print "adding publisher for topic '%s' with type '%s'"%(topic,msg.__class__.__name__)
00069 out_publishers[topic] = rospy.Publisher(topic, msg.__class__)
00070
00071 out_publishers[topic].publish(msg)
00072 if t<rospy.Duration(self.start_t) + first_t:
00073 continue
00074 if self.stop_t>0 and t>rospy.Duration(self.stop_t) + first_t:
00075 continue
00076 if topic=="/r_cart/state/x":
00077 identity = PoseStamped(msg.header,Pose(Point(0,0,0),Quaternion(0,0,0,1)))
00078 try:
00079 msgtime = msg.header.stamp
00080 msg.header.stamp = rospy.Time(0)
00081 msg = self.tf.transformPose('wide_stereo_optical_frame',msg)
00082 msg.header.stamp = msgtime
00083 identity.header.stamp = rospy.Time(0)
00084 identity = self.tf.transformPose('wide_stereo_optical_frame',identity)
00085 identity.header.stamp = msgtime
00086 except (tf.LookupException, tf.ConnectivityException,tf.ExtrapolationException) as e:
00087 print "transform failure",e,e.__class__
00088 continue
00089
00090 count += 1
00091 if(count % self.every == 0):
00092 self.object_parts[0].pose.append(identity.pose)
00093 self.object_parts[1].pose.append(msg.pose)
00094
00095 if(len(self.object_parts[0].pose) <self.save_frame):
00096 continue
00097
00098
00099
00100 rospy.loginfo('sending tracks with '+('/'.join(["%d"%len(track.pose) for track in self.object_parts]))+' poses')
00101 for track in self.object_parts:
00102 track.header = msg.header
00103
00104 self.object_msg.header = msg.header
00105 self.object_msg.parts = copy.deepcopy(self.object_parts)
00106 if self.downsample:
00107 for part in self.object_msg.parts:
00108 if len(part.pose)>self.samples:
00109 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]
00110 else:
00111 for part in self.object_msg.parts:
00112 if len(part.pose)>self.samples:
00113 part.pose = part.pose[len(part.pose) - self.samples:]
00114
00115 rospy.loginfo('fitting tracks with '+('/'.join(["%d"%len(track.pose) for track in self.object_msg.parts]))+' poses')
00116
00117 request = ArticulatedObjectSrvRequest()
00118 request.object = self.object_msg
00119 parts = len(self.object_parts)
00120 print "fitting models.."
00121 response = self.fit_models(request)
00122 print '\n'.join(
00123 ['(%s:%d,%d) bic=%f pos_err=%f rot_err=%f sigma_pos=%f sigma_orient=%f'%(
00124 model.name[0:2],
00125 model.id/parts,
00126 model.id%parts,
00127 get_param(model,'bic'),
00128 get_param(model,'avg_error_position'),
00129 get_param(model,'avg_error_orientation'),
00130 get_param(model,'sigma_position'),
00131 get_param(model,'sigma_orientation')
00132 ) for model in response.object.models])
00133 request.object = copy.deepcopy(response.object)
00134
00135 print "selecting models and structure.."
00136 response = self.get_fast_graph(request)
00137 self.object_msg.models = response.object.models
00138
00139 request.object = copy.deepcopy(response.object)
00140 print "visualizing models.."
00141 response = self.visualize_graph(request)
00142
00143 rospy.loginfo('received articulation model: '+(' '.join(
00144 ['(%s:%d,%d)'%(model.name[0:2],model.id/parts,model.id%parts) for model in response.object.models])))
00145
00146 print '\n'.join(
00147 ['(%s:%d,%d) bic=%f pos_err=%f rot_err=%f'%(
00148 model.name[0:2],
00149 model.id/parts,
00150 model.id%parts,
00151 get_param(model,'bic'),
00152 get_param(model,'avg_error_position'),
00153 get_param(model,'avg_error_orientation')
00154 ) for model in response.object.models])
00155 print "dofs=%d, nominal dofs=%d, bic=%f pos_err=%f rot_err=%f"%(
00156 get_param(response.object,'dof'),
00157 get_param(response.object,'dof.nominal'),
00158 get_param(response.object,'bic'),
00159 get_param(response.object,'avg_error_position'),
00160 get_param(response.object,'avg_error_orientation')
00161 )
00162 print "done evaluating new pose.."
00163
00164 print "rendering image.."
00165 self.render_image( RenderImageSrvRequest() )
00166 print "done rendering image.."
00167 break
00168
00169
00170
00171 if __name__ == '__main__':
00172 rospy.init_node('process_robotbag')
00173
00174 ProcessRobotBag()
