[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

@@ -392,13 +392,37 @@ 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. servos that are off).
+        '''
+        # If you're using real robot AND servos are off, simply return False
+        # and indicate why.
+        joints_servooff = None
+        if self.simulation_mode:
+            print('On simulation so servos are always on.')
+        if not self.simulation_mode:
+            joints_servooff = self.isServoOn(joint_examined, return_servos=True)
+            if joints_servooff:
+                print('One or more servos are off: {}'.format(joints_servooff))
+            else:
+                print('All servos are on.')
+        return joints_servooff
+
     def goInitial(self, tm=7, wait=True, init_pose_type=0):
         '''
         Move arms to the predefined (as member variable) "initialized" pose.
@@ -414,24 +438,32 @@ def goInitial(self, tm=7, wait=True, init_pose_type=0):
                                1: factory init pose (specified as
                                   _InitialPose_Factory)
         '''
+        servos_not_ready = self._servo_ready()
+
         if init_pose_type == self.INITPOS_TYPE_FACTORY:
             _INITPOSE = self._InitialPose_Factory
         else:
             _INITPOSE = self._InitialPose
 
         ret = True
+        joints_failed = None
         for i in range(len(self.Groups)):
             # radangles = [x/180.0*math.pi for x in self._InitialPose[i]]
             print(
                 '{}, JntAnglesOfGr={}, INITPOSE[i]={}, tm={}, wait={}'.format(
                     self.configurator_name, self.Groups[i][0], _INITPOSE[i],
                     tm, wait))
-            ret &= self.setJointAnglesOfGroup(self.Groups[i][0],
+            ret_joint = self.setJointAnglesOfGroup(self.Groups[i][0],
                                               _INITPOSE[i],
                                               tm, wait=False)
+            if not ret_joint:
+                joints_failed.extend(self.Groups[i][0])
+            ret &= ret_joint
         if wait:
             for i in range(len(self.Groups)):
                 self.seq_svc.waitInterpolationOfGroup(self.Groups[i][0])
+        if not ret:
+            print('Joints failed to execute the command: {}'.format(joints_failed))
         return ret
 
     def getRTCList(self):
@@ -606,7 +638,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.
@@ -617,25 +649,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,22 @@ 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).
+        '''
+        # 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.
+        self.robot.simulation_mode = False
+        # We want to test cases where the tests fail, but as mentioned above
+        # it's not yet figured out how. So here just simply test if the methods return True,
+        # which should always happen on simulation.
+        self.assertTrue(self.robot.isServoOn('all'))
+        self.assertIsNotNone(self.robot.isServoOn(return_servos=True))
+
+
 if __name__ == '__main__':
     import rostest
     rostest.rosrun(PKG, 'test_hronx_limb', TestHiroLimb)