20 from uuv_thrusters
import ThrusterManager
22 PKG =
'uuv_thruster_manager' 23 roslib.load_manifest(PKG)
25 rospy.init_node(
'test_thruster_manager_proportional_correct', anonymous=
True)
27 MANAGER = ThrusterManager()
29 REFERENCE_TAM = np.array([
31 [0.87758256, 0, -0.47942554, 0.47942554, 0.47942554, 0.87758256],
32 [0.87758256, 0.47942554, 0, -0.47942554, 0.87758256, -0.87758256]
37 self.assertEqual(MANAGER.namespace,
'/test_vehicle/')
39 self.assertEqual(MANAGER.config[
'tf_prefix'],
'test_vehicle')
40 self.assertEqual(MANAGER.config[
'base_link'],
'base_link')
41 self.assertEqual(MANAGER.config[
'thruster_topic_prefix'],
'thrusters/')
42 self.assertEqual(MANAGER.config[
'thruster_frame_base'],
'thruster_')
43 self.assertEqual(MANAGER.config[
'thruster_topic_suffix'],
'/input')
44 self.assertEqual(MANAGER.config[
'timeout'], -1)
45 self.assertEqual(MANAGER.config[
'max_thrust'], 1000.0)
46 self.assertEqual(MANAGER.config[
'update_rate'], 50)
48 self.assertEqual(MANAGER.n_thrusters, 3)
50 self.assertEqual(REFERENCE_TAM.shape, MANAGER.configuration_matrix.shape)
52 self.assertTrue(np.isclose(REFERENCE_TAM, MANAGER.configuration_matrix).all())
55 self.assertEqual(len(MANAGER.thrusters), 3)
57 for i
in range(len(MANAGER.thrusters)):
58 self.assertEqual(MANAGER.thrusters[i].index, i)
59 self.assertEqual(MANAGER.thrusters[i].topic,
'thrusters/{}/input'.format(i))
60 self.assertEqual(MANAGER.thrusters[i].LABEL,
'proportional')
61 self.assertTrue(np.isclose(REFERENCE_TAM[:, i].flatten(), MANAGER.thrusters[i].tam_column).all())
65 gen_force = np.random.rand(6) * 100
66 thrust_forces = MANAGER.compute_thruster_forces(gen_force)
68 ref_thrust_forces = np.linalg.pinv(REFERENCE_TAM).dot(gen_force)
70 self.assertTrue(np.isclose(ref_thrust_forces, thrust_forces).all())
72 if __name__ ==
'__main__':
74 rosunit.unitrun(PKG,
'test_thruster_manager_proportional_correct', TestThrusterManagerProportionalCorrect)
def test_initialization(self)
def test_processing_gen_forces(self)