diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_limits_urdf.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_limits_urdf.py index 7cdd52c9d..2a14784db 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_limits_urdf.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_limits_urdf.py @@ -11,14 +11,15 @@ import rospy -def get_joint_limits(key='robot_description', use_smallest_joint_limits=True): +def get_joint_limits(key='robot_description', use_smallest_joint_limits=True, description=None): use_small = use_smallest_joint_limits use_mimic = True # Code inspired on the joint_state_publisher package by David Lu!!! # https://github.com/ros/robot_model/blob/indigo-devel/ # joint_state_publisher/joint_state_publisher/joint_state_publisher - description = rospy.get_param(key) + if description is None: + description = rospy.get_param(key) robot = xml.dom.minidom.parseString(description)\ .getElementsByTagName('robot')[0] free_joints = {} diff --git a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py index e74395973..3e0f76c0f 100644 --- a/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/src/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -186,6 +186,7 @@ def __init__(self, context): self._state_sub = None # Controller state subscriber self._list_controllers = None + self._cm_checked = set() def shutdown_plugin(self): self._update_cmd_timer.stop() @@ -239,8 +240,20 @@ def _update_jtc_list(self): # List of running controllers with a valid joint limits specification # for _all_ their joints running_jtc = self._running_jtc_info() - if running_jtc and not self._robot_joint_limits: - self._robot_joint_limits = get_joint_limits() # Lazy evaluation + if running_jtc: + ns = self._cm_ns.rsplit('/', 1)[0] + if ns not in self._cm_checked: + try: + for jnt, lims in get_joint_limits(description=rospy.get_param('{}/robot_description'.format(ns))).iteritems(): + self._robot_joint_limits[jnt] = lims + self._cm_checked.add(ns) + except KeyError: + rospy.logwarn('Could not find a valid robot_description parameter in namespace {}'.format(ns)) + try: + for jnt, lims in get_joint_limits(description=rospy.get_param('robot_description')).iteritems(): + self._robot_joint_limits[jnt] = lims + except KeyError: + rospy.logwarn('Could not find robot_description parameter') valid_jtc = [] for jtc_info in running_jtc: has_limits = all(name in self._robot_joint_limits @@ -268,9 +281,9 @@ def _on_cm_change(self, cm_ns): # might have controllers with the same name but different # configurations. Clearing forces controller re-discovery self._widget.jtc_combo.clear() - self._update_jtc_list() else: self._list_controllers = None + self._update_jtc_list() def _on_jtc_change(self, jtc_name): self._unload_jtc()