Merge pull request #35 from hello-robot/feature/testing

Feature/testing

stretch_core/CMakeLists.txt

@@ -204,3 +204,9 @@ include_directories(
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(tests/core_lib_test.launch)
+  add_rostest(tests/stretch_driver_test.launch)
+endif()

stretch_core/LICENSE.md

@@ -7,5 +7,5 @@ The Contents are licensed under the Apache License, Version 2.0 (the "License").
 http://www.apache.org/licenses/LICENSE-2.0
 
 Unless required by applicable law or agreed to in writing, the Contents are distributed under the License are 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.
-   
+
 For further information about the Contents including inquiries about dual licensing, please contact Hello Robot Inc.

stretch_core/README.md

@@ -2,7 +2,7 @@
 
 ## Overview
 
-*stretch_core* provides the core ROS interfaces to the Stretch RE1 mobile manipulator from Hello Robot Inc. It includes the following nodes: 
+*stretch_core* provides the core ROS interfaces to the Stretch RE1 mobile manipulator from Hello Robot Inc. It includes the following nodes:
 
 *stretch_driver* : node that communicates with the low-level Python library (stretch_body) to interface with the Stretch RE1
 
@@ -12,6 +12,116 @@
 
 *keyboard_teleop* : node that provides a keyboard interface to control the robot's joints
 
+## Testing
+
+The primary testing framework being used within *stretch_ros* is pytest. Pytest is an open source testing framework that scales well and takes a functional approach resulting in minimal boiler plate code. First we should ensure that pytest is installed and up to date:
+
+
+```bash
+>>$ pip3 install -U pytest
+>>$ pytest --version
+pytest 6.2.4
+```
+Testing can be split into four different categories:
+
+1. Python library unit tests (pytest)
+2. ROS node unit tests (pytest + rostest)
+3. ROS node integration tests (pytest + rostest)
+4. ROS functional tests (pytest + rostest)
+
+Tests suites are organized using the rostest framework which is an extension of standard ROS launch files. Test suites can be created simply by adding the following lines to the desired test file:
+
+Python Library Tests:
+
+```xml
+<launch>
+  <param name="test_module" value="../src"/>
+  <test test-name="test_lib" pkg="my_pkg" type="pytest_runner.py" />
+</launch>
+```
+
+ROS Node Unit/Integration Tests:
+
+```xml
+<launch>
+  <node pkg="my_pkg" type="publisher" name="publisher" />
+  <param name="test_module" value="listener"/>
+  <test test-name="test_listener" pkg="my_pkg" type="pytest_runner.py" />
+</launch>
+```
+
+It is also important that test suites are created in a directory that has access to the *pytest_runner.py* file. This script converts the output file parameter to the pytest supported format. Additionally, it passes the test directory to pytest.
+
+Below are some optional plugins that help make test reports more readable and with decoupling of test dependencies:
+
+ * Pytest Clarity: https://github.com/darrenburns/pytest-clarity
+ * Pytest Randomly: https://github.com/pytest-dev/pytest-randomly
+
+Before running any tests we should run the *stretch_robot_home.py* script at least once after startup and launch test suites that require nodes to be running via the roslaunch command as follows (we should not launch any test suites if we are using the *catkin_tools* package to build and run tests):
+
+```bash
+>>$ stretch_robot_home.py
+```
+
+```bash
+>>$ roslaunch <package_name> <test_suite_name>
+```
+
+In order to run tests the following commands can be typed into the command line:
+
+```bash
+>>$ pytest -vv
+```
+A test session will bootup that reports the root directory and all the plugins currently running. Within the test session pytest will search for all tests in the current directory and all subdirectories.
+
+We can also run individual tests by specify the test name after the option -k as follows:
+
+```bash
+>>$ pytest -vv -k 'test_name'
+```
+In order to run all tests in all packages, we should call the *pytest* command inside of our *catkin_ws*.
+
+Note, we can also run tests using the *catkin_tools* package. Thus we can assume we will be using *catkin build* instead of *catkin_make*. To install *catkin_tools* first ensure that we have the ROS repositories that contain *.deb* and *catkin_tools*:
+
+```bash
+>>$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > /etc/apt/sources.list.d/ros-latest.list'
+>>$ wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+```
+
+Next we can install *catkin_tools* by running:
+
+```bash
+>>$ sudo apt-get update
+>>$ sudo apt-get install python-catkin-tools
+```
+
+If we are currently using the *catkin_make* build system we should delete the *devel* and *build* directories in our *catkin_ws* before running *catkin build*. Next, we can type the following inside of our *catkin_ws* to compile and run all tests from all packages:
+
+```bash
+>>$ catkin run_tests
+```
+
+The following line can be modified to specify individual packages as such:
+
+```bash
+>>$ catkin run_tests <package_name>
+```
+
+Finally, if we navigate to any ROS package within our work space we can run all of the tests associated with the package with the following command:
+
+```bash
+>>$ catkin run_tests --this
+```
+
+Results can be visualized by typing in the following command:
+
+```bash
+>>$ catkin_test_results
+```
+
+This will show descriptive messages based on how many tests were run, errors, failures, skipped tests, and the respective package where the failure occurred. However, when running tests with *catkin_tools* some plugins will lose functionality such as Pytest Clarity.
+
+
 ## License
 
-For license information, please see the LICENSE files. 
+For license information, please see the LICENSE files.

stretch_core/nodes/tests/test_command_groups.py

@@ -0,0 +1,68 @@
+import pytest
+import math
+import stretch_body.robot as robot
+
+from .. import command_groups as cg
+
+#Initalize fixtures for all possible command groups
+@pytest.fixture
+def head_pan_cg():
+    range_rad = (-math.pi/4,math.pi/4)
+    head_pan_calibrated_offset_rad = math.pi/16
+    head_pan_calibrated_looked_left_offset_rad = 0.03
+    return cg.HeadPanCommandGroup(range_rad, head_pan_calibrated_offset_rad,
+                                   head_pan_calibrated_looked_left_offset_rad)
+
+@pytest.fixture
+def head_tilt_cg():
+    range_rad = (-math.pi/4,math.pi/4)
+    head_tilt_calibrated_offset_rad = math.pi/16
+    head_tilt_calibrated_looking_up_offset_rad = 0.03
+    head_tilt_backlash_transition_angle_rad = 0.2
+    return cg.HeadTiltCommandGroup(range_rad,head_tilt_calibrated_offset_rad,
+                                    head_tilt_calibrated_looking_up_offset_rad,
+                                    head_tilt_backlash_transition_angle_rad)
+
+@pytest.fixture
+def wrist_yaw_cg():
+    range_rad = (-math.pi/4,math.pi/4)
+    return cg.WristYawCommandGroup(range_rad)
+
+@pytest.fixture
+def gripper_cg():
+    range_robotis = (0,3)
+    return cg.GripperCommandGroup(range_robotis)
+
+@pytest.fixture
+def telescoping_cg():
+    range_m = (0,0.8)
+    wrist_extension_calibrated_retracted_offset_m = 0.03
+    return cg.TelescopingCommandGroup(range_m,
+                                wrist_extension_calibrated_retracted_offset_m)
+
+@pytest.fixture
+def lift_cg():
+    range_m = (0,1)
+    return cg.LiftCommandGroup(range_m)
+
+@pytest.fixture
+def mobile_base_cg():
+    virtual_range_m = (-0.5,0.5)
+    return cg.MobileBaseCommandGroup(virtual_range_m)
+
+@pytest.fixture
+def invalid_joints_callback():
+    pass
+
+
+#Begin tests
+def test_get_num_valid_commands(head_pan_cg,head_tilt_cg,wrist_yaw_cg,gripper_cg,telescoping_cg,lift_cg,mobile_base_cg):
+    command_groups = [head_pan_cg,head_tilt_cg,wrist_yaw_cg,gripper_cg,
+                      telescoping_cg,lift_cg,mobile_base_cg]
+
+    failed_cg = [cg for cg in command_groups if cg.get_num_valid_commands() != 0]
+
+    if len(failed_cg) == 0:
+        assert True
+    else:
+        assert False

stretch_core/package.xml

@@ -86,7 +86,7 @@
   <exec_depend>joy</exec_depend>
   <exec_depend>teleop_twist_joy</exec_depend>
   <exec_depend>teleop_twist_keyboard</exec_depend>
-
+  <test_depend>rostest</test_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

stretch_core/tests/core_lib_test.launch

@@ -0,0 +1,4 @@
+<launch>
+    <param name = "test_module" value = "../nodes"/>
+    <test test-name = "test_command_groups" pkg = "stretch_core" type = "pytest_runner.py"/>
+</launch>

stretch_core/tests/node_tests/test_stretch_driver.py

@@ -0,0 +1,346 @@
+import pytest
+import rospy
+import time
+import actionlib
+import math
+
+from std_msgs.msg import String
+from sensor_msgs.msg import JointState, Imu, MagneticField
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from std_srvs.srv import Trigger, TriggerRequest
+from std_srvs.srv import SetBool, SetBoolRequest
+from control_msgs.msg import FollowJointTrajectoryAction ,FollowJointTrajectoryGoal
+from trajectory_msgs.msg import JointTrajectoryPoint
+
+######## DEFINE TEST FIXTURES #######
+
+@pytest.fixture
+def node():
+    rospy.init_node("test_stretch_driver", anonymous=True)
+
+@pytest.fixture
+def waiter():
+    class Waiter:
+        def __init__(self):
+            self.received = []
+            self.condition = lambda x: False
+
+        @property
+        def success(self):
+            return True in self.received
+
+        def callback(self, data):
+            self.received.append(self.condition(data))
+
+        def wait(self, timeout):
+            timeout_t = time.time() + timeout
+            while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
+                time.sleep(0.1)
+
+        def reset(self):
+            self.received = []
+
+    return Waiter()
+
+@pytest.fixture
+def action_server_client():
+    class ActionServerClient:
+
+        def __init__(self):
+            self.trajectory_client = None
+            self.joint_state = None
+            self.initial_position = None
+            self.final_position = None
+
+            self.joint_initial_rotate = []
+            self.joint_final_rotate = []
+
+            self.joint_initial_translate = []
+            self.joint_final_translate = []
+
+        def joint_state_callback(self,joint_state):
+            self.joint_state = joint_state
+
+        def start_trajectory_client(self):
+            self.trajectory_client = actionlib.SimpleActionClient("/stretch_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
+            server_reached = self.trajectory_client.wait_for_server(timeout=rospy.Duration(60.0))
+            return server_reached
+
+        def move_joint(self,command):
+
+            #Unpack joint name and store inital state in member variable
+            joint_name = command['joint']
+            joint_index = self.joint_state.name.index(joint_name)
+            self.initial_position = self.joint_state.position[joint_index]
+
+            #Loop through desired deltas in command dictionary and send goal
+            for delta in command['deltas']:
+                point = JointTrajectoryPoint()
+                point.time_from_start = rospy.Duration(0.0)
+                trajectory_goal = FollowJointTrajectoryGoal()
+                trajectory_goal.goal_time_tolerance = rospy.Time(3.0)
+                trajectory_goal.trajectory.joint_names = [joint_name]
+                joint_value = self.joint_state.position[joint_index]
+                delta_val = command['deltas'][delta]
+                new_value = joint_value + delta_val
+                point.positions = [new_value]
+                trajectory_goal.trajectory.points = [point]
+
+                rospy.loginfo('Done sending pose.')
+                self.trajectory_client.send_goal(trajectory_goal)
+                time.sleep(2)
+
+            #Store final state in memeber variable
+            self.final_position = self.joint_state.position[joint_index]
+
+
+        def move_multiple_joints(self,joint_names,rotate,translate):
+
+            #Unpack joint names and store inital states in member variable
+            joint_names_rotate = [joint_name for joint_name in joint_names if joint_name != 'joint_lift' and joint_name != 'wrist_extension']
+            joint_names_translate = [joint_name for joint_name in joint_names if joint_name == 'joint_lift' or joint_name == 'wrist_extension']
+            joint_index_rotate = [self.joint_state.name.index(joint_name) for joint_name in joint_names_rotate]
+            joint_index_translate = [self.joint_state.name.index(joint_name) for joint_name in joint_names_translate]
+            self.joint_initial_rotate = [self.joint_state.position[joint_index] for joint_index in joint_index_rotate]
+            self.joint_initial_translate = [self.joint_state.position[joint_index] for joint_index in joint_index_translate]
+
+            #Declare points in trajectory
+            point_move_forward = JointTrajectoryPoint()
+            point_move_back = JointTrajectoryPoint()
+
+            point_move_forward.time_from_start = rospy.Duration(0.0)
+            point_move_back.time_from_start = rospy.Duration(5.0)
+
+            trajectory_goal = FollowJointTrajectoryGoal()
+            trajectory_goal.goal_time_tolerance = rospy.Time(10.0)
+
+            trajectory_goal.trajectory.joint_names = joint_names_rotate + joint_names_translate
+
+            #Set forward point to inital_values + desired_movements
+            new_values_rotate_forward = [joint_values + rotate for joint_values in self.joint_initial_rotate]
+            new_values_translate_forward = [joint_values + translate for joint_values in self.joint_initial_translate]
+            new_value_forward = new_values_rotate_forward + new_values_translate_forward
+
+            #Set back point to inital values
+            new_values_rotate_back = [joint_values for joint_values in self.joint_initial_rotate]
+            new_values_translate_back = [joint_values for joint_values in self.joint_initial_translate]
+            new_value_back = new_values_rotate_back + new_values_translate_back
+
+            point_move_forward.positions = new_value_forward
+            point_move_back.positions = new_value_back
+
+            #Add points to trajectory and send goal
+            trajectory_goal.trajectory.points = [point_move_forward,point_move_back]
+            self.trajectory_client.send_goal(trajectory_goal)
+            time.sleep(3)
+
+            #Store final states in member variable
+            self.joint_final_rotate = [self.joint_state.position[joint_index] for joint_index in joint_index_rotate]
+            self.joint_final_translate = [self.joint_state.position[joint_index] for joint_index in joint_index_translate]
+
+    return ActionServerClient()
+
+
+######## TEST PUBLISHERS #######
+
+def test_joint_states_receives_something(node, waiter):
+    waiter.condition = lambda data: True  # any message is good
+
+    rospy.Subscriber("/stretch/joint_states", JointState, waiter.callback)
+    waiter.wait(10.0)
+
+    assert waiter.success
+
+def test_odom_receives_something(node, waiter):
+    waiter.condition = lambda data: True  # any message is good
+
+    rospy.Subscriber("/odom", Odometry, waiter.callback)
+    waiter.wait(10.0)
+
+    assert waiter.success
+
+def test_imu_mobile_base_receives_something(node, waiter):
+    waiter.condition = lambda data: True  # any message is good
+
+    rospy.Subscriber("/imu_mobile_base", Imu, waiter.callback)
+    waiter.wait(10.0)
+
+    assert waiter.success
+
+def test_imu_wrist_receives_something(node, waiter):
+    waiter.condition = lambda data: True  # any message is good
+
+    rospy.Subscriber("/imu_wrist", Imu, waiter.callback)
+    waiter.wait(10.0)
+
+    assert waiter.success
+
+def test_magnetometer_mobile_base_receives_something(node, waiter):
+    waiter.condition = lambda data: True  # any message is good
+
+    rospy.Subscriber("/magnetometer_mobile_base", MagneticField, waiter.callback)
+    waiter.wait(10.0)
+
+    assert waiter.success
+
+######## TEST SUBSCRIBERS #######
+
+def test_cmd_vel_subbed(node):
+    cmd_pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size = None)
+    time.sleep(0.5)
+    assert cmd_pub.get_num_connections() == 1
+
+
+######## TEST SERVICES #######
+
+def test_switch_to_position_mode(node):
+    rospy.wait_for_service('switch_to_position_mode')
+
+    s = rospy.ServiceProxy('switch_to_position_mode', Trigger)
+
+    s_request = TriggerRequest()
+
+    result = s(s_request)
+
+    assert result.success
+
+def test_switch_to_navigation_mode(node):
+    rospy.wait_for_service('switch_to_navigation_mode')
+
+    s = rospy.ServiceProxy('switch_to_navigation_mode', Trigger)
+
+    s_request = TriggerRequest()
+
+    result = s(s_request)
+
+    assert result.success
+
+
+def test_stop_the_robot(node):
+    rospy.wait_for_service('stop_the_robot')
+
+    s = rospy.ServiceProxy('stop_the_robot', Trigger)
+
+    s_request = TriggerRequest()
+
+    result = s(s_request)
+
+    assert result.success
+
+def test_runstop(node):
+    rospy.wait_for_service('runstop')
+
+    s = rospy.ServiceProxy('runstop', SetBool)
+
+    s_request = SetBoolRequest()
+
+    result = s(s_request)
+
+    assert result.success
+
+
+######## TEST ACTION SERVER #######
+
+def test_move_lift(node, action_server_client):
+    rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback)
+    time.sleep(0.5)
+    translate = 0.1
+    deltas = {'down' : -translate , 'up' : translate}
+    command = {'joint' : 'joint_lift' , 'deltas' : deltas}
+
+    if not action_server_client.start_trajectory_client():
+        assert False
+    else:
+        action_server_client.move_joint(command)
+        assert (action_server_client.initial_position) == pytest.approx(action_server_client.final_position, abs=1e-2)
+
+def test_move_wrist(node, action_server_client):
+    rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback)
+    time.sleep(0.5)
+    translate = 0.1
+    deltas = { 'out' : translate, 'in' : -translate ,}
+    command = {'joint' : 'wrist_extension' , 'deltas' : deltas}
+
+    if not action_server_client.start_trajectory_client():
+        assert False
+    else:
+        action_server_client.move_joint(command)
+
+        assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, abs=1e-2)
+
+def test_move_wrist_yaw(node, action_server_client):
+    rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback)
+    time.sleep(0.5)
+    deg = 15
+    deltas = {'ccw' : ((math.pi/180) * deg) , 'cw' : -((math.pi/180) * deg)}
+    command = {'joint' : 'joint_wrist_yaw' , 'deltas' : deltas}
+
+    if not action_server_client.start_trajectory_client():
+        assert False
+    else:
+        action_server_client.move_joint(command)
+
+        assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, abs=1e-1)
+
+def test_move_gripper_finger(node, action_server_client):
+    rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback)
+    time.sleep(0.5)
+    deg = 5
+    deltas = {'ccw' : ((math.pi/180) * deg) , 'cw' : -((math.pi/180) * deg)}
+    command = {'joint' : 'joint_gripper_finger_left' , 'deltas' : deltas}
+
+    if not action_server_client.start_trajectory_client():
+        assert False
+    else:
+        action_server_client.move_joint(command)
+
+        assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, abs=1e-1) #0.01
+
+def test_move_head_tilt(node, action_server_client):
+    rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback)
+    time.sleep(0.5)
+    deg = 20
+    deltas = {'ccw' : ((math.pi/180) * deg) , 'cw' : -((math.pi/180) * deg)}
+    command = {'joint' : 'joint_head_tilt' , 'deltas' : deltas}
+
+    if not action_server_client.start_trajectory_client():
+        assert False
+    else:
+        action_server_client.move_joint(command)
+
+        assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, abs = 1e-1)
+
+def test_move_head_pan(node, action_server_client):
+    rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback)
+    time.sleep(0.5)
+    deg = 20
+    deltas = {'ccw' : ((math.pi/180) * deg) , 'cw' : -((math.pi/180) * deg)}
+    command = {'joint' : 'joint_head_pan' , 'deltas' : deltas}
+
+    if not action_server_client.start_trajectory_client():
+        assert False
+    else:
+        action_server_client.move_joint(command)
+
+        assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, abs = 1e-1)
+
+
+def test_move_multiple_joints(node, action_server_client):
+    rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback)
+    time.sleep(0.5)
+    deg = 30
+    rad = ((math.pi/180) * deg)
+    translate = 0.09
+
+    joint_names = ['joint_lift','joint_wrist_yaw','joint_head_pan','wrist_extension']
+
+    if not action_server_client.start_trajectory_client():
+        assert False
+    else:
+        action_server_client.move_multiple_joints(joint_names,rad,translate)
+
+        translate_results = all([initial  == pytest.approx(final,abs=1e-2) for initial, final in zip(action_server_client.joint_initial_translate, action_server_client.joint_final_translate)])
+        rotate_results = all([initial  == pytest.approx(final,abs=1e-2) for initial, final in zip(action_server_client.joint_initial_rotate, action_server_client.joint_final_rotate)])
+        assert translate_results + rotate_results
+

