def get_pos_vel_data(db_name): docs = DBUtils.get_docs(db_name, 'ros_amcl_pose') num_of_docs = len(docs) data = np.zeros((num_of_docs, 5)) # data (nx6) is arranged as follows for i, doc in enumerate(docs): # get position information from amcl_pose localisation data[i][0] = DataUtils.get_var_value(doc, 'pose/pose/position/x') data[i][1] = DataUtils.get_var_value(doc, 'pose/pose/position/y') quat_x = DataUtils.get_var_value(doc, 'pose/pose/orientation/x') quat_y = DataUtils.get_var_value(doc, 'pose/pose/orientation/y') quat_z = DataUtils.get_var_value(doc, 'pose/pose/orientation/z') quat_w = DataUtils.get_var_value(doc, 'pose/pose/orientation/w') theta = tf.euler_from_quaternion((quat_w, quat_x, quat_y, quat_z))[2] data[i][2] = theta timestamp = DataUtils.get_var_value(doc, 'timestamp') # get velocity information from odom at the same timestamp as position odom_doc = DBUtils.get_last_doc_before(db_name, 'ros_ropod_odom', timestamp) vel_x = DataUtils.get_var_value(odom_doc, 'twist/twist/linear/x') vel_y = DataUtils.get_var_value(odom_doc, 'twist/twist/linear/y') data[i][3] = vel_x data[i][4] = vel_y return data
def dump_and_reinit_bbdb(self, data_descriptor): '''Dumps the current black box database in self.bb_config_params.default.db_export_dir, drops the database, and then reinitialises a new black box database with the black box metadata. Keyword arguments: data_descriptor: str -- descriptive name of the directory in which the data is be exported ''' dump_dir = '{0}/{1}_{2}'.format( self.bb_config_params.default.db_export_dir, data_descriptor, datetime.datetime.now().isoformat()) # we dump the black box database print('Dumping database {0} to {1}'.format(self.data_logger.db_name, dump_dir)) DBUtils.dump_db(db_name=self.data_logger.db_name, data_dir=dump_dir, delete_db=True) # we reinitialise the black box database # by writing the metadata print('Reinitialising database {0}'.format(self.data_logger.db_name)) self.data_logger.write_metadata(self.bb_config_params) print('Database {0} reinitialised'.format(self.data_logger.db_name))
def main(config_file, test_duration, print_output=False): """ main function which uses AutomaticTester class and tests if bb works or not :config_file: string (file path of the config file to be used) :test_duration: float/int (time for which the publishers should be on) """ # only proceed if black box is running if not is_bb_running(): print('Blackbox is not running. Please make sure it is running before', 'executing this test script.') sys.exit(1) config_params = ConfigFileReader.load_config(config_file) DBUtils.clear_db(config_params.default.db_name) tester = AutomaticTester(config_params, test_duration) print("initialised all publisher") tester.start() print("publishers running") tester.stop() print("publishers stopped") return check_logs(config_params, test_duration, print_output=print_output)
def get_variables(self, data_source): '''Returns a list of all stored variables corresponding to the input data source Keyword arguments: @param data_source -- a string denoting the name of a data source; collection names corresponding to data from this data source are expected to start with the data source name ''' (host, port) = DBUtils.get_db_host_and_port() if port != self.db_port: port = self.db_port client = pm.MongoClient(host=host, port=port) database = client[self.db_name] collection_names = database.list_collection_names() variable_list = [] for collection_name in collection_names: if collection_name == 'system.indexes' or \ not collection_name.startswith(data_source): continue collection = database[collection_name] variable_names = DataUtils.get_variable_list( collection_name, collection) variable_list.extend(variable_names) return variable_list
def test_get_newest_doc(self): doc = DBUtils.get_newest_doc(db_name=self.test_db_name, collection_name='ros_ropod_cmd_vel') del doc['_id'] self.assertDictEqual( doc, {'angular': {'x': 0.0, 'y': 0.0, 'z': 0.0}, 'linear': {'x': 0.0, 'y': 0.0, 'z': 0.0}, 'timestamp': 1544441432.830365})
def get_handle_position(log_db_name, event_timestamp): object_detection_result = DBUtils.get_last_doc_before(log_db_name, 'ros_detect_handle_server_result', event_timestamp) detected_handle = get_closest_handle(object_detection_result['result']['objects']['objects']) detected_handle_bb = detected_handle['bounding_box'] center_position = [detected_handle_bb['center']['x'], detected_handle_bb['center']['y'], detected_handle_bb['center']['z']] return np.array(center_position)
def test_get_docs(self): start_time = 1544441409.88607 stop_time = 1544441411.28594 docs = DBUtils.get_docs(db_name=self.test_db_name, collection_name='ros_ropod_cmd_vel', start_time=start_time, stop_time=stop_time) self.assertEqual(len(docs), 9) for doc in docs: self.assertLessEqual(doc['timestamp'], stop_time) self.assertGreaterEqual(doc['timestamp'], start_time)
def test_dump_db_with_delete(self): dump_dir = '/tmp/bb_tools_test_dump_' + str(time.time()).replace('.', '_') os.makedirs(dump_dir) success = DBUtils.dump_db( db_name=self.test_db_name, data_dir=dump_dir) self.assertTrue(success) files = os.listdir(os.path.join(dump_dir, self.test_db_name)) self.assertSetEqual(set(files) - {'system.indexes.bson'}, set(os.listdir(self.test_db_dir))) self.assertNotIn(self.test_db_name, self.client.list_database_names())
def test_get_collection_metadata(self): doc = DBUtils.get_collection_metadata(db_name=self.test_db_name, collection_name='ros_ropod_cmd_vel') del doc['_id'] self.assertDictEqual( doc, { 'collection_name': 'ros_ropod_cmd_vel', 'ros': { 'msg_type': 'geometry_msgs/Twist', 'direct_msg_mapping': True, 'topic_name': '/ropod/cmd_vel'} })
def get_goal_pose(log_db_name, event_timestamp): goal_pose_doc = DBUtils.get_last_doc_before(log_db_name, 'ros_grasped_handle_pose', event_timestamp) goal_position = [goal_pose_doc['pose']['position']['x'], goal_pose_doc['pose']['position']['y'], goal_pose_doc['pose']['position']['z']] goal_orientation_q = [goal_pose_doc['pose']['orientation']['x'], goal_pose_doc['pose']['orientation']['y'], goal_pose_doc['pose']['orientation']['z'], goal_pose_doc['pose']['orientation']['w']] orientation_euler = tf.euler_from_quaternion(goal_orientation_q) return np.array(goal_position), np.array(orientation_euler)
def get_handle_bbox(log_db_name, event_timestamp): object_detection_result = DBUtils.get_last_doc_before(log_db_name, 'ros_detect_handle_server_result', event_timestamp) detected_handle = get_closest_handle(object_detection_result['result']['objects']['objects']) detected_handle_bb = detected_handle['bounding_box'] bb_min = [detected_handle_bb['center']['x'] - (detected_handle_bb['dimensions']['x'] / 2.), detected_handle_bb['center']['y'] - (detected_handle_bb['dimensions']['y'] / 2.), detected_handle_bb['center']['z'] - (detected_handle_bb['dimensions']['z'] / 2.)] bb_max = [detected_handle_bb['center']['x'] + (detected_handle_bb['dimensions']['x'] / 2.), detected_handle_bb['center']['y'] + (detected_handle_bb['dimensions']['y'] / 2.), detected_handle_bb['center']['z'] + (detected_handle_bb['dimensions']['z'] / 2.)] return bb_min, bb_max
def write_metadata(self, config_params): '''If a "black_box_metadata" collection doesn't exist in the black box database, creates the collection and stores the metadata for all logged variables there. @param config_params -- a black_box.config.config_params.ConfigParams instance ''' if config_params: (host, port) = DBUtils.get_db_host_and_port() if port != self.db_port: port = self.db_port client = pm.MongoClient(host=host, port=port) db = client[self.db_name] collection_names = db.list_collection_names() if 'black_box_metadata' in collection_names: print('[write_metadata] {0} already has a "black_box_metadata" collection'.format(self.db_name)) else: print('[write_metadata] Saving metadata') collection = db['black_box_metadata'] if config_params.ros: for topic_params in config_params.ros.topic: collection_name = ConfigUtils.get_full_variable_name('ros', topic_params.name) if topic_params.metadata: metadata = {} metadata['collection_name'] = collection_name metadata['ros'] = {} metadata['ros']['topic_name'] = topic_params.metadata.topic_name metadata['ros']['msg_type'] = topic_params.metadata.msg_type metadata['ros']['direct_msg_mapping'] = topic_params.metadata.direct_msg_mapping collection.insert_one(metadata) if config_params.zmq: for topic_params in config_params.zmq.topics: collection_name = ConfigUtils.get_full_variable_name('zmq', topic_params.name) if topic_params.metadata: metadata = {} metadata['collection_name'] = collection_name metadata['ros'] = {} metadata['ros']['topic_name'] = topic_params.metadata.topic_name metadata['ros']['msg_type'] = topic_params.metadata.msg_type metadata['ros']['direct_msg_mapping'] = topic_params.metadata.direct_msg_mapping collection.insert_one(metadata) else: raise AssertionError('[write_metadata] config_params needs to be of type ' + \ 'black_box.config.config_params.ConfigParams')
def log_data(self, variable, timestamp, data): '''Logs the data dictionary for the given variable; adds the timestamp to the data dictionary before logging. @param variable -- name of a variable to be logged @param timestamp -- a timestamp (epoch in seconds) @param data -- a dictionary to be logged ''' (host, port) = DBUtils.get_db_host_and_port() if port != self.db_port: port = self.db_port client = pm.MongoClient(host=host, port=port) db = client[self.db_name] collection = db[variable] data['timestamp'] = timestamp collection.insert_one(data)
def get_object_pose(log_db_name, event_timestamp): goal_pose_doc = DBUtils.get_last_doc_before( log_db_name, 'ros_push_pull_object_server_goal', event_timestamp) goal_position = [ goal_pose_doc['goal']['object_pose']['pose']['position']['x'], goal_pose_doc['goal']['object_pose']['pose']['position']['y'], goal_pose_doc['goal']['object_pose']['pose']['position']['z'] ] goal_orientation_q = [ goal_pose_doc['goal']['object_pose']['pose']['orientation']['x'], goal_pose_doc['goal']['object_pose']['pose']['orientation']['y'], goal_pose_doc['goal']['object_pose']['pose']['orientation']['z'], goal_pose_doc['goal']['object_pose']['pose']['orientation']['w'] ] orientation_euler = tf.euler_from_quaternion(goal_orientation_q) return np.array(goal_position), np.array(orientation_euler)
def get_data(self, collection_name, variable_names, start_time=-1, end_time=-1): '''Returns a dictionary in which each key is a full variable name (namely a variable name of the format "collection_name/variable_name", where "variable_name" is a flattened version of a variable stored in the collection) and each value is a list of "[timestamp, value]" strings, namely each entry in the list corresponds to a value of the variable at a particular timestamp (the entries are in string format to allow "value" to be of different types) Keyword arguments: @param collection_name -- name corresponding to a collection from the log database @param variable_names -- list of variable names that should be retrieved from the collection @param start_time -- a UNIX timestamp in seconds representing the start time for the queried data (default -1, in which case there is no lower bound for the timestamp of the retrieved data) @param end_time -- a UNIX timestamp in seconds representing the end time for the queried data (default -1, in which case there is no upper bound for the timestamp of the retrieved data) ''' docs = DBUtils.get_doc_cursor(self.db_name, collection_name, start_time, end_time) var_data = {} var_full_names = {} for var_name in variable_names: full_var_name = '{0}/{1}'.format(collection_name, var_name) var_data[full_var_name] = [] var_full_names[var_name] = full_var_name for doc in docs: for var_name in variable_names: var_value = DataUtils.get_var_value(doc, var_name) var_data[var_full_names[var_name]].append('[{0}, {1}]'.format( doc['timestamp'], var_value)) return var_data
def get_latest_data(self, collection_name, variable_names): '''Returns a dictionary in which each key is a full variable name (namely a variable name of the format "collection_name/variable_name", where "variable_name" is a flattened version of a variable stored in the collection) and the value is a list string "[timestamp, value]", namely the latest value of the variable together with its timestamp (the list is a string to allow "value" to be of different types). If the given collection does not exist or there are no documents in it, returns an empty dictionary. Keyword arguments: @param collection_name -- name corresponding to a collection from the log database @param variable_names -- list of variable names that should be retrieved from the collection ''' (host, port) = DBUtils.get_db_host_and_port() if port != self.db_port: port = self.db_port client = pm.MongoClient(host=host, port=port) database = client[self.db_name] collection = database[collection_name] doc = collection.find_one(sort=[('timestamp', pm.DESCENDING)]) var_data = {} var_full_names = {} for var_name in variable_names: full_var_name = '{0}/{1}'.format(collection_name, var_name) var_data[full_var_name] = None var_full_names[var_name] = full_var_name if doc: for var_name in variable_names: var_value = DataUtils.get_var_value(doc, var_name) var_data[var_full_names[var_name]] = '[{0}, {1}]'.format( doc['timestamp'], var_value) return var_data
time.sleep(config_param['sleep_duration']) stdscr.refresh() if __name__ == '__main__': # rospy.init_node('rosbag_play') config_param = get_config_param() run_events = False parser = argparse.ArgumentParser(description="Play rosbag from mongo db") parser.add_argument('-db', help='name of the mongo db', default=config_param['db_name']) parser.add_argument('-e', '--events', action='store_true', help='Enable events playback') args = parser.parse_args() db_name = args.db if args.events and 'ros_ropod_event' in DBUtils.get_data_collection_names(db_name) \ and len(DBUtils.get_all_docs(db_name, 'ros_ropod_event')) > 0: events = DBUtils.get_all_docs(db_name, 'ros_ropod_event') chosen_event_index = choose_event(events, 1) while chosen_event_index != 0 : chosen_event = events[chosen_event_index-1] event_time = chosen_event['timestamp'] play_rosbag(event_time-config_param['event_playback_duration'], event_time+config_param['event_playback_duration']) chosen_event_index = choose_event(events, chosen_event_index) else : start_time = DBUtils.get_db_oldest_timestamp(db_name) stop_time = DBUtils.get_db_newest_timestamp(db_name) start_time, stop_time = get_desired_duration(start_time, stop_time) play_rosbag(start_time, stop_time)
return duration_doc['data'] def get_label(event_start, event_end): if event_start and event_end: if event_start == 'success': return 1 else: return 0 return 0 if __name__ == '__main__': log_db_name = 'pull_logs' event_docs = DBUtils.get_all_docs(log_db_name, 'ros_event') event_timestamps = DataUtils.get_all_measurements(event_docs, 'timestamp') event_descriptions = DataUtils.get_all_measurements( event_docs, 'description') data_point_count = len(event_timestamps) // 2 object_positions = np.zeros((data_point_count, 3)) goal_positions = np.zeros((data_point_count, 3)) distances_from_edge = np.zeros((data_point_count, 1)) motion_durations = np.zeros((data_point_count, 1)) labels = np.zeros(data_point_count, dtype=int) data_idx = 0 for i in range(0, data_point_count * 2, 2): object_positions[data_idx], _ = get_object_pose( log_db_name, event_timestamps[i])
#!/usr/bin/env python3 import ast import argparse from black_box_tools.db_utils import DBUtils if __name__ == '__main__': parser = argparse.ArgumentParser( description='Dump database (and optionally delete it afterwards)') parser.add_argument('-db', help='name of the mongo db', default='logs') parser.add_argument('-path', help='directory of the dump', default='.') parser.add_argument('-delete', help='name of the mongo db', default='True') args = parser.parse_args() db_name = args.db dump_path = args.path delete_db = ast.literal_eval(args.delete) print('Dumping database {0} to {1}'.format(db_name, dump_path)) DBUtils.dump_db(db_name=db_name, data_dir=dump_path, delete_db=delete_db) print('{0} successfully dumped to {1}'.format(db_name, dump_path))
def get_distance_from_edge(log_db_name, event_timestamp): distance_doc = DBUtils.get_last_doc_before(log_db_name, 'ros_distance_from_edge', event_timestamp) return distance_doc['data']
def __init__(self, **kwargs): self.black_box_db_name = kwargs.get('db_name', 'logs') self.sync_time = kwargs.get('sync_time', True) self.time_step = kwargs.get('time_step', 1.0) self.sleep_duration = kwargs.get('sleep_duration', 0.5) actual_start_time = DBUtils.get_db_oldest_timestamp( self.black_box_db_name) actual_stop_time = DBUtils.get_db_newest_timestamp( self.black_box_db_name) self.start_timestamp = kwargs.get('start_time', actual_start_time) self.stop_timestamp = kwargs.get('stop_time', actual_stop_time) if actual_start_time > self.start_timestamp or \ actual_stop_time < self.stop_timestamp or \ self.start_timestamp > self.stop_timestamp : print( "WARNING: Incorrect start or stop time. Using default duration" ) self.start_timestamp = actual_start_time self.stop_timestamp = actual_stop_time self.current_time = self.start_timestamp self.status = "PAUSED" self.topic_managers = [] self.topic_manager_threads = [] data_collections = DBUtils.get_data_collection_names( self.black_box_db_name) # create list of locks for syncing (each for one topic) self.locks = [ multiprocessing.Lock() for collection in data_collections ] # create list of queues to access global current time (maintained by syncronizer) self.queues = [ multiprocessing.Queue() for collection in data_collections ] self.queues.append( multiprocessing.Queue()) # for current(parent) process sync_pause_conn, self.rosbag_pause_conn = multiprocessing.Pipe( duplex=True) # create syncronizer object and assign it to a thread self.sync = Syncronizer(self.start_timestamp, self.locks, self.queues, sync_pause_conn, time_step=self.time_step, sleep_duration=self.sleep_duration) self.sync_thread = multiprocessing.Process( target=self.sync.increment_time, daemon=True) # create topic_utils object and assign it to a thread for each topic for i, collection in enumerate(data_collections): collection_metadata = DBUtils.get_collection_metadata( self.black_box_db_name, collection) topic_manager = TopicUtils( collection_metadata['ros']['topic_name'], collection_metadata['ros']['msg_type'], collection_metadata['ros']['direct_msg_mapping']) self.topic_managers.append(topic_manager) data_cursor = DBUtils.get_doc_cursor(self.black_box_db_name, collection, self.start_timestamp, self.stop_timestamp) data_thread = multiprocessing.Process( target=topic_manager.publish_data, kwargs={ 'dict_msgs': data_cursor, 'sync_time': self.sync_time, 'global_clock_start': self.start_timestamp, 'lock': self.locks[i], 'queue': self.queues[i] }, daemon=True) # data_thread.daemon = True self.topic_manager_threads.append(data_thread) for data_thread in self.topic_manager_threads: data_thread.start() self.sync_thread.start()
def get_motion_duration(log_db_name, event_timestamp): duration_doc = DBUtils.get_last_doc_before(log_db_name, 'ros_motion_duration', event_timestamp) return duration_doc['data']
def check_logs(config_params, test_duration, print_output=False): """Check the logs in mongodb and print the status. :config_params: black_box.config.ConfigParams :test_duration: float :print_output: bool :returns: None """ db_name = config_params.default.db_name collection_names = DBUtils.get_data_collection_names(db_name) # check if all topics are present in db fail = False size_status = [] if config_params.ros: for topic_params in config_params.ros.topic: topic_name = ConfigUtils.get_full_variable_name( "ros", topic_params.name) if topic_name not in collection_names: fail = True print(colored(topic_name + " not present in mongoDB", "red")) size_status.append({ 'collection': topic_name, 'expected_size': topic_params.max_frequency * test_duration, 'collection_size': 0 }) continue collection_size = len(DBUtils.get_all_docs(db_name, topic_name)) size_status.append({ 'collection': topic_name, 'expected_size': topic_params.max_frequency * test_duration, 'collection_size': collection_size }) if config_params.zyre: expected_size = test_duration * config_params.default.max_frequency for message_type in config_params.zyre.message_types: topic_name = ConfigUtils.get_full_variable_name( "zyre", message_type) if topic_name not in collection_names: fail = True print(colored(topic_name + " not present in mongoDB", "red")) size_status.append({ 'collection': topic_name, 'expected_size': expected_size, 'collection_size': 0 }) continue collection_size = len(DBUtils.get_all_docs(db_name, topic_name)) size_status.append({ 'collection': topic_name, 'expected_size': expected_size, 'collection_size': collection_size }) if not fail and print_output: print( colored("All topics have their respective collection in mongoDB", "green")) for comparison in size_status: color = "green" if comparison['expected_size'] == comparison[ 'collection_size'] else "red" string = comparison['collection'] + ': ' + str(comparison['collection_size']) \ + '/' + str(comparison['expected_size']) if print_output: print(colored(string, color)) return size_status
def test_get_db_oldest_timestamp(self): timestamp = DBUtils.get_db_oldest_timestamp(db_name=self.test_db_name) self.assertEqual(timestamp, 1544441408.0746574)
def test_get_docs_of_last_n_secs(self): docs = DBUtils.get_docs_of_last_n_secs(db_name=self.test_db_name, collection_name='ros_ropod_cmd_vel', n=2) self.assertEqual(len(docs), 6)
def test_get_db_newest_timestamp(self): timestamp = DBUtils.get_db_newest_timestamp(db_name=self.test_db_name) self.assertEqual(timestamp, 1544441433.7863772)
def test_restore_db(self): success = DBUtils.restore_db(data_dir=self.test_db_dir, drop_existing_records=True) self.assertTrue(success) self.assertIn(self.test_db_name, self.client.list_database_names())
def test_get_all_docs(self): docs = DBUtils.get_all_docs(db_name=self.test_db_name, collection_name='ros_ropod_cmd_vel') self.assertEqual(len(docs), 149)
def test_clear_db(self): DBUtils.clear_db(db_name=self.test_db_name) database = self.client[self.test_db_name] self.assertSetEqual(set(database.list_collection_names()) - {'system.indexes'}, {'black_box_metadata'})
def test_get_data_collection_names(self): collection_names = DBUtils.get_data_collection_names(db_name=self.test_db_name) self.assertSetEqual(set(collection_names), {'ros_sw_ethercat_parser_data', 'ros_ropod_cmd_vel'})