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