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 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