From c6e4cb7887b5c264fe523d66780ce60d7c8ee514 Mon Sep 17 00:00:00 2001 From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp> Date: Tue, 8 Dec 2015 14:40:02 -0800 Subject: [PATCH] [hironx_client, test limb] isServoOn returns list of joint names that are off. Add unit test for isServoOn. --- .../src/hironx_ros_bridge/hironx_client.py | 53 ++++++++++++++++--- hironx_ros_bridge/test/test_hironx_limb.py | 15 ++++++ 2 files changed, 62 insertions(+), 6 deletions(-) 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 057ef785..77462e9b 100644 --- a/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py +++ b/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,6 +186,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) @@ -192,6 +198,26 @@ 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. 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): diff --git a/hironx_ros_bridge/test/test_hironx_limb.py b/hironx_ros_bridge/test/test_hironx_limb.py index a8c095bb..c7f100bc 100755 --- a/hironx_ros_bridge/test/test_hironx_limb.py +++ b/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)