def biotacCallback(self,data):
#Grab all Joint efforts
(valid, position, velocity, effort) = self.joint_states.return_joint_states(self.joint_names)
for ind,joint_name in enumerate(self.joint_names):
self.pr2_biotac_log.joint_states[ind].position = position[ind]
self.pr2_biotac_log.joint_states[ind].velocity = velocity[ind]
self.pr2_biotac_log.joint_states[ind].effort = effort[ind]
#print time
for ind,xform_name in enumerate(self.tf_child_names):
#Grab Transformations for each finger, and tool frame
(tf_trans, tf_rot, tf_valid) = self.tfLookUp(self.tf_parent_name, xform_name)
self.pr2_biotac_log.transforms[ind].transform.translation = tf_trans
self.pr2_biotac_log.transforms[ind].transform.rotation = tf_rot
self.pr2_biotac_log.transforms[ind].transform_valid = tf_valid
# Store off the BioTac Data Message
self.pr2_biotac_log.bt_hand = data
# Store the accelerometer and gripper aperture position
self.pr2_biotac_log.gripper_accelerometer = self.gripper_accelerometer
# Store the controller state
self.pr2_biotac_log.controller_state = self.controller_state
self.pr2_biotac_log.controller_state_detail = self.controller_state_detail
# Stores the frame count into the message
self.pr2_biotac_log.frame_count = self.frame_count
# Uses rosjson_time to convert message to JSON
toWrite = rosjson_time.ros_message_to_json(self.pr2_biotac_log) + "\n"
self.fout.write(toWrite)
# Move to next frame
self.frame_count += 1
def main():
""" Usage
First log the data using rosbag:
rosbag record /biotac_pub [other topics] -o file_prefix
Many topics can be recorded, but this file only parses biotac msgs.
The call rosrun rosrun biotac_log_parser parse_log_json.py -i bag_file -o output.json
Multiple bag files can be specified on the command line using wildcards, but they need to be
enclosed in quotes:
The call rosrun rosrun biotac_log_parser parse_log_json.py -i "*.bag" -o output.json
"""
parser = OptionParser()
parser.add_option("-i", "--input_file", dest="input_file",
help="input bag FILEs. Use wildcards for multiple files",
metavar="FILE", action="store")
parser.add_option("-o", "--output_file", dest="output_file",
help="output FILE", metavar="FILE")
(options, _) = parser.parse_args()
if options.input_file is None:
rospy.logerr("The input bag has to be specified")
sys.exit()
if options.output_file is None:
rospy.logerr("The output file has to be specified")
sys.exit()
output_file = open(options.output_file, "w")
gen = (glob.glob(f) for f in options.input_file.split())
frame_count = 0
elements = []
for filename in itertools.chain.from_iterable(gen):
rospy.loginfo("Opening bag %s"% filename)
bag = rosbag.Bag(filename)
for _, msg, _ in bag.read_messages(topics="/biotac_pub"):
isinstance(msg, BioTacHand)
msg.header.frame_id = frame_count
toWrite = rosjson_time.ros_message_to_json(msg) + '\n'
elements.append(toWrite)
frame_count +=1
output_file.write("[\n")
output_file.write( ",".join(elements))
output_file.write("]\n")
output_file.close()
def convert_data(self, raw_data):
raw_data = json.loads(rosjson_time.ros_message_to_json(raw_data))
data = {}
data['t_send'] = raw_data['bt_time']['frame_end_time']
data['temperature'] = [raw_data['bt_data'][0]['tdc_data'],
raw_data['bt_data'][1]['tdc_data']]
data['thermal_flux'] = [raw_data['bt_data'][0]['tac_data'],
raw_data['bt_data'][1]['tac_data']]
data['fluid_pressure'] = [raw_data['bt_data'][0]['pdc_data'],
raw_data['bt_data'][1]['pdc_data']]
data['microvibration'] = [raw_data['bt_data'][0]['pac_data'][0],
raw_data['bt_data'][1]['pac_data'][0]]
data['force'] = [sum(raw_data['bt_data'][0]['electrode_data']),
sum(raw_data['bt_data'][1]['electrode_data'])]
data['x'] = 0
data['y'] = 0
data['z'] = 0
return json.dumps(data)
def biotacCallback(self, data):
#rospy.loginfo(rospy.get_name()+' FRAME ID: %d',self.frame_count)
# Stores the frame count into the message
data.header.frame_id = self.frame_count
# Uses rosjson to convert message to JSON
toWrite = rosjson_time.ros_message_to_json(data) + '\n'
self.fout.write(toWrite)
#Check to see if Elapsed time has run out
if self.node_log_time is not 'inf':
elapsed_time = rospy.get_time() - self.start_time
if elapsed_time >= self.node_log_time:
finish_msg = 'Logging for %f second(s) Complete!' % self.node_log_time
rospy.loginfo(finish_msg)
self.__del__()
rospy.signal_shutdown(finish_msg)
# Move to next frame
self.frame_count += 1
def biotacCallback(self,data):
#rospy.loginfo(rospy.get_name()+' FRAME ID: %d',self.frame_count)
# Stores the frame count into the message
data.header.frame_id = self.frame_count
# Uses rosjson to convert message to JSON
toWrite = rosjson_time.ros_message_to_json(data) + '\n'
self.fout.write(toWrite);
#Check to see if Elapsed time has run out
if self.node_log_time is not 'inf':
elapsed_time = rospy.get_time() - self.start_time
if elapsed_time >= self.node_log_time:
finish_msg = 'Logging for %f second(s) Complete!' % self.node_log_time
rospy.loginfo(finish_msg)
self.__del__()
rospy.signal_shutdown(finish_msg)
# Move to next frame
self.frame_count += 1
