Add unit tests

Signed-off-by: Musa Morena Marcusso Manhães <[email protected]>
akash1306 · Jul 24, 2019 · b9c193d · b9c193d
1 parent b58f42e
commit b9c193d
Show file tree

Hide file tree

Showing 8 changed files with 437 additions and 0 deletions.
diff --git a/uuv_control/uuv_thruster_manager/test/test_thruster_allocator.py b/uuv_control/uuv_thruster_manager/test/test_thruster_allocator.py
@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+# Copyright (c) 2016-2019 The UUV Simulator Authors.
+# All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+import rospy
+import roslib
+import numpy as np
+import unittest
+import time
+from geometry_msgs.msg import WrenchStamped
+from uuv_thruster_manager.srv import GetThrusterManagerConfig, ThrusterManagerInfo
+from uuv_gazebo_ros_plugins_msgs.msg import FloatStamped
+# Other imports
+
+PKG = 'uuv_thruster_manager'
+roslib.load_manifest(PKG)
+
+NS = 'test_vehicle'
+
+AXIS_X_TAM = np.array([
+    [1, 0, 0, 0, 0, 0],
+    [0.87758256, 0, -0.47942554, 0.47942554, 0.47942554, 0.87758256],
+    [0.87758256, 0.47942554, 0, -0.47942554, 0.87758256, -0.87758256]
+]).T
+
+AXIS_Y_TAM = np.array([
+    [0, 0.87758256, 0.47942554, 0, 0.47942554, -0.87758256],
+    [0, 1, 0, 0, 0, 1],
+    [-0.47942554, 0.87758256, 0, -0.87758256, -0.47942554, 0.47942554]
+]).T
+
+AXIS_Z_TAM = np.array([
+    [0, -0.47942554, 0.87758256, 0, 0.87758256, 0.47942554],
+    [0.47942554, 0, 0.87758256, -0.87758256, -0.87758256, 0.47942554],
+    [0., 0., 1., 1., 0., 0.]
+]).T
+
+class TestThrusterAllocator(unittest.TestCase):
+    def test_services_exist(self):
+        srvs = [
+            'thruster_manager/get_thrusters_info',
+            'thruster_manager/get_thruster_curve',
+            'thruster_manager/set_config',
+            'thruster_manager/get_config'
+        ]
+
+        for srv in srvs:
+            rospy.wait_for_service('/{}/{}'.format(NS, srv), timeout=1000)
+
+    def test_config(self):
+        axis = rospy.get_param('axis')
+        ref_config = rospy.get_param('/{}/thruster_manager'.format(NS))
+
+        rospy.wait_for_service('/{}/thruster_manager/get_config'.format(NS), timeout=1000)
+        srv = rospy.ServiceProxy('/{}/thruster_manager/get_config'.format(NS), GetThrusterManagerConfig)
+        tm_config = srv()
+
+        self.assertEqual(tm_config.tf_prefix, '/test_vehicle/')
+        self.assertEqual(tm_config.base_link, 'base_link')
+        self.assertEqual(tm_config.thruster_frame_base, 'thruster_')
+        self.assertEqual(tm_config.thruster_topic_suffix, '/input')
+        self.assertEqual(tm_config.timeout, -1)
+        self.assertEqual(tm_config.max_thrust, 1000.0)
+        self.assertEqual(tm_config.n_thrusters, 3)
+
+        if axis == 'x':
+            tam_flat = AXIS_X_TAM.flatten()
+        elif axis == 'y':
+            tam_flat = AXIS_Y_TAM.flatten()
+        elif axis == 'z':
+            tam_flat = AXIS_Z_TAM.flatten()
+
+        self.assertEqual(len(tm_config.allocation_matrix), tam_flat.size)
+        for x, y in zip(tam_flat, tm_config.allocation_matrix):
+            self.assertAlmostEqual(x, y)
+
+
+if __name__ == '__main__':
+    import rosunit
+    rosunit.unitrun(PKG, 'test_thruster_allocator', TestThrusterAllocator)
+
+
diff --git a/uuv_control/uuv_thruster_manager/test/test_thruster_allocator_x_axis.test b/uuv_control/uuv_thruster_manager/test/test_thruster_allocator_x_axis.test
@@ -0,0 +1,36 @@
+<launch>
+
+    <rosparam param="axis">"x"</rosparam>
+
+    <group ns="test_vehicle">
+        <param name="robot_description"
+            command="$(find xacro)/xacro '$(find uuv_thruster_manager)/test/test_vehicle_x_axis.urdf.xacro' --inorder"/>
+
+        <rosparam 
+            command="load"
+            file="$(find uuv_thruster_manager)/test/test_vehicle_thruster_manager_proportional.yaml"/>                
+    </group>    
+
+    <include file="$(find uuv_thruster_manager)/launch/thruster_manager.launch">
+        <arg name="model_name" value="test_vehicle"/>
+        <arg name="output_dir" value="/tmp" />
+        <arg name="config_file" value="$(find uuv_thruster_manager)/test/test_vehicle_thruster_manager_proportional.yaml" />        
+        <arg name="reset_tam" value="true"/>
+    </include>
+
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ns="test_vehicle"/>
+
+    <node name="robot_state_publisher" 
+        pkg="robot_state_publisher" 
+        type="robot_state_publisher" 
+        respawn="true" 
+        output="screen"
+        ns="test_vehicle">
+        <param name="robot_description" value="/test_vehicle/robot_description" />
+        <param name="publish_frequency" value="5" />
+    </node>
+
+    <test test-name="test_thruster_allocator_x_axis"
+      pkg="uuv_thruster_manager"
+      type="test_thruster_allocator.py"/>
+</launch>
diff --git a/uuv_control/uuv_thruster_manager/test/test_thruster_allocator_y_axis.test b/uuv_control/uuv_thruster_manager/test/test_thruster_allocator_y_axis.test
@@ -0,0 +1,36 @@
+<launch>
+
+    <rosparam param="axis">"y"</rosparam>
+
+    <group ns="test_vehicle">
+        <param name="robot_description"
+            command="$(find xacro)/xacro '$(find uuv_thruster_manager)/test/test_vehicle_y_axis.urdf.xacro' --inorder"/>
+
+        <rosparam 
+            command="load"
+            file="$(find uuv_thruster_manager)/test/test_vehicle_thruster_manager_proportional.yaml"/>                
+    </group>    
+
+    <include file="$(find uuv_thruster_manager)/launch/thruster_manager.launch">
+        <arg name="model_name" value="test_vehicle"/>
+        <arg name="output_dir" value="/tmp" />
+        <arg name="config_file" value="$(find uuv_thruster_manager)/test/test_vehicle_thruster_manager_proportional.yaml" />        
+        <arg name="reset_tam" value="true"/>
+    </include>
+
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ns="test_vehicle"/>
+
+    <node name="robot_state_publisher" 
+        pkg="robot_state_publisher" 
+        type="robot_state_publisher" 
+        respawn="true" 
+        output="screen"
+        ns="test_vehicle">
+        <param name="robot_description" value="/test_vehicle/robot_description" />
+        <param name="publish_frequency" value="5" />
+    </node>
+
+    <test test-name="test_thruster_allocator_x_axis"
+      pkg="uuv_thruster_manager"
+      type="test_thruster_allocator.py"/>
+</launch>
diff --git a/uuv_control/uuv_thruster_manager/test/test_thruster_allocator_z_axis.test b/uuv_control/uuv_thruster_manager/test/test_thruster_allocator_z_axis.test
@@ -0,0 +1,36 @@
+<launch>
+
+    <rosparam param="axis">"z"</rosparam>
+
+    <group ns="test_vehicle">
+        <param name="robot_description"
+            command="$(find xacro)/xacro '$(find uuv_thruster_manager)/test/test_vehicle_z_axis.urdf.xacro' --inorder"/>
+
+        <rosparam 
+            command="load"
+            file="$(find uuv_thruster_manager)/test/test_vehicle_thruster_manager_proportional.yaml"/>                
+    </group>    
+
+    <include file="$(find uuv_thruster_manager)/launch/thruster_manager.launch">
+        <arg name="model_name" value="test_vehicle"/>
+        <arg name="output_dir" value="/tmp" />
+        <arg name="config_file" value="$(find uuv_thruster_manager)/test/test_vehicle_thruster_manager_proportional.yaml" />        
+        <arg name="reset_tam" value="true"/>
+    </include>
+
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ns="test_vehicle"/>
+
+    <node name="robot_state_publisher" 
+        pkg="robot_state_publisher" 
+        type="robot_state_publisher" 
+        respawn="true" 
+        output="screen"
+        ns="test_vehicle">
+        <param name="robot_description" value="/test_vehicle/robot_description" />
+        <param name="publish_frequency" value="5" />
+    </node>
+
+    <test test-name="test_thruster_allocator_x_axis"
+      pkg="uuv_thruster_manager"
+      type="test_thruster_allocator.py"/>
+</launch>
diff --git a/uuv_control/uuv_thruster_manager/test/test_thruster_manager_proportional_correct.py b/uuv_control/uuv_thruster_manager/test/test_thruster_manager_proportional_correct.py
@@ -0,0 +1,74 @@
+#!/usr/bin/env python
+# Copyright (c) 2016-2019 The UUV Simulator Authors.
+# All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+import rospy
+import roslib
+import unittest
+import numpy as np
+from uuv_thrusters import ThrusterManager
+
+PKG = 'uuv_thruster_manager'
+roslib.load_manifest(PKG)
+
+rospy.init_node('test_thruster_manager_proportional_correct', anonymous=True)
+
+MANAGER = ThrusterManager()
+
+REFERENCE_TAM = np.array([
+    [1, 0, 0, 0, 0, 0],
+    [0.87758256, 0, -0.47942554, 0.47942554, 0.47942554, 0.87758256],
+    [0.87758256, 0.47942554, 0, -0.47942554, 0.87758256, -0.87758256]
+]).T
+
+class TestThrusterManagerProportionalCorrect(unittest.TestCase):
+    def test_initialization(self):        
+        self.assertEqual(MANAGER.namespace, '/test_vehicle/')
+
+        self.assertEqual(MANAGER.config['tf_prefix'], 'test_vehicle')
+        self.assertEqual(MANAGER.config['base_link'], 'base_link')
+        self.assertEqual(MANAGER.config['thruster_topic_prefix'], 'thrusters/')        
+        self.assertEqual(MANAGER.config['thruster_frame_base'], 'thruster_')
+        self.assertEqual(MANAGER.config['thruster_topic_suffix'], '/input')
+        self.assertEqual(MANAGER.config['timeout'], -1)
+        self.assertEqual(MANAGER.config['max_thrust'], 1000.0)
+        self.assertEqual(MANAGER.config['update_rate'], 50)
+
+        self.assertEqual(MANAGER.n_thrusters, 3)
+
+        self.assertEqual(REFERENCE_TAM.shape, MANAGER.configuration_matrix.shape)        
+
+        self.assertTrue(np.isclose(REFERENCE_TAM, MANAGER.configuration_matrix).all())
+
+    def test_thrusters(self):
+        self.assertEqual(len(MANAGER.thrusters), 3)
+
+        for i in range(len(MANAGER.thrusters)):
+            self.assertEqual(MANAGER.thrusters[i].index, i)
+            self.assertEqual(MANAGER.thrusters[i].topic, 'thrusters/{}/input'.format(i))
+            self.assertEqual(MANAGER.thrusters[i].LABEL, 'proportional')
+            self.assertTrue(np.isclose(REFERENCE_TAM[:, i].flatten(), MANAGER.thrusters[i].tam_column).all())
+
+    def test_processing_gen_forces(self):
+        for _ in range(10):
+            gen_force = np.random.rand(6) * 100
+            thrust_forces = MANAGER.compute_thruster_forces(gen_force)
+
+            ref_thrust_forces = np.linalg.pinv(REFERENCE_TAM).dot(gen_force)
+
+            self.assertTrue(np.isclose(ref_thrust_forces, thrust_forces).all())
+
+if __name__ == '__main__':
+    import rosunit
+    rosunit.unitrun(PKG, 'test_thruster_manager_proportional_correct', TestThrusterManagerProportionalCorrect)
diff --git a/uuv_control/uuv_thruster_manager/test/test_thruster_manager_proportional_correct.test b/uuv_control/uuv_thruster_manager/test/test_thruster_manager_proportional_correct.test
@@ -0,0 +1,30 @@
+<launch>
+    <rosparam param="axis">"x"</rosparam>
+
+    <group ns="test_vehicle">
+        <param name="robot_description"
+            command="$(find xacro)/xacro '$(find uuv_thruster_manager)/test/test_vehicle_x_axis.urdf.xacro' --inorder"/>
+
+        <rosparam 
+            command="load"
+            file="$(find uuv_thruster_manager)/test/test_vehicle_thruster_manager_proportional.yaml"/>                
+    </group>    
+
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ns="test_vehicle"/>
+
+    <node name="robot_state_publisher" 
+        pkg="robot_state_publisher" 
+        type="robot_state_publisher" 
+        respawn="true" 
+        output="screen"
+        ns="test_vehicle">
+        <param name="robot_description" value="/test_vehicle/robot_description" />
+        <param name="publish_frequency" value="5" />
+    </node>
+
+    <group ns="test_vehicle">        
+        <test test-name="test_thruster_manager_proportional_correct"
+        pkg="uuv_thruster_manager"
+        type="test_thruster_manager_proportional_correct.py"/>
+    </group>
+</launch>