stretch_core/tests/pytest_runner.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+from __future__ import print_function
+
+import os
+import sys
+import rospy
+import pytest
+
+
+def get_output_file():
+    for arg in sys.argv:
+        if arg.startswith('--gtest_output'):
+            return arg.split('=xml:')[1]
+
+    raise RuntimeError('No output file has been passed')
+
+
+if __name__ == '__main__':
+    output_file = get_output_file()
+    test_module = rospy.get_param('test_module')
+    runner_path = os.path.dirname(os.path.realpath(__file__))
+    module_path = os.path.join(runner_path, test_module)
+
+    sys.exit(
+        pytest.main([module_path, '--junitxml={}'.format(output_file)])
+    )

stretch_core/tests/stretch_driver_test.launch

@@ -0,0 +1,32 @@
+<launch>
+    <param name="robot_description" textfile="$(find stretch_description)/urdf/stretch.urdf" />
+    <arg name="calibrated_controller_yaml_file" value="$(find stretch_core)/config/controller_calibration_head.yaml"/>
+
+    <node
+        name="joint_state_publisher"
+        pkg="joint_state_publisher"
+        type="joint_state_publisher" >
+        <param name="rate" value="15.0"/>
+        <rosparam param="source_list">
+            [/stretch/joint_states]
+        </rosparam>
+    </node>
+
+    <node
+        name="robot_state_publisher"
+        pkg="robot_state_publisher"
+        type="robot_state_publisher" >
+        <param name="publish_frequency" value="15.0"/>
+    </node>
+
+    <node name="stretch_driver" pkg="stretch_core" type="stretch_driver" output="screen">
+        <param name="rate" type="double" value="25.0"/>
+        <param name="timeout" type="double" value="0.5"/>
+        <remap from="cmd_vel" to="/stretch/cmd_vel" />
+        <remap from="joint_states" to="/stretch/joint_states" />
+        <param name="controller_calibration_file" type="string" value="$(arg calibrated_controller_yaml_file)"/>
+    </node>
+
+    <param name="test_module" value="node_tests"/>
+    <test test-name="test_stretch_driver" pkg ="stretch_core" type ="pytest_runner.py"/>
+</launch>