def test_normalize_vector(self):
'''Test vector normalization'''
for k in range(10):
rand_vec = np.random.random(k) # Make a random k-length vector
# Ignore the astronomically unlikely case that a vector has near 0 norm
if not np.isclose(np.sum(rand_vec), 0):
normalized = normalize(rand_vec)
norm = np.linalg.norm(normalized)
# Test that the norm is 1
np.testing.assert_almost_equal(norm, 1.0, err_msg="The normalized vector did not have length 1")
def test_normalize_vector(self):
'''Test vector normalization'''
for k in range(10):
rand_vec = np.random.random(k) # Make a random k-length vector
# Ignore the astronomically unlikely case that a vector has near 0 norm
if not np.isclose(np.sum(rand_vec), 0):
normalized = normalize(rand_vec)
norm = np.linalg.norm(normalized)
# Test that the norm is 1
np.testing.assert_almost_equal(
norm,
1.0,
err_msg="The normalized vector did not have length 1")
def set_pose_server(self, srv):
'''Set the pose of the submarine
TODO: Make this an 'abstract' method of Entity, and assign each new Entity a name/id
'''
rospy.logwarn("Manually setting position of simulated vehicle")
position = sub8_utils.rosmsg_to_numpy(srv.pose.position)
self.body.setPosition(position)
rotation_q = sub8_utils.rosmsg_to_numpy(srv.pose.orientation)
rotation_norm = np.linalg.norm(rotation_q)
velocity = sub8_utils.rosmsg_to_numpy(srv.twist.linear)
angular = sub8_utils.rosmsg_to_numpy(srv.twist.angular)
self.body.setLinearVel(velocity)
self.body.setAngularVel(angular)
# If the commanded rotation is valid
if np.isclose(rotation_norm, 1., atol=1e-2):
self.body.setQuaternion(sub8_utils.normalize(rotation_q))
else:
rospy.logwarn("Commanded quaternion was not a unit quaternion, NOT setting rotation")
return SimSetPoseResponse()
