Skip to content

Commit

Permalink
Fixed timeout in python test (#169) and reduced CPU load while waiting (
Browse files Browse the repository at this point in the history 
#170)
  • Loading branch information
@guihomework
guihomework authored Apr 22, 2021
1 parent 96cedd2 commit 65e9866
Show file tree
Hide file tree
Showing 2 changed files with 24 additions and 17 deletions.
17 changes: 12 additions & 5 deletions sr_utilities/scripts/sr_utilities/hand_finder.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,9 @@
import rospy
import rospkg
from urdf_parser_py.urdf import URDF
import time

DEFAULT_TIMEOUT = 60.0


class HandControllerTuning(object):
Expand Down Expand Up @@ -91,14 +94,14 @@ def get_default_joints(cls):
'THJ5', 'WRJ1', 'WRJ2']
return joints

def __init__(self, mapping, joint_prefix):
def __init__(self, mapping, joint_prefix, timeout=DEFAULT_TIMEOUT):
"""
"""
self.joints = {}
hand_joints = []
joints = self.get_default_joints()
TIMEOUT_WAIT_FOR_PARAMS_IN_SECS = 60.0
TIMEOUT_WAIT_FOR_PARAMS_IN_SECS = timeout
start_time = rospy.get_time()
while not rospy.has_param("/robot_description"):
if (rospy.get_time() - start_time > TIMEOUT_WAIT_FOR_PARAMS_IN_SECS):
Expand All @@ -115,6 +118,8 @@ def __init__(self, mapping, joint_prefix):
"in joint_prefix parameters")
self.joints[mapping[hand]] = hand_joints
return
else:
time.sleep(1)

robot_description = rospy.get_param('robot_description')

Expand Down Expand Up @@ -155,7 +160,7 @@ class HandFinder(object):
using this library to handle prefixes, joint prefixes etc...
"""

def __init__(self):
def __init__(self, timeout=DEFAULT_TIMEOUT):
"""
Parses the parameter server to extract the necessary information.
"""
Expand All @@ -164,12 +169,12 @@ def __init__(self):
self._hand_h = False
self._hand_h_parameters = {}

TIMEOUT_WAIT_FOR_PARAMS_IN_SECS = 60.0
TIMEOUT_WAIT_FOR_PARAMS_IN_SECS = timeout
self.wait_for_hand_params(TIMEOUT_WAIT_FOR_PARAMS_IN_SECS)

self.hand_config = HandConfig(self._hand_parameters["mapping"],
self._hand_parameters["joint_prefix"])
self.hand_joints = HandJoints(self.hand_config.mapping, self.hand_config.joint_prefix).joints
self.hand_joints = HandJoints(self.hand_config.mapping, self.hand_config.joint_prefix, timeout).joints
self.calibration_path = HandCalibration(self.hand_config.mapping).calibration_path
self.hand_control_tuning = HandControllerTuning(self.hand_config.mapping)

Expand All @@ -179,6 +184,8 @@ def wait_for_hand_params(self, timeout_in_secs):
if (rospy.get_time() - start_time > timeout_in_secs):
rospy.logerr("No hand is detected")
break
else:
time.sleep(1)
if rospy.has_param("/hand"):
rospy.loginfo("Found hand E")
self._hand_e = True
Expand Down
24 changes: 12 additions & 12 deletions sr_utilities/test/test_hand_finder.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ def test_no_hand_no_robot_description_finder(self):
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")

hand_finder = HandFinder()
hand_finder = HandFinder(1.0)

self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 0, "correct parameters without a hand")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 0, "correct parameters without a hand")
Expand All @@ -66,7 +66,7 @@ def test_one_hand_no_robot_description_finder(self):
rospy.set_param("hand/joint_prefix/1", "rh_")
rospy.set_param("hand/mapping/1", "right")

hand_finder = HandFinder()
hand_finder = HandFinder(1.0)
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
Expand Down Expand Up @@ -111,7 +111,7 @@ def test_one_hand_no_mapping_no_robot_description_finder(self):
rospy.set_param("hand/joint_prefix/1", "rh_")
rospy.set_param("hand/mapping/1", "")

hand_finder = HandFinder()
hand_finder = HandFinder(1.0)
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
Expand Down Expand Up @@ -157,7 +157,7 @@ def test_one_hand_no_prefix_no_robot_description_finder(self):
rospy.set_param("hand/joint_prefix/1", "")
rospy.set_param("hand/mapping/1", "rh")

hand_finder = HandFinder()
hand_finder = HandFinder(1.0)
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
Expand Down Expand Up @@ -202,7 +202,7 @@ def test_one_hand_no_prefix_no_mapping_no_robot_description_finder(self):
rospy.set_param("hand/joint_prefix/1", "")
rospy.set_param("hand/mapping/1", "")

hand_finder = HandFinder()
hand_finder = HandFinder(1.0)
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
Expand Down Expand Up @@ -249,7 +249,7 @@ def test_two_hand_no_robot_description_finder(self):
rospy.set_param("hand/joint_prefix/2", "lh_")
rospy.set_param("hand/mapping/2", "left")

hand_finder = HandFinder()
hand_finder = HandFinder(1.0)

# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
Expand Down Expand Up @@ -320,7 +320,7 @@ def test_no_hand_robot_description_exists_finder(self):

rospy.set_param("robot_description", rospy.get_param("two_hands_description"))

hand_finder = HandFinder()
hand_finder = HandFinder(1.0)

self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix), 0, "correct parameters without a hand")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 0, "correct parameters without a hand")
Expand All @@ -343,7 +343,7 @@ def test_one_hand_robot_description_exists_finder(self):
rospy.set_param("hand/mapping/1", "right")
rospy.set_param("robot_description", rospy.get_param("right_hand_description"))

hand_finder = HandFinder()
hand_finder = HandFinder(1.0)
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
Expand Down Expand Up @@ -391,7 +391,7 @@ def test_one_hand_no_mapping_robot_description_exists_finder(self):
rospy.set_param("hand/mapping/1", "")
rospy.set_param("robot_description", rospy.get_param("right_hand_description"))

hand_finder = HandFinder()
hand_finder = HandFinder(1.0)
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
Expand Down Expand Up @@ -438,7 +438,7 @@ def test_one_hand_no_prefix_robot_description_exists_finder(self):
rospy.set_param("hand/mapping/1", "rh")
rospy.set_param("robot_description", rospy.get_param("right_hand_description_no_prefix"))

hand_finder = HandFinder()
hand_finder = HandFinder(1.0)
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
Expand Down Expand Up @@ -485,7 +485,7 @@ def test_one_hand_no_prefix_no_ns_robot_description_exists_finder(self):
rospy.set_param("hand/mapping/1", "")
rospy.set_param("robot_description", rospy.get_param("right_hand_description_no_prefix"))

hand_finder = HandFinder()
hand_finder = HandFinder(1.0)
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1, "It should be only one mapping")
Expand Down Expand Up @@ -534,7 +534,7 @@ def test_two_hand_robot_description_exists_finder(self):
rospy.set_param("hand/mapping/2", "left")
rospy.set_param("robot_description", rospy.get_param("two_hands_description"))

hand_finder = HandFinder()
hand_finder = HandFinder(1.0)
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(), "Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 2, "It should be two mappings")
Expand Down

0 comments on commit 65e9866

Please sign in to comment.