Minor bug fix in rqt_joint_trajectory_controller

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 = {}

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.__ns_checked =[]
 
     def shutdown_plugin(self):
         self._update_cmd_timer.stop()
@@ -239,8 +240,21 @@ 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
+        try:
+            __description = rospy.get_param('robot_description')
+            if running_jtc and not self._robot_joint_limits:
+                self._robot_joint_limits = get_joint_limits(description=__description)
+        except KeyError:
+            rospy.loginfo('Could not find robot_description parameter')
+        ns=self._cm_ns.rsplit('/', 1)[0]
+        if ns not in self.__ns_checked:
+            try:
+                __description = rospy.get_param('{}/robot_description'.format(ns))
+                for _jnt, _lims in  get_joint_limits(description=__description).iteritems():
+                    self._robot_joint_limits[_jnt] = _lims
+                self.__ns_checked.append(ns)
+            except KeyError:
+                rospy.loginfo('Could not find a valid robot_description parameter in namespace {}'.format(ns))
         valid_jtc = []
         for jtc_info in running_jtc:
             has_limits = all(name in self._robot_joint_limits
@@ -268,9 +282,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()