def test_one_hand_no_robot_description_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("hand/joint_prefix/1", "rh_")
rospy.set_param("hand/mapping/1", "right")
hand_finder = HandFinder()
# 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")
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
1, "It should be only one joint_prefix")
self.assertEqual(hand_finder.get_hand_parameters().mapping['1'],
"right", "It should be only right mapping")
self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'],
"rh_", "It should be only rh_ prefix")
# hand joints
self.assertIsNotNone(hand_finder.get_hand_joints(),
"Joints extracted.")
self.assertEqual(len(list(hand_finder.get_hand_joints().keys())), 1,
"It should be only one mapping")
self.assertIn("right", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['right']
self.assertEqual(len(joints), 24, "Joint number should be 24")
self.assertIn("rh_FFJ3",
hand_finder.get_hand_joints()["right"],
"FFJ3 joint should be in the joints list")
def test_no_hand_robot_description_exists_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("robot_description",
rospy.get_param("two_hands_description"))
hand_finder = HandFinder()
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")
self.assertEqual(len(hand_finder.get_hand_joints()), 0,
"correct joints without a hand")
self.assertEqual(len(hand_finder.get_calibration_path()), 0,
"correct calibration path without a hand")
self.assertEqual(
len(hand_finder.get_hand_control_tuning().friction_compensation),
0, "correct tuning without a hands")
self.assertEqual(
len(hand_finder.get_hand_control_tuning().host_control), 0,
"correct tuning without a hands")
self.assertEqual(
len(hand_finder.get_hand_control_tuning().motor_control), 0,
"correct tuning without a hands")
def test_no_hand_no_robot_description_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
hand_finder = HandFinder()
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")
self.assertEqual(len(hand_finder.get_hand_joints()), 0,
"correct joints without a hand")
def __init__(self, config_file_path, service_timeout, excluded_joints=[]):
self._config_file_path = config_file_path
self._service_timeout = service_timeout
hand_finder = HandFinder()
self._hand_mapping = hand_finder.get_hand_parameters().mapping
self._joints = hand_finder.get_hand_joints()
self._nonpresent_joints = list(ControllerSpawner.JOINT_NAMES)
for side in self._joints:
for joint in self._joints[side]:
if joint in self._nonpresent_joints:
self._nonpresent_joints.remove(joint)
self._excluded_joints = excluded_joints
self._excluded_joints = list(
set(self._excluded_joints) | set(self._nonpresent_joints))
rospy.logwarn("Excluded joints:")
rospy.logwarn(self._excluded_joints)
def generate_controllers():
controllers = []
hand_finder = HandFinder()
joints = hand_finder.get_hand_joints()
mapping = hand_finder.get_hand_parameters().mapping
for hand in mapping:
prefix = mapping[hand]
for joint in joints[prefix]:
if joint[3:5].lower() == 'th' or joint[3:5].lower(
) == 'wr' or (joint[6] != '1' and joint[6] != '2'):
joint_controller = 'cal_sh_' + joint.lower()
else:
joint = joint[:6] + '0'
joint_controller = 'cal_sh_' + joint.lower()
if joint_controller not in controllers:
controllers.append(joint_controller)
return controllers
def generate_reset_services_list(self):
reset_service_list = []
generated_reset_service_list = []
service_list = []
# We first read the list of available motor reset services in this namespace
# this will allow us to avoid having to know the name of the robot driver node
ns = rospy.get_namespace()
while not reset_service_list:
rospy.sleep(0.5)
service_list = rosservice.get_service_list(namespace=ns)
reset_service_list = [
srv for srv in service_list if '/reset_motor_' in srv
]
if not reset_service_list:
rospy.loginfo("Waiting for motor reset services")
# We generate a list of the services that we expect to find
# and merely throw a warning if some of them didn't turn up in the available service list
hand_finder = HandFinder()
joints = hand_finder.get_hand_joints()
mapping = hand_finder.get_hand_parameters().mapping
for hand in mapping:
prefix = mapping[hand]
for joint in joints[prefix]:
if not (joint[3:5].lower() == 'th'
or joint[3:5].lower() == 'wr' or
(joint[6] != '1' and joint[6] != '2')):
joint = joint[:6] + '0'
joint_reset_service = joint[:2] + '/reset_motor_' + joint[
3:].upper()
if joint_reset_service not in generated_reset_service_list:
generated_reset_service_list.append(joint_reset_service)
for gen_srv in generated_reset_service_list:
gen_srv_match_list = [
srv for srv in reset_service_list if gen_srv in srv
]
if not gen_srv_match_list:
rospy.logwarn("Expected service not found: %s", gen_srv)
return reset_service_list
def test_one_hand_no_prefix_no_ns_robot_description_exists_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("hand/joint_prefix/1", "")
rospy.set_param("hand/mapping/1", "")
rospy.set_param("robot_description",
rospy.get_param("right_hand_description_no_prefix"))
hand_finder = HandFinder()
# 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")
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
1, "It should be only one joint_prefix")
self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "1",
"It should be the serial id as mapping")
self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'],
"", "It should be only an empty prefix")
# hand joints
self.assertIsNotNone(hand_finder.get_hand_joints(),
"Joints extracted.")
self.assertEqual(len(list(hand_finder.get_hand_joints().keys())), 1,
"It should be only one mapping")
self.assertIn("1", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['1']
self.assertEqual(len(joints), 1, "Joint number should be 1")
self.assertNotIn("FFJ3",
hand_finder.get_hand_joints()["1"],
"FFJ3 joint should not be in the joints list")
self.assertIn("RFJ4",
hand_finder.get_hand_joints()["1"],
"RFJ4 joint should be in the joints list")
for key in joint_trajectory.joint_names:
trajectory_point.positions.append(posture[key])
return trajectory_point
hand_finder = HandFinder()
hand_parameters = hand_finder.get_hand_parameters()
hand_serial = hand_parameters.mapping.keys()[0]
hand_commander = SrHandCommander(hand_parameters=hand_parameters,
hand_serial=hand_serial)
hand_mapping = hand_parameters.mapping[hand_serial]
# Hand joints are detected
joints = hand_finder.get_hand_joints()[hand_mapping]
open_hand = {'rh_FFJ1': 0.0, 'rh_FFJ2': 0.0, 'rh_FFJ3': 0.0, 'rh_FFJ4': 0.0,
'rh_MFJ1': 0.0, 'rh_MFJ2': 0.0, 'rh_MFJ3': 0.0, 'rh_MFJ4': 0.0,
'rh_RFJ1': 0.0, 'rh_RFJ2': 0.0, 'rh_RFJ3': 0.0, 'rh_RFJ4': 0.0,
'rh_LFJ1': 0.0, 'rh_LFJ2': 0.0, 'rh_LFJ3': 0.0, 'rh_LFJ4': 0.0,
'rh_LFJ5': 0.0, 'rh_THJ1': 0.0, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0,
'rh_THJ4': 0.0, 'rh_THJ5': 0.0}
keys = ['rh_FFJ1', 'rh_FFJ2', 'rh_FFJ3', 'rh_FFJ4', 'rh_LFJ1', 'rh_LFJ2',
'rh_LFJ3', 'rh_LFJ4', 'rh_LFJ5', 'rh_MFJ1', 'rh_MFJ2', 'rh_MFJ3',
'rh_MFJ4', 'rh_RFJ1', 'rh_RFJ2', 'rh_RFJ3', 'rh_RFJ4', 'rh_THJ1',
'rh_THJ2', 'rh_THJ3', 'rh_THJ4', 'rh_THJ5']
position = [1.07, 0.26, 0.88, -0.34, 0.85, 0.60,
0.21, -0.23, 0.15, 1.06, 0.16, 1.04,
def test_two_hand_robot_description_exists_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("hand/joint_prefix/1", "rh_")
rospy.set_param("hand/mapping/1", "right")
rospy.set_param("hand/joint_prefix/2", "lh_")
rospy.set_param("hand/mapping/2", "left")
rospy.set_param("robot_description",
rospy.get_param("two_hands_description"))
hand_finder = HandFinder()
# 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")
self.assertIn("right",
hand_finder.get_hand_parameters().mapping.values(),
"It should be right mapping")
self.assertIn("left",
hand_finder.get_hand_parameters().mapping.values(),
"It should be left mapping")
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
2, "It should be two joint_prefixes")
self.assertIn("rh_",
hand_finder.get_hand_parameters().joint_prefix.values(),
"It should be rh_ prefix")
self.assertIn("lh_",
hand_finder.get_hand_parameters().joint_prefix.values(),
"It should be rh_ prefix")
# hand joints
self.assertIsNotNone(hand_finder.get_hand_joints(),
"Joints extracted.")
self.assertEqual(len(hand_finder.get_hand_joints().keys()), 2,
"It should be two mappings")
self.assertIn("right", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['right']
self.assertEqual(len(joints), 1, "Joint number should be 1")
self.assertNotIn("rh_FFJ3",
hand_finder.get_hand_joints()["right"],
"rh_FFJ3 joint should not be in the joints list")
self.assertIn("rh_RFJ4",
hand_finder.get_hand_joints()["right"],
"rh_RFJ4 joint should be in the joints list")
self.assertIn("left", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['left']
self.assertEqual(len(joints), 1, "Joint number should be 1")
self.assertNotIn("lh_FFJ3",
hand_finder.get_hand_joints()["left"],
"lh_FFJ3 joint should not be in the joints list")
self.assertIn("lh_LFJ4",
hand_finder.get_hand_joints()["left"],
"lh_LFJ4 joint should be in the joints list")
# calibration
self.assertIsNotNone(hand_finder.get_calibration_path(),
"Calibration extracted.")
calibration_path = hand_finder.get_calibration_path()['right']
self.assertEqual(
calibration_path,
self.ethercat_path + "/calibrations/right/" + "calibration.yaml",
"incorrect calibration file")
calibration_path = hand_finder.get_calibration_path()['left']
self.assertEqual(
calibration_path,
self.ethercat_path + "/calibrations/left/" + "calibration.yaml",
"incorrect calibration file")
# tuning
self.assertIsNotNone(hand_finder.get_hand_control_tuning(),
"Control tuning parameters extracted.")
ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning(
).friction_compensation['right']
self.assertEqual(
ctrl_tun_friction_comp_path,
self.ethercat_path + "/controls/friction_compensation.yaml",
"incorrect friction compensation file")
ctrl_tun_motors_path = hand_finder.get_hand_control_tuning(
).motor_control['right']
self.assertEqual(
ctrl_tun_motors_path, self.ethercat_path +
"/controls/motors/right/motor_board_effort_controllers.yaml",
"incorrect motor config file")
ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning(
).host_control['right']
self.assertEqual(len(ctrl_tun_host_control_paths),
len(controller_params),
"incorrect number of host controllers param")
for controller_path, controller_param in zip(
ctrl_tun_host_control_paths, controller_params):
self.assertEqual(
controller_path, self.ethercat_path + "/controls/host/right/" +
controller_param, "incorrect controller config file")
ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning(
).friction_compensation['left']
self.assertEqual(
ctrl_tun_friction_comp_path,
self.ethercat_path + "/controls/friction_compensation.yaml",
"incorrect friction compensation file")
ctrl_tun_motors_path = hand_finder.get_hand_control_tuning(
).motor_control['left']
self.assertEqual(
ctrl_tun_motors_path, self.ethercat_path +
"/controls/motors/left/motor_board_effort_controllers.yaml",
"incorrect motor config file")
ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning(
).host_control['left']
self.assertEqual(len(ctrl_tun_host_control_paths),
len(controller_params),
"incorrect number of host controllers param")
for controller_path, controller_param in zip(
ctrl_tun_host_control_paths, controller_params):
self.assertEqual(
controller_path,
self.ethercat_path + "/controls/host/left/" + controller_param,
"incorrect controller config file")
def test_one_hand_no_prefix_no_ns_robot_description_exists_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("hand/joint_prefix/1", "")
rospy.set_param("hand/mapping/1", "")
rospy.set_param("robot_description",
rospy.get_param("right_hand_description_no_prefix"))
hand_finder = HandFinder()
# 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")
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
1, "It should be only one joint_prefix")
self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "1",
"It should be the serial id as mapping")
self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'],
"", "It should be only an empty prefix")
# hand joints
self.assertIsNotNone(hand_finder.get_hand_joints(),
"Joints extracted.")
self.assertEqual(len(hand_finder.get_hand_joints().keys()), 1,
"It should be only one mapping")
self.assertIn("1", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['1']
self.assertEqual(len(joints), 1, "Joint number should be 1")
self.assertNotIn("FFJ3",
hand_finder.get_hand_joints()["1"],
"FFJ3 joint should not be in the joints list")
self.assertIn("RFJ4",
hand_finder.get_hand_joints()["1"],
"RFJ4 joint should be in the joints list")
# calibration
self.assertIsNotNone(hand_finder.get_calibration_path(),
"Calibration extracted.")
calibration_path = hand_finder.get_calibration_path()['1']
self.assertEqual(
calibration_path,
self.ethercat_path + "/calibrations/1/" + "calibration.yaml",
"incorrect calibration file")
# tuning
self.assertIsNotNone(hand_finder.get_hand_control_tuning(),
"Control tuning parameters extracted.")
ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning(
).friction_compensation['1']
self.assertEqual(
ctrl_tun_friction_comp_path,
self.ethercat_path + "/controls/friction_compensation.yaml",
"incorrect friction compensation file")
ctrl_tun_motors_path = hand_finder.get_hand_control_tuning(
).motor_control['1']
self.assertEqual(
ctrl_tun_motors_path, self.ethercat_path +
"/controls/motors/1/motor_board_effort_controllers.yaml",
"incorrect motor config file")
ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning(
).host_control['1']
self.assertEqual(len(ctrl_tun_host_control_paths),
len(controller_params),
"incorrect number of host controllers param")
for controller_path, controller_param in zip(
ctrl_tun_host_control_paths, controller_params):
self.assertEqual(
controller_path,
self.ethercat_path + "/controls/host/1/" + controller_param,
"incorrect controller config file")
def test_two_hand_robot_description_exists_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("hand/joint_prefix/1", "rh_")
rospy.set_param("hand/mapping/1", "right")
rospy.set_param("hand/joint_prefix/2", "lh_")
rospy.set_param("hand/mapping/2", "left")
rospy.set_param("robot_description",
rospy.get_param("two_hands_description"))
hand_finder = HandFinder()
# 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")
self.assertIn("right",
list(hand_finder.get_hand_parameters().mapping.values()),
"It should be right mapping")
self.assertIn("left",
list(hand_finder.get_hand_parameters().mapping.values()),
"It should be left mapping")
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
2, "It should be two joint_prefixes")
self.assertIn(
"rh_",
list(hand_finder.get_hand_parameters().joint_prefix.values()),
"It should be rh_ prefix")
self.assertIn(
"lh_",
list(hand_finder.get_hand_parameters().joint_prefix.values()),
"It should be rh_ prefix")
# hand joints
self.assertIsNotNone(hand_finder.get_hand_joints(),
"Joints extracted.")
self.assertEqual(len(list(hand_finder.get_hand_joints().keys())), 2,
"It should be two mappings")
self.assertIn("right", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['right']
self.assertEqual(len(joints), 1, "Joint number should be 1")
self.assertNotIn("rh_FFJ3",
hand_finder.get_hand_joints()["right"],
"rh_FFJ3 joint should not be in the joints list")
self.assertIn("rh_RFJ4",
hand_finder.get_hand_joints()["right"],
"rh_RFJ4 joint should be in the joints list")
self.assertIn("left", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['left']
self.assertEqual(len(joints), 1, "Joint number should be 1")
self.assertNotIn("lh_FFJ3",
hand_finder.get_hand_joints()["left"],
"lh_FFJ3 joint should not be in the joints list")
self.assertIn("lh_LFJ4",
hand_finder.get_hand_joints()["left"],
"lh_LFJ4 joint should be in the joints list")
class TrajectoryControllerSpawner(object):
def __init__(self, trajectory, service_timeout):
self.trajectory = trajectory
self.service_timeout = service_timeout
self.hand_finder = HandFinder()
self.joints = self.hand_finder.get_hand_joints()
ros_pack = rospkg.RosPack()
sr_robot_launch_path = ros_pack.get_path('sr_robot_launch')
self.hand_mapping = self.hand_finder.get_hand_parameters().mapping
self.yaml_file_path = {}
for hand in self.hand_mapping:
self.yaml_file_path[self.hand_mapping[hand]] = (
sr_robot_launch_path + "/config/" + self.hand_mapping[hand] + "_trajectory_controller.yaml")
def generate_parameters(self):
for hand in self.yaml_file_path:
with open(self.yaml_file_path[hand], 'r') as yaml_file:
yaml_content = yaml.load(yaml_file)
if hand + "_trajectory_controller" not in yaml_content.keys():
rospy.logerr("there are errors opening trajectory controller yaml file")
else:
hand_trajectory = yaml_content[hand + "_trajectory_controller"]
if rospy.has_param('~exclude_wrist') and rospy.get_param('~exclude_wrist'):
hand_trajectory['joints'] = [s for s in self.joints[hand] if "WR" not in s]
else:
hand_trajectory['joints'] = self.joints[hand]
for joint_name in hand_trajectory['constraints'].keys():
if (joint_name not in hand_trajectory['joints'] and
joint_name != 'goal_time' and joint_name != 'stopped_velocity_tolerance'):
del hand_trajectory['constraints'][joint_name]
param_prefix = hand + "_trajectory_controller/"
rospy.set_param(param_prefix + 'allow_partial_joints_goal',
hand_trajectory['allow_partial_joints_goal'])
rospy.set_param(param_prefix + 'joints', hand_trajectory['joints'])
rospy.set_param(param_prefix + 'stop_trajectory_duration', hand_trajectory['stop_trajectory_duration'])
rospy.set_param(param_prefix + 'type', hand_trajectory['type'])
constrain_prefix = param_prefix + 'constraints/'
for constraint in hand_trajectory['constraints']:
if constraint == 'goal_time' or constraint == 'stopped_velocity_tolerance':
rospy.set_param(constrain_prefix + constraint, hand_trajectory['constraints'][constraint])
else:
rospy.set_param(constrain_prefix + constraint + '/goal',
hand_trajectory['constraints'][constraint]['goal'])
rospy.set_param(constrain_prefix + constraint + '/trajectory',
hand_trajectory['constraints'][constraint]['trajectory'])
@staticmethod
def check_joint(joint, controllers_to_start, controller_names):
if joint[3:5].lower() == 'th' or joint[3:5].lower() == 'wr' or (joint[6] != '1' and joint[6] != '2'):
joint_controller = 'sh_' + joint.lower() + "_position_controller"
else:
joint = joint[:6] + '0'
joint_controller = 'sh_' + joint.lower() + "_position_controller"
if joint_controller not in controller_names and joint_controller not in controllers_to_start:
controllers_to_start.append(joint_controller)
def set_controller(self):
controllers_to_start = []
for hand_serial in self.hand_mapping:
hand_prefix = self.hand_mapping[hand_serial]
success = True
try:
rospy.wait_for_service('controller_manager/list_controllers', self.service_timeout)
list_controllers = rospy.ServiceProxy(
'controller_manager/list_controllers', ListControllers)
running_controllers = list_controllers()
except rospy.ServiceException:
success = False
rospy.logerr("Failed to load trajectory controller")
if success:
already_running = False
controller_names = []
for controller_state in running_controllers.controller:
controller_names.append(controller_state.name)
if controller_state.name == hand_prefix + '_trajectory_controller':
already_running = True
if self.trajectory and not already_running:
controllers_to_start.append(hand_prefix + '_trajectory_controller')
for joint in self.joints[hand_prefix]:
TrajectoryControllerSpawner.check_joint(joint, controllers_to_start, controller_names)
for load_control in controllers_to_start:
try:
rospy.wait_for_service('controller_manager/load_controller', self.service_timeout)
load_controllers = rospy.ServiceProxy('controller_manager/load_controller', LoadController)
loaded_controllers = load_controllers(load_control)
except rospy.ServiceException:
success = False
if not loaded_controllers.ok:
success = False
try:
rospy.wait_for_service('controller_manager/switch_controller', self.service_timeout)
switch_controllers = rospy.ServiceProxy('controller_manager/switch_controller', SwitchController)
switched_controllers = switch_controllers(controllers_to_start, None,
SwitchController._request_class.BEST_EFFORT)
except rospy.ServiceException:
success = False
if not switched_controllers.ok:
success = False
if not success:
rospy.logerr("Failed to launch trajectory controller!")
@staticmethod
def wait_for_topic(topic_name, timeout):
if not topic_name:
return True
# This has to be a list since Python has a peculiar mechanism to determine
# whether a variable is local to a function or not:
# if the variable is assigned in the body of the function, then it is
# assumed to be local. Modifying a mutable object (like a list)
# works around this debatable "design choice".
wait_for_topic_result = [None]
def wait_for_topic_cb(msg):
wait_for_topic_result[0] = msg
rospy.logdebug("Heard from wait-for topic: %s" % str(msg.data))
rospy.Subscriber(topic_name, Bool, wait_for_topic_cb)
started_waiting = rospy.Time.now().to_sec()
# We might not have received any time messages yet
warned_about_not_hearing_anything = False
while not wait_for_topic_result[0]:
rospy.sleep(0.01)
if rospy.is_shutdown():
return False
if not warned_about_not_hearing_anything:
if rospy.Time.now().to_sec() - started_waiting > timeout:
warned_about_not_hearing_anything = True
rospy.logwarn("Controller Spawner hasn't heard anything from its \"wait for\" topic (%s)" %
topic_name)
while not wait_for_topic_result[0].data:
rospy.sleep(0.01)
if rospy.is_shutdown():
return False
return True
