Merge pull request RethinkRobotics#2 from RethinkRobotics/limb_joint_…

…names

Limb.py joint_names fetched from ROS Param

intera_interface/src/intera_interface/limb.py

@@ -56,27 +56,28 @@ class Limb(object):
     Point = collections.namedtuple('Point', ['x', 'y', 'z'])
     Quaternion = collections.namedtuple('Quaternion', ['x', 'y', 'z', 'w'])
 
-    def __init__(self, limb):
+    def __init__(self, limb="right"):
         """
         Constructor.
 
         @type limb: str
         @param limb: limb to interface
         """
+        try:
+            joint_names = rospy.get_param(
+                            "robot_config/{0}_config/joint_names".format(limb))
+        except KeyError:
+            rospy.logerr(("intera_interface:Limb cannot detect joint_names for"
+                          " arm \"{0}\". Limb init() failed.").format(limb))
+            return
         self.name = limb
         self._joint_angle = dict()
         self._joint_velocity = dict()
         self._joint_effort = dict()
         self._cartesian_pose = dict()
         self._cartesian_velocity = dict()
         self._cartesian_effort = dict()
-
-        self._joint_names = {
-            'left': ['left_s0', 'left_s1', 'left_e0', 'left_e1',
-                     'left_w0', 'left_w1', 'left_w2'],
-            'right': ['right_s0', 'right_s1', 'right_e0', 'right_e1',
-                      'right_w0', 'right_w1', 'right_w2']
-            }
+        self._joint_names = { limb: joint_names }
 
         ns = '/robot/limb/' + limb + '/'
 
@@ -118,7 +119,7 @@ def __init__(self, limb):
         err_msg = ("%s limb init failed to get current joint_states "
                    "from %s") % (self.name.capitalize(), joint_state_topic)
         intera_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
-                                 timeout_msg=err_msg)
+                                 timeout_msg=err_msg, timeout=5.0)
         err_msg = ("%s limb init failed to get current endpoint_state "
                    "from %s") % (self.name.capitalize(), ns + 'endpoint_state')
         intera_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,