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