Exemple #1
0
    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
Exemple #2
0
    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
Exemple #3
0
    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
Exemple #4
0
    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
Exemple #5
0
    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
Exemple #6
0
    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
Exemple #7
0
    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
Exemple #9
0
    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