From 286d4d97a80a69e5d3645daf7829eb9036a118f8 Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Tue, 20 Jul 2021 11:43:49 -0400 Subject: [PATCH 01/16] Test format --- stretch_core/CMakeLists.txt | 5 ++++ .../nodes/tests/test_stretch_driver.py | 9 +++++++ stretch_core/package.xml | 2 +- stretch_core/tests/core_lib_test.test | 0 stretch_core/tests/pytest_runner.py | 26 +++++++++++++++++++ stretch_core/tests/stretch_driver_test.test | 4 +++ 6 files changed, 45 insertions(+), 1 deletion(-) create mode 100644 stretch_core/nodes/tests/test_stretch_driver.py create mode 100644 stretch_core/tests/core_lib_test.test create mode 100755 stretch_core/tests/pytest_runner.py create mode 100644 stretch_core/tests/stretch_driver_test.test diff --git a/stretch_core/CMakeLists.txt b/stretch_core/CMakeLists.txt index 596e2a1..b850453 100644 --- a/stretch_core/CMakeLists.txt +++ b/stretch_core/CMakeLists.txt @@ -204,3 +204,8 @@ 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/stretch_driver_test.test) +endif() diff --git a/stretch_core/nodes/tests/test_stretch_driver.py b/stretch_core/nodes/tests/test_stretch_driver.py new file mode 100644 index 0000000..2c70829 --- /dev/null +++ b/stretch_core/nodes/tests/test_stretch_driver.py @@ -0,0 +1,9 @@ +import pytest + + +def test_hello_works(): + assert True + + +def test_hello_does_not_work(): + assert False \ No newline at end of file diff --git a/stretch_core/package.xml b/stretch_core/package.xml index 9d1e5bb..41c0bda 100644 --- a/stretch_core/package.xml +++ b/stretch_core/package.xml @@ -82,7 +82,7 @@ std_srvs tf tf2 - + rostest diff --git a/stretch_core/tests/core_lib_test.test b/stretch_core/tests/core_lib_test.test new file mode 100644 index 0000000..e69de29 diff --git a/stretch_core/tests/pytest_runner.py b/stretch_core/tests/pytest_runner.py new file mode 100755 index 0000000..fd8e0b0 --- /dev/null +++ b/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)]) + ) diff --git a/stretch_core/tests/stretch_driver_test.test b/stretch_core/tests/stretch_driver_test.test new file mode 100644 index 0000000..8aea652 --- /dev/null +++ b/stretch_core/tests/stretch_driver_test.test @@ -0,0 +1,4 @@ + + + + \ No newline at end of file From ab75fbcf9a6c3de68b14afe9a82f10f7f745cbeb Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Tue, 20 Jul 2021 14:06:07 -0400 Subject: [PATCH 02/16] Begin unit tests for command groups and action server --- stretch_core/CMakeLists.txt | 3 ++- .../core_lib_test.test => nodes/__init__.py} | 0 stretch_core/nodes/tests/__init__.py | 0 stretch_core/nodes/tests/test_command_groups.py | 15 +++++++++++++++ stretch_core/tests/core_lib_test.launch | 4 ++++ ...river_test.test => stretch_driver_test.launch} | 0 6 files changed, 21 insertions(+), 1 deletion(-) rename stretch_core/{tests/core_lib_test.test => nodes/__init__.py} (100%) create mode 100644 stretch_core/nodes/tests/__init__.py create mode 100644 stretch_core/nodes/tests/test_command_groups.py create mode 100644 stretch_core/tests/core_lib_test.launch rename stretch_core/tests/{stretch_driver_test.test => stretch_driver_test.launch} (100%) diff --git a/stretch_core/CMakeLists.txt b/stretch_core/CMakeLists.txt index b850453..6c1a9f2 100644 --- a/stretch_core/CMakeLists.txt +++ b/stretch_core/CMakeLists.txt @@ -207,5 +207,6 @@ include_directories( if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - add_rostest(tests/stretch_driver_test.test) + add_rostest(tests/core_lib_test.launch) + add_rostest(tests/stretch_driver_test.launch) endif() diff --git a/stretch_core/tests/core_lib_test.test b/stretch_core/nodes/__init__.py similarity index 100% rename from stretch_core/tests/core_lib_test.test rename to stretch_core/nodes/__init__.py diff --git a/stretch_core/nodes/tests/__init__.py b/stretch_core/nodes/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/stretch_core/nodes/tests/test_command_groups.py b/stretch_core/nodes/tests/test_command_groups.py new file mode 100644 index 0000000..be29bd9 --- /dev/null +++ b/stretch_core/nodes/tests/test_command_groups.py @@ -0,0 +1,15 @@ +import pytest +import math +from .. import command_groups as cg + + +@pytest.fixture +def head_pan_cg(): + range_rad = (0,100) + 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) + + +def test_get_num_valid_commands(head_pan_cg): + assert head_pan_cg.get_num_valid_commands() == 0 diff --git a/stretch_core/tests/core_lib_test.launch b/stretch_core/tests/core_lib_test.launch new file mode 100644 index 0000000..1d91361 --- /dev/null +++ b/stretch_core/tests/core_lib_test.launch @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/stretch_core/tests/stretch_driver_test.test b/stretch_core/tests/stretch_driver_test.launch similarity index 100% rename from stretch_core/tests/stretch_driver_test.test rename to stretch_core/tests/stretch_driver_test.launch From abee2ad1d49911cc65fde34ccbd51b2979b7058f Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Thu, 22 Jul 2021 10:58:41 -0400 Subject: [PATCH 03/16] node test skeleton --- .../nodes/tests/test_command_groups.py | 62 +++++++++++++++++-- .../nodes/tests/test_stretch_driver.py | 9 --- .../tests/node_tests/test_stretch_driver.py | 49 +++++++++++++++ stretch_core/tests/stretch_driver_test.launch | 30 ++++++++- 4 files changed, 134 insertions(+), 16 deletions(-) delete mode 100644 stretch_core/nodes/tests/test_stretch_driver.py create mode 100644 stretch_core/tests/node_tests/test_stretch_driver.py diff --git a/stretch_core/nodes/tests/test_command_groups.py b/stretch_core/nodes/tests/test_command_groups.py index be29bd9..9c16c4c 100644 --- a/stretch_core/nodes/tests/test_command_groups.py +++ b/stretch_core/nodes/tests/test_command_groups.py @@ -1,15 +1,65 @@ import pytest -import math -from .. import command_groups as cg +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 = (0,100) + 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) + 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) + +#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] -def test_get_num_valid_commands(head_pan_cg): - assert head_pan_cg.get_num_valid_commands() == 0 + 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 + diff --git a/stretch_core/nodes/tests/test_stretch_driver.py b/stretch_core/nodes/tests/test_stretch_driver.py deleted file mode 100644 index 2c70829..0000000 --- a/stretch_core/nodes/tests/test_stretch_driver.py +++ /dev/null @@ -1,9 +0,0 @@ -import pytest - - -def test_hello_works(): - assert True - - -def test_hello_does_not_work(): - assert False \ No newline at end of file diff --git a/stretch_core/tests/node_tests/test_stretch_driver.py b/stretch_core/tests/node_tests/test_stretch_driver.py new file mode 100644 index 0000000..ce2a88c --- /dev/null +++ b/stretch_core/tests/node_tests/test_stretch_driver.py @@ -0,0 +1,49 @@ +import pytest +import rospy +import time + +from std_msgs.msg import String +from sensor_msgs.msg import JointState + +@pytest.fixture +def node(): + rospy.init_node('test_stretch_driver', anonymous=True) + +@pytest.fixture +def waiter(): + class Waiter(object): + 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() + +def test_listener_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_hello_works(): + assert True + + +def test_hello_does_not_work(): + assert False \ No newline at end of file diff --git a/stretch_core/tests/stretch_driver_test.launch b/stretch_core/tests/stretch_driver_test.launch index 8aea652..7e226cb 100644 --- a/stretch_core/tests/stretch_driver_test.launch +++ b/stretch_core/tests/stretch_driver_test.launch @@ -1,4 +1,32 @@ - + + + + + + + [/stretch/joint_states] + + + + + + + + + + + + + + + + \ No newline at end of file From 6c4ef1c22c00479c3694d7283b94b07cf159fabd Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Thu, 22 Jul 2021 11:04:22 -0400 Subject: [PATCH 04/16] remove failed test case in test_stretch_driver --- stretch_core/tests/node_tests/test_stretch_driver.py | 2 -- stretch_core/tests/stretch_driver_test.launch | 4 ++-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/stretch_core/tests/node_tests/test_stretch_driver.py b/stretch_core/tests/node_tests/test_stretch_driver.py index ce2a88c..603a5a6 100644 --- a/stretch_core/tests/node_tests/test_stretch_driver.py +++ b/stretch_core/tests/node_tests/test_stretch_driver.py @@ -45,5 +45,3 @@ def test_hello_works(): assert True -def test_hello_does_not_work(): - assert False \ No newline at end of file diff --git a/stretch_core/tests/stretch_driver_test.launch b/stretch_core/tests/stretch_driver_test.launch index 7e226cb..1706697 100644 --- a/stretch_core/tests/stretch_driver_test.launch +++ b/stretch_core/tests/stretch_driver_test.launch @@ -27,6 +27,6 @@ - - + + \ No newline at end of file From df3956bd8acc1b03829a50f0e8d289246abf866b Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Fri, 23 Jul 2021 12:33:57 -0400 Subject: [PATCH 05/16] pub tests in stretch_driver --- .../nodes/tests/test_command_groups.py | 25 ++++++++++- .../tests/node_tests/test_stretch_driver.py | 45 ++++++++++++++++--- 2 files changed, 64 insertions(+), 6 deletions(-) diff --git a/stretch_core/nodes/tests/test_command_groups.py b/stretch_core/nodes/tests/test_command_groups.py index 9c16c4c..bcdae07 100644 --- a/stretch_core/nodes/tests/test_command_groups.py +++ b/stretch_core/nodes/tests/test_command_groups.py @@ -50,6 +50,12 @@ 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): @@ -62,4 +68,21 @@ def test_get_num_valid_commands(head_pan_cg,head_tilt_cg,wrist_yaw_cg,gripper_cg assert True else: assert False - + + +''' IN DEVELOPMENT +def test_update(head_pan_cg,head_tilt_cg,wrist_yaw_cg,gripper_cg,telescoping_cg,lift_cg,mobile_base_cg,invalid_joints_callback): + + command_groups = [head_pan_cg,head_tilt_cg,wrist_yaw_cg,gripper_cg, + telescoping_cg,lift_cg,mobile_base_cg] + + commanded_joint_names = [cg.name for cg in command_groups] + + + updates = [c.update(commanded_joint_names, invalid_joints_callback) + for c in command_groups] + + if not all(updates): + assert False + assert True +''' diff --git a/stretch_core/tests/node_tests/test_stretch_driver.py b/stretch_core/tests/node_tests/test_stretch_driver.py index 603a5a6..4384565 100644 --- a/stretch_core/tests/node_tests/test_stretch_driver.py +++ b/stretch_core/tests/node_tests/test_stretch_driver.py @@ -3,7 +3,10 @@ import rospy import time from std_msgs.msg import String -from sensor_msgs.msg import JointState +from sensor_msgs.msg import JointState, Imu, MagneticField +from geometry_msgs.msg import Twist +from nav_msgs.msg import Odometry + @pytest.fixture def node(): @@ -11,7 +14,7 @@ def node(): @pytest.fixture def waiter(): - class Waiter(object): + class Waiter: def __init__(self): self.received = [] self.condition = lambda x: False @@ -33,7 +36,8 @@ def waiter(): return Waiter() -def test_listener_receives_something(node, waiter): + +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) @@ -41,7 +45,38 @@ def test_listener_receives_something(node, waiter): assert waiter.success -def test_hello_works(): - assert True +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 + From 3cb821339dd3b6353ae70b94402f1d8439efb75d Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Wed, 28 Jul 2021 10:16:55 -0400 Subject: [PATCH 06/16] test services --- .../tests/node_tests/test_stretch_driver.py | 74 ++++++++++++++++++- 1 file changed, 73 insertions(+), 1 deletion(-) diff --git a/stretch_core/tests/node_tests/test_stretch_driver.py b/stretch_core/tests/node_tests/test_stretch_driver.py index 4384565..64a042f 100644 --- a/stretch_core/tests/node_tests/test_stretch_driver.py +++ b/stretch_core/tests/node_tests/test_stretch_driver.py @@ -1,12 +1,18 @@ import pytest import rospy import time +import actionlib 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 control_msgs.msg import FollowJointTrajectoryFeedback +from control_msgs.msg import FollowJointTrajectoryResult +######## DEFINE TEST FIXTURES ####### @pytest.fixture def node(): @@ -37,6 +43,8 @@ def waiter(): return Waiter() +######## TEST PUBLISHERS ####### + def test_joint_states_receives_something(node, waiter): waiter.condition = lambda data: True # any message is good @@ -80,3 +88,67 @@ def test_magnetometer_mobile_base_receives_something(node, waiter): assert waiter.success + + +######## TEST SERVICES ####### + +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 == True + +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 == True + +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 == True + +def test_runstop(node): + rospy.wait_for_service('runstop') + + s = rospy.ServiceProxy('runstop', SetBool) + + s_request = SetBoolRequest() + + result = s(s_request) + + assert result.success == True + + +''' Test timing out (60s), error could be due to calibration + +def test_switch_to_manipulation_mode(node): + rospy.wait_for_service('switch_to_manipulation_mode') + + s = rospy.ServiceProxy('switch_to_manipulation_mode', Trigger) + + s_request = TriggerRequest() + + result = s(s_request) + + assert result.success == True +''' + +######## TEST ACTION SERVER ####### From 6bc52dd371888914ea1814d8ef540f2a80f59439 Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Thu, 29 Jul 2021 13:12:48 -0400 Subject: [PATCH 07/16] First draft of docs --- README.md | 5 +++ stretch_core/README.md | 84 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 89 insertions(+) diff --git a/README.md b/README.md index 68bb27f..ea7ac39 100644 --- a/README.md +++ b/README.md @@ -33,6 +33,7 @@ The *stretch_ros* repository holds ROS related code for the Stretch RE1 mobile m [stretch_moveit_config](stretch_gazebo/README.md) | Config files to use Stretch with the MoveIt Motion Planning Framework [stretch_navigation](stretch_navigation/README.md) | Support for the ROS navigation stack, including move_base, gmapping, and AMCL + ## Licenses This software is intended for use with the Stretch RE1 mobile manipulator, which is a robot produced and sold by Hello Robot Inc. For further information, including inquiries about dual licensing, please contact Hello Robot Inc. @@ -51,3 +52,7 @@ stretch_funmap | [LGPLv3](https://www.gnu.org/licenses/lgpl-3.0.en.html) stretch_gazebo | [Apache 2.0](http://www.apache.org/licenses/LICENSE-2.0) stretch_moveit_config | [Apache 2.0](http://www.apache.org/licenses/LICENSE-2.0) stretch_navigation | [Apache 2.0](http://www.apache.org/licenses/LICENSE-2.0) + + + + diff --git a/stretch_core/README.md b/stretch_core/README.md index 7a60a62..696bc58 100644 --- a/stretch_core/README.md +++ b/stretch_core/README.md @@ -12,6 +12,90 @@ *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. Running tests are depedent on the catkin_tools library. Thus we can assume we will be using *catkin build* instead of *catkin_make*. To install catkin_tools first ensure that you have the ROS repositories that contain .deb and catkin_tools: + +''' +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 you can install catkin_tools by running: + +'''bash +>>$ sudo apt-get update +>>$ sudo apt-get install python-catkin-tools +''' + +If you are currently using the *catkin_make* build system you should delete the devel and build directories in your catkin_ws before running *catkin build*. Once catkin_tools is up and running you should then ensure that pytest is installed by running: + +'''bash +>>$ pip3 install -U pytest +''' + +And you can ensure the version is correct by running: + +'''bash +>>$ 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 extention on standard ROS launch files. The runner script converts the output file parameter to the pytest supported format, additionally, it passes the test directory to pytest. Test suites can be created simply by adding the following line to the desired test file: + +Python Library Tests: + +'''html + + + + +''' + +ROS Node Unit Tests: + +'''html + + + + + +''' + +Next, you can type the following inside of your catkin_ws to compile and run all tests from all packages: + +'''bash +>>$ catkin run_tests +''' + +The following line can be modifed to specify indvidual packages as such: + +'''bash +>>$ catkin run_tests +''' + +Finally, if you navigate to any ROS package within your work space you 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 pacakge where the failure occured. + + ## License For license information, please see the LICENSE files. From 7c7c964885ec4608d46e5197cfd4abf004d7c385 Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Thu, 29 Jul 2021 13:18:19 -0400 Subject: [PATCH 08/16] Fix docs --- stretch_core/README.md | 44 +++++++++++++++++++++--------------------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/stretch_core/README.md b/stretch_core/README.md index 696bc58..2d1feed 100644 --- a/stretch_core/README.md +++ b/stretch_core/README.md @@ -16,30 +16,30 @@ The primary testing framework being used within *stretch_ros* is pytest. Running tests are depedent on the catkin_tools library. Thus we can assume we will be using *catkin build* instead of *catkin_make*. To install catkin_tools first ensure that you have the ROS repositories that contain .deb and catkin_tools: -''' -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 - -''' +```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 you can install catkin_tools by running: -'''bash +```bash >>$ sudo apt-get update >>$ sudo apt-get install python-catkin-tools -''' +``` If you are currently using the *catkin_make* build system you should delete the devel and build directories in your catkin_ws before running *catkin build*. Once catkin_tools is up and running you should then ensure that pytest is installed by running: -'''bash +```bash >>$ pip3 install -U pytest -''' +``` And you can ensure the version is correct by running: -'''bash +```bash >>$ pytest --version pytest 6.2.4 -''' +``` Testing can be split into four different categories: @@ -52,46 +52,46 @@ Tests suites are organized using the rostest framework which is an extention on Python Library Tests: -'''html +```html -''' +``` ROS Node Unit Tests: -'''html +```html -''' +``` Next, you can type the following inside of your catkin_ws to compile and run all tests from all packages: -'''bash +```bash >>$ catkin run_tests -''' +``` The following line can be modifed to specify indvidual packages as such: -'''bash +```bash >>$ catkin run_tests -''' +``` Finally, if you navigate to any ROS package within your work space you can run all of the tests associated with the package with the following command: -'''bash +```bash >>$ catkin run_tests --this -''' +``` Results can be visualized by typing in the following command: -'''bash +```bash >>$ catkin_test_results -''' +``` This will show descriptive messages based on how many tests were run, errors, failures, skipped tests, and the respective pacakge where the failure occured. From 8d56ae9867d9515f6017780374e3a388853fd198 Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Mon, 2 Aug 2021 12:45:08 -0400 Subject: [PATCH 09/16] Adding action server tests --- .../tests/node_tests/test_stretch_driver.py | 137 ++++++++++++++++-- 1 file changed, 124 insertions(+), 13 deletions(-) diff --git a/stretch_core/tests/node_tests/test_stretch_driver.py b/stretch_core/tests/node_tests/test_stretch_driver.py index 64a042f..20e2b3f 100644 --- a/stretch_core/tests/node_tests/test_stretch_driver.py +++ b/stretch_core/tests/node_tests/test_stretch_driver.py @@ -2,6 +2,7 @@ import pytest import rospy import time import actionlib +import math from std_msgs.msg import String from sensor_msgs.msg import JointState, Imu, MagneticField @@ -9,9 +10,9 @@ 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 control_msgs.msg import FollowJointTrajectoryFeedback -from control_msgs.msg import FollowJointTrajectoryResult +from control_msgs.msg import FollowJointTrajectoryAction ,FollowJointTrajectoryGoal +from trajectory_msgs.msg import JointTrajectoryPoint + ######## DEFINE TEST FIXTURES ####### @pytest.fixture @@ -42,6 +43,48 @@ def waiter(): return Waiter() +@pytest.fixture +def action_server_client(): + class ActionServerClient: + + def __init__(self): + self.trajectory_client = None + self.joint_state = None + + 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 send_trajectory(self,command): + 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) + joint_name = command['joint'] + trajectory_goal.trajectory.joint_names = [joint_name] + joint_index = self.joint_state.name.index(joint_name) + joint_value = self.joint_state.position[joint_index] + delta_val = command['deltas'][delta] + rospy.loginfo('delta = {0}, joint_index = {1}, joint_value = {2}'.format(delta, joint_index, joint_value)) + new_value = joint_value + delta_val + point.positions = [new_value] + trajectory_goal.trajectory.points = [point] + trajectory_goal.trajectory.header.stamp = rospy.Time.now() + rospy.loginfo('joint_name = {0}, trajectory_goal = {1}'.format(joint_name, trajectory_goal)) + rospy.loginfo('Done sending pose.') + time.sleep(3) + self.trajectory_client.send_goal(trajectory_goal) + + return ActionServerClient() + + + + ######## TEST PUBLISHERS ####### @@ -103,16 +146,6 @@ def test_switch_to_navigation_mode(node): assert result.success == True -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 == True def test_stop_the_robot(node): rospy.wait_for_service('stop_the_robot') @@ -136,6 +169,16 @@ def test_runstop(node): assert result.success == True +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 == True ''' Test timing out (60s), error could be due to calibration @@ -152,3 +195,71 @@ def test_switch_to_manipulation_mode(node): ''' ######## 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.05 + 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.send_trajectory(command) + assert True + +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.05 + deltas = {'in' : -translate , 'out' : translate} + command = {'joint' : 'wrist_extension' , 'deltas' : deltas} + + if not action_server_client.start_trajectory_client(): + assert False + else: + action_server_client.send_trajectory(command) + assert True + + + +def test_wrist_yaw(node, action_server_client): + rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback) + time.sleep(0.5) + deg = 10 + 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.send_trajectory(command) + assert True + +def test_head_tilt(node, action_server_client): + rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback) + time.sleep(0.5) + deg = 10 + 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.send_trajectory(command) + assert True + + +def test_head_pan(node, action_server_client): + rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback) + time.sleep(0.5) + deg = 10 + 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.send_trajectory(command) + assert True \ No newline at end of file From c751d9c08f81e910f597ae66216f408a8fa4edd6 Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Wed, 4 Aug 2021 14:50:09 -0400 Subject: [PATCH 10/16] Updated action server tests --- .../tests/node_tests/test_stretch_driver.py | 141 +++++++++++++----- 1 file changed, 103 insertions(+), 38 deletions(-) diff --git a/stretch_core/tests/node_tests/test_stretch_driver.py b/stretch_core/tests/node_tests/test_stretch_driver.py index 20e2b3f..ecce1d6 100644 --- a/stretch_core/tests/node_tests/test_stretch_driver.py +++ b/stretch_core/tests/node_tests/test_stretch_driver.py @@ -50,40 +50,48 @@ def action_server_client(): def __init__(self): self.trajectory_client = None self.joint_state = None - + self.initial_position = None + self.final_position = None + 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 send_trajectory(self,command): + joint_name = command['joint'] + joint_index = self.joint_state.name.index(joint_name) + self.initial_position = self.joint_state.position[joint_index] + 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) - joint_name = command['joint'] trajectory_goal.trajectory.joint_names = [joint_name] - joint_index = self.joint_state.name.index(joint_name) joint_value = self.joint_state.position[joint_index] delta_val = command['deltas'][delta] rospy.loginfo('delta = {0}, joint_index = {1}, joint_value = {2}'.format(delta, joint_index, joint_value)) new_value = joint_value + delta_val point.positions = [new_value] trajectory_goal.trajectory.points = [point] - trajectory_goal.trajectory.header.stamp = rospy.Time.now() + rospy.loginfo('joint_name = {0}, trajectory_goal = {1}'.format(joint_name, trajectory_goal)) rospy.loginfo('Done sending pose.') - time.sleep(3) self.trajectory_client.send_goal(trajectory_goal) + time.sleep(2) - return ActionServerClient() - + + self.final_position = self.joint_state.position[joint_index] + + def move_multiple_joints(self,joint_names,translate,rotate): + pass + return ActionServerClient() ######## TEST PUBLISHERS ####### @@ -104,7 +112,6 @@ def test_odom_receives_something(node, waiter): assert waiter.success - def test_imu_mobile_base_receives_something(node, waiter): waiter.condition = lambda data: True # any message is good @@ -113,7 +120,6 @@ def test_imu_mobile_base_receives_something(node, waiter): assert waiter.success - def test_imu_wrist_receives_something(node, waiter): waiter.condition = lambda data: True # any message is good @@ -122,7 +128,6 @@ def test_imu_wrist_receives_something(node, waiter): assert waiter.success - def test_magnetometer_mobile_base_receives_something(node, waiter): waiter.condition = lambda data: True # any message is good @@ -131,10 +136,32 @@ def test_magnetometer_mobile_base_receives_something(node, waiter): assert waiter.success +######## TEST SUBSCRIBERS ####### + +#pub.get_num_connections() continues to return 0 when cmd_vel should be subscribed to in stretch_driver +''' +def test_cmd_vel_subbed(node): + cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size = None) + time.sleep(0.5) + connections = cmd_pub.get_num_connections() + + assert 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') @@ -144,7 +171,7 @@ def test_switch_to_navigation_mode(node): result = s(s_request) - assert result.success == True + assert result.success def test_stop_the_robot(node): @@ -156,7 +183,7 @@ def test_stop_the_robot(node): result = s(s_request) - assert result.success == True + assert result.success def test_runstop(node): rospy.wait_for_service('runstop') @@ -167,21 +194,12 @@ def test_runstop(node): result = s(s_request) - assert result.success == True + assert result.success -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 == True - -''' Test timing out (60s), error could be due to calibration +# Test timing out (60s), error could be due to calibration +''' def test_switch_to_manipulation_mode(node): rospy.wait_for_service('switch_to_manipulation_mode') @@ -191,9 +209,8 @@ def test_switch_to_manipulation_mode(node): result = s(s_request) - assert result.success == True + assert result.success ''' - ######## TEST ACTION SERVER ####### def test_move_lift(node, action_server_client): @@ -202,17 +219,17 @@ def test_move_lift(node, action_server_client): translate = 0.05 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.send_trajectory(command) - assert True + assert (action_server_client.initial_position) == pytest.approx(action_server_client.final_position, 0.01) 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.05 + translate = 0.08 deltas = {'in' : -translate , 'out' : translate} command = {'joint' : 'wrist_extension' , 'deltas' : deltas} @@ -220,14 +237,13 @@ def test_move_wrist(node, action_server_client): assert False else: action_server_client.send_trajectory(command) - assert True - + assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) def test_wrist_yaw(node, action_server_client): rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback) time.sleep(0.5) - deg = 10 + deg = 15 deltas = {'ccw' : -((math.pi/180) * deg) , 'cw' : ((math.pi/180) * deg)} command = {'joint' : 'joint_wrist_yaw' , 'deltas' : deltas} @@ -235,12 +251,27 @@ def test_wrist_yaw(node, action_server_client): assert False else: action_server_client.send_trajectory(command) - assert True + + assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) + +def test_gripper_finger(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_gripper_finger_left' , 'deltas' : deltas} + + if not action_server_client.start_trajectory_client(): + assert False + else: + action_server_client.send_trajectory(command) + + assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.1) #0.01 def test_head_tilt(node, action_server_client): rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback) time.sleep(0.5) - deg = 10 + deg = 15 deltas = {'ccw' : -((math.pi/180) * deg) , 'cw' : ((math.pi/180) * deg)} command = {'joint' : 'joint_head_tilt' , 'deltas' : deltas} @@ -248,13 +279,13 @@ def test_head_tilt(node, action_server_client): assert False else: action_server_client.send_trajectory(command) - assert True + assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.1) def test_head_pan(node, action_server_client): rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback) time.sleep(0.5) - deg = 10 + deg = 15 deltas = {'ccw' : -((math.pi/180) * deg) , 'cw' : ((math.pi/180) * deg)} command = {'joint' : 'joint_head_pan' , 'deltas' : deltas} @@ -262,4 +293,38 @@ def test_head_pan(node, action_server_client): assert False else: action_server_client.send_trajectory(command) - assert True \ No newline at end of file + + assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) + #assert True + +''' IN DEVELOPMENT +def test_rotate_mobile_base(node, action_server_client): + rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback) + time.sleep(0.5) + deg = 10 + deltas = {'ccw' : -((math.pi/180) * deg) , 'cw' : ((math.pi/180) * deg)} + command = {'joint' : 'rotate_mobile_base' , 'deltas' : deltas} + + if not action_server_client.start_trajectory_client(): + assert False + else: + action_server_client.send_trajectory(command) + + #assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) + assert True + +def test_translate_mobile_base(node, action_server_client): + rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback) + time.sleep(0.5) + translate = 0.08 + deltas = {'in' : -translate , 'out' : translate} + command = {'joint' : 'joint_mobile_base_translation' , 'deltas' : deltas} + + if not action_server_client.start_trajectory_client(): + assert False + else: + action_server_client.send_trajectory(command) + + #assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) + assert True +''' \ No newline at end of file From 0cb957b57ec826a56d0fc998b076623839ad557f Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Thu, 5 Aug 2021 13:42:31 -0400 Subject: [PATCH 11/16] add move_multiple_joints and updated tutorial --- stretch_core/README.md | 70 ++++++++++------ .../tests/node_tests/test_stretch_driver.py | 81 ++++++++++++++----- 2 files changed, 109 insertions(+), 42 deletions(-) diff --git a/stretch_core/README.md b/stretch_core/README.md index 2d1feed..93f7b06 100644 --- a/stretch_core/README.md +++ b/stretch_core/README.md @@ -14,33 +14,14 @@ ## Testing -The primary testing framework being used within *stretch_ros* is pytest. Running tests are depedent on the catkin_tools library. Thus we can assume we will be using *catkin build* instead of *catkin_make*. To install catkin_tools first ensure that you have the ROS repositories that contain .deb and catkin_tools: +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 approah resulting in minimal boiler plate code. First you should ensure that pytest is installed and up to date: -```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 you can install catkin_tools by running: - -```bash ->>$ sudo apt-get update ->>$ sudo apt-get install python-catkin-tools -``` - -If you are currently using the *catkin_make* build system you should delete the devel and build directories in your catkin_ws before running *catkin build*. Once catkin_tools is up and running you should then ensure that pytest is installed by running: ```bash >>$ pip3 install -U pytest -``` - -And you can ensure the version is correct by running: - -```bash >>$ pytest --version pytest 6.2.4 ``` - Testing can be split into four different categories: 1. Python library unit tests (pytest) @@ -48,7 +29,7 @@ Testing can be split into four different categories: 3. ROS node integration tests (pytest + rostest) 4. ROS functional tests (pytest + rostest) -Tests suites are organized using the rostest framework which is an extention on standard ROS launch files. The runner script converts the output file parameter to the pytest supported format, additionally, it passes the test directory to pytest. Test suites can be created simply by adding the following line to the desired test file: +Tests suites are organized using the rostest framework which is an extention of standard ROS launch files. Test suites can be created simply by adding the following lines to the desired test file: Python Library Tests: @@ -59,7 +40,7 @@ Python Library Tests: ``` -ROS Node Unit Tests: +ROS Node Unit/Integration Tests: ```html @@ -69,7 +50,48 @@ ROS Node Unit Tests: ``` -Next, you can type the following inside of your catkin_ws to compile and run all tests from all packages: +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 you should launch test suites that require nodes to be running via the roslaunch command as follows (you should not launch any test suites if you are using the *catkin_tools* package to build and run tests): + +```bash +>>$ roslaunch +``` + +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. + +You 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, you should call the *pytest* command inside of your *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 you 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 you can install *catkin_tools* by running: + +```bash +>>$ sudo apt-get update +>>$ sudo apt-get install python-catkin-tools +``` +If you are currently using the *catkin_make* build system you should delete the *devel* and *build* directories in your *catkin_ws* before running *catkin build*. Next, you can type the following inside of your *catkin_ws* to compile and run all tests from all packages: ```bash >>$ catkin run_tests @@ -93,7 +115,7 @@ Results can be visualized by typing in the following command: >>$ catkin_test_results ``` -This will show descriptive messages based on how many tests were run, errors, failures, skipped tests, and the respective pacakge where the failure occured. +This will show descriptive messages based on how many tests were run, errors, failures, skipped tests, and the respective pacakge where the failure occured. However, when running tests with *catkin_tools* some plugins will lose functionality such as Pytest Clarity. ## License diff --git a/stretch_core/tests/node_tests/test_stretch_driver.py b/stretch_core/tests/node_tests/test_stretch_driver.py index ecce1d6..4c5a722 100644 --- a/stretch_core/tests/node_tests/test_stretch_driver.py +++ b/stretch_core/tests/node_tests/test_stretch_driver.py @@ -61,7 +61,7 @@ def action_server_client(): server_reached = self.trajectory_client.wait_for_server(timeout=rospy.Duration(60.0)) return server_reached - def send_trajectory(self,command): + def move_joint(self,command): joint_name = command['joint'] joint_index = self.joint_state.name.index(joint_name) self.initial_position = self.joint_state.position[joint_index] @@ -88,8 +88,37 @@ def action_server_client(): self.final_position = self.joint_state.position[joint_index] - def move_multiple_joints(self,joint_names,translate,rotate): - pass + def move_multiple_joints(self,joint_names,rotate,translate): + + 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] + joint_initial_rotate = [self.joint_state.position[joint_index] for joint_index in joint_index_rotate] + joint_initial_translate = [self.joint_state.position[joint_index] for joint_index in joint_index_translate] + + self.initial_position = joint_initial_rotate + joint_initial_translate + + 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.clear() + trajectory_goal.trajectory.joint_names = joint_names_rotate + joint_names_translate + + new_values_rotate = [joint_values + rotate for joint_values in joint_initial_rotate] + new_values_translate = [joint_values + translate for joint_values in joint_initial_translate] + new_value = new_values_rotate + new_values_translate + + point.positions = new_value + trajectory_goal.trajectory.points = [point] + self.trajectory_client.send_goal(trajectory_goal) + time.sleep(2) + + joint_final_rotate = [self.joint_state.position[joint_index] for joint_index in joint_index_rotate] + joint_final_translate = [self.joint_state.position[joint_index] for joint_index in joint_index_translate] + self.final_position = joint_final_rotate + joint_final_translate return ActionServerClient() @@ -223,7 +252,7 @@ def test_move_lift(node, action_server_client): if not action_server_client.start_trajectory_client(): assert False else: - action_server_client.send_trajectory(command) + action_server_client.move_joint(command) assert (action_server_client.initial_position) == pytest.approx(action_server_client.final_position, 0.01) def test_move_wrist(node, action_server_client): @@ -236,7 +265,7 @@ def test_move_wrist(node, action_server_client): if not action_server_client.start_trajectory_client(): assert False else: - action_server_client.send_trajectory(command) + action_server_client.move_joint(command) assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) @@ -244,27 +273,27 @@ def test_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)} + 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.send_trajectory(command) - - assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) + action_server_client.move_joint(command) + + assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.1) def test_gripper_finger(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)} + 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.send_trajectory(command) + action_server_client.move_joint(command) assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.1) #0.01 @@ -272,13 +301,13 @@ def test_head_tilt(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)} + 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.send_trajectory(command) + action_server_client.move_joint(command) assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.1) @@ -286,16 +315,32 @@ def test_head_pan(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)} + 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.send_trajectory(command) + action_server_client.move_joint(command) assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) - #assert True + + +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.1 + + 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) + action_server_client.move_multiple_joints(joint_names,-rad,-translate) + assert True ''' IN DEVELOPMENT def test_rotate_mobile_base(node, action_server_client): @@ -308,7 +353,7 @@ def test_rotate_mobile_base(node, action_server_client): if not action_server_client.start_trajectory_client(): assert False else: - action_server_client.send_trajectory(command) + action_server_client.move_joint(command) #assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) assert True @@ -323,7 +368,7 @@ def test_translate_mobile_base(node, action_server_client): if not action_server_client.start_trajectory_client(): assert False else: - action_server_client.send_trajectory(command) + action_server_client.move_joint(command) #assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) assert True From b4994d7e6c583e8e4152627a1ce6131051911cd1 Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Thu, 5 Aug 2021 13:47:45 -0400 Subject: [PATCH 12/16] tutorial lang fix --- stretch_core/README.md | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/stretch_core/README.md b/stretch_core/README.md index 93f7b06..fb7b383 100644 --- a/stretch_core/README.md +++ b/stretch_core/README.md @@ -14,7 +14,7 @@ ## 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 approah resulting in minimal boiler plate code. First you should ensure that pytest is installed and up to date: +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 approah resulting in minimal boiler plate code. First we should ensure that pytest is installed and up to date: ```bash @@ -59,7 +59,7 @@ Pytest Clarity: https://github.com/darrenburns/pytest-clarity Pytest Randomly: https://github.com/pytest-dev/pytest-randomly ``` -Before running any tests you should launch test suites that require nodes to be running via the roslaunch command as follows (you should not launch any test suites if you are using the *catkin_tools* package to build and run tests): +Before running any tests we should 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 >>$ roslaunch @@ -72,26 +72,26 @@ In order to run tests the following commands can be typed into the command line: ``` 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. -You can also run individual tests by specify the test name after the option -k as follows: +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, you should call the *pytest* command inside of your *catkin_ws*. +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 you have the ROS repositories that contain *.deb* and *catkin_tools*: +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 you can install *catkin_tools* by running: +Next we can install *catkin_tools* by running: ```bash >>$ sudo apt-get update >>$ sudo apt-get install python-catkin-tools ``` -If you are currently using the *catkin_make* build system you should delete the *devel* and *build* directories in your *catkin_ws* before running *catkin build*. Next, you can type the following inside of your *catkin_ws* to compile and run all tests from all packages: +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 @@ -103,7 +103,7 @@ The following line can be modifed to specify indvidual packages as such: >>$ catkin run_tests ``` -Finally, if you navigate to any ROS package within your work space you can run all of the tests associated with the package with the following command: +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 From a23e1e7c747334b91358bd46f21d16e5ec0fe509 Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Mon, 9 Aug 2021 12:40:46 -0400 Subject: [PATCH 13/16] tutorial and move_multiple_joints fix --- stretch_core/README.md | 7 +- .../tests/node_tests/test_stretch_driver.py | 107 ++++++++---------- 2 files changed, 53 insertions(+), 61 deletions(-) diff --git a/stretch_core/README.md b/stretch_core/README.md index fb7b383..f086a25 100644 --- a/stretch_core/README.md +++ b/stretch_core/README.md @@ -59,7 +59,12 @@ Pytest Clarity: https://github.com/darrenburns/pytest-clarity Pytest Randomly: https://github.com/pytest-dev/pytest-randomly ``` -Before running any tests we should 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): + +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 diff --git a/stretch_core/tests/node_tests/test_stretch_driver.py b/stretch_core/tests/node_tests/test_stretch_driver.py index 4c5a722..82da073 100644 --- a/stretch_core/tests/node_tests/test_stretch_driver.py +++ b/stretch_core/tests/node_tests/test_stretch_driver.py @@ -53,6 +53,12 @@ def action_server_client(): 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 @@ -94,31 +100,41 @@ def action_server_client(): 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] - joint_initial_rotate = [self.joint_state.position[joint_index] for joint_index in joint_index_rotate] - joint_initial_translate = [self.joint_state.position[joint_index] for joint_index in joint_index_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] + + + point_move_forward = JointTrajectoryPoint() + point_move_back = JointTrajectoryPoint() - self.initial_position = joint_initial_rotate + joint_initial_translate + point_move_forward.time_from_start = rospy.Duration(0.0) + + + point_move_back.time_from_start = rospy.Duration(5.0) - point = JointTrajectoryPoint() - point.time_from_start = rospy.Duration(0.0) trajectory_goal = FollowJointTrajectoryGoal() - trajectory_goal.goal_time_tolerance = rospy.Time(3.0) + trajectory_goal.goal_time_tolerance = rospy.Time(10.0) - trajectory_goal.trajectory.joint_names.clear() + trajectory_goal.trajectory.joint_names = joint_names_rotate + joint_names_translate - new_values_rotate = [joint_values + rotate for joint_values in joint_initial_rotate] - new_values_translate = [joint_values + translate for joint_values in joint_initial_translate] - new_value = new_values_rotate + new_values_translate + new_values_rotate = [joint_values + rotate for joint_values in self.joint_initial_rotate] + new_values_translate = [joint_values + translate for joint_values in self.joint_initial_translate] + new_value_forward = new_values_rotate + new_values_translate + + new_values_rotate = [joint_values for joint_values in self.joint_initial_rotate] + new_values_translate = [joint_values for joint_values in self.joint_initial_translate] + new_value_back = new_values_rotate + new_values_translate + + point_move_forward.positions = new_value_forward + point_move_back.positions = new_value_back - point.positions = new_value - trajectory_goal.trajectory.points = [point] + trajectory_goal.trajectory.points = [point_move_forward,point_move_back] self.trajectory_client.send_goal(trajectory_goal) - time.sleep(2) + time.sleep(3) - joint_final_rotate = [self.joint_state.position[joint_index] for joint_index in joint_index_rotate] - joint_final_translate = [self.joint_state.position[joint_index] for joint_index in joint_index_translate] - self.final_position = joint_final_rotate + joint_final_translate + 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() @@ -245,7 +261,7 @@ def test_switch_to_manipulation_mode(node): 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.05 + translate = 0.1 deltas = {'down' : -translate , 'up' : translate} command = {'joint' : 'joint_lift' , 'deltas' : deltas} @@ -258,8 +274,8 @@ def test_move_lift(node, action_server_client): 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.08 - deltas = {'in' : -translate , 'out' : translate} + translate = 0.1 + deltas = { 'out' : translate, 'in' : -translate ,} command = {'joint' : 'wrist_extension' , 'deltas' : deltas} if not action_server_client.start_trajectory_client(): @@ -269,7 +285,7 @@ def test_move_wrist(node, action_server_client): assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) -def test_wrist_yaw(node, action_server_client): +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 @@ -281,12 +297,12 @@ def test_wrist_yaw(node, action_server_client): else: action_server_client.move_joint(command) - assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.1) + assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) -def test_gripper_finger(node, action_server_client): +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 = 15 + deg = 8 deltas = {'ccw' : ((math.pi/180) * deg) , 'cw' : -((math.pi/180) * deg)} command = {'joint' : 'joint_gripper_finger_left' , 'deltas' : deltas} @@ -297,7 +313,7 @@ def test_gripper_finger(node, action_server_client): assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.1) #0.01 -def test_head_tilt(node, action_server_client): +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 = 15 @@ -311,7 +327,7 @@ def test_head_tilt(node, action_server_client): assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.1) -def test_head_pan(node, action_server_client): +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 = 15 @@ -323,7 +339,7 @@ def test_head_pan(node, action_server_client): else: action_server_client.move_joint(command) - assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) + assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.1) def test_move_multiple_joints(node, action_server_client): @@ -331,45 +347,16 @@ def test_move_multiple_joints(node, action_server_client): time.sleep(0.5) deg = 30 rad = ((math.pi/180) * deg) - translate = 0.1 + 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) - action_server_client.move_multiple_joints(joint_names,-rad,-translate) - assert True + action_server_client.move_multiple_joints(joint_names,rad,translate) -''' IN DEVELOPMENT -def test_rotate_mobile_base(node, action_server_client): - rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback) - time.sleep(0.5) - deg = 10 - deltas = {'ccw' : -((math.pi/180) * deg) , 'cw' : ((math.pi/180) * deg)} - command = {'joint' : 'rotate_mobile_base' , '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, 0.01) - assert True + translate_results = all([initial == pytest.approx(final,0.1) for initial, final in zip(action_server_client.joint_initial_translate, action_server_client.joint_final_translate)]) + rotate_results = all([initial == pytest.approx(final,0.1) for initial, final in zip(action_server_client.joint_initial_rotate, action_server_client.joint_final_rotate)]) + assert translate_results + rotate_results -def test_translate_mobile_base(node, action_server_client): - rospy.Subscriber("/stretch/joint_states", JointState, action_server_client.joint_state_callback) - time.sleep(0.5) - translate = 0.08 - deltas = {'in' : -translate , 'out' : translate} - command = {'joint' : 'joint_mobile_base_translation' , '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, 0.01) - assert True -''' \ No newline at end of file From ece26beaf1312d6b11be8c78d7b847fa5b9eb768 Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Tue, 10 Aug 2021 19:51:07 -0400 Subject: [PATCH 14/16] test_cmd_sub fix --- stretch_core/tests/node_tests/test_stretch_driver.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/stretch_core/tests/node_tests/test_stretch_driver.py b/stretch_core/tests/node_tests/test_stretch_driver.py index 82da073..22a676d 100644 --- a/stretch_core/tests/node_tests/test_stretch_driver.py +++ b/stretch_core/tests/node_tests/test_stretch_driver.py @@ -184,14 +184,11 @@ def test_magnetometer_mobile_base_receives_something(node, waiter): ######## TEST SUBSCRIBERS ####### #pub.get_num_connections() continues to return 0 when cmd_vel should be subscribed to in stretch_driver -''' + def test_cmd_vel_subbed(node): - cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size = None) + cmd_pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size = None) time.sleep(0.5) - connections = cmd_pub.get_num_connections() - - assert connections == 1 -''' + assert cmd_pub.get_num_connections() == 1 ######## TEST SERVICES ####### From e357079351926ae57d5e49768ee3cd2c895ddc78 Mon Sep 17 00:00:00 2001 From: hello-adamc Date: Wed, 11 Aug 2021 16:22:16 -0400 Subject: [PATCH 15/16] switch rel comparison to abs in approx functions --- .../nodes/tests/test_command_groups.py | 20 ------ .../tests/node_tests/test_stretch_driver.py | 71 ++++++++----------- 2 files changed, 29 insertions(+), 62 deletions(-) diff --git a/stretch_core/nodes/tests/test_command_groups.py b/stretch_core/nodes/tests/test_command_groups.py index bcdae07..c70fc70 100644 --- a/stretch_core/nodes/tests/test_command_groups.py +++ b/stretch_core/nodes/tests/test_command_groups.py @@ -55,8 +55,6 @@ 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, @@ -68,21 +66,3 @@ def test_get_num_valid_commands(head_pan_cg,head_tilt_cg,wrist_yaw_cg,gripper_cg assert True else: assert False - - -''' IN DEVELOPMENT -def test_update(head_pan_cg,head_tilt_cg,wrist_yaw_cg,gripper_cg,telescoping_cg,lift_cg,mobile_base_cg,invalid_joints_callback): - - command_groups = [head_pan_cg,head_tilt_cg,wrist_yaw_cg,gripper_cg, - telescoping_cg,lift_cg,mobile_base_cg] - - commanded_joint_names = [cg.name for cg in command_groups] - - - updates = [c.update(commanded_joint_names, invalid_joints_callback) - for c in command_groups] - - if not all(updates): - assert False - assert True -''' diff --git a/stretch_core/tests/node_tests/test_stretch_driver.py b/stretch_core/tests/node_tests/test_stretch_driver.py index 22a676d..27d3a0b 100644 --- a/stretch_core/tests/node_tests/test_stretch_driver.py +++ b/stretch_core/tests/node_tests/test_stretch_driver.py @@ -17,7 +17,7 @@ from trajectory_msgs.msg import JointTrajectoryPoint @pytest.fixture def node(): - rospy.init_node('test_stretch_driver', anonymous=True) + rospy.init_node("test_stretch_driver", anonymous=True) @pytest.fixture def waiter(): @@ -68,10 +68,13 @@ def action_server_client(): 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) @@ -80,59 +83,59 @@ def action_server_client(): trajectory_goal.trajectory.joint_names = [joint_name] joint_value = self.joint_state.position[joint_index] delta_val = command['deltas'][delta] - rospy.loginfo('delta = {0}, joint_index = {1}, joint_value = {2}'.format(delta, joint_index, joint_value)) new_value = joint_value + delta_val point.positions = [new_value] trajectory_goal.trajectory.points = [point] - rospy.loginfo('joint_name = {0}, trajectory_goal = {1}'.format(joint_name, trajectory_goal)) 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 - new_values_rotate = [joint_values + rotate for joint_values in self.joint_initial_rotate] - new_values_translate = [joint_values + translate for joint_values in self.joint_initial_translate] - new_value_forward = new_values_rotate + new_values_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 - new_values_rotate = [joint_values for joint_values in self.joint_initial_rotate] - new_values_translate = [joint_values for joint_values in self.joint_initial_translate] - new_value_back = new_values_rotate + new_values_translate + #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] @@ -183,10 +186,8 @@ def test_magnetometer_mobile_base_receives_something(node, waiter): ######## TEST SUBSCRIBERS ####### -#pub.get_num_connections() continues to return 0 when cmd_vel should be subscribed to in stretch_driver - def test_cmd_vel_subbed(node): - cmd_pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size = None) + cmd_pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size = None) time.sleep(0.5) assert cmd_pub.get_num_connections() == 1 @@ -239,20 +240,6 @@ def test_runstop(node): assert result.success - -# Test timing out (60s), error could be due to calibration -''' -def test_switch_to_manipulation_mode(node): - rospy.wait_for_service('switch_to_manipulation_mode') - - s = rospy.ServiceProxy('switch_to_manipulation_mode', Trigger) - - s_request = TriggerRequest() - - result = s(s_request) - - assert result.success -''' ######## TEST ACTION SERVER ####### def test_move_lift(node, action_server_client): @@ -266,7 +253,7 @@ def test_move_lift(node, action_server_client): assert False else: action_server_client.move_joint(command) - assert (action_server_client.initial_position) == pytest.approx(action_server_client.final_position, 0.01) + 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) @@ -280,7 +267,7 @@ def test_move_wrist(node, action_server_client): else: action_server_client.move_joint(command) - assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) + 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) @@ -294,12 +281,12 @@ def test_move_wrist_yaw(node, action_server_client): else: action_server_client.move_joint(command) - assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.01) + 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 = 8 + deg = 5 deltas = {'ccw' : ((math.pi/180) * deg) , 'cw' : -((math.pi/180) * deg)} command = {'joint' : 'joint_gripper_finger_left' , 'deltas' : deltas} @@ -308,12 +295,12 @@ def test_move_gripper_finger(node, action_server_client): else: action_server_client.move_joint(command) - assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.1) #0.01 + 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 = 15 + deg = 20 deltas = {'ccw' : ((math.pi/180) * deg) , 'cw' : -((math.pi/180) * deg)} command = {'joint' : 'joint_head_tilt' , 'deltas' : deltas} @@ -322,12 +309,12 @@ def test_move_head_tilt(node, action_server_client): else: action_server_client.move_joint(command) - assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.1) + 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 = 15 + deg = 20 deltas = {'ccw' : ((math.pi/180) * deg) , 'cw' : -((math.pi/180) * deg)} command = {'joint' : 'joint_head_pan' , 'deltas' : deltas} @@ -336,7 +323,7 @@ def test_move_head_pan(node, action_server_client): else: action_server_client.move_joint(command) - assert action_server_client.initial_position == pytest.approx(action_server_client.final_position, 0.1) + 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): @@ -353,7 +340,7 @@ def test_move_multiple_joints(node, action_server_client): else: action_server_client.move_multiple_joints(joint_names,rad,translate) - translate_results = all([initial == pytest.approx(final,0.1) for initial, final in zip(action_server_client.joint_initial_translate, action_server_client.joint_final_translate)]) - rotate_results = all([initial == pytest.approx(final,0.1) for initial, final in zip(action_server_client.joint_initial_rotate, action_server_client.joint_final_rotate)]) + 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 From 7cc51d20a7ba751bf3349bad669bf4a9564c3b86 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Tue, 7 Sep 2021 12:57:43 -0700 Subject: [PATCH 16/16] Correct spelling and cleanup whitespace --- README.md | 5 - stretch_core/CMakeLists.txt | 2 +- stretch_core/LICENSE.md | 2 +- stretch_core/README.md | 55 +++--- .../nodes/tests/test_command_groups.py | 40 ++-- stretch_core/tests/core_lib_test.launch | 2 +- .../tests/node_tests/test_stretch_driver.py | 182 +++++++++--------- stretch_core/tests/stretch_driver_test.launch | 4 +- 8 files changed, 143 insertions(+), 149 deletions(-) diff --git a/README.md b/README.md index ea7ac39..68bb27f 100644 --- a/README.md +++ b/README.md @@ -33,7 +33,6 @@ The *stretch_ros* repository holds ROS related code for the Stretch RE1 mobile m [stretch_moveit_config](stretch_gazebo/README.md) | Config files to use Stretch with the MoveIt Motion Planning Framework [stretch_navigation](stretch_navigation/README.md) | Support for the ROS navigation stack, including move_base, gmapping, and AMCL - ## Licenses This software is intended for use with the Stretch RE1 mobile manipulator, which is a robot produced and sold by Hello Robot Inc. For further information, including inquiries about dual licensing, please contact Hello Robot Inc. @@ -52,7 +51,3 @@ stretch_funmap | [LGPLv3](https://www.gnu.org/licenses/lgpl-3.0.en.html) stretch_gazebo | [Apache 2.0](http://www.apache.org/licenses/LICENSE-2.0) stretch_moveit_config | [Apache 2.0](http://www.apache.org/licenses/LICENSE-2.0) stretch_navigation | [Apache 2.0](http://www.apache.org/licenses/LICENSE-2.0) - - - - diff --git a/stretch_core/CMakeLists.txt b/stretch_core/CMakeLists.txt index 6c1a9f2..c3f2704 100644 --- a/stretch_core/CMakeLists.txt +++ b/stretch_core/CMakeLists.txt @@ -208,5 +208,5 @@ include_directories( if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest(tests/core_lib_test.launch) - add_rostest(tests/stretch_driver_test.launch) + add_rostest(tests/stretch_driver_test.launch) endif() diff --git a/stretch_core/LICENSE.md b/stretch_core/LICENSE.md index c45b1e2..ebb2d6f 100644 --- a/stretch_core/LICENSE.md +++ b/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. diff --git a/stretch_core/README.md b/stretch_core/README.md index f086a25..fd73895 100644 --- a/stretch_core/README.md +++ b/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 @@ -14,7 +14,7 @@ ## 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 approah resulting in minimal boiler plate code. First we should ensure that pytest is installed and up to date: +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 @@ -22,27 +22,27 @@ The primary testing framework being used within *stretch_ros* is pytest. Pytest >>$ pytest --version pytest 6.2.4 ``` -Testing can be split into four different categories: +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 extention of standard ROS launch files. Test suites can be created simply by adding the following lines to the desired test file: +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: +Python Library Tests: -```html +```xml ``` -ROS Node Unit/Integration Tests: +ROS Node Unit/Integration Tests: -```html +```xml @@ -52,37 +52,34 @@ ROS Node Unit/Integration Tests: 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 -``` +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 +>>$ stretch_robot_home.py ``` ```bash ->>$ roslaunch +>>$ roslaunch ``` -In order to run tests the following commands can be typed into the command line: +In order to run tests the following commands can be typed into the command line: ```bash ->>$ pytest -vv +>>$ 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. +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: +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*. +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*: @@ -90,39 +87,41 @@ Note, we can also run tests using the *catkin_tools* package. Thus we can assume >>$ 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 modifed to specify indvidual packages as such: +The following line can be modified to specify individual packages as such: ```bash >>$ catkin run_tests ``` -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: +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 +```bash +>>$ catkin run_tests --this ``` -Results can be visualized by typing in the following command: +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 pacakge where the failure occured. However, when running tests with *catkin_tools* some plugins will lose functionality such as Pytest Clarity. +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. diff --git a/stretch_core/nodes/tests/test_command_groups.py b/stretch_core/nodes/tests/test_command_groups.py index c70fc70..4bc53ff 100644 --- a/stretch_core/nodes/tests/test_command_groups.py +++ b/stretch_core/nodes/tests/test_command_groups.py @@ -1,35 +1,35 @@ -import pytest +import pytest import math -import stretch_body.robot as robot +import stretch_body.robot as robot -from .. import command_groups as cg +from .. import command_groups as cg #Initalize fixtures for all possible command groups -@pytest.fixture +@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 + 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(): +def head_tilt_cg(): range_rad = (-math.pi/4,math.pi/4) - head_tilt_calibrated_offset_rad = math.pi/16 + 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) + head_tilt_backlash_transition_angle_rad) -@pytest.fixture +@pytest.fixture def wrist_yaw_cg(): range_rad = (-math.pi/4,math.pi/4) - return cg.WristYawCommandGroup(range_rad) + return cg.WristYawCommandGroup(range_rad) -@pytest.fixture -def gripper_cg(): +@pytest.fixture +def gripper_cg(): range_robotis = (0,3) return cg.GripperCommandGroup(range_robotis) @@ -45,24 +45,24 @@ def lift_cg(): range_m = (0,1) return cg.LiftCommandGroup(range_m) -@pytest.fixture +@pytest.fixture def mobile_base_cg(): virtual_range_m = (-0.5,0.5) return cg.MobileBaseCommandGroup(virtual_range_m) -@pytest.fixture +@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): +#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 + assert False diff --git a/stretch_core/tests/core_lib_test.launch b/stretch_core/tests/core_lib_test.launch index 1d91361..2e8fa47 100644 --- a/stretch_core/tests/core_lib_test.launch +++ b/stretch_core/tests/core_lib_test.launch @@ -1,4 +1,4 @@ - \ No newline at end of file + diff --git a/stretch_core/tests/node_tests/test_stretch_driver.py b/stretch_core/tests/node_tests/test_stretch_driver.py index 27d3a0b..7070200 100644 --- a/stretch_core/tests/node_tests/test_stretch_driver.py +++ b/stretch_core/tests/node_tests/test_stretch_driver.py @@ -1,6 +1,6 @@ import pytest -import rospy -import time +import rospy +import time import actionlib import math @@ -43,15 +43,15 @@ def waiter(): return Waiter() -@pytest.fixture +@pytest.fixture def action_server_client(): - class ActionServerClient: + class ActionServerClient: def __init__(self): self.trajectory_client = None self.joint_state = None - self.initial_position = None - self.final_position = None + self.initial_position = None + self.final_position = None self.joint_initial_rotate = [] self.joint_final_rotate = [] @@ -60,8 +60,8 @@ def action_server_client(): self.joint_final_translate = [] def joint_state_callback(self,joint_state): - self.joint_state = 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)) @@ -69,18 +69,18 @@ def action_server_client(): def move_joint(self,command): - #Unpack joint name and store inital state in member variable - joint_name = command['joint'] + #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']: + #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] + 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 @@ -91,21 +91,21 @@ def action_server_client(): self.trajectory_client.send_goal(trajectory_goal) time.sleep(2) - #Store final state in memeber variable + #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 + #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 + 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() @@ -114,88 +114,88 @@ def action_server_client(): 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 + + 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 + new_value_forward = new_values_rotate_forward + new_values_translate_forward - #Set back point to inital values + #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 + 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 + #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] + #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) + + rospy.Subscriber("/stretch/joint_states", JointState, waiter.callback) waiter.wait(10.0) - assert waiter.success + 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) + + rospy.Subscriber("/odom", Odometry, waiter.callback) waiter.wait(10.0) - assert waiter.success + 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) + + rospy.Subscriber("/imu_mobile_base", Imu, waiter.callback) waiter.wait(10.0) - assert waiter.success + 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) + + rospy.Subscriber("/imu_wrist", Imu, waiter.callback) waiter.wait(10.0) - assert waiter.success + 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) + + rospy.Subscriber("/magnetometer_mobile_base", MagneticField, waiter.callback) waiter.wait(10.0) - assert waiter.success + 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 + assert cmd_pub.get_num_connections() == 1 ######## TEST SERVICES ####### -def test_switch_to_position_mode(node): - rospy.wait_for_service('switch_to_position_mode') +def test_switch_to_position_mode(node): + rospy.wait_for_service('switch_to_position_mode') s = rospy.ServiceProxy('switch_to_position_mode', Trigger) @@ -203,10 +203,10 @@ def test_switch_to_position_mode(node): result = s(s_request) - assert result.success + assert result.success -def test_switch_to_navigation_mode(node): - rospy.wait_for_service('switch_to_navigation_mode') +def test_switch_to_navigation_mode(node): + rospy.wait_for_service('switch_to_navigation_mode') s = rospy.ServiceProxy('switch_to_navigation_mode', Trigger) @@ -214,11 +214,11 @@ def test_switch_to_navigation_mode(node): result = s(s_request) - assert result.success + assert result.success -def test_stop_the_robot(node): - rospy.wait_for_service('stop_the_robot') +def test_stop_the_robot(node): + rospy.wait_for_service('stop_the_robot') s = rospy.ServiceProxy('stop_the_robot', Trigger) @@ -228,8 +228,8 @@ def test_stop_the_robot(node): assert result.success -def test_runstop(node): - rospy.wait_for_service('runstop') +def test_runstop(node): + rospy.wait_for_service('runstop') s = rospy.ServiceProxy('runstop', SetBool) @@ -237,90 +237,90 @@ def test_runstop(node): result = s(s_request) - assert result.success + 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) + time.sleep(0.5) translate = 0.1 deltas = {'down' : -translate , 'up' : translate} - command = {'joint' : 'joint_lift' , 'deltas' : deltas} - + command = {'joint' : 'joint_lift' , 'deltas' : deltas} + if not action_server_client.start_trajectory_client(): - assert False - else: + 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) + time.sleep(0.5) translate = 0.1 deltas = { 'out' : translate, 'in' : -translate ,} - command = {'joint' : 'wrist_extension' , 'deltas' : deltas} + command = {'joint' : 'wrist_extension' , 'deltas' : deltas} if not action_server_client.start_trajectory_client(): - assert False - else: + 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) + time.sleep(0.5) deg = 15 deltas = {'ccw' : ((math.pi/180) * deg) , 'cw' : -((math.pi/180) * deg)} - command = {'joint' : 'joint_wrist_yaw' , 'deltas' : deltas} + command = {'joint' : 'joint_wrist_yaw' , 'deltas' : deltas} if not action_server_client.start_trajectory_client(): - assert False - else: + 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) + 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} + command = {'joint' : 'joint_gripper_finger_left' , 'deltas' : deltas} if not action_server_client.start_trajectory_client(): - assert False - else: + 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) + time.sleep(0.5) deg = 20 deltas = {'ccw' : ((math.pi/180) * deg) , 'cw' : -((math.pi/180) * deg)} - command = {'joint' : 'joint_head_tilt' , 'deltas' : deltas} + command = {'joint' : 'joint_head_tilt' , 'deltas' : deltas} if not action_server_client.start_trajectory_client(): - assert False - else: + 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) + time.sleep(0.5) deg = 20 deltas = {'ccw' : ((math.pi/180) * deg) , 'cw' : -((math.pi/180) * deg)} - command = {'joint' : 'joint_head_pan' , 'deltas' : deltas} + command = {'joint' : 'joint_head_pan' , 'deltas' : deltas} if not action_server_client.start_trajectory_client(): - assert False - else: + 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) @@ -328,19 +328,19 @@ def test_move_head_pan(node, action_server_client): 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) + 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) + 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)]) + 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 diff --git a/stretch_core/tests/stretch_driver_test.launch b/stretch_core/tests/stretch_driver_test.launch index 1706697..6d26928 100644 --- a/stretch_core/tests/stretch_driver_test.launch +++ b/stretch_core/tests/stretch_driver_test.launch @@ -11,7 +11,7 @@ [/stretch/joint_states] - + - \ No newline at end of file +