def _assert_bag_valid(self, filename, topics=None, start_time=None, stop_time=None): ''' Open the bagfile at the specified filename and read it to ensure topic limits were enforced and the optional topic list and start/stop times are also enforced. ''' bag = Bag(filename) topics_dict = bag.get_type_and_topic_info()[1] bag_topics = set(topics_dict.keys()) param_topics = set(self.topic_limits.keys()) if topics: self.assertEqual(bag_topics, set(topics)) self.assertTrue(bag_topics.issubset(param_topics)) for topic in topics_dict: size = topics_dict[ topic].message_count * 8 # Calculate stored message size as each message is 8 bytes count = topics_dict[topic].message_count gen = bag.read_messages(topics=topic) _, _, first_time = next(gen) last_time = first_time # in case the next for loop does not execute if start_time: self.assertGreaterEqual(first_time, start_time) for _, _, last_time in gen: # Read through all messages so last_time is valid pass if stop_time: self.assertLessEqual(last_time, stop_time) duration = last_time - first_time self._assert_limits_enforced(topic, duration, size, count)
def stream(input_file: Bag, output_file: Bag, topics: list) -> None: """ Stream data from an input bag to an output bag. Args: input_file: the input bag to stream from output_file: the output bag to write data to topics: a list of the topics to include Returns: None """ included = 0 skipped = 0 # create a progress bar for iterating over the messages in the bag with tqdm(total=input_file.get_message_count(), unit='message') as prog: # iterate over the messages in this input bag for topic, msg, time in input_file: # check for matches between the topics filter and this topic if any(fnmatchcase(topic, pattern) for pattern in topics): # write this message to the output bag output_file.write(topic, msg, time) # increment the counter of included messages included += 1 else: # increment the counter of excluded messages skipped += 1 # update the progress bar with a single iteration prog.update(1) # update the progress bar post fix text with statistics prog.set_postfix(included=included, skipped=skipped)
def load_trajectory_from_bag(file,fast=True,fast_only=False): traj = [] fast_traj = [] bag = Bag(file) normal_topics = ['raven_state', 'raven_state/array'] normal_topics = normal_topics + ['/' + topic for topic in normal_topics] fast_topic = ['raven_state/1000Hz'] #fast_topic = fast_topic + ['/' + topic for topic in fast_topic] if fast_only: for topic, msg, t in bag.read_messages(topics=fast_topic): fast_traj.append(msg) return get_trajectory(fast_traj) for topic, msg, t in bag.read_messages(topics=normal_topics): traj.append(msg) if fast: for topic, msg, t in bag.read_messages(topics=fast_topic): fast_traj.append(msg) if fast_traj: return get_trajectory(fast_traj,traj) else: return get_trajectory(traj)
def incoming_ask(ask_msg): rospy.loginfo('Incoming ask:\n' + str(ask_msg)) if ask_msg.model == self.model and ask_msg.token == self.token: rospy.loginfo('Incoming ask with right model and token') with TemporaryDirectory() as tmpdir: rospy.logdebug('Temporary directory created: %s', tmpdir) os.chdir(tmpdir) rospy.loginfo('Waiting for objective file from IPFS...') try: self.ipfs.get(ask_msg.objective, timeout=60) rospy.logdebug('Objective is written to %s', tmpdir + '/' + ask_msg.objective) bag = Bag(str(tmpdir + '/' + ask_msg.objective)) msg_type = list(bag.get_type_and_topic_info()[1].values())[0][0] if msg_type != Mission()._type: rospy.logwarn('Wrong message type in objective topic') return except ipfsapi.exceptions.TimeoutError: rospy.logwarn('Timeout of waiting objective file: ' + str(ask_msg.objective)) rospy.logwarn('Skip incoming ask') return self.make_bid(ask_msg) else: rospy.logwarn('Incoming ask with wrong model and token, skip')
def main(): parser = argparse.ArgumentParser( description= 'Merge one or more bag files with the possibilities of filtering topics.' ) parser.add_argument('outputbag', help='Output bag file with topics merged.') parser.add_argument('inputbag', nargs='+', help='Input bag files.') parser.add_argument('-v', '--verbose', action="store_true", default=False, help='Verbose output.') parser.add_argument( '-t', '--topics', default="*", help= 'String interpreted as a list of topics (wildcards \'*\' and \'?\' allowed) to include in the merged bag file.' ) args = parser.parse_args() topics = args.topics.split(' ') total_included_count = 0 total_skipped_count = 0 if (args.verbose): print("Writing bag file: " + args.outputbag) print("Matching topics against patters: '%s'" % ' '.join(topics)) with Bag(args.outputbag, 'w') as o: for ifile in args.inputbag: matchedtopics = [] included_count = 0 skipped_count = 0 if (args.verbose): print("> Reading bag file: " + ifile) with Bag(ifile, 'r') as ib: for topic, msg, t in ib: if any(fnmatchcase(topic, pattern) for pattern in topics): if not topic in matchedtopics: matchedtopics.append(topic) if (args.verbose): print("Including matched topic '%s'" % topic) o.write(topic, msg, t) included_count += 1 else: skipped_count += 1 total_included_count += included_count total_skipped_count += skipped_count if (args.verbose): print("< Included %d messages and skipped %d" % (included_count, skipped_count)) if (args.verbose): print("Total: Included %d messages and skipped %d" % (total_included_count, total_skipped_count))
def dump(input_file: Bag, topics: list, output_file: 'file' = None) -> None: """ Dump messages from a bag. Args: input_file: the input bag to dump topics from topics: the topics to dump output_file: an optional file to dump to Returns: None """ # create a progress bar for iterating over the messages in the bag with tqdm(total=input_file.get_message_count( topic_filters=topics)) as prog: # iterate over the messages in this input bag for topic, msg, _ in input_file.read_messages(topics=topics): # update the progress bar with a single iteration prog.update(1) # create the line to print line = '{} {}\n\n'.format(topic, msg) # print the line to the terminal print(line) # if there is an output file, write the line to it if output_file is not None: output_file.write(line)
def get_info(bag_file, topic_filter=None): bag = Bag(bag_file) topics = bag.get_type_and_topic_info().topics for topic in topics: if topic_filter and topics[topic].msg_type not in topic_filter: continue print("{}: {} Hz".format(topic, round(topics[topic].frequency, 3))) print(topics[topic].message_count) times = np.ones(shape=bag.get_message_count(topic_filters=topic)) n = 0 for _, msg, t in bag.read_messages(topics=topic): times[n] = msg.header.stamp.to_sec() n += 1 times = 1 / np.gradient(times) times = times[np.where((times > np.percentile(times, 10)) & (times < np.percentile(times, 90)))] print("mean: {}, median: {}".format(np.mean(times), np.median(times))) print("min: {}, max: {}".format(np.min(times), np.max(times))) # plt.scatter(times, np.gradient(times)) plt.hist(times) plt.yscale("log") plt.title("{}: {}".format(os.path.basename(bag_file), topic)) if not os.path.exists("images"): os.makedirs("images") plt.savefig(os.path.join("images/", "{}.{}.png".format(os.path.basename(bag_file), topic.replace("/", "")))) plt.cla()
def load_trajectory_from_bag(file, fast=True, fast_only=False): traj = [] fast_traj = [] bag = Bag(file) normal_topics = ['raven_state', 'raven_state/array'] normal_topics = normal_topics + ['/' + topic for topic in normal_topics] fast_topic = ['raven_state/1000Hz'] #fast_topic = fast_topic + ['/' + topic for topic in fast_topic] if fast_only: for topic, msg, t in bag.read_messages(topics=fast_topic): fast_traj.append(msg) return get_trajectory(fast_traj) for topic, msg, t in bag.read_messages(topics=normal_topics): traj.append(msg) if fast: for topic, msg, t in bag.read_messages(topics=fast_topic): fast_traj.append(msg) if fast_traj: return get_trajectory(fast_traj, traj) else: return get_trajectory(traj)
def shift( input_file: Bag, output_file: Bag, shift_topic: list, shift_sec: float, ) -> None: """ Shift a topic in a bag forward or backwards in time. Args: input_file: the input bag to stream from output_file: the output bag to write data to topic: the topic to shift in time shift_sec: the amount of time to shift by Returns: None """ # create a progress bar for iterating over the messages in the bag with tqdm(total=input_file.get_message_count()) as prog: # iterate over the messages in this input bag for topic, msg, time in input_file: # if the topic is the sentinel topic, increase the count if topic == shift_topic: # adjust time by the shift value time += rospy.Duration(shift_sec) # update the message header with the new time stamp msg.header.stamp = time # update the progress bar with a single iteration prog.update(1) # write the message output_file.write(topic, msg, time)
def main(): parser = argparse.ArgumentParser() parser.add_argument("--bag", dest="bag", help="Path to ROS bag.", required=True) parser.add_argument("--left_image_0", help="Path to 'left_image_00000.png'") parser.add_argument("--right_image_0", help="Path to 'right_image_00000.png'") parser.add_argument("--start_time", help="A start time to check", type=float, default=None) args = parser.parse_args() bridge = CvBridge() bag = Bag(args.bag) t_start = bag.get_start_time() n_msgs = 0 if args.left_image_0 is None and args.right_image_0 is None: raise ValueError( "You must provide either left_image_0 or right_image_0") isLeft = args.left_image_0 is not None ref_img = args.left_image_0 if isLeft else args.right_image_0 print("Using ref image %s" % ref_img) ref_image_0 = cv2.imread(ref_img)[:, :, 0] assert ref_image_0 is not None topic = '/davis/left/image_raw' if isLeft else '/davis/right/image_raw' if args.start_time: topic, msg, t = next( bag.read_messages(topics=[topic], start_time=rospy.Time(args.start_time + t_start))) width = msg.width height = msg.height image = np.asarray(bridge.imgmsg_to_cv2(msg, msg.encoding)) image = np.reshape(image, (height, width)) print("Correct? {}".format(bool(np.all(np.isclose(image, ref_image_0))))) else: for topic, msg, t in bag.read_messages(topics=[topic]): n_msgs += 1 width = msg.width height = msg.height image = np.asarray(bridge.imgmsg_to_cv2(msg, msg.encoding)) image = np.reshape(image, (height, width)) if bool(np.all(np.isclose(image, ref_image_0))): print("Ref image found at {} ({} from start)".format( t.to_sec(), t.to_sec() - t_start)) return print("Processed {} images, ref not found!".format(n_msgs))
def write_bag(bag_origin, bag_write: rosbag.Bag, t_start): count = 0 for topic, msg, t in bag_origin.read_messages("/tf", t_start): time_stamp = rospy.Time(0) + (t - t_start) msg.transforms[0].header.stamp = time_stamp bag_write.write(topic, msg, time_stamp) count += 1 bag_write.close()
def import_schema(self, schema, srv_options, options, restriction_type, restricts): Bag = import_bag(srv_options) pcid_str = options.pop('pcid', srv_options.pop('pcid', 0)) pcid = int(pcid_str) patch_column = options.pop('patch_column', srv_options.pop('patch_column', 'points')) patch_columns = options.pop('patch_columns', '*').strip() patch_columns = [ col.strip() for col in patch_columns.split(',') if col.strip() ] filename = srv_options.pop('rosbag_path', "") + options.pop( 'rosbag_path', "") + schema bag = Bag(filename, 'r') tablecols = [] topics = bag.get_type_and_topic_info().topics pcid_for_topic = {k: pcid + 1 + i for i, k in enumerate(topics.keys())} pointcloud_formats = True if restriction_type is 'limit': topics = {k: v for k, v in topics.items() if k in restricts} pointcloud_formats = 'pointcloud_formats' in restricts elif restriction_type is 'except': topics = {k: v for k, v in topics.items() if k not in restricts} pointcloud_formats = 'pointcloud_formats' not in restricts tabledefs = [] if pointcloud_formats: tablecols = [ ColumnDefinition('pcid', type_name='integer'), ColumnDefinition('srid', type_name='integer'), ColumnDefinition('schema', type_name='text'), ColumnDefinition('format', type_name='text'), ColumnDefinition('rostype', type_name='text'), ColumnDefinition('columns', type_name='text[]'), ColumnDefinition('ply_header', type_name='text'), ] tableopts = { 'metadata': 'true', 'rosbag': schema, 'pcid': pcid_str } tabledefs.append( TableDefinition("pointcloud_formats", columns=tablecols, options=tableopts)) for topic, infos in topics.items(): pcid = pcid_for_topic[topic] columns, _, _, _, _, _ = get_columns(bag, topic, infos, pcid, patch_column, patch_columns) tablecols = [get_column_def(k, *v) for k, v in columns.items()] tableopts = {'topic': topic, 'rosbag': schema, 'pcid': str(pcid)} tabledefs.append( TableDefinition(topic, columns=tablecols, options=tableopts)) return tabledefs
def merge(): # Setting arguments parser = argparse.ArgumentParser(description='Merge one or more bag files with the possibilities of filtering topics.') parser.add_argument('outputbag', help='output bag file with topics merged') parser.add_argument('inputbag', nargs='+', help='input bag files') parser.add_argument('-v', '--verbose', action="store_true", default=False, help='verbose output') parser.add_argument('-t', '--topics', default="*", help='string interpreted as a list of topics (wildcards \'*\' and \'?\' allowed) to include in the merged/filtered bag file') parser.add_argument('-nt', '--new_topic', default=False, help='store all the potentially found topics with this "new_topic" name in the merged/filtered bag file') args = parser.parse_args() # Varialbe initialisation topics = args.topics.split(' ') total_included_count = 0 total_skipped_count = 0 if (args.verbose): print("Writing bag file: " + args.outputbag) print("Matching topics against patters: '%s'" % ' '.join(topics)) # Start the process with Bag(args.outputbag, 'w') as o: for ifile in args.inputbag: matchedtopics = [] included_count = 0 skipped_count = 0 if (args.verbose): print("> Reading bag file: " + ifile) # Read each potentially available bag file with Bag(ifile, 'r') as ib: for topic, msg, t in ib: if any(fnmatchcase(topic, pattern) for pattern in topics): if not topic in matchedtopics: matchedtopics.append(topic) if (args.verbose): print("Including matched topic '%s'" % topic) if args.new_topic: # If required, store with new topic name o.write(args.new_topic, msg, t) else: # otherwise, continue as it is o.write(topic, msg, t) included_count += 1 else: skipped_count += 1 total_included_count += included_count total_skipped_count += skipped_count if (args.verbose): print("< Included %d messages and skipped %d" % (included_count, skipped_count)) if (args.verbose): print("Total: Included %d messages and skipped %d" % (total_included_count, total_skipped_count))
def RenameTopics(bag_file, location, collect_type, car_name): print("Renaming topics...") new_bag_file = bag_file + "_" + location + "_" + collect_type + "_" + car_name + ".bag" velodyne_name = '/vlp16_1/velodyne_points' new_velodyne_name = '/points_raw' with Bag(new_bag_file, 'w') as Y: for topic, msg, t in Bag(bag_file + ".bag"): if topic == velodyne_name: Y.write(new_velodyne_name, msg, t) else: Y.write(topic, msg, t)
def remap(input_bag_path, output_bag_path): """ Remaps the topics of a single bag :param input_bag_path: the bag with the topic that should be renamed :param output_bag_path: the path where the output bag should be created """ with Bag(output_bag_path, 'w') as output_bag: # create output bag # write all topics into the output bag and rename the specified one: for topic, msg, t in Bag(input_bag_path): if topic == '/image_raw': output_bag.write('/camera/image_proc', msg, t) else: output_bag.write(topic, msg, t)
def decompress( input_file: Bag, output_file: Bag, camera_info: list, compressed_camera: list, raw_camera: list, ) -> None: """ Decompress a compressed image into a raw image. Args: input_file: the input bag to stream from output_file: the output bag to write data to camera_info: a list of info topics for each compressed image topic compressed_camera: a list of compressed image topics raw_camera: a list of raw image topics to publish Returns: None """ # get the dims for each compressed camera info topic dims = [get_camera_dimensions(input_file, info) for info in camera_info] # convert the list of dims to a dictionary of compressed topics to dims dims = dict(zip(compressed_camera, dims)) # setup a dictionary mapping compressed topics to raw camera topics raw_camera = dict(zip(compressed_camera, raw_camera)) # get the number of camera info topics to leave out camera_info_total = input_file.get_message_count(topic_filters=camera_info) # get the total number of output messages as total minus camera info topics total = input_file.get_message_count() - camera_info_total # create a progress bar for iterating over the messages in the output bag with tqdm(total=total) as prog: # iterate over the messages in this input bag for topic, msg, time in input_file: # don't write the camera info to the bag if topic in camera_info: continue # if the topic is the compress image topic, decompress the image if topic in compressed_camera: # get the image from the compressed topic img = get_camera_image(msg.data, dims[topic]) # create a raw image message and replace the message msg = image_msg(img, time, dims[topic], 'rgb8') # reset the topic topic = raw_camera[topic] # update the progress bar with a single iteration prog.update(1) # write the message output_file.write(topic, msg, time)
def bag_to_video( input_file: Bag, output_file: str, topic: str, fps: float = 30, codec: str = 'MJPG', ) -> None: """ Convert a ROS bag with image topic to a video file. Args: input_file: the bag file to get image data from output_file: the path to an output video file to create topic: the topic to read image data from fps: the frame rate of the video codec: the codec to use when outputting to the video file Returns: None """ # create an empty reference for the output video file video = None # get the total number of frames to write total = input_file.get_message_count(topic_filters=topic) # get an iterator for the topic with the frame data iterator = input_file.read_messages(topics=topic) # iterate over the image messages of the given topic for _, msg, _ in tqdm(iterator, total=total): # open the video file if it isn't open if video is None: # create the video codec codec = cv2.VideoWriter_fourcc(*codec) # open the output video file cv_dims = (msg.width, msg.height) video = cv2.VideoWriter(output_file, codec, fps, cv_dims) # read the image data into a NumPy tensor img = get_camera_image(msg.data, (msg.height, msg.width)) # write the image to the video file video.write(img) # if the video file is open, close it if video is not None: video.release() # read the start time of the video from the bag file start = input_file.get_start_time() # set the access and modification times for the video file os.utime(output_file, (start, start))
def create_objective(period: str, stream_id: str, email: str) -> str: ipfs = connect() tempdir = gettempdir() os.chdir(tempdir) with NamedTemporaryFile(delete=False) as f: bag = Bag(f.name, 'w') bag.write('/period', String(period)) bag.write('/stream_id', String(stream_id)) bag.write('/email', String(email)) bag.close() res = ipfs.add(f.name) rospy.loginfo("Hash of the objective is {}".format(res['Hash'])) return res['Hash']
def process_bag_file(file: Union[str, BinaryIO], topic_to_cb: Mapping[str, Callable[[ROSMsgT], DataPoint]]) \ -> DistTrace: """ Process messages from bag files Notes ----- Only the timestamp of messages from the same topic is guaranteed to be monotonically increasing. Be careful when merging messages from different topics. """ ret_dist_trace = {topic: [] for topic in topic_to_cb.keys() } # type: Dict[str, List[StampedDataPoint]] with Bag(file) as bag: for topic, msg, t in bag.read_messages(topics=topic_to_cb.keys()): if hasattr(msg, "header"): t = msg.header.stamp # Use sender's timestamp if available t_nsec = t.to_nsec() # type: int quo, rem = divmod(t_nsec, (5**6)) if t_nsec > MAX_PRECISE_INT or rem != 0: raise RuntimeWarning( "Timestamp %s nano-sec may lose precision when converted to milli-sec" "in floating point." % str(t)) t_msec_f = float(quo) / (2**6) if topic in topic_to_cb: dp = topic_to_cb[topic](msg) ret_dist_trace[topic].append(StampedDataPoint(t_msec_f, dp)) else: continue # Ignore other topics return ret_dist_trace
def d8n_get_all_images_topic_bag(bag: rosbag.Bag, min_messages: int = 0) -> List[Tuple[str, type]]: """ Returns the (name, type) of all topics that look like images and that have nonzero message count. """ tat = bag.get_type_and_topic_info() consider_images = [ "sensor_msgs/Image", "sensor_msgs/CompressedImage", ] all_types = set() found = [] topics = tat.topics for t, v in list(topics.items()): msg_type = v.msg_type all_types.add(msg_type) message_count = v.message_count if msg_type in consider_images: # quick fix: ignore image_raw if we have image_compressed version if "raw" in t: other = t.replace("raw", "compressed") if other in topics: continue if message_count < min_messages: # print('ignoring topic %r because message_count = 0' % t) continue found.append((t, msg_type)) return found
def main(): ifile = './mvsec_data_bags/outdoor_day2_events.bag' write_bag = Bag(ifile, 'w') print('write bag opened') ofile = './mvsec_data_bags/outdoor_day2_data.bag' read_bag = Bag(ofile, 'r') print('read bag opened') iteration = 1 for topic, msg, t in read_bag: if ('events' in topic): write_bag.write(topic, msg, t) iteration = iteration + 1 print(iteration, topic)
def get_start_stamp(bag: rosbag.Bag): pos_prev = [] t_prev = 0 count = 0 t_start = 0 for topic, msg, t in bag.read_messages('/tf'): if msg.transforms[0].child_frame_id == "Puck": pos = np.array([ msg.transforms[0].transform.translation.x, msg.transforms[0].transform.translation.y, msg.transforms[0].transform.translation.z ]) t_sec = msg.transforms[0].header.stamp.to_sec() if t_prev != 0: vel = np.linalg.norm((pos - pos_prev) / (t_sec - t_prev)) if vel > 0.1: if count == 0: t_start = t count += 1 if count > 10: return t_start else: count = 0 pos_prev = pos t_prev = t_sec
def ipfs_download(multihash: Multihash) -> (dict, Bag): rospy.wait_for_service('/ipfs/get_file') download = rospy.ServiceProxy('/ipfs/get_file', IpfsDownloadFile) tmpfile = NamedTemporaryFile(delete=False) res = download(multihash, Filepath(tmpfile.name)) tmpfile.close() if not res.success: raise Exception(res.error_msg) messages = {} bag = Bag(tmpfile.name, 'r') for topic, msg, timestamp in bag.read_messages(): if topic not in messages: messages[topic] = [msg] else: messages[topic].append(msg) os.unlink(tmpfile.name) return (messages, bag)
def import_schema(self, schema, srv_options, options, restriction_type, restricts): Bag = import_bag(srv_options) pcid_str = options.pop('pcid', srv_options.pop('pcid', 0)) pcid = int(pcid_str) patch_column = options.pop('patch_column', srv_options.pop('patch_column', 'points')) patch_columns = options.pop('patch_columns', '*').strip() patch_columns = [col.strip() for col in patch_columns.split(',') if col.strip()] filename = srv_options.pop('rosbag_path', "") + options.pop('rosbag_path', "") + schema bag = Bag(filename, 'r') tablecols = [] topics = bag.get_type_and_topic_info().topics pcid_for_topic = {k: pcid+1+i for i, k in enumerate(topics.keys())} pointcloud_formats = True if restriction_type is 'limit': topics = {k: v for k, v in topics.items() if k in restricts} pointcloud_formats = 'pointcloud_formats' in restricts elif restriction_type is 'except': topics = {k: v for k, v in topics.items() if k not in restricts} pointcloud_formats = 'pointcloud_formats' not in restricts tabledefs = [] if pointcloud_formats: tablecols = [ ColumnDefinition('pcid', type_name='integer'), ColumnDefinition('srid', type_name='integer'), ColumnDefinition('schema', type_name='text'), ColumnDefinition('format', type_name='text'), ColumnDefinition('rostype', type_name='text'), ColumnDefinition('columns', type_name='text[]'), ColumnDefinition('ply_header', type_name='text'), ] tableopts = {'metadata': 'true', 'rosbag': schema, 'pcid': pcid_str} tabledefs.append(TableDefinition("pointcloud_formats", columns=tablecols, options=tableopts)) for topic, infos in topics.items(): pcid = pcid_for_topic[topic] columns, _, _, _, _, _ = get_columns(bag, topic, infos, pcid, patch_column, patch_columns) tablecols = [get_column_def(k, *v) for k, v in columns.items()] tableopts = {'topic': topic, 'rosbag': schema, 'pcid': str(pcid)} tabledefs.append(TableDefinition(topic, columns=tablecols, options=tableopts)) return tabledefs
def writepc2frombag2file(input_bagfile, pc2_topic, out_file_path,start_time=None, end_time=None, msg_count=None): try: output_file_name = get_output_file_name(input_bagfile) + '_xyz.txt' if out_file_path.endswith('/'): output_file = out_file_path + output_file_name else: output_file = out_file_path+'/' + output_file_name output_file_fh = open(output_file,'w') print('writing output_file at', output_file) # check start and end time condition if start_time is not None and end_time is not None: assert (end_time - start_time) > 0 , "end_time should be higher than start time" if msg_count is None: use_msg_count = False else: use_msg_count = True # msg_count is already static casted in arg parse, just checking again for sanity ad if function is re used individially msg_count = int(msg_count) assert msg_count > 0, "should have positive msg_count" input_bag = Bag(input_bagfile,'r') print('bag load success') count = 0; for topic, msg, ts in input_bag.read_messages(topics=[pc2_topic],start_time=start_time, end_time=end_time): output_file_fh.write('msg_timestamp: %f\n' % msg.header.stamp.to_sec()) output_file_fh.write('%d\n' %msg.width) for data_pts in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True): output_file_fh.write('%f %f %f %d\n' % (data_pts[0], data_pts[1], data_pts[2], data_pts[3])) count +=1 print(count,msg_count) if use_msg_count and count >= msg_count: break input_bag.close() output_file_fh.close() except Exception as e_init: print(traceback.format_exc(e_init))
def _read_info(self): """Read some metadata from inside this rosbag.""" from rosbag import Bag, ROSBagUnindexedException, ROSBagException try: b = Bag(self.path) except ROSBagUnindexedException: b = Bag(self.path, allow_unindexed=True) print('Reindexing', self.filename) b.reindex() try: duration = b.get_end_time() - b.get_start_time() except ROSBagException: duration = 0 return { 'n_messages': b.get_message_count(), 'duration': duration, 'topic_list': b.get_type_and_topic_info()[1].keys() }
def clip( input_file: Bag, output_file: Bag, sentinel_topic: list, start: int, stop: int, ) -> None: """ Clip a bag using a range of messages for a given topic. Args: input_file: the input bag to stream from output_file: the output bag to write data to sentinel_topic: the topic to watch for starting and stopping the clip start: the starting message for the clip stop: the stopping message for the clip Returns: None """ # initialize a counter for the sentinel topic messages count = 0 # create a progress bar for iterating over the messages in the bag with tqdm(total=stop - start, unit='sentinel') as prog: # iterate over the messages in this input bag for topic, msg, time in input_file: # if the topic is the sentinel topic, increase the count if topic == sentinel_topic: count += 1 # if the count hasn't reached the starting point, continue if count < start: continue # if the count has exceeded the stopping point, break if count >= stop: break # update the progress bar with a single iteration if topic == sentinel_topic: prog.update(1) # write the message output_file.write(topic, msg, time)
def ipfs_download(multihash): rospy.wait_for_service('/liability/infochan/ipfs/get_file') download = rospy.ServiceProxy('/liability/infochan/ipfs/get_file', IpfsDownloadFile) with NamedTemporaryFile(delete=False) as tmpfile: res = download(multihash, Filepath(tmpfile.name)) if not res.success: raise Exception(res.error_msg) messages = {} for topic, msg, timestamp in Bag(tmpfile.name, 'r').read_messages(): messages[topic] = msg return messages
def _get_objective(self, lia: Liability) -> dict: mhash = lia.objective.multihash tempdir = gettempdir() os.chdir(tempdir) # temp_obj = NamedTemporaryFile(delete=False) self.ipfs.get(mhash) messages = {} for topic, msg, timestamp in Bag(mhash, 'r').read_messages(): messages[topic] = msg return messages
def extract_depth( bag_file: Bag, rgb_directory: str, camera_info: str, depth: str ) -> None: """ Extract depth data from a bag file. Args: bag_file: the bag file to play rgb_directory: the directory to find RGB image to match depths to camera_info: the topic to use to read metadata about the camera depth: the topic to use to read 32-bit floating point depth measures Returns: None """ # extract the camera dimensions from the bag dims = get_camera_dimensions(bag, camera_info) # get the images from a glob images = glob.glob(os.path.join(rgb_directory, 'X', 'data', '*.png')) # convert the images to numbers path_to_int = lambda x: int(os.path.basename(x).replace('.png', '')) images = sorted([path_to_int(image) for image in images]) # create the output directory output_dir = os.path.join(rgb_directory, 'D', 'data') try: os.makedirs(output_dir) except FileExistsError: pass # iterate over the messages progress = tqdm(total=len(images)) for _, msg, time in bag_file.read_messages(topics=depth): # if there are no more images left, break out of the loop if not len(images): break # if the time is less than the current image, continue if int(str(time)) < images[0]: continue # update the progress bar progress.update(1) # get the depth image img = get_depth_image(msg.data, dims, as_rgb=False) # save the depth image to disk output_file = os.path.join(output_dir, '{}-{}.npz'.format(images[0], time)) np.savez_compressed(output_file, y=img) # remove the first item from the list of times images.pop(0) # close the progress bar progress.close()
def ipfs_download(multihash): tempdir = gettempdir() os.chdir(tempdir) temp_obj = NamedTemporaryFile(delete=False) res = ipfs_download_file(connect(), multihash.multihash, temp_obj.name) if not res: raise Exception("Can't download objective") messages = {} for topic, msg, timestamp in Bag(temp_obj.name, 'r').read_messages(): messages[topic] = msg return messages
def pinata_rosbag(ipfs_hash: str) -> str: data = requests.get(f'https://gateway.pinata.cloud/ipfs/{ipfs_hash}') rospy.loginfo(data.status_code) if data.status_code != 200: rospy.loginfo(f'Error downloading rosbag! {data.content}') return else: tmpfile = NamedTemporaryFile(delete=False) tmpfile.write(data.content) bag = Bag(tmpfile.name) messages = {} for topic, msg, timestamp in bag.read_messages(): if topic not in messages: messages[topic] = [msg] else: messages[topic].append(msg) os.unlink(tmpfile.name) return (messages, bag)
def __init__(self, options, columns=None): super(Rosbag, self).__init__(options, columns) Bag = import_bag(options) self.filename = options.pop('rosbag_path', "") + options.pop('rosbag') self.topic = options.pop('topic', None) pointcloud_formats = strtobool(options.pop('metadata', 'false')) self.patch_column = options.pop('patch_column', 'points').strip() self.patch_columns = options.pop('patch_columns', '*').strip() self.patch_columns = [col.strip() for col in self.patch_columns.split(',') if col.strip()] self.patch_count_default = int(options.pop('patch_count_default', 1000)) # 0 => 1 patch per message self.patch_count_pointcloud = int(options.pop('patch_count_pointcloud', 0)) assert(self.patch_count_default > 0) assert(self.patch_count_pointcloud >= 0) self.pcid = int(options.pop('pcid', 0)) self.bag = Bag(self.filename, 'r') self.topics = self.bag.get_type_and_topic_info().topics self.pointcloud_formats = None if pointcloud_formats: self.pointcloud_formats = [] topics = self.topic.split(',') if self.topic else self.topics for i, topic in enumerate(self.topics): if topic not in topics: continue infos = self.topics[topic] columns, patch_schema, patch_ply_header, _, patch_columns, patch_srid = \ get_columns(self.bag, topic, infos, self.pcid+i+1, self.patch_column, self.patch_columns) self.pointcloud_formats.append({ 'pcid': self.pcid+i+1, 'srid': patch_srid, 'schema': patch_schema, 'format': columns[self.patch_column][3], 'rostype': infos.msg_type, 'columns': patch_columns, 'ply_header': patch_ply_header, }) return if not self.topic: log_to_postgres('"topic" option is required', ERROR) self.infos = self.topics[self.topic] (self.columns, self.patch_schema, self.patch_ply_header, self.endianness, self.patch_columns, self.patch_srid) = \ get_columns(self.bag, self.topic, self.infos, self.pcid, self.patch_column, self.patch_columns) if columns: missing = set(columns) - set(self.columns.keys()) columns = list(c for c in self.columns.keys() if c in columns) if missing: missing = ", ".join(sorted(missing)) support = ", ".join(sorted(self.columns.keys())) log_to_postgres( "extra unsupported columns : {}".format(missing), WARNING, hint="supported columns : {}".format(support)) self.columns = {col: self.columns[col] for col in columns} if options: log_to_postgres("extra unsupported options : {}".format( options.keys()), WARNING)
class Rosbag(ForeignDataWrapper): def __init__(self, options, columns=None): super(Rosbag, self).__init__(options, columns) Bag = import_bag(options) self.filename = options.pop('rosbag_path', "") + options.pop('rosbag') self.topic = options.pop('topic', None) pointcloud_formats = strtobool(options.pop('metadata', 'false')) self.patch_column = options.pop('patch_column', 'points').strip() self.patch_columns = options.pop('patch_columns', '*').strip() self.patch_columns = [col.strip() for col in self.patch_columns.split(',') if col.strip()] self.patch_count_default = int(options.pop('patch_count_default', 1000)) # 0 => 1 patch per message self.patch_count_pointcloud = int(options.pop('patch_count_pointcloud', 0)) assert(self.patch_count_default > 0) assert(self.patch_count_pointcloud >= 0) self.pcid = int(options.pop('pcid', 0)) self.bag = Bag(self.filename, 'r') self.topics = self.bag.get_type_and_topic_info().topics self.pointcloud_formats = None if pointcloud_formats: self.pointcloud_formats = [] topics = self.topic.split(',') if self.topic else self.topics for i, topic in enumerate(self.topics): if topic not in topics: continue infos = self.topics[topic] columns, patch_schema, patch_ply_header, _, patch_columns, patch_srid = \ get_columns(self.bag, topic, infos, self.pcid+i+1, self.patch_column, self.patch_columns) self.pointcloud_formats.append({ 'pcid': self.pcid+i+1, 'srid': patch_srid, 'schema': patch_schema, 'format': columns[self.patch_column][3], 'rostype': infos.msg_type, 'columns': patch_columns, 'ply_header': patch_ply_header, }) return if not self.topic: log_to_postgres('"topic" option is required', ERROR) self.infos = self.topics[self.topic] (self.columns, self.patch_schema, self.patch_ply_header, self.endianness, self.patch_columns, self.patch_srid) = \ get_columns(self.bag, self.topic, self.infos, self.pcid, self.patch_column, self.patch_columns) if columns: missing = set(columns) - set(self.columns.keys()) columns = list(c for c in self.columns.keys() if c in columns) if missing: missing = ", ".join(sorted(missing)) support = ", ".join(sorted(self.columns.keys())) log_to_postgres( "extra unsupported columns : {}".format(missing), WARNING, hint="supported columns : {}".format(support)) self.columns = {col: self.columns[col] for col in columns} if options: log_to_postgres("extra unsupported options : {}".format( options.keys()), WARNING) @classmethod def import_schema(self, schema, srv_options, options, restriction_type, restricts): Bag = import_bag(srv_options) pcid_str = options.pop('pcid', srv_options.pop('pcid', 0)) pcid = int(pcid_str) patch_column = options.pop('patch_column', srv_options.pop('patch_column', 'points')) patch_columns = options.pop('patch_columns', '*').strip() patch_columns = [col.strip() for col in patch_columns.split(',') if col.strip()] filename = srv_options.pop('rosbag_path', "") + options.pop('rosbag_path', "") + schema bag = Bag(filename, 'r') tablecols = [] topics = bag.get_type_and_topic_info().topics pcid_for_topic = {k: pcid+1+i for i, k in enumerate(topics.keys())} pointcloud_formats = True if restriction_type is 'limit': topics = {k: v for k, v in topics.items() if k in restricts} pointcloud_formats = 'pointcloud_formats' in restricts elif restriction_type is 'except': topics = {k: v for k, v in topics.items() if k not in restricts} pointcloud_formats = 'pointcloud_formats' not in restricts tabledefs = [] if pointcloud_formats: tablecols = [ ColumnDefinition('pcid', type_name='integer'), ColumnDefinition('srid', type_name='integer'), ColumnDefinition('schema', type_name='text'), ColumnDefinition('format', type_name='text'), ColumnDefinition('rostype', type_name='text'), ColumnDefinition('columns', type_name='text[]'), ColumnDefinition('ply_header', type_name='text'), ] tableopts = {'metadata': 'true', 'rosbag': schema, 'pcid': pcid_str} tabledefs.append(TableDefinition("pointcloud_formats", columns=tablecols, options=tableopts)) for topic, infos in topics.items(): pcid = pcid_for_topic[topic] columns, _, _, _, _, _ = get_columns(bag, topic, infos, pcid, patch_column, patch_columns) tablecols = [get_column_def(k, *v) for k, v in columns.items()] tableopts = {'topic': topic, 'rosbag': schema, 'pcid': str(pcid)} tabledefs.append(TableDefinition(topic, columns=tablecols, options=tableopts)) return tabledefs def execute(self, quals, columns): if self.pointcloud_formats is not None: for f in self.pointcloud_formats: yield f return self.patch_data = '' from rospy.rostime import Time tmin = None tmax = None for qual in quals: if qual.field_name == "time": t = int(qual.value) t = Time(t / 1000000000, t % 1000000000) if qual.operator in ['=', '>', '>=']: tmin = t if qual.operator in ['=', '<', '<=']: tmax = t for topic, msg, t in self.bag.read_messages( topics=self.topic, start_time=tmin, end_time=tmax): for row in self.get_rows(topic, msg, t, columns): yield row # flush leftover patch data if self.patch_data and self.last_row: count = int((len(self.patch_data) / self.point_size)) # in replicating mode, a single leftover point must not be reported if count > 1 or self.patch_step_size == self.patch_size: res = self.last_row if self.patch_column in columns: res[self.patch_column] = hexlify( pack('=b3I', self.endianness, self.pcid, 0, count) + self.patch_data) if self.patch_ply_header and 'ply' in columns: self.ply_info['count'] = count res['ply'] = self.patch_ply_header.format(**self.ply_info) + self.patch_data yield res def get_rows(self, topic, msg, t, columns, toplevel=True): if toplevel and len(msg.__slots__) == 1: attr = getattr(msg, msg.__slots__[0]) if isinstance(attr, list): for msg in attr: for row in self.get_rows(topic, msg, t, columns, False): yield row return res = {} data_columns = set(columns) if self.patch_column in columns: data_columns = data_columns.union(self.patch_columns) - set([self.patch_column]) if "filename" in data_columns: res["filename"] = self.filename if "topic" in data_columns: res["topic"] = topic if "time" in data_columns: res["time"] = t.to_nsec() if self.infos.msg_type == 'sensor_msgs/PointCloud2': self.patch_count = self.patch_count_pointcloud or (msg.width*msg.height) self.point_size = msg.point_step self.patch_size = self.patch_count * self.point_size self.patch_step_size = self.patch_size self.endianness = 0 if msg.is_bigendian else 1 data_columns = data_columns - set(['ply', self.patch_column]) self.patch_data += msg.data data_columns = data_columns - set(res.keys()) for column in data_columns: attr = msg for col in column.split('.'): if isinstance(attr, list): attr = tuple(getattr(a, col) for a in attr) else: attr = getattr(attr, col) if hasattr(attr, "to_nsec"): attr = attr.to_nsec() elif hasattr(attr, "x"): if hasattr(attr, "w"): attr = (attr.x, attr.y, attr.z, attr.w) else: attr = (attr.x, attr.y, attr.z) elif isinstance(attr, str): fmt = self.columns[column][3] if fmt: attr = unpack(fmt, attr) res[column] = attr if self.patch_column in columns and not self.infos.msg_type == 'sensor_msgs/PointCloud2': fmt = self.columns[self.patch_column][3] self.patch_count = self.patch_count_default self.point_size = calcsize(fmt) self.patch_size = self.patch_count * self.point_size self.patch_step_size = self.patch_size - self.point_size self.patch_data += get_point_data(res, self.patch_columns, fmt) res = {k: v for k, v in res.items() if k not in self.patch_columns} if not self.patch_data: yield res else: # todo: ensure current res and previous res are equal if there is some leftover # patch_data while len(self.patch_data) >= self.patch_size: data = self.patch_data[0:self.patch_size] count = int(self.patch_size / self.point_size) res[self.patch_column] = hexlify( pack('=b3I', self.endianness, self.pcid, 0, count) + data) if self.patch_ply_header and 'ply' in columns: self.ply_info = { 'endianness': 'big' if self.endianness else 'little', 'filename': self.filename, 'topic': self.topic, 'count': count } res['ply'] = self.patch_ply_header.format(**self.ply_info) + data self.patch_data = self.patch_data[self.patch_step_size:] yield res self.last_row = res
def on_enter(self, userdata): self._failed = False try: # Initialization # userdata self._bag_filename = userdata.bag_filename self._trajectories_command = userdata.trajectories_command # joint definitions l_arm_range = range(16,23); r_arm_range = range(23,30); atlasJointNames = [ 'back_bkz', 'back_bky', 'back_bkx', 'neck_ry', 'l_leg_hpz', 'l_leg_hpx', 'l_leg_hpy', 'l_leg_kny', 'l_leg_aky', 'l_leg_akx', 'r_leg_hpz', 'r_leg_hpx', 'r_leg_hpy', 'r_leg_kny', 'r_leg_aky', 'r_leg_akx', 'l_arm_shz', 'l_arm_shx', 'l_arm_ely', 'l_arm_elx', 'l_arm_wry', 'l_arm_wrx', 'l_arm_wry2', 'r_arm_shz', 'r_arm_shx', 'r_arm_ely', 'r_arm_elx', 'r_arm_wry', 'r_arm_wrx', 'r_arm_wry2'] joint_position_cmd = [0]*len(l_arm_range) # starting and stopping time of measurement data to be taken into account for cailbration time_start = 0 time_end = 0 time_tf = 0 # gravity vector in world frame g_world = np.resize(np.array([0,0,-9.81]),(3,1)) # take this number of data points for each pose. max_data_points_per_pose = 100 # rotation of the force torque sensor in world frame (quaternions) ft_rotation = [0,0,0,0] # loop over all ft sensors to calibrate for chain in self._calibration_chain: tf_chain=['/pelvis', 'ltorso', 'mtorso', 'utorso'] if chain == 'left_arm': joint_range = l_arm_range tfname = 'l_hand' tf_chain.extend(['l_clav', 'l_scap', 'l_uarm', 'l_larm', 'l_ufarm', 'l_lfarm', 'l_hand']) elif chain == 'right_arm': joint_range = r_arm_range tf_chain.extend(['r_clav', 'r_scap', 'r_uarm', 'r_larm', 'r_ufarm', 'r_lfarm', 'r_hand']) tfname = 'r_hand' else: Logger.logwarn('CalculateForceTorqueCalibration: Undefined chain %s', chain) # initialize transformations tf_data = [] # frame to frame transformation with numerical indexes from the tf_chain tf_data_cum = [] # world to frame transformation with numerical indexes from the tf_chain for i in range(len(tf_chain)): tf_data.append([0,0,0,0]) tf_data_cum.append([0,0,0,0]) # get number of poses from the commanded trajectories. 2 Points per Pose number_of_poses = len(self._trajectories_command[chain].points) / 2 # define information matrix for calibration InfMat = np.zeros((6*max_data_points_per_pose*number_of_poses, 10)) InfMat_i = 0 # Index in this Matrix # definie measurement vector for calibration MeasVec = np.zeros((6*max_data_points_per_pose*number_of_poses, 1)) # commanded joint trajectory for current chain # all commanded positions in this trajectory current_traj_cmd = self._trajectories_command[chain] # flag if the time interval was already set: timesetflag = False point_index_cmd = 0 # start with index 0 transformation_available = False # read time series from bag file # check in desired input trajectory, at which time a position is reached # take the data in a defined time period after the commanded new position bag_from_robot = Bag(os.path.expanduser(self._bag_filename)) # open bag file Logger.loginfo('CalculateForceTorqueCalibration: Calibrate %s from %s. Using %d different positions from trajectory command. Expecting 2 commanded positions per pose' % (chain, self._bag_filename, len(current_traj_cmd.points)) ) for topic, msg, t in bag_from_robot.read_messages(topics=['/flor/controller/atlas_state', '/tf', '/flor/controller/joint_command']): # Check if all desired poses have been reached if point_index_cmd > len(current_traj_cmd.points)-1: break current_position_cmd = current_traj_cmd.points[point_index_cmd].positions #################################################### # Check tf message: remember last transformation from world to ft sensor if topic == '/tf': # loop over all transformations inside this tf message for j in range(len(msg.transforms)): data = msg.transforms[j] tr = data.transform # current transformation header = data.header # check if frame matches on of the saved frames for i in range(len(tf_chain)): if data.child_frame_id == tf_chain[i]: tf_data[i] = np.array([tr.rotation.x, tr.rotation.y, tr.rotation.z, tr.rotation.w]) # calculate the newest transformation to the force torque sensor tf_data_cum[0] = tf_data[0] for i in range(1,len(tf_chain)): tf_data_cum[i] = q = tf.transformations.quaternion_multiply(tf_data_cum[i-1], tf_data[i]) if i == len(tf_chain)-1: time_tf = msg.transforms[0].header.stamp.to_sec() # nothing to do with tf message # check if all transformations are available. (up to the last link) if np.any(tf_data_cum[-1]): # real part of quaternion unequal to zero: data exists transformation_available = True continue else: time_msg = msg.header.stamp.to_sec() #################################################### # check if timestamp is interesting. Then check the if not (time_msg > time_start and time_msg < time_end) and not timesetflag: # the timestamp is not in evaluation interval. Look for reaching of the pose # record data (see below) if topic == '/flor/controller/joint_command': # get the commanded position for i in range(len(joint_range)): osrf_ndx = joint_range[i] joint_position_cmd[i] = msg.position[osrf_ndx] # Check if position command matches the currently expected commanded position pos_reached = True for i in range(len(joint_range)): if abs(joint_position_cmd[i]-current_position_cmd[i]) > 0.001: pos_reached = False break if pos_reached: # end time of the values regarded for calibration data: take the latest time possible for this pose time_end = msg.header.stamp.to_sec() + self._settlingtime # starting time for calibration: Take 100ms time_start = time_end - 0.1 data_points_for_this_pose = 0 timesetflag = True # take the next point next time. Each pose consists of two trajectory points (one for reaching, one for settling). # take the second one point_index_cmd = point_index_cmd + 2 if not pos_reached: # The commanded position has not been reached. Skip continue continue # continue here, because data aquisition is triggered by the time_start, time_end if time_msg > time_end: timesetflag = False # prepare for new evaluation interval #################################################### # check if enough datapoints for this pose have been collected if data_points_for_this_pose > max_data_points_per_pose: # already enough data points for this pose continue #################################################### # Check if message is atlas_state # IF this is the case, fill information matrix if topic != '/flor/controller/atlas_state': continue # Extract measured force and torque if chain == 'left_arm': FT = [msg.l_hand.force.x, msg.l_hand.force.y, msg.l_hand.force.z, msg.l_hand.torque.x, msg.l_hand.torque.y, msg.l_hand.torque.z ] elif chain == 'right_arm': FT = [msg.r_hand.force.x, msg.r_hand.force.y, msg.r_hand.force.z, msg.r_hand.torque.x, msg.r_hand.torque.y, msg.r_hand.torque.z ] # calculate gravitation vector if not transformation_available: Logger.logwarn('No tf messages available at time %1.4f.' % time_msg) continue R = tf.transformations.quaternion_matrix(tf_data_cum[-1]) g = np.dot((R[0:3,0:3]).transpose(), g_world) gx = g[0] gy = g[1] gz = g[2] # fill information matrix for this data point. Source: [1], equ. (7) M = np.zeros((6, 10)) M[0,0] = gx M[1,0] = gy M[2,0] = gz M[3,2] = gz M[3,3] = -gy M[4,1] = -gz M[4,3] = gx M[5,1] = gy M[5,2] = -gx M[0,4] = 1.0 M[1,5] = 1.0 M[2,6] = 1.0 M[3,7] = 1.0 M[4,8] = 1.0 M[5,9] = 1.0 # fill big information matrix and vector (stack small information matrizes) for i in range(6): for j in range(10): InfMat[InfMat_i*6+i,j] = M[i,j] MeasVec[InfMat_i*6+i,0] = FT[i] InfMat_i = InfMat_i + 1 # increase index data_points_for_this_pose = data_points_for_this_pose + 1 # shorten big information matrix if InfMat_i < max_data_points_per_pose*number_of_poses: InfMat_calc = InfMat[0:(6*InfMat_i)-1,:] MeasVec_calc = MeasVec[0:(6*InfMat_i-1),:] else: InfMat_calc = InfMat MeasVec_calc = MeasVec # calculate calibration data if chain in self._static_calibration_data.keys(): # calculate calibration with given static parameters # bring colums with first parameters on the other side of the equation if len(self._static_calibration_data[chain]) != 4: Logger.logwarn( "CalculateForceTorqueCalibration: Given static calibration data for %s has length %d. Required 4 entries." % (chain, len(k_fix)) ) self._failed = True return # convert physical parameters to identification parameters (mass, 1st moment) m = self._static_calibration_data[chain][0] if m == 0: mom_x = 0 mom_y = 0 mom_z = 0 elif m > 0: mom_x = self._static_calibration_data[chain][1]/m mom_y = self._static_calibration_data[chain][2]/m mom_z = self._static_calibration_data[chain][3]/m else: Logger.logwarn( "CalculateForceTorqueCalibration: Negative mass (%f) for calibration requested. Abort." % mass ) self._failed = True return k_fix = np.resize(np.array([m, mom_x, mom_y, mom_z]),(4,1)) Logger.loginfo( "CalculateForceTorqueCalibration:static calibration data for %s given: %s. Reduce equation system" % (chain, str(self._static_calibration_data[chain])) ) MeasVec_calc_corr = np.subtract(np.array(MeasVec_calc), np.dot(InfMat_calc[:,0:4], k_fix)) InfMat_calc_corr = InfMat_calc[:,4:10] # only the last 6 colums which correspond to the sensor offsets k = np.linalg.lstsq(InfMat_calc_corr, MeasVec_calc_corr)[0] # solve reduced equation system k_calibration = self._static_calibration_data[chain] k_calibration.extend(k) else: # calculate normally with all parameters unknown k = np.linalg.lstsq(InfMat_calc, MeasVec_calc)[0] # convert to physical parameters (first moment -> mass) if k[0] > 0: k_calibration = [k[0], k[1]/k[0], k[2]/k[0], k[3]/k[0], k[4], k[5], k[6], k[7], k[8], k[9]] else: Logger.loginfo("CalculateForceTorqueCalibration:Calibration brought negative mass %f" % k[0]) k_calibration = [0.0, 0.0, 0.0, 0.0, k[4], k[5], k[6], k[7], k[8], k[9]] Logger.loginfo("CalculateForceTorqueCalibration:calibration data for %s" % chain) Logger.loginfo("CalculateForceTorqueCalibration:mass: %f" % float(k_calibration[0])) Logger.loginfo("CalculateForceTorqueCalibration:center of mass: %f %f %f" % (k_calibration[1], k_calibration[2], k_calibration[3])) Logger.loginfo("CalculateForceTorqueCalibration:F offset: %f %f %f" % (k_calibration[4], k_calibration[5], k_calibration[6])) Logger.loginfo("CalculateForceTorqueCalibration:M offset: %f %f %f" % (k_calibration[7], k_calibration[8], k_calibration[9])) self._ft_calib_data[chain] = k_calibration bag_from_robot.close() userdata.ft_calib_data = self._ft_calib_data Logger.loginfo('CalculateForceTorqueCalibration:Calibration finished') self._done = True except Exception as e: Logger.logwarn('CalculateForceTorqueCalibration:Unable to calculate calibration:\n%s' % str(e)) self._failed = True