def clear_error(self):
try:
# clear error
req = bcapRequest()
req.func_id = 17
req.vntArgs = [variant() for _ in range(2)]
req.vntArgs[0].vt = 3
req.vntArgs[0].value = self._bcap_handle
req.vntArgs[1].vt = 8
req.vntArgs[1].value = "ClearError"
resp = self._bcap_srv(req)
if resp.HRESULT != 0:
rospy.logerr("Failed to clear error: {}.".format(resp.HRESULT))
return False
else:
rospy.loginfo("Clear error.")
self._rate.sleep()
return True
except rospy.ServiceException as e:
rospy.logerr("Failed to clear error: {}.".format(e))
return False
def move_arm(self, x, y, z, alpha, beta, gamma, robot_handle):
try:
# Get bcap service handle
req = bcapRequest()
req.func_id = 72
req.vntArgs = [variant() for _ in range(4)]
req.vntArgs[0].vt = 3
req.vntArgs[0].value = robot_handle
req.vntArgs[1].vt = 3
req.vntArgs[1].value = "1"
req.vntArgs[2].vt = 8
req.vntArgs[2].value = "P(" + str(x) + "," + str(y) + "," + str(z) + "," \
+ str(alpha) + "," + str(beta) + "," + str(gamma) + ")"
req.vntArgs[3].vt = 8
req.vntArgs[3].value = ""
resp = self._bcap_srv(req)
if resp.HRESULT != 0:
rospy.logerr("Failed to move arm: {}.".format(resp.HRESULT))
return False
else:
rospy.loginfo("Move arm")
self._rate.sleep()
return True
except rospy.ServiceException as e:
rospy.logerr("Failed to move arm: {}.".format(e))
return False
def close_gripper(self):
try:
# close gripper
req = bcapRequest()
req.func_id = 17
req.vntArgs = [variant() for _ in range(3)]
req.vntArgs[0].vt = 3
req.vntArgs[0].value = self._bcap_handle
req.vntArgs[1].vt = 8
req.vntArgs[1].value = "HandMoveA"
req.vntArgs[2].vt = 8195
req.vntArgs[2].value = "20, 100"
resp = self._bcap_srv(req)
if resp.HRESULT != 0:
rospy.logerr("Failed to close gripper: {}.".format(
resp.HRESULT))
return False
else:
rospy.loginfo("Close gripper.")
self._rate.sleep()
self._open = False
return True
except rospy.ServiceException as e:
rospy.logerr("Failed to close gripper: {}.".format(e))
return False
def motor_off(self, robot_handle):
try:
# Get bcap service handle
req = bcapRequest()
req.func_id = 64
req.vntArgs = [variant() for _ in range(3)]
req.vntArgs[0].vt = 3
req.vntArgs[0].value = robot_handle
req.vntArgs[1].vt = 8
req.vntArgs[1].value = "Motor"
req.vntArgs[2].vt = 8
req.vntArgs[2].value = "0"
resp = self._bcap_srv(req)
if resp.HRESULT != 0:
rospy.logerr("Failed to bring down motor: {}.".format(
resp.HRESULT))
return False
else:
rospy.loginfo("Motor off")
self._rate.sleep()
return True
except rospy.ServiceException as e:
rospy.logerr("Failed to bring down motor: {}.".format(e))
return False
def give_arm(self, robot_handle):
try:
# Get bcap service handle
req = bcapRequest()
req.func_id = 64
req.vntArgs = [variant() for _ in range(3)]
req.vntArgs[0].vt = 3
req.vntArgs[0].value = robot_handle
req.vntArgs[1].vt = 8
req.vntArgs[1].value = "TakeArm"
req.vntArgs[2].vt = 8
req.vntArgs[2].value = ""
resp = self._bcap_srv(req)
if resp.HRESULT != 0:
rospy.logerr("Failed to give arm: {}.".format(resp.HRESULT))
return False
else:
rospy.loginfo("Give arm")
self._rate.sleep()
return True
except rospy.ServiceException as e:
rospy.logerr("Failed to give arm: {}.".format(e))
return False
def add_robot(self):
try:
# Get bcap service handle
req = bcapRequest()
req.func_id = 7
req.vntArgs = [variant() for _ in range(3)]
req.vntArgs[0].vt = 3
req.vntArgs[0].value = self._bcap_handle
req.vntArgs[1].vt = 8
req.vntArgs[1].value = "Robot0"
req.vntArgs[2].vt = 8
req.vntArgs[2].value = ""
resp = self._bcap_srv(req)
if resp.HRESULT != 0:
rospy.logerr("Failed to add robot: {}.".format(resp.HRESULT))
return False
else:
self._robot_handle = resp.vntRet.value
rospy.loginfo("Get the robot handle: {}".format(
self._robot_handle))
self._rate.sleep()
return True
except rospy.ServiceException as e:
rospy.logerr("Failed to add robot: {}.".format(e))
return False
def powerup_controller(self):
try:
# Get bcap service handle
req = bcapRequest()
req.func_id = 3
req.vntArgs = [variant() for _ in range(4)]
req.vntArgs[0].vt = 8
req.vntArgs[0].value = "b-CAP"
req.vntArgs[1].vt = 8
req.vntArgs[1].value = "CaoProv.DENSO.VRC"
req.vntArgs[2].vt = 8
req.vntArgs[2].value = "localhost"
req.vntArgs[3].vt = 8
req.vntArgs[3].value = ""
resp = self._bcap_srv(req)
if resp.HRESULT != 0:
rospy.logerr("Failed to get controller handle: {}.".format(
resp.HRESULT))
return False
else:
self._bcap_handle = resp.vntRet.value
rospy.loginfo("Get the controller handle: {}".format(
self._bcap_handle))
self._rate.sleep()
return True
except rospy.ServiceException as e:
rospy.logerr("Failed to get controller handle: {}.".format(e))
return False
def release_controller(self, controller_handle):
try:
# release controller
req = bcapRequest()
req.func_id = 4
req.vntArgs = [variant() for _ in range(1)]
req.vntArgs[0].vt = 3
req.vntArgs[0].value = controller_handle
self._bcap_srv(req)
rospy.loginfo("release controller.")
self._rate.sleep()
return True
except rospy.ServiceException as e:
rospy.logerr("Failed to release controller: {}.".format(e))
return False
def release_robot(self, robot_handle):
try:
# release controller
req = bcapRequest()
req.func_id = 84
req.vntArgs = [variant() for _ in range(1)]
req.vntArgs[0].vt = 3
req.vntArgs[0].value = robot_handle
resp = self._bcap_srv(req)
if resp.HRESULT != 0:
rospy.logerr("Failed to release robot: {}.".format(
resp.HRESULT))
return False
else:
rospy.loginfo("Release robot")
self._rate.sleep()
return True
except rospy.ServiceException as e:
rospy.logerr("Failed to release robot: {}.".format(e))
return False
