def success_failure_classification_preprocess(folder_name): data_dict = None #counter = 0 for bag_name in glob.glob('%s/*.bag' % folder_name): print 'Loading bag %s' % bag_name topics_dict = ru.bag_sel(bag_name, [ '/imitate_behavior_marker', '/pressure/l_gripper_motor', '/pressure/r_gripper_motor', '/accelerometer/l_gripper_motor', '/accelerometer/r_gripper_motor', '/joint_states', '/l_cart/command_pose', '/r_cart/command_pose' ]) ##break each mat into segments based on states #/imitate_behavior_marker #topics_dict['/imitate_behavior_marker']['t'] print 'There are %d marker messages.' % len( topics_dict['/imitate_behavior_marker']['msg']) time_segments = [[]] for marker_msg in topics_dict['/imitate_behavior_marker']['msg']: if len(time_segments[-1]) >= 2: time_segments.append([]) time_segments[-1].append(marker_msg.header.stamp.to_time()) if data_dict == None: data_dict = {} #organize by segment, then by type, then by what that type will store for i in range(len(time_segments)): data_dict[i] = {} ###pressure mat with times ##/pressure/l_gripper_motor ##/pressure/r_gripper_motor for ptop in ['/pressure/l_gripper_motor', '/pressure/r_gripper_motor']: p = topics_dict[ptop] psegs = bp.segment_msgs(time_segments, p['msg']) print '>> Separated records of %s (%s) into %d segments' % ( ptop, bag_name, len(psegs)) #convert segments into mats for i, pseg in enumerate(psegs): #each col is an example in left-f and right_f left_f, right_f, ptimes = hpr2.pressure_state_to_mat(pseg) if not data_dict[i].has_key(ptop): data_dict[i][ptop] = [] data_dict[i][ptop].append({ 'left': left_f, 'right': right_f, 't': ptimes }) for ptop in ['/pressure/l_gripper_motor', '/pressure/r_gripper_motor']: for k in data_dict.keys(): print '>>', k, ptop, len(data_dict[k][ptop]) return data_dict
def success_failure_classification_preprocess(folder_name): data_dict = None #counter = 0 for bag_name in glob.glob('%s/*.bag' % folder_name): print 'Loading bag %s' % bag_name topics_dict = ru.bag_sel(bag_name, ['/imitate_behavior_marker', '/pressure/l_gripper_motor', '/pressure/r_gripper_motor', '/accelerometer/l_gripper_motor', '/accelerometer/r_gripper_motor', '/joint_states', '/l_cart/command_pose', '/r_cart/command_pose']) ##break each mat into segments based on states #/imitate_behavior_marker #topics_dict['/imitate_behavior_marker']['t'] print 'There are %d marker messages.' % len(topics_dict['/imitate_behavior_marker']['msg']) time_segments = [[]] for marker_msg in topics_dict['/imitate_behavior_marker']['msg']: if len(time_segments[-1]) >= 2: time_segments.append([]) time_segments[-1].append(marker_msg.header.stamp.to_time()) if data_dict == None: data_dict = {} #organize by segment, then by type, then by what that type will store for i in range(len(time_segments)): data_dict[i] = {} ###pressure mat with times ##/pressure/l_gripper_motor ##/pressure/r_gripper_motor for ptop in ['/pressure/l_gripper_motor', '/pressure/r_gripper_motor']: p = topics_dict[ptop] psegs = bp.segment_msgs(time_segments, p['msg']) print '>> Separated records of %s (%s) into %d segments' % (ptop, bag_name, len(psegs)) #convert segments into mats for i, pseg in enumerate(psegs): #each col is an example in left-f and right_f left_f, right_f, ptimes = hpr2.pressure_state_to_mat(pseg) if not data_dict[i].has_key(ptop): data_dict[i][ptop] = [] data_dict[i][ptop].append({'left': left_f, 'right': right_f, 't': ptimes}) for ptop in ['/pressure/l_gripper_motor', '/pressure/r_gripper_motor']: for k in data_dict.keys(): print '>>', k, ptop, len(data_dict[k][ptop]) return data_dict
def process_bag(full_bag_name, prosilica_image_file, model_image_file, experiment_start_condition_pkl, arm_used='left'): bag_path, bag_name_ext = os.path.split(full_bag_name) filename, ext = os.path.splitext(bag_name_ext) ############################################################################### # Playback the bag bag_playback = Process(target=playback_bag, args=(full_bag_name, )) bag_playback.start() ############################################################################### ## Listen for transforms using rosbag rospy.init_node('bag_proceessor') tl = tf.TransformListener() print 'waiting for transform' tl.waitForTransform('map', 'base_footprint', rospy.Time(), rospy.Duration(20)) # Extract the starting location map_T_bf p_base = tfu.transform('map', 'base_footprint', tl) \ * tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0))) t, r = tfu.matrix_as_tf(p_base) pose_base = (t, r) print 'done with tf' ########################################################## ## Find contact locations start_conditions = ut.load_pickle(experiment_start_condition_pkl) start_conditions['highdef_image'] = prosilica_image_file start_conditions['model_image'] = model_image_file rospy.loginfo('extracting object localization features') start_conditions[ 'pose_parameters'] = extract_object_localization_features2( start_conditions, tl, arm_used, p_base) if bag_playback.is_alive(): rospy.loginfo('Terminating playback process') bag_playback.terminate() time.sleep(1) bag_playback.terminate() time.sleep(1) rospy.loginfo('Playback process terminated? %s' % str(not bag_playback.is_alive())) ############################################################################### #Read bag using programmatic API pr2_kinematics = pr2k.PR2Kinematics(tl) converter = JointMsgConverter() rospy.loginfo('opening bag, reading state topics') topics_dict = ru.bag_sel(full_bag_name, [ '/joint_states', '/l_cart/command_pose', '/r_cart/command_pose', '/torso_controller/state', '/pressure/l_gripper_motor', '/pressure/r_gripper_motor' ]) ## Select the arm that has been moving, segment joint states based on contact states. if arm_used == 'left': pressures = topics_dict['/pressure/l_gripper_motor'] elif arm_used == 'right': pressures = topics_dict['/pressure/r_gripper_motor'] else: raise RuntimeError('arm_used invalid') ## find contact times rospy.loginfo('Finding contact times') left_f, right_f, ptimes = hpr2.pressure_state_to_mat(pressures['msg']) ## create segments based on contacts # TODO: make this accept more contact stages contact_times = find_contact_times(left_f, right_f, ptimes, 250) if len(contact_times) > 2: time_segments = [['start', contact_times[0]], [contact_times[0], contact_times[-1]], [contact_times[-1], 'end']] else: time_segments = [['start', 'end']] rospy.loginfo('Splitting messages based on contact times') ## split pressure readings based on contact times pressure_lseg = segment_msgs( time_segments, topics_dict['/pressure/l_gripper_motor']['msg']) pressure_rseg = segment_msgs( time_segments, topics_dict['/pressure/r_gripper_motor']['msg']) ## split cartesian commands based on contact times lcart_seg = segment_msgs(time_segments, topics_dict['/l_cart/command_pose']['msg']) rcart_seg = segment_msgs(time_segments, topics_dict['/r_cart/command_pose']['msg']) ## split joint states joint_states = topics_dict['/joint_states']['msg'] print 'there are %d joint state messages in bag' % len(joint_states) j_segs = segment_msgs(time_segments, topics_dict['/joint_states']['msg']) jseg_dicts = [converter.msgs_to_dict(seg) for seg in j_segs] # find the first set of joint states j0_dict = jseg_dicts[0][0] ## perform FK rospy.loginfo('Performing FK to find tip locations') bf_T_obj = htf.composeHomogeneousTransform( start_conditions['pose_parameters']['frame_bf'], start_conditions['pose_parameters']['center_bf']) obj_T_bf = np.linalg.inv(bf_T_obj) for jseg_dict in jseg_dicts: for d in jseg_dict: rtip_bf = pr2_kinematics.right.fk('base_footprint', 'r_wrist_roll_link', 'r_gripper_tool_frame', d['poses']['rarm'].A1.tolist()) ltip_bf = pr2_kinematics.left.fk('base_footprint', 'l_wrist_roll_link', 'l_gripper_tool_frame', d['poses']['larm'].A1.tolist()) rtip_obj = obj_T_bf * rtip_bf ltip_obj = obj_T_bf * ltip_bf d['rtip_obj'] = tfu.matrix_as_tf(rtip_obj) d['ltip_obj'] = tfu.matrix_as_tf(ltip_obj) d['rtip_bf'] = tfu.matrix_as_tf(rtip_bf) d['ltip_bf'] = tfu.matrix_as_tf(ltip_bf) ############################################################################### # make movement state dictionaries, one for each state movement_states = [] for i, seg in enumerate(time_segments): name = "state_%d" % i start_times = [ lcart_seg[i][0].header.stamp.to_time(), rcart_seg[i][0].header.stamp.to_time(), jseg_dicts[i][0]['time'], pressure_lseg[i][0].header.stamp.to_time(), pressure_rseg[i][0].header.stamp.to_time() ] sdict = { 'name': name, 'start_time': np.min(start_times), 'cartesian': [[ru.ros_to_dict(ps) for ps in lcart_seg[i]], [ru.ros_to_dict(ps) for ps in rcart_seg[i]]], 'joint_states': jseg_dicts[i] #'pressure': [pressure_lseg[i], pressure_rseg[i]] } movement_states.append(sdict) # store in a dict data = { 'start_conditions': start_conditions, # ['camera_info', 'map_T_bf', 'pro_T_bf', 'points' (in base_frame), # 'highdef_image', 'model_image', ## 'pose_parameters' ## 'descriptors' ## 'directions' (wrt to cannonical orientation) ## 'closest_feature' 'base_pose': pose_base, 'robot_pose': j0_dict, 'arm': arm_used, 'movement_states': movement_states } # save dicts to pickles processed_bag_name = '%s_processed.pkl' % os.path.join(bag_path, filename) rospy.loginfo('saving to %s' % processed_bag_name) ut.save_pickle(data, processed_bag_name) bag_playback.join() rospy.loginfo('finished!')
def process_bag(full_bag_name, prosilica_image_file, model_image_file, experiment_start_condition_pkl, arm_used='left'): bag_path, bag_name_ext = os.path.split(full_bag_name) filename, ext = os.path.splitext(bag_name_ext) ############################################################################### # Playback the bag bag_playback = Process(target=playback_bag, args=(full_bag_name,)) bag_playback.start() ############################################################################### ## Listen for transforms using rosbag rospy.init_node('bag_proceessor') tl = tf.TransformListener() print 'waiting for transform' tl.waitForTransform('map', 'base_footprint', rospy.Time(), rospy.Duration(20)) # Extract the starting location map_T_bf p_base = tfu.transform('map', 'base_footprint', tl) \ * tfu.tf_as_matrix(([0., 0., 0., 1.], tr.quaternion_from_euler(0,0,0))) t, r = tfu.matrix_as_tf(p_base) pose_base = (t, r) print 'done with tf' ########################################################## ## Find contact locations start_conditions = ut.load_pickle(experiment_start_condition_pkl) start_conditions['highdef_image'] = prosilica_image_file start_conditions['model_image'] = model_image_file rospy.loginfo('extracting object localization features') start_conditions['pose_parameters'] = extract_object_localization_features2(start_conditions, tl, arm_used, p_base) if bag_playback.is_alive(): rospy.loginfo('Terminating playback process') bag_playback.terminate() time.sleep(1) bag_playback.terminate() time.sleep(1) rospy.loginfo('Playback process terminated? %s' % str(not bag_playback.is_alive())) ############################################################################### #Read bag using programmatic API pr2_kinematics = pr2k.PR2Kinematics(tl) converter = JointMsgConverter() rospy.loginfo('opening bag, reading state topics') topics_dict = ru.bag_sel(full_bag_name, ['/joint_states', '/l_cart/command_pose', '/r_cart/command_pose', '/torso_controller/state', '/pressure/l_gripper_motor', '/pressure/r_gripper_motor']) ## Select the arm that has been moving, segment joint states based on contact states. if arm_used == 'left': pressures = topics_dict['/pressure/l_gripper_motor'] elif arm_used == 'right': pressures = topics_dict['/pressure/r_gripper_motor'] else: raise RuntimeError('arm_used invalid') ## find contact times rospy.loginfo('Finding contact times') left_f, right_f, ptimes = hpr2.pressure_state_to_mat(pressures['msg']) ## create segments based on contacts # TODO: make this accept more contact stages contact_times = find_contact_times(left_f, right_f, ptimes, 250) if len(contact_times) > 2: time_segments = [['start', contact_times[0]], [contact_times[0], contact_times[-1]], [contact_times[-1], 'end']] else: time_segments = [['start', 'end']] rospy.loginfo('Splitting messages based on contact times') ## split pressure readings based on contact times pressure_lseg = segment_msgs(time_segments, topics_dict['/pressure/l_gripper_motor']['msg']) pressure_rseg = segment_msgs(time_segments, topics_dict['/pressure/r_gripper_motor']['msg']) ## split cartesian commands based on contact times lcart_seg = segment_msgs(time_segments, topics_dict['/l_cart/command_pose']['msg']) rcart_seg = segment_msgs(time_segments, topics_dict['/r_cart/command_pose']['msg']) ## split joint states joint_states = topics_dict['/joint_states']['msg'] print 'there are %d joint state messages in bag' % len(joint_states) j_segs = segment_msgs(time_segments, topics_dict['/joint_states']['msg']) jseg_dicts = [converter.msgs_to_dict(seg) for seg in j_segs] # find the first set of joint states j0_dict = jseg_dicts[0][0] ## perform FK rospy.loginfo('Performing FK to find tip locations') bf_T_obj = htf.composeHomogeneousTransform(start_conditions['pose_parameters']['frame_bf'], start_conditions['pose_parameters']['center_bf']) obj_T_bf = np.linalg.inv(bf_T_obj) for jseg_dict in jseg_dicts: for d in jseg_dict: rtip_bf = pr2_kinematics.right.fk('base_footprint', 'r_wrist_roll_link', 'r_gripper_tool_frame', d['poses']['rarm'].A1.tolist()) ltip_bf = pr2_kinematics.left.fk('base_footprint', 'l_wrist_roll_link', 'l_gripper_tool_frame', d['poses']['larm'].A1.tolist()) rtip_obj = obj_T_bf * rtip_bf ltip_obj = obj_T_bf * ltip_bf d['rtip_obj'] = tfu.matrix_as_tf(rtip_obj) d['ltip_obj'] = tfu.matrix_as_tf(ltip_obj) d['rtip_bf'] = tfu.matrix_as_tf(rtip_bf) d['ltip_bf'] = tfu.matrix_as_tf(ltip_bf) ############################################################################### # make movement state dictionaries, one for each state movement_states = [] for i, seg in enumerate(time_segments): name = "state_%d" % i start_times = [lcart_seg[i][0].header.stamp.to_time(), rcart_seg[i][0].header.stamp.to_time(), jseg_dicts[i][0]['time'], pressure_lseg[i][0].header.stamp.to_time(), pressure_rseg[i][0].header.stamp.to_time()] sdict = {'name': name, 'start_time': np.min(start_times), 'cartesian': [[ru.ros_to_dict(ps) for ps in lcart_seg[i]], [ru.ros_to_dict(ps) for ps in rcart_seg[i]]], 'joint_states': jseg_dicts[i] #'pressure': [pressure_lseg[i], pressure_rseg[i]] } movement_states.append(sdict) # store in a dict data = {'start_conditions': start_conditions, # ['camera_info', 'map_T_bf', 'pro_T_bf', 'points' (in base_frame), # 'highdef_image', 'model_image', ## 'pose_parameters' ## 'descriptors' ## 'directions' (wrt to cannonical orientation) ## 'closest_feature' 'base_pose': pose_base, 'robot_pose': j0_dict, 'arm': arm_used, 'movement_states': movement_states} # save dicts to pickles processed_bag_name = '%s_processed.pkl' % os.path.join(bag_path, filename) rospy.loginfo('saving to %s' % processed_bag_name) ut.save_pickle(data, processed_bag_name) bag_playback.join() rospy.loginfo('finished!')