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 +