[hironx_client, test limb] isServoOn returns list of joint names that…

… are off. Add unit test for isServoOn.

hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py

@@ -45,6 +45,7 @@
 import OpenRTM_aist.RTM_IDL
 import rtm
 from waitInput import waitInputConfirm, waitInputSelect
+import rospy
 
 
 SWITCH_ON = OpenHRP.RobotHardwareService.SWITCH_ON
@@ -185,13 +186,38 @@ def goOffPose(self, tm=7):
         @type tm: float
         @param tm: Second to complete.
         '''
+        # If you're using real robot AND servos are off, simply return False
+        # and indicate why.
+        if (not self.simulation_mode) and (not self.isServoOn('any')):
+            return False
+
         for i in range(len(self.Groups)):
             self.setJointAnglesOfGroup(self.Groups[i][0], self.OffPose[i], tm,
                                        wait=False)
         for i in range(len(self.Groups)):
             self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0])
         self.servoOff(wait=False)
 
+    def _servo_ready(self, joint_examined='any'):
+        '''
+        @param joint_examined: Same as `jname` in HIRONX.isServoOn
+        @rtype: [str]
+        @return List of joints that are not ready (e.g. servo off).
+        '''
+        # If you're using real robot AND servos are off, simply return False
+        # and indicate why.
+        result = None
+        if self.simulation_mode:
+            rospy.loginfo('On simulation so servos are always on.')
+        if not self.simulation_mode:
+            joints_servooff = self.isServoOn(joint_examined, return_servos=True)
+            if joints_servooff:
+                rospy.logerr('One or more servos are off.')
+                result = joints_servooff
+            else:
+                pass
+        return result
+
     def goInitial(self, tm=7, wait=True, init_pose_type=0):
         '''
         Move arms to the predefined (as member variable) "initialized" pose.
@@ -207,6 +233,8 @@ def goInitial(self, tm=7, wait=True, init_pose_type=0):
                                1: factory init pose (specified as
                                   _InitialPose_Factory)
         '''
+        self._servo_ready()
+
         if init_pose_type == self.INITPOS_TYPE_FACTORY:
             _INITPOSE = self._InitialPose_Factory
         else:
@@ -420,7 +448,7 @@ def isCalibDone(self):
                     return False
         return True
 
-    def isServoOn(self, jname='any'):
+    def isServoOn(self, jname='any', return_servos=False):
         '''
         Check whether servo control has been turned on. Check is done by
         HIRONX.getActualState().servoState.
@@ -431,25 +459,38 @@ def isServoOn(self, jname='any'):
                       Reserved values:
                       - any: This command will check all servos available.
                       - all: Same as 'any'.
-        @rtype bool
-        @return: If jname is specified either 'any' or 'all', return False
-                 if the control of any of servos isn't available.
+        @type return_servos: bool
+        @param return_servos: Set True to return the tuple (see @return section).
+        @rtype bool (or [str] if return_servos is True)
+        @return: - If jname is specified either 'any' or 'all', return False
+                   if the control of any of servos isn't available.
+                 - If return_servos is True, list of the joint names that are not
+                   ready to take commands.
         '''
         if self.simulation_mode:
             return True
         else:
             s_s = self.getActualState().servoState
+            joints_off = []
             if jname.lower() == 'any' or jname.lower() == 'all':
                 for s in s_s:
                     # print self.configurator_name, 's = ', s
                     if (s[0] & 2) == 0:
-                        return False
+                        if return_servos:
+                            joints_off.append(s[0])
             else:
                 jid = eval('self.' + jname)
                 print self.configurator_name, s_s[jid]
                 if s_s[jid][0] & 1 == 0:
+                    joints_off.append(s[0])
+            # If no servos are off, return True.
+            if len(joints_off) == 0:
+                return True
+            else:
+                if return_servos:
+                    return joints_off
+                else:
                     return False
-            return True
         return False
 
     def flat2Groups(self, flatList):

hironx_ros_bridge/test/test_hironx_limb.py

@@ -226,6 +226,21 @@ def test_movejoints_neck_waist(self):
         self.robot.goInitial()
         self.assertTrue(self.robot.setTargetPoseRelative('torso', 'CHEST_JOINT0', dw=min_waist_yaw*safety_coeffiecient, tm=durtion_operation))
 
+    def test_isServoOn_virtual(self):
+        '''
+        Test isServoOn method virtually (ie. without robot hardware).
+        '''
+        # Have to hack since no api in hrpsys to turn this mode.
+        # Also, in sim mode isServoOn is always True so we the mode False.
+        self.robot.simulation_mode = False
+        self.assertFalse(self.robot.isServoOn('all'))
+        self.assertIsNone(self.robot.isServoOn(return_servos=True))
+
+        # TODO @130s tried to add more tests, such as passing specific joints
+        #      as an arg to HIRONX.isServoOn, but the approached tried doesn't
+        #      work (servoOff a joint on simulation with robot.simulation_mode == False),
+        #      so solution is required here.
+
 if __name__ == '__main__':
     import rostest
     rostest.rosrun(PKG, 'test_hronx_limb', TestHiroLimb)