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!')