def getBody(id):
_wait_for_service(GraspitCommander.GRASPIT_NODE_NAME + 'getBody')
serviceProxy = rospy.ServiceProxy(GraspitCommander.GRASPIT_NODE_NAME + 'getBody', GetBody)
body = serviceProxy(id)
if body.result is GetBody._response_class.RESULT_SUCCESS:
return body
elif body.result is GetBody._response_class.RESULT_INVALID_ID:
raise InvalidBodyIDException(id)
def getDistance(id1, id2):
_wait_for_service(GraspitCommander.GRASPIT_NODE_NAME + 'getDistance')
serviceProxy = rospy.ServiceProxy(
GraspitCommander.GRASPIT_NODE_NAME + 'getDistance', GetDistance)
distance = serviceProxy(id1, id2)
if distance.result is GetDistance._response_class.RESULT_SUCCESS:
return distance
elif distance.result is GetDistance._response_class.RESULT_INVALID_ID:
raise InvalidBodyIDException()
def toggleCollisions(enableCollisions, id1, id2):
_wait_for_service(GraspitCommander.GRASPIT_NODE_NAME +
'toggleCollisions')
serviceProxy = rospy.ServiceProxy(
GraspitCommander.GRASPIT_NODE_NAME + 'toggleCollisions',
ToggleCollisions)
result = serviceProxy(enableCollisions, id1, id2)
if result.result is ToggleCollisions._response_class.RESULT_SUCCESS:
return
elif result.result is ToggleCollisions._response_class.RESULT_INVALID_ID:
raise InvalidBodyIDException()
def setBodyPose(id, pose):
_wait_for_service(GraspitCommander.GRASPIT_NODE_NAME + 'setBodyPose')
serviceProxy = rospy.ServiceProxy(GraspitCommander.GRASPIT_NODE_NAME + 'setBodyPose', SetBodyPose)
result = serviceProxy(id, pose)
if result.result is SetBodyPose._response_class.RESULT_SUCCESS:
return
elif result.result is SetBodyPose._response_class.RESULT_INVALID_ID:
raise InvalidBodyIDException(id)
elif result.result is SetBodyPose._response_class.RESULT_INVALID_POSE:
raise InvalidBodyPoseException()
elif result.result is SetBodyPose._response_class.RESULT_ROBOT_IN_COLLISION:
raise BodyCollisionException()