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'})
