diff --git a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py index 5471469e..1d0d5584 100644 --- a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py +++ b/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py @@ -352,6 +352,11 @@ 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) @@ -359,6 +364,25 @@ def goOffPose(self, tm=7): 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. @@ -374,24 +398,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): @@ -566,7 +598,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. @@ -577,25 +609,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): diff --git a/hironx_ros_bridge/test/test_hironx_limb.py b/hironx_ros_bridge/test/test_hironx_limb.py index a8c095bb..e208fd7a 100755 --- a/hironx_ros_bridge/test/test_hironx_limb.py +++ b/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)