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 test_get_windowed_correlation(self):
var_pairs, correlation = DataUtils.get_windowed_correlations(
variable_names=["var1", "var2", "var3"],
data=np.array([[1, 2, 3], [4, 6, 8], [3, 6, 6]]),
window_size=2)
self.assertListEqual([("var1", "var2"), ("var1", "var3"), ("var2", "var3")], var_pairs)
self.assertTrue(np.array_equal(np.array([[1.0, 1.0], [1.0, 0.0], [1.0, 0.0]]), correlation))
var_pairs, correlation = DataUtils.get_windowed_correlations(
variable_names=["var1", "var2", "var3"],
data=np.array([[1, 2, 3], [4, 8, 8], [3, 6, 6]]),
window_size=2)
self.assertListEqual([("var1", "var2"), ("var1", "var3"), ("var2", "var3")], var_pairs)
self.assertTrue(np.array_equal(np.array([[1.0, 0.0], [1.0, 0.0], [1.0, 1.0]]), correlation))
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
'''
client = pm.MongoClient(port=self.db_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
def test_parse_bb_data_msg(self):
response_msg = {
'payload': {
'dataList': {
'ros_ropod_cmd_vel/linear/x':
['[1544441409.0861013, -0.0]', '[1544441409.191301, -0.0]',
'[1544441409.3861327, -0.0]', '[1544441409.586039, 0.030635764822363853]',
'[1544441409.695998, 0.04727638512849808]',
'[1544441409.8860662, 0.18040134012699127]']
},
'receiverId': '347b6674-c915-4940-831e-9e86a0c194f7'
},
'header': {
'metamodel': 'ropod-msg-schema.json',
'type': 'DATA-QUERY',
'timestamp': 1561631692.9753046,
'msgId': 'b62c41fa-f98f-4fb9-9957-51f59819f900'
}
}
variables, data = DataUtils.parse_bb_data_msg(response_msg)
self.assertIsInstance(variables, list)
self.assertIsInstance(data, list)
self.assertEqual(len(variables), len(data))
for variable_data in data:
for entry in variable_data:
self.assertEqual(len(entry), 2)
def get_e_stop_info(self):
"""Sends a query to bb query interface. Parses the data to
get the required info.
:returns: (bool, bool) == (status, e_stop_pressed)
"""
variables = [self.variable_name_pattern.replace('*', str(i)) for i\
in range(self.num_of_wheels)]
dict_msg = self.black_box_comm.send_latest_data_query(variables)
if dict_msg is None:
return (False, True)
_, data = DataUtils.parse_bb_latest_data_msg(dict_msg)
if not data or all(not x for x in data):
return (False, True)
for i in data:
if i is None:
return (False, True)
status_list = [i[1] for i in data]
# if different wheel have different status
if status_list.count(status_list[0]) != len(status_list):
return (False, True)
e_stop_pressed_list = []
for wheel_number in range(self.num_of_wheels):
status1 = status_list[wheel_number]
# list of flags as described in https://git.ropod.org/ropod/smartwheel/blob/master/README.md
flag_list = [
i == '1' for i in list(bin(int(status1))[2:].zfill(5))[::-1]
]
e_stop_pressed_list.append(flag_list[2])
return (True, all(e_stop_pressed_list))
def get_experiment_feedback(session_id, robot_id):
global data_thread
experiment_ongoing = True
feedback_received = False
black_box_id = BBUtils.get_bb_id(robot_id)
robot_smart_wheel_count = config.get_robot_smart_wheel_count(robot_id)
diagnostic_vars = DataUtils.expand_var_names(
experiment_diagnostic_vars, robot_smart_wheel_count)
zyre_communicator.reset_experiment_feedback(robot_id)
while experiment_ongoing:
feedback_msg = zyre_communicator.get_experiment_feedback(robot_id)
if feedback_msg and feedback_msg['robot_id'] == robot_id:
feedback_received = True
experiment_ongoing = send_experiment_feedback(
robot_id, feedback_msg, feedback_received)
if experiment_ongoing:
with data_thread_lock:
if not data_thread:
data_thread = threading.Thread(
target=send_diagnostic_data,
kwargs={
'session_id': session_id,
'black_box_id': black_box_id,
'diagnostic_vars': diagnostic_vars
})
data_thread.start()
global feedback_thread
feedback_thread = None
def get_download_query():
'''Responds to a data download query by sending a query to the appropriate
black box and then saving the data to a temporary file for download.
'''
robot_id = request.args.get('robot_id', '', type=str)
black_box_id = BBUtils.get_bb_id(robot_id)
variable_list = request.args.get('variables').split(',')
start_query_time = request.args.get('start_timestamp')
end_query_time = request.args.get('end_timestamp')
query_msg = DataUtils.get_bb_query_msg(session['uid'].hex,
black_box_id,
variable_list,
start_query_time,
end_query_time)
query_result = zyre_communicator.get_query_data(query_msg)
message = ''
try:
with open(query_result_file_path, 'w') as download_file:
json.dump(query_result, download_file)
return jsonify(success=True)
except Exception as exc:
print('[get_download_query_robot_data] %s' % str(exc))
message = 'Data could not be retrieved'
return jsonify(message=message)
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_all_measurement(self):
database = self.client[self.test_db_name]
collection = database[self.collection_name]
doc_cursor = collection.find({})
docs = [doc for doc in doc_cursor]
measurements = DataUtils.get_all_measurements(docs, 'linear/x')
self.assertEqual(measurements.shape, (149,))
def test_get_bb_query_msg_template(self):
query_msg = DataUtils.get_bb_query_msg_template()
self.assertIsInstance(query_msg, dict)
self.assertIn('header', query_msg)
self.assertIsInstance(query_msg['header'], dict)
self.assertIn('metamodel', query_msg['header'])
self.assertEqual('ropod-msg-schema.json', query_msg['header']['metamodel'])
self.assertIn('payload', query_msg)
self.assertIsInstance(query_msg['payload'], dict)
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)
'''
client = pm.MongoClient(port=self.db_port)
database = client[self.db_name]
collection = database[collection_name]
docs = {}
if start_time == -1 and end_time == -1:
docs = collection.find({})
elif start_time == -1:
docs = collection.find({'timestamp': {'$lte': end_time}})
elif end_time == -1:
docs = collection.find({'timestamp': {'$gte': start_time}})
else:
docs = collection.find(
{'timestamp': {
'$gte': start_time,
'$lte': 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 send_diagnostic_data(session_id, black_box_id, diagnostic_vars):
end_query_time = int(time.time())
start_query_time = end_query_time - 10
query_msg = DataUtils.get_bb_query_msg(session_id, black_box_id,
diagnostic_vars,
start_query_time,
end_query_time)
query_result = zyre_communicator.get_query_data(query_msg)
try:
variables, data = DataUtils.parse_bb_data_msg(query_result)
vel_vars, vel_data = get_variable_data('velocity', variables, data)
socketio.emit('vel_data',
json.dumps({
'variables': vel_vars,
'data': vel_data
}),
namespace='/experiments')
accel_vars, accel_data = get_variable_data('accel', variables,
data)
socketio.emit('accel_data',
json.dumps({
'variables': accel_vars,
'data': accel_data
}),
namespace='/experiments')
gyro_vars, gyro_data = get_variable_data('gyro', variables, data)
socketio.emit('gyro_data',
json.dumps({
'variables': gyro_vars,
'data': gyro_data
}),
namespace='/experiments')
except Exception as exc:
print('[send_diagnostic_data] {0} does not seem to be responding'.
format(black_box_id))
print(str(exc))
global data_thread
data_thread = None
def get_diff_drive_statuses(self):
current_time = time.time()
variables = []
for inp in self.variable_names:
v = [inp.replace('*', str(i)) for i\
in range(self.num_wheels)]
variables.extend(v)
dict_msg = self.black_box_comm.send_query(current_time - 1.0,
current_time, variables)
var_names, data = DataUtils.parse_bb_data_msg(dict_msg)
result, overall_result = self.__process_data(var_names, data)
return result, overall_result
def send_latest_data_query(self, variables):
"""
create and send a query message to black box query interface through
pyre shout.
:variables: list of strings
:returns: None
"""
msg_sender_id = str(uuid.uuid4())
data_query_msg = DataUtils.get_bb_latest_data_query_msg(
msg_sender_id, self.black_box_id, variables)
return self.get_black_box_data(data_query_msg)
def setup_robot_query_params(robots):
for robot in robots:
# for each robot, we expand the smart wheel variable names
# depending on the robot's smart wheel count, combine the
# query variables into a single list, and generate a robot-specific
# data query message
black_box_id = BBUtils.get_bb_id(robot)
robot_smart_wheel_count = config.get_robot_smart_wheel_count(robot)
expanded_wheel_vars = DataUtils.expand_var_names(wheel_vars, robot_smart_wheel_count)
query_vars = expanded_wheel_vars + cmd_vel_vars
query_msg = DataUtils.get_bb_latest_data_query_msg(session['uid'].hex,
black_box_id,
query_vars)
robot_data_query_msgs[robot] = query_msg
# for each robot, we get a short variable name mapping for the smart wheel variables
robot_wheel_var_name_mapping[robot] = []
for i in range(robot_smart_wheel_count):
robot_wheel_var_name_mapping[robot].append({})
for var in wheel_var_name_mapping:
var_name = var.replace('*', str(i+1))
robot_wheel_var_name_mapping[robot][i][var_name] = wheel_var_name_mapping[var]
def get_robot_data():
robot_id = request.args.get('robot_id', '', type=str)
black_box_id = BBUtils.get_bb_id(robot_id)
variable_list = request.args.get('variables').split(',')
start_query_time = request.args.get('start_timestamp')
end_query_time = request.args.get('end_timestamp')
query_msg = DataUtils.get_bb_query_msg(session['uid'].hex,
black_box_id,
variable_list,
start_query_time,
end_query_time)
query_result = zyre_communicator.get_query_data(query_msg)
variables = list()
data = list()
message = ''
try:
variables, data = DataUtils.parse_bb_data_msg(query_result)
except Exception as exc:
print('[get_robot_data] %s' % str(exc))
message = 'Data could not be retrieved'
return jsonify(variables=variables, data=data, message=message)
def send_query(self, variables):
"""create and send a query message to black box query interface through
pyre shout.
:variables: list of strings
:returns: None
"""
if len(variables) == 0:
return None
msg_sender_id = str(uuid.uuid4())
data_query_msg = DataUtils.get_bb_latest_data_query_msg(
msg_sender_id, self.black_box_id, variables)
self.sender_ids.append(msg_sender_id)
self.data['bb_variables'] = {}
self.shout(data_query_msg)
def test_get_bb_latest_data_query_msg(self):
query_msg = DataUtils.get_bb_latest_data_query_msg(
sender_id='some_unique_id',
bb_id='black_box_101',
variable_list=['variable_name'])
self.assertIsInstance(query_msg, dict)
self.assertIn('header', query_msg)
self.assertIsInstance(query_msg['header'], dict)
self.assertIn('type', query_msg['header'])
self.assertEqual('LATEST-DATA-QUERY', query_msg['header']['type'])
self.assertIn('timestamp', query_msg['header'])
self.assertIn('payload', query_msg)
self.assertIsInstance(query_msg['payload'], dict)
self.assertEqual('some_unique_id', query_msg['payload'].get('senderId', None))
self.assertEqual('black_box_101', query_msg['payload'].get('blackBoxId', None))
self.assertListEqual(['variable_name'], query_msg['payload'].get('variables', None))
def get_robot_variables():
robot_id = request.args.get('robot_id', '', type=str)
black_box_id = BBUtils.get_bb_id(robot_id)
query_msg = dict(msg_data)
query_msg['header']['type'] = 'VARIABLE-QUERY'
query_msg['payload']['senderId'] = session['uid'].hex
query_msg['payload']['blackBoxId'] = black_box_id
query_result = zyre_communicator.get_query_data(query_msg)
variables = dict()
message = ''
try:
variables = DataUtils.parse_bb_variable_msg(query_result)
except Exception as exc:
print('[get_robot_variables] %s' % str(exc))
message = 'Variable list could not be retrieved'
return jsonify(robot_variables=variables, message=message)
def test_parse_bb_variable_msg(self):
response_msg = {
'payload':
{'variableList':
{'ros':
['ros_cmd_vel/linear/x', 'ros_cmd_vel/linear/y']
}
}
}
response = DataUtils.parse_bb_variable_msg(response_msg)
self.assertIsInstance(response, dict)
self.assertIn('ros_cmd_vel', response)
self.assertIsInstance(response['ros_cmd_vel'], dict)
self.assertIn('linear', response['ros_cmd_vel'])
self.assertIsInstance(response['ros_cmd_vel']['linear'], dict)
self.assertIn('x', response['ros_cmd_vel']['linear'])
self.assertIn('y', response['ros_cmd_vel']['linear'])
def test_parse_bb_latest_data_msg(self):
response_msg = {
'payload': {
'receiverId': '7276ff82-b265-4176-a177-b7936c3c1341',
'dataList': {'ros_ropod_cmd_vel/linear/x': '[1544441432.830365, 0.0]'}
},
'header': {
'type': 'LATEST-DATA-QUERY',
'metamodel': 'ropod-msg-schema.json',
'msgId': 'e46325a0-a57d-4ec9-919b-a32c91f342af',
'timestamp': 1561636142.9828317}
}
variables, data = DataUtils.parse_bb_latest_data_msg(response_msg)
self.assertIsInstance(variables, list)
self.assertIsInstance(data, list)
self.assertEqual(len(variables), len(data))
for entry in data:
self.assertEqual(len(entry), 2)
def _get_localisation_avg_variance(self):
"""Provide a localisation average variance along with status after querying
black box.
:returns: (bool, float, float, float, bool)
"""
dict_msg = self.black_box_comm.send_latest_data_query(
[self.bb_variable_name])
if not dict_msg:
return (False, 0.0, 0.0, 0.0, False)
_, data = DataUtils.parse_bb_latest_data_msg(dict_msg)
if not data or not data[0]:
return (False, 0.0, 0.0, 0.0, False)
covariance_matrix = data[0][1]
avg_variance = (covariance_matrix[0] + covariance_matrix[7] +
covariance_matrix[35]) / 3.0
return (True, covariance_matrix[0], covariance_matrix[7],
covariance_matrix[35],
avg_variance < self.avg_variance_threshold)
def get_robot_data(robot_id):
robot_smart_wheel_count = config.get_robot_smart_wheel_count(robot_id)
while True:
data_msg = zyre_communicator.get_query_data(robot_data_query_msgs[robot_id])
variables, data = DataUtils.parse_bb_latest_data_msg(data_msg)
short_vel_vars = [cmd_vel_var_name_mapping[x] for x in cmd_vel_vars]
wheel_data = []
for i in range(robot_smart_wheel_count):
wheel_data.append({})
for var in wheel_vars:
expanded_var_name = var.replace('*', str(i))
wheel_data[i][expanded_var_name] = None
vel_data = list()
if variables:
try:
for i in range(robot_smart_wheel_count):
for var in wheel_vars:
expanded_var_name = var.replace('*', str(i))
wheel_data[i][expanded_var_name] = data[variables.index(expanded_var_name)]
socketio.emit('wheel_data',
json.dumps({'variables': robot_wheel_var_name_mapping[robot_id],
'data': wheel_data}),
namespace='/real_time_monitoring')
except ValueError as exc:
print('[real_time_monitoring] Smart wheel data error')
print(str(exc))
try:
vel_data = [data[variables.index(x)] for x in cmd_vel_vars]
socketio.emit('cmd_vel_data',
json.dumps({'variables': short_vel_vars,
'data': vel_data}),
namespace='/real_time_monitoring')
except ValueError as exc:
print('[real_time_monitoring] Velocity data error')
print(str(exc))
socketio.sleep(1)
def get_avg_battery(self):
"""Send a query message to bb query interface for voltage values for
each wheel. Take average voltage and find percentage.
:returns: (bool, float) == (status, percentage)
"""
variables = [self.variable_name_pattern.replace('*', str(i)) for i\
in range(self.num_of_wheels)]
dict_msg = self.black_box_comm.send_latest_data_query(variables)
if dict_msg is None:
return (False, 0.0)
_, data = DataUtils.parse_bb_latest_data_msg(dict_msg)
if not data or all(not x for x in data):
return (False, 0.0)
for i in data:
if i is None:
return (False, 0.0)
battery_voltage = sum([i[1] for i in data]) / self.num_of_wheels
battery_percentage = ((battery_voltage - self.lowest_voltage) / \
(self.highest_voltage - self.lowest_voltage)) * 100.0
return (True, battery_percentage)
def test_get_variable_list(self):
database = self.client[self.test_db_name]
collection = database[self.collection_name]
variable_list = DataUtils.get_variable_list(self.collection_name, collection)
self.assertCountEqual(variable_list, ['ros_ropod_cmd_vel/linear/x', 'ros_ropod_cmd_vel/linear/y', 'ros_ropod_cmd_vel/linear/z', 'ros_ropod_cmd_vel/angular/x', 'ros_ropod_cmd_vel/angular/y', 'ros_ropod_cmd_vel/angular/z'])
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])
goal_positions[data_idx], _ = get_goal_pose(log_db_name,
def test_expand_var_names(self):
var_names = DataUtils.expand_var_names(['var1/*'], 2)
self.assertListEqual(var_names, ['var1/0', 'var1/1'])
def test_find_correlated_variables(self):
var_pairs = DataUtils.find_correlated_variables(
variable_names=["var1", "var2", "var3"],
measurement_matrix=np.array([[1, 2, 3], [4, 6, 8], [3, 6, 7]]))
self.assertListEqual([("var1", "var2"), ("var1", "var3"), ("var2", "var3")], var_pairs)