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