Browse Source

Correct spelling and cleanup whitespace

pull/35/head
Binit Shah 3 years ago
parent
commit
7cc51d20a7
8 changed files with 143 additions and 149 deletions
  1. +0
    -5
      README.md
  2. +1
    -1
      stretch_core/CMakeLists.txt
  3. +1
    -1
      stretch_core/LICENSE.md
  4. +27
    -28
      stretch_core/README.md
  5. +20
    -20
      stretch_core/nodes/tests/test_command_groups.py
  6. +1
    -1
      stretch_core/tests/core_lib_test.launch
  7. +91
    -91
      stretch_core/tests/node_tests/test_stretch_driver.py
  8. +2
    -2
      stretch_core/tests/stretch_driver_test.launch

+ 0
- 5
README.md View File

@ -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)

+ 1
- 1
stretch_core/CMakeLists.txt View File

@ -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()

+ 1
- 1
stretch_core/LICENSE.md View File

@ -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.

+ 27
- 28
stretch_core/README.md View File

@ -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
<launch>
<param name="test_module" value="../src"/>
<test test-name="test_lib" pkg="my_pkg" type="pytest_runner.py" />
</launch>
```
ROS Node Unit/Integration Tests:
ROS Node Unit/Integration Tests:
```html
```xml
<launch>
<node pkg="my_pkg" type="publisher" name="publisher" />
<param name="test_module" value="listener"/>
@ -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 <package_name> <test_suite_name>
>>$ roslaunch <package_name> <test_suite_name>
```
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 <package_name>
```
Finally, if we navigate to any ROS package within our work space we can run all of the tests associated with the package with the following command:
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.

+ 20
- 20
stretch_core/nodes/tests/test_command_groups.py View File

@ -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

+ 1
- 1
stretch_core/tests/core_lib_test.launch View File

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

+ 91
- 91
stretch_core/tests/node_tests/test_stretch_driver.py View File

@ -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

+ 2
- 2
stretch_core/tests/stretch_driver_test.launch View File

@ -11,7 +11,7 @@
[/stretch/joint_states]
</rosparam>
</node>
<node
name="robot_state_publisher"
pkg="robot_state_publisher"
@ -29,4 +29,4 @@
<param name="test_module" value="node_tests"/>
<test test-name="test_stretch_driver" pkg ="stretch_core" type ="pytest_runner.py"/>
</launch>
</launch>

Loading…
Cancel
Save