def test_to_dict(self):
p0 = TrajectoryPoint()
p0.t = 1
p0.pos = [1, 2, 3]
p0.rotq = [0, 0, 1, 1]
p0.vel = [1, 2, 3, 4, 5, 6]
p0.acc = [1, 2, 3, 4, 5, 6]
p1 = TrajectoryPoint()
p1.from_dict(p0.to_dict())
self.assertEquals(p0, p1, 'Point to dict conversion failed')
def _read_data(self):
try:
self._trajectories['desired'] = TrajectoryGenerator()
for topic, msg, time in self._bag.read_messages(self._topics['trajectory']):
self._trajectories['desired'].add_trajectory_point_from_msg(msg)
self._trajectories['actual'] = TrajectoryGenerator()
if 'odometry' in self._topics:
for topic, msg, time in self._bag.read_messages(self._topics['odometry']):
t = msg.header.stamp.to_sec()
p = msg.pose.pose.position
q = msg.pose.pose.orientation
v = msg.twist.twist.linear
w = msg.twist.twist.angular
point = TrajectoryPoint(
t, np.array([p.x, p.y, p.z]),
np.array([q.x, q.y, q.z, q.w]),
np.array([v.x, v.y, v.z]),
np.array([w.x, w.y, w.z]),
np.array([0, 0, 0]),
np.array([0, 0, 0]))
# Store sampled trajectory point
self._trajectories['actual'].add_trajectory_point(point)
# Find all thruster topics
for i in range(16):
for topic, msg, time in self._bag.read_messages('%s/%d/thrust' % (self._thruster_prefix, i)):
if i not in self._thrusters:
self._thrusters[i] = dict(time=list(), values=list())
t = msg.header.stamp.to_sec()
self._thrusters[i]['time'].append(t)
self._thrusters[i]['values'].append(float(msg.data))
if 'current_velocity' in self._topics:
for topic, msg, time in self._bag.read_messages(self._topics['current_velocity']):
time = msg.header.stamp.to_sec()
v = msg.twist.linear
self._current_vel['time'].append(time)
self._current_vel['vel'].append([v.x, v.y, v.z])
if 'wrench_perturbation' in self._topics:
for topic, msg, time in self._bag.read_messages(self._topics['wrench_perturbation']):
time = msg.header.stamp.to_sec()
self._disturbance_wrench['time'].append(time)
self._disturbance_wrench['force'].append([msg.wrench.force.x,
msg.wrench.force.y,
msg.wrench.force.z])
self._disturbance_wrench['torque'].append([msg.wrench.torque.x,
msg.wrench.torque.y,
msg.wrench.torque.z])
self._is_init = True
except Exception, e:
self._logger.error('Error retrieving data from rosbag, message=' + str(e))
self._is_init = False
def __init__(self, bag):
super(TrajectoryData, self).__init__(message_type='nav_msgs/Odometry')
self._topic_name = dict()
for x in bag.get_type_and_topic_info():
for k in x:
if 'nav_msgs/Odometry' in x[k][0]:
self._topic_name['odometry'] = k
self._logger.info('Odometry topic found <%s>' % k)
if 'reference' in k and 'uuv_control_msgs/TrajectoryPoint' in x[k][0]:
self._topic_name['reference'] = k
self._logger.info('Trajectory topic found <%s>' % k)
try:
self._recorded_data['desired'] = TrajectoryGenerator()
for topic, msg, time in bag.read_messages(self._topic_name['reference']):
self._recorded_data['desired'].add_trajectory_point_from_msg(msg)
self._logger.info('%s=loaded' % self._topic_name['reference'])
except Exception as e:
self._logger.error('Error trajectories from rosbag, message=' + str(e))
self._recorded_data['desired'] = None
try:
self._recorded_data['actual'] = TrajectoryGenerator()
for topic, msg, time in bag.read_messages(self._topic_name['odometry']):
t = msg.header.stamp.to_sec()
p = msg.pose.pose.position
q = msg.pose.pose.orientation
v = msg.twist.twist.linear
w = msg.twist.twist.angular
point = TrajectoryPoint(
t, np.array([p.x, p.y, p.z]),
np.array([q.x, q.y, q.z, q.w]),
np.array([v.x, v.y, v.z]),
np.array([w.x, w.y, w.z]),
np.array([0, 0, 0]),
np.array([0, 0, 0]))
# Store sampled trajectory point
self._recorded_data['actual'].add_trajectory_point(point)
self._logger.info('%s=loaded' % self._topic_name['odometry'])
except Exception as e:
self._logger.error('Error retrieving odometry data from rosbag, message=' + str(e))
self._recorded_data['actual'] = None
def test_to_dict(self):
p0 = TrajectoryPoint()
p0.t = 1
p0.pos = [1, 2, 3]
p0.rotq = [0, 0, 1, 1]
p0.vel = [1, 2, 3, 4, 5, 6]
p0.acc = [1, 2, 3, 4, 5, 6]
p1 = TrajectoryPoint()
p1.from_dict(p0.to_dict())
self.assertEqual(p0, p1, 'Point to dict conversion failed')
def test_init_quat_vector(self):
p = TrajectoryPoint()
self.assertEqual(p.rotq.size, 4, 'Quaternion vector len() is incorrect')
self.assertTrue(np.array_equal(p.rotq, [0, 0, 0, 1]), 'Quaternion initialization failed')
def test_set_pos_vector(self):
p = TrajectoryPoint()
p.pos = [1, 2, 3]
self.assertEqual(p.pos[0], 1, 'X position was initialized wrong')
self.assertEqual(p.pos[1], 2, 'Y position was initialized wrong')
self.assertEqual(p.pos[2], 3, 'Z position was initialized wrong')
def test_init_pos_vector(self):
p = TrajectoryPoint()
self.assertEqual(p.pos.size, 3, 'Position vector len() is incorrect')
self.assertTrue(np.array_equal(p.pos, [0, 0, 0]), 'Position initialization failed')
def test_set_pos_vector(self):
p = TrajectoryPoint()
p.pos = [1, 2, 3]
self.assertEquals(p.pos[0], 1, 'X position was initialized wrong')
self.assertEquals(p.pos[1], 2, 'Y position was initialized wrong')
self.assertEquals(p.pos[2], 3, 'Z position was initialized wrong')