Пример #1
0
def setupMoveService():
    """Initialize the DriveMotors service

    Establish a connection with the Phidget HC Motor Control and
    then with the ROS Master as the service DriveMotors

    """

    global motorControl, minAcceleration, maxAcceleration

    try:
        motorControl = MotorControl()
    except:
        rospy.logerr("Unable to connect to Phidget HC Motor Control")
        return

    try:
        motorControl.setOnAttachHandler(mcAttached)
        motorControl.setOnDetachHandler(mcDetached)
        motorControl.setOnErrorhandler(mcError)
        motorControl.setOnCurrentChangeHandler(mcCurrentChanged)
        motorControl.setOnInputChangeHandler(mcInputChanged)
        motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
    except:
        rospy.logerr("Unable to register the handlers")
        return

    try:
        motorControl.openPhidget()
    except PhidgetException as e:
        rospy.logerr("Fail to openPhidget() %i: %s", e.code, e.details)
        return

    try:
        motorControl.waitForAttach(10000)
    except PhidgetException as e:
        rospy.logerr("Fail to attach to Phidget %i: %s", e.code, e.details)
        return

    if motorControl.isAttached():
        rospy.loginfo("Device: %s, Serial: %d, Version: %d",
                      motorControl.getDeviceName(),
                      motorControl.getSerialNum(),
                      motorControl.getDeviceVersion())

    minAcceleration = motorControl.getAccelerationMin(leftWheels)
    maxAcceleration = motorControl.getAccelerationMax(leftWheels)

    rospy.init_node('DriveMotors', log_level=rospy.DEBUG)

    phidgetMotorService = rospy.Service('DriveMotors', Move, move)

    rospy.spin()
Пример #2
0
class PhidgetMotorController:

    def __init__(
        self,
        leftMotorId,
        rightMotorId,
        leftSignAdjust,
        rightSignAdjust
        ):

        self.leftWheels = leftMotorId
        self.rightWheels = rightMotorId
        self.defaultMotorSpeed = 100.0
        self.motorMaxSpeed = 100
        self.motorMinSpeed = 0
        self.leftSignAdjust = leftSignAdjust
        self.rightSignAdjust = rightSignAdjust
        self.whichMotorFirst = self.rightWheels

        self.motorControl = MotorControl()

        self.motorControl.setOnAttachHandler(self.mcAttached)
        self.motorControl.setOnDetachHandler(self.mcDetached)
        self.motorControl.setOnErrorhandler(self.mcError)
        self.motorControl.setOnCurrentChangeHandler(self.mcCurrentChanged)
        self.motorControl.setOnInputChangeHandler(self.mcInputChanged)
        self.motorControl.setOnVelocityChangeHandler(self.mcVelocityChanged)

        try:
            self.motorControl.openPhidget()

        except PhidgetException, e:
            print "openPhidget() failed"
            print "code: %d" % e.code
            print "message", e.message

            raise

        try:
            self.motorControl.waitForAttach(10000)

        except PhidgetException, e:
            print "waitForAttach() failed"
            print "code: %d" % e.code
            print "message", e.message
    
            raise
Пример #3
0
    source = e.device
    print("Motor Control %i: Input %i State: %s" % (source.getSerialNum(), e.index, e.state))

def motorControlVelocityChanged(e):
    source = e.device
    print("Motor Control %i: Motor %i Current Velocity: %f" % (source.getSerialNum(), e.index, e.velocity))

#Main Program Code
try:
	#logging example, uncomment to generate a log file
    #motorControl.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, "phidgetlog.log")

    motorControl.setOnAttachHandler(motorControlAttached)
    motorControl.setOnDetachHandler(motorControlDetached)
    motorControl.setOnErrorhandler(motorControlError)
    motorControl.setOnCurrentChangeHandler(motorControlCurrentChanged)
    motorControl.setOnInputChangeHandler(motorControlInputChanged)
    motorControl.setOnVelocityChangeHandler(motorControlVelocityChanged)
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))
    print("Exiting....")
    exit(1)

print("Opening phidget object....")

try:
    motorControl.openPhidget()
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))
    print("Exiting....")
    exit(1)
def setupMoveService():
    """Initialize the PhidgetMotor service

    Establish a connection with the Phidget HC Motor Control and
    then with the ROS Master as the service PhidgetMotor

    """
    node_name = 'PhidgetMotor'

    rospy.init_node(node_name, log_level=rospy.DEBUG)

    serial_no = rospy.get_param("~serial_no", 0)
    if not serial_no == 0:
        rospy.loginfo("Using motor controller with serial number %d",
                      serial_no)
    else:
        rospy.loginfo(
            "No serial number specified.  This is fine for systems with one controller, but may behave unpredictably for systems with multiple motor controllers"
        )

    global motorControl, motorControlRight, minAcceleration, maxAcceleration, timer, motors_inverted, phidget1065, rightWheels, posdataPub
    timer = 0
    try:
        motorControl = MotorControl()
    except:
        rospy.logerr("Unable to connect to Phidget HC Motor Control")
        return

    try:
        motorControl.setOnAttachHandler(mcAttached)
        motorControl.setOnDetachHandler(mcDetached)
        motorControl.setOnErrorhandler(mcError)
        motorControl.setOnCurrentChangeHandler(mcCurrentChanged)
        motorControl.setOnInputChangeHandler(mcInputChanged)
        motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
        if serial_no != 0:
            motorControl.openPhidget(serial_no)
        else:
            motorControl.openPhidget()

        #attach the board
        motorControl.waitForAttach(10000)
        """use the function getMotorCount() to know how many motors the Phidget board can take

        if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. The corobot_phidgetIK handles the Phidget encoder board that is 
        separated of the phidget 1064.
        if the result is equal to 1, we have two phidget 1065 boards. The one with serial number that is the lowest will be the left will, the other the right weel. The encoder has to be handled 
        in this file as it is part of the 1065 board. 

        """
        if motorControl.getMotorCount() == 1:
            phidget1065 = True
            rightWheels = 0
            motorControlRight.setOnAttachHandler(mcAttached)
            motorControlRight.setOnDetachHandler(mcDetached)
            motorControlRight.setOnErrorhandler(mcError)
            motorControlRight.setOnCurrentChangeHandler(mcCurrentChanged)
            motorControlRight.setOnInputChangeHandler(mcInputChanged)
            motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)

            if motorControl.getSerialNum() > motorControlRight.getSerialNum():
                """ As a rule, we need the serial number of the left board to be lower than for the right board. This is for consistancy for all the robots
                """
                motorControlTemp = motorControl
                motorControl = motorControlRight
                motorControlRight = motorControlTemp

#Set up the encoders handler
            motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
            motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)

            #attach the board
            motorControlRight.waitForAttach(10000)
    except PhidgetException as e:
        rospy.logerr("Unable to register the handlers: %i: %s", e.code,
                     e.details)
        return
    except AttributeError as e:
        rospy.logerr("Unable to register the handlers: %s", e)
        return
    except:
        rospy.logerr("Unable to register the handlers: %s", sys.exc_info()[0])
        return

    if motorControl.isAttached():
        rospy.loginfo("Device: %s, Serial: %d, Version: %d",
                      motorControl.getDeviceName(),
                      motorControl.getSerialNum(),
                      motorControl.getDeviceVersion())
    if phidget1065 == True:
        if motorControlRight.isAttached():
            rospy.loginfo("Device: %s, Serial: %d, Version: %d",
                          motorControlRight.getDeviceName(),
                          motorControlRight.getSerialNum(),
                          motorControlRight.getDeviceVersion())

    minAcceleration = motorControl.getAccelerationMin(leftWheels)
    maxAcceleration = motorControl.getAccelerationMax(leftWheels)

    motors_inverted = rospy.get_param('~motors_inverted', False)

    phidgetMotorTopic = rospy.Subscriber(node_name, MotorCommand, move)
    phidgetMotorService = rospy.Service(node_name, Move, move)
    if phidget1065 == True:
        posdataPub = rospy.Publisher("position_data", PosMsg)
    rospy.spin()
Пример #5
0
class IOTools:
    def __init__(self, onRobot):
        self.onRobot = onRobot

        # Camera
        self._openCam=False
        
        # Motor Control
        self._snMot=-1
        self._openMot=False
        self._attachedMot=False
        self._cur = [0, 0]

        # Servo
        self._snSer=-1
        self._openSer=False
        self._attachedSer=False
        self._limits = [0, 0]
        
        # IF Kit
        self._snIF=-1
        self._openIF=False
        self._attachedIF=False        
        self._inp = [0, 0, 0, 0, 0, 0, 0, 0]
        self._sen = [0, 0, 0, 0, 0, 0, 0, 0]
        
        # LEDs
        self._fistTime = True
        self._status = [0,0,0]
        self._mod = [8, 1, 1]
        self._rep = [-1, 5, 6]
        self._val = [True, False, False]
        self._ofs = [0, 0, 0]
        self._updaterThread = threading.Thread(target=self.__updateLED)
        self._stop = False
        self._updaterThread.setDaemon(True)
        self._updaterThread.start()

    def destroy(self):
        # LEDs
        self._stop = True
        if self._attachedIF:
            self._interfaceKit.setOutputState(0, 0)
            self._interfaceKit.setOutputState(1, 0)
            self._interfaceKit.setOutputState(2, 0)
        # Servo
        self.servoDisengage()
        # Camera
        self._openCam = False
        if self._openCam:
            self._cap.release()

    def open(self):
        self.__openIF()
        self.__openMot()
        self.__openSer()
        self.__openCam()

###################### Camera ######################
        
    def __openCam(self):
        if not os.path.exists('/dev/video0'):
            return False
        self._cap = cv2.VideoCapture()
        if not self._cap.open(-1):
            return False
        self._openCam = True

    def cameraGrab(self):
        if self._openCam:
            return self._cap.grab()
        else:
            return False

    def cameraRead(self):
        if self._openCam:
            (ret, img)=self._cap.retrieve()
            return img
        else:
            return False

    def cameraSetResolution(self, sz):
        """

        :rtype : object
        """
        if self._openCam:
            sz=sz.lower()
            if sz=='low':
                self._cap.set(3,160)
                self._cap.set(4,120)
            if sz=='medium':
                self._cap.set(3,640)
                self._cap.set(4,480)
            if sz=='high':
                self._cap.set(3,800)
                self._cap.set(4,600)
            if sz=='full':
                self._cap.set(3,1280)
                self._cap.set(4,720)

    def imshow(self, wnd, img):
        if not self.onRobot:
            if img.__class__ != numpy.ndarray:
                print "imshow - invalid image"
                return False
            else:
                cv2.imshow(wnd,img)
                cv2.waitKey(5)
        
####################### Servo ######################

    def __openSer(self):
        try:
            self._advancedServo = AdvancedServo()
        except RuntimeError as e:
            print("Servo - Runtime Exception: %s" % e.details)
            return False        

        try:
            self._advancedServo.setOnAttachHandler(self.__onAttachedSer)
            self._advancedServo.setOnDetachHandler(self.__onDetachedSer)
            self._advancedServo.setOnErrorhandler(self.__onErrorSer)
        except PhidgetException as e:
            print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        
        try:
            self._advancedServo.openPhidget()
        except PhidgetException as e:
            print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        self._openSer=True
        return True

    def __onAttachedSer(self,e):
        self._snSer = e.device.getSerialNum()
        self._advancedServo.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD)
        self._advancedServo.setAcceleration(0, self._advancedServo.getAccelerationMax(0))
        self._advancedServo.setVelocityLimit(0, self._advancedServo.getVelocityMax(0))
        self._limits[0] = self._advancedServo.getPositionMin(0)
        self._limits[1] = self._advancedServo.getPositionMax(0)
        print("Servo %i Attached! Range: %f - %f" % (self._snSer, self._limits[0], self._limits[1]))
        self._attachedSer=True

    def __onDetachedSer(self,e ):
        print("Servo %i Detached!" % (self._snSer))
        self._snSer = -1
        self._attachedSer=False
              
    def __onErrorSer(self, e):
        try:
            source = e.device
            print("Servo %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
        except PhidgetException as e:
            print("Servo - Phidget Exception %i: %s" % (e.code, e.details))

    def __closeSer(self):
        if self._openSer==True:
            self.servoDisengage()
            self._advancedServo.closePhidget()

    def servoEngage(self):
        if self._attachedSer==True:
            self._advancedServo.setEngaged(0, True)

    def servoDisengage(self):
        if self._attachedSer==True:
            self._advancedServo.setEngaged(0, False)

    def servoSet(self, pos):
        if self._attachedSer==True:
            self._advancedServo.setPosition(0, min(max(pos,self._limits[0]),self._limits[1]))
        
############### Motor Control ######################

    def __openMot(self):
        try:
            self._motorControl = MotorControl()
        except RuntimeError as e:
            print("Motor Control - Runtime Exception: %s" % e.details)
            return False        

        try:
            self._motorControl.setOnAttachHandler(self.__onAttachedMot)
            self._motorControl.setOnDetachHandler(self.__onDetachedMot)
            self._motorControl.setOnErrorhandler(self.__onErrorMot)
            self._motorControl.setOnCurrentChangeHandler(self.__onCurrentChangedMot)
        except PhidgetException as e:
            print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        
        try:
            self._motorControl.openPhidget()
        except PhidgetException as e:
            print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        self._openMot=True
        return True

    def __onAttachedMot(self,e):
        self._snMot = e.device.getSerialNum()
        print("Motor Control %i Attached!" % (self._snMot))
        self._attachedMot=True

    def __onDetachedMot(self,e ):
        print("Motor Control %i Detached!" % (self._snMot))
        self._snMot = -1
        self._attachedMot=False
              
    def __onErrorMot(self, e):
        try:
            source = e.device
            print("Motor Control %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
        except PhidgetException as e:
            print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details))

    def __onCurrentChangedMot(self, e):
        self._cur[e.index] = e.current

    def __closeMot(self):
        if self._openMot==True:
            self._motorControl.closePhidget()

    def setMotors(self, speed1=0.0, speed2=0.0, acceleration1=100.0, acceleration2=100.0 ):
        if self._openMot==True:
            self._motorControl.setAcceleration(0, acceleration1)
            self._motorControl.setVelocity(0, speed1)
            self._motorControl.setAcceleration(1, acceleration2)
            self._motorControl.setVelocity(1, speed2)
            
############### Interface Kit ######################

    def __closeIF(self):
        if self._openIF==True:
            self._interfaceKit.closePhidget()
            
    def __openIF(self):
        try:
            self._interfaceKit = InterfaceKit()
        except RuntimeError as e:
            print("IF Kit - Runtime Exception: %s" % e.details)
            return False
        try:
            self._interfaceKit.setOnAttachHandler(self.__onAttachedIF)
            self._interfaceKit.setOnDetachHandler(self.__onDetachedIF)
            self._interfaceKit.setOnErrorhandler(self.__onErrorIF)
            self._interfaceKit.setOnInputChangeHandler(self.__onInputChangedIF)
            self._interfaceKit.setOnSensorChangeHandler(self.__onSensorChangedIF)
        except PhidgetException as e:
            print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        try:
            self._interfaceKit.openPhidget()
        except PhidgetException as e:
            print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        self._openIF=True
        return True
        
    def __onAttachedIF(self,e):
        self._snIF = e.device.getSerialNum()
        print("InterfaceKit %i Attached!" % (self._snIF))
        self._attachedIF=True
        if self._fistTime:
            for i in range(0,3):
                self._interfaceKit.setOutputState(0, 1)
                self._interfaceKit.setOutputState(1, 1)
                self._interfaceKit.setOutputState(2, 1)
                time.sleep(0.1)
                self._interfaceKit.setOutputState(0, 0)
                self._interfaceKit.setOutputState(1, 0)
                self._interfaceKit.setOutputState(2, 0)
                time.sleep(0.1)
        self._fistTime = False

    def __onDetachedIF(self,e ):
        print("InterfaceKit %i Detached!" % (self._snIF))
        self._snIF = -1
        self._inp = [0, 0, 0, 0, 0, 0, 0, 0]
        self._sen = [0, 0, 0, 0, 0, 0, 0, 0]
        self._attachedIF=False
              
    def __onErrorIF(self, e):
        try:
            source = e.device
            print("InterfaceKit %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
        except PhidgetException as e:
            print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))

    def __onSensorChangedIF(self, e):
        self._sen[e.index] = e.value

    def __onInputChangedIF(self, e):
        self._inp[e.index] = e.state

    def getSensors(self):
        """

        :rtype : object
        """
        return self._sen;

    def getInputs(self):
        return self._inp;

################ LEDs #######################

    def __updateLED(self):
        t=0
        while self._stop==False:
            t=(t+1)%100
            for i in range(0,3):
                self._status[i]=((t+self._ofs[i])%self._mod[i]==0) and self._val[i] and bool(self._rep[i])
                self._rep[i]=self._rep[i]-int(self._rep[i]>0 and self._status[i])
                if self._attachedIF:
                    self._interfaceKit.setOutputState(i, self._status[i])
            time.sleep(0.15)
            
    def __setModeLED(self, i, mode, hz=2, cnt=1, ofs=0):
        if mode=='on':
            self._rep[i]=-1
            self._val[i]=True
            self._ofs[i]=0
            self._mod[i]=1
        if mode=='off':
            self._rep[i]=-1
            self._val[i]=False
            self._ofs[i]=0
            self._mod[i]=1
        if mode=='flash':
            hz=min(max(hz,1),100)
            self._rep[i]=min(max(cnt,1),20)
            self._val[i]=True
            self._ofs[i]=min(max(ofs,0),hz)
            self._mod[i]=hz

    def setStatus(self, mode, hz=2, cnt=1, ofs=0):
        self.__setModeLED(1,mode, hz, cnt, ofs)            
                
    def setError(self, mode, hz=2, cnt=1, ofs=0):
        self.__setModeLED(2,mode, hz, cnt, ofs)

    def setSemaphor(self):
        self.__setModeLED(1,'flash', 2, 6, 0)
        self.__setModeLED(2,'flash', 2, 6, 1)
Пример #6
0
    print("Motor Control %i: Input %i State: %s" %
          (source.getSerialNum(), e.index, e.state))


def motorControlVelocityChanged(e):
    source = e.device
    print("Motor Control %i: Motor %i Current Velocity: %f" %
          (source.getSerialNum(), e.index, e.velocity))


#Main Program Code
try:
    motorControl.setOnAttachHandler(motorControlAttached)
    motorControl.setOnDetachHandler(motorControlDetached)
    motorControl.setOnErrorhandler(motorControlError)
    motorControl.setOnCurrentChangeHandler(motorControlCurrentChanged)
    motorControl.setOnInputChangeHandler(motorControlInputChanged)
    motorControl.setOnVelocityChangeHandler(motorControlVelocityChanged)
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))
    print("Exiting....")
    exit(1)

print("Opening phidget object....")

try:
    motorControl.openPhidget()
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))
    print("Exiting....")
    exit(1)
def setupMoveService():
    """Initialize the PhidgetLinear service

    Establish a connection with the Phidget 1065 Motor Control and
    then with the ROS Master as the service PhidgetLinear

    """

    rospy.init_node('PhidgetLinear', log_level=rospy.DEBUG)

    serial_no = rospy.get_param("~serial_no", 0)
    if not serial_no == 0:
        rospy.loginfo("Using motor controller with serial number %d",
                      serial_no)
    else:
        rospy.loginfo(
            "No serial number specified.  This is fine for systems with one controller, but may behave unpredictably for systems with multiple motor controllers"
        )

    global motorControl, minAcceleration, maxAcceleration, timer, invert_speed, posdataPub
    timer = 0
    try:
        motorControl = MotorControl()
    except:
        rospy.logerr("Unable to connect to Phidget Motor Control")
        return

    try:
        motorControl.setOnAttachHandler(mcAttached)
        motorControl.setOnDetachHandler(mcDetached)
        motorControl.setOnErrorhandler(mcError)
        motorControl.setOnCurrentChangeHandler(mcCurrentChanged)
        motorControl.setOnInputChangeHandler(mcInputChanged)
        motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
        motorControl.setOnSensorUpdateHandler(mcSensorUpdated)
        if serial_no != 0:
            motorControl.openPhidget(serial_no)
        else:
            motorControl.openPhidget()

        #attach the board
        motorControl.waitForAttach(10000)
    except PhidgetException as e:
        rospy.logerr("Unable to register the handlers: %i: %s", e.code,
                     e.details)
        return
    except AttributeError as e:
        rospy.logerr("Unable to register the handlers: %s", e)
        return
    except:
        rospy.logerr("Unable to register the handlers: %s", sys.exc_info()[0])
        return

    if motorControl.isAttached():
        rospy.loginfo("Device: %s, Serial: %d, Version: %d",
                      motorControl.getDeviceName(),
                      motorControl.getSerialNum(),
                      motorControl.getDeviceVersion())
    # ensure the motor controller attempts to brake the linear
    # when velocity is cut to 0
    motorControl.setBraking(linear, 100)
    #    print motorControl.getBraking(linear)
    minAcceleration = motorControl.getAccelerationMin(linear)
    maxAcceleration = motorControl.getAccelerationMax(linear)

    invert_speed = rospy.get_param('~invert_speed', False)
    maxPos = rospy.get_param('~max_pos', 1014)
    minPos = rospy.get_param('~min_pos', 10)

    phidgetMotorTopic = rospy.Subscriber("PhidgetLinear", LinearCommand, move)
    phidgetMotorService = rospy.Service('PhidgetLinear', Move, move)
    posdataPub = rospy.Publisher("position", UInt32, latch=True)
    rospy.spin()
Пример #8
0
class IOTools:
    def __init__(self, onRobot):
        self.onRobot = onRobot

        # Camera
        self._openCam = False

        # Motor Control
        self._snMot = -1
        self._openMot = False
        self._attachedMot = False
        self._cur = [0, 0]

        # Servo
        self._snSer = -1
        self._openSer = False
        self._attachedSer = False
        self._limits = [0, 0]

        # IF Kit
        self._snIF = -1
        self._openIF = False
        self._attachedIF = False
        self._inp = [0, 0, 0, 0, 0, 0, 0, 0]
        self._sen = [0, 0, 0, 0, 0, 0, 0, 0]

        # LEDs
        self._fistTime = True
        self._status = [0, 0, 0]
        self._mod = [8, 1, 1]
        self._rep = [-1, 5, 6]
        self._val = [True, False, False]
        self._ofs = [0, 0, 0]
        self._updaterThread = threading.Thread(target=self.__updateLED)
        self._stop = False
        self._updaterThread.setDaemon(True)
        self._updaterThread.start()

    def destroy(self):
        # LEDs
        self._stop = True
        if self._attachedIF:
            self._interfaceKit.setOutputState(0, 0)
            self._interfaceKit.setOutputState(1, 0)
            self._interfaceKit.setOutputState(2, 0)
        # Servo
        self.servoDisengage()
        # Camera
        self._openCam = False
        if self._openCam:
            self._cap.release()

    def open(self):
        self.__openIF()
        self.__openMot()
        self.__openSer()
        self.__openCam()

###################### Camera ######################

    def __openCam(self):
        if not os.path.exists('/dev/video0'):
            return False
        self._cap = cv2.VideoCapture()
        if not self._cap.open(-1):
            return False
        self._openCam = True

    def cameraGrab(self):
        if self._openCam:
            return self._cap.grab()
        else:
            return False

    def cameraRead(self):
        if self._openCam:
            (ret, img) = self._cap.retrieve()
            return img
        else:
            return False

    def cameraSetResolution(self, sz):
        if self._openCam:
            sz = sz.lower()
            if sz == 'low':
                self._cap.set(3, 160)
                self._cap.set(4, 120)
            if sz == 'medium':
                self._cap.set(3, 640)
                self._cap.set(4, 480)
            if sz == 'high':
                self._cap.set(3, 800)
                self._cap.set(4, 600)
            if sz == 'full':
                self._cap.set(3, 1280)
                self._cap.set(4, 720)

    def imshow(self, wnd, img):
        if not self.onRobot:
            if img.__class__ != numpy.ndarray:
                print "imshow - invalid image"
                return False
            else:
                cv2.imshow(wnd, img)
                cv2.waitKey(5)

####################### Servo ######################

    def __openSer(self):
        try:
            self._advancedServo = AdvancedServo()
        except RuntimeError as e:
            print("Servo - Runtime Exception: %s" % e.details)
            return False

        try:
            self._advancedServo.setOnAttachHandler(self.__onAttachedSer)
            self._advancedServo.setOnDetachHandler(self.__onDetachedSer)
            self._advancedServo.setOnErrorhandler(self.__onErrorSer)
        except PhidgetException as e:
            print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
            return False

        try:
            self._advancedServo.openPhidget()
        except PhidgetException as e:
            print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        self._openSer = True
        return True

    def __onAttachedSer(self, e):
        self._snSer = e.device.getSerialNum()
        self._advancedServo.setServoType(
            0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD)
        self._advancedServo.setAcceleration(
            0, self._advancedServo.getAccelerationMax(0))
        self._advancedServo.setVelocityLimit(
            0, self._advancedServo.getVelocityMax(0))
        self._limits[0] = self._advancedServo.getPositionMin(0)
        self._limits[1] = self._advancedServo.getPositionMax(0)
        print("Servo %i Attached! Range: %f - %f" %
              (self._snSer, self._limits[0], self._limits[1]))
        self._attachedSer = True

    def __onDetachedSer(self, e):
        print("Servo %i Detached!" % (self._snSer))
        self._snSer = -1
        self._attachedSer = False

    def __onErrorSer(self, e):
        try:
            source = e.device
            print("Servo %i: Phidget Error %i: %s" %
                  (source.getSerialNum(), e.eCode, e.description))
        except PhidgetException as e:
            print("Servo - Phidget Exception %i: %s" % (e.code, e.details))

    def __closeSer(self):
        if self._openSer == True:
            self.servoDisengage()
            self._advancedServo.closePhidget()

    def servoEngage(self):
        if self._attachedSer == True:
            self._advancedServo.setEngaged(0, True)

    def servoDisengage(self):
        if self._attachedSer == True:
            self._advancedServo.setEngaged(0, False)

    def servoSet(self, pos):
        if self._attachedSer == True:
            self._advancedServo.setPosition(
                0, min(max(pos, self._limits[0]), self._limits[1]))

############### Motor Control ######################

    def __openMot(self):
        try:
            self._motorControl = MotorControl()
        except RuntimeError as e:
            print("Motor Control - Runtime Exception: %s" % e.details)
            return False

        try:
            self._motorControl.setOnAttachHandler(self.__onAttachedMot)
            self._motorControl.setOnDetachHandler(self.__onDetachedMot)
            self._motorControl.setOnErrorhandler(self.__onErrorMot)
            self._motorControl.setOnCurrentChangeHandler(
                self.__onCurrentChangedMot)
        except PhidgetException as e:
            print("Motor Control - Phidget Exception %i: %s" %
                  (e.code, e.details))
            return False

        try:
            self._motorControl.openPhidget()
        except PhidgetException as e:
            print("Motor Control - Phidget Exception %i: %s" %
                  (e.code, e.details))
            return False
        self._openMot = True
        return True

    def __onAttachedMot(self, e):
        self._snMot = e.device.getSerialNum()
        print("Motor Control %i Attached!" % (self._snMot))
        self._attachedMot = True

    def __onDetachedMot(self, e):
        print("Motor Control %i Detached!" % (self._snMot))
        self._snMot = -1
        self._attachedMot = False

    def __onErrorMot(self, e):
        try:
            source = e.device
            print("Motor Control %i: Phidget Error %i: %s" %
                  (source.getSerialNum(), e.eCode, e.description))
        except PhidgetException as e:
            print("Motor Control - Phidget Exception %i: %s" %
                  (e.code, e.details))

    def __onCurrentChangedMot(self, e):
        self._cur[e.index] = e.current

    def __closeMot(self):
        if self._openMot == True:
            self._motorControl.closePhidget()

    def setMotors(self,
                  speed1=0.0,
                  speed2=0.0,
                  acceleration1=100.0,
                  acceleration2=100.0):
        if self._openMot == True:
            self._motorControl.setAcceleration(0, acceleration1)
            self._motorControl.setVelocity(0, speed1)
            self._motorControl.setAcceleration(1, acceleration2)
            self._motorControl.setVelocity(1, speed2)

############### Interface Kit ######################

    def __closeIF(self):
        if self._openIF == True:
            self._interfaceKit.closePhidget()

    def __openIF(self):
        try:
            self._interfaceKit = InterfaceKit()
        except RuntimeError as e:
            print("IF Kit - Runtime Exception: %s" % e.details)
            return False
        try:
            self._interfaceKit.setOnAttachHandler(self.__onAttachedIF)
            self._interfaceKit.setOnDetachHandler(self.__onDetachedIF)
            self._interfaceKit.setOnErrorhandler(self.__onErrorIF)
            self._interfaceKit.setOnInputChangeHandler(self.__onInputChangedIF)
            self._interfaceKit.setOnSensorChangeHandler(
                self.__onSensorChangedIF)
        except PhidgetException as e:
            print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        try:
            self._interfaceKit.openPhidget()
        except PhidgetException as e:
            print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        self._openIF = True
        return True

    def __onAttachedIF(self, e):
        self._snIF = e.device.getSerialNum()
        print("InterfaceKit %i Attached!" % (self._snIF))
        self._attachedIF = True
        if self._fistTime:
            for i in range(0, 3):
                self._interfaceKit.setOutputState(0, 1)
                self._interfaceKit.setOutputState(1, 1)
                self._interfaceKit.setOutputState(2, 1)
                time.sleep(0.1)
                self._interfaceKit.setOutputState(0, 0)
                self._interfaceKit.setOutputState(1, 0)
                self._interfaceKit.setOutputState(2, 0)
                time.sleep(0.1)
        self._fistTime = False

    def __onDetachedIF(self, e):
        print("InterfaceKit %i Detached!" % (self._snIF))
        self._snIF = -1
        self._inp = [0, 0, 0, 0, 0, 0, 0, 0]
        self._sen = [0, 0, 0, 0, 0, 0, 0, 0]
        self._attachedIF = False

    def __onErrorIF(self, e):
        try:
            source = e.device
            print("InterfaceKit %i: Phidget Error %i: %s" %
                  (source.getSerialNum(), e.eCode, e.description))
        except PhidgetException as e:
            print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))

    def __onSensorChangedIF(self, e):
        self._sen[e.index] = e.value

    def __onInputChangedIF(self, e):
        self._inp[e.index] = e.state

    def getSensors(self):
        return self._sen

    def getInputs(self):
        return self._inp

################ LEDs #######################

    def __updateLED(self):
        t = 0
        while self._stop == False:
            t = (t + 1) % 100
            for i in range(0, 3):
                self._status[i] = ((t + self._ofs[i]) % self._mod[i]
                                   == 0) and self._val[i] and bool(
                                       self._rep[i])
                self._rep[i] = self._rep[i] - int(self._rep[i] > 0
                                                  and self._status[i])
                if self._attachedIF:
                    self._interfaceKit.setOutputState(i, self._status[i])
            time.sleep(0.15)

    def __setModeLED(self, i, mode, hz=2, cnt=1, ofs=0):
        if mode == 'on':
            self._rep[i] = -1
            self._val[i] = True
            self._ofs[i] = 0
            self._mod[i] = 1
        if mode == 'off':
            self._rep[i] = -1
            self._val[i] = False
            self._ofs[i] = 0
            self._mod[i] = 1
        if mode == 'flash':
            hz = min(max(hz, 1), 100)
            self._rep[i] = min(max(cnt, 1), 20)
            self._val[i] = True
            self._ofs[i] = min(max(ofs, 0), hz)
            self._mod[i] = hz

    def setStatus(self, mode, hz=2, cnt=1, ofs=0):
        self.__setModeLED(1, mode, hz, cnt, ofs)

    def setError(self, mode, hz=2, cnt=1, ofs=0):
        self.__setModeLED(2, mode, hz, cnt, ofs)

    def setSemaphor(self):
        self.__setModeLED(1, 'flash', 2, 6, 0)
        self.__setModeLED(2, 'flash', 2, 6, 1)
Пример #9
0
def setupMoveService():
    """Initialize the PhidgetMotor service

    Establish a connection with the Phidget HC Motor Control and
    then with the ROS Master as the service PhidgetMotor

    """

    rospy.init_node(
            'PhidgetMotor',
            log_level = rospy.DEBUG
            )

    global motorControl, motorControlRight, minAcceleration, maxAcceleration, timer, motors_inverted, phidget1065, rightWheels, posdataPub
    timer = 0
    try:
        motorControl = MotorControl()
    except:
        rospy.logerr("Unable to connect to Phidget HC Motor Control")
     	return

    try:
        motorControl.setOnAttachHandler(mcAttached)
        motorControl.setOnDetachHandler(mcDetached)
        motorControl.setOnErrorhandler(mcError)
        motorControl.setOnCurrentChangeHandler(mcCurrentChanged)
        motorControl.setOnInputChangeHandler(mcInputChanged)
        motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
        motorControl.openPhidget()

        #attach the board
        motorControl.waitForAttach(10000)
        """use the function getMotorCount() to know how many motors the Phidget board can take

        if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. The corobot_phidgetIK handles the Phidget encoder board that is 
        separated of the phidget 1064.
        if the result is equal to 1, we have two phidget 1065 boards. The one with serial number that is the lowest will be the left will, the other the right weel. The encoder has to be handled 
        in this file as it is part of the 1065 board. 

        """
        if motorControl.getMotorCount() == 1:
            phidget1065 = True 
            rightWheels = 0
            motorControlRight.setOnAttachHandler(mcAttached)
            motorControlRight.setOnDetachHandler(mcDetached)
            motorControlRight.setOnErrorhandler(mcError)
            motorControlRight.setOnCurrentChangeHandler(mcCurrentChanged)
            motorControlRight.setOnInputChangeHandler(mcInputChanged)
            motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)
			
            if motorControl.getSerialNum() > motorControlRight.getSerialNum(): 
                """ As a rule, we need the serial number of the left board to be lower than for the right board. This is for consistancy for all the robots
                """
                motorControlTemp = motorControl
                motorControl = motorControlRight
                motorControlRight = motorControlTemp

			#Set up the encoders handler
            motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
            motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)

            #attach the board
            motorControlRight.waitForAttach(10000)
    except PhidgetException as e:
        rospy.logerr("Unable to register the handlers: %i: %s", e.code, e.details)
        return
    except:
        rospy.logerr("Unable to register the handlers")
        return
     

    if motorControl.isAttached():
        rospy.loginfo(
                "Device: %s, Serial: %d, Version: %d",
                motorControl.getDeviceName(),
                motorControl.getSerialNum(),
                motorControl.getDeviceVersion()
                )
    if phidget1065 == True:
        if motorControlRight.isAttached():
            rospy.loginfo(
		            "Device: %s, Serial: %d, Version: %d",
		            motorControlRight.getDeviceName(),
		            motorControlRight.getSerialNum(),
		            motorControlRight.getDeviceVersion()
		            )

    minAcceleration = motorControl.getAccelerationMin(leftWheels)
    maxAcceleration = motorControl.getAccelerationMax(leftWheels)

    motors_inverted = rospy.get_param('~motors_inverted', False)

    phidgetMotorTopic = rospy.Subscriber("PhidgetMotor", MotorCommand ,move)
    phidgetMotorService = rospy.Service('PhidgetMotor',Move, move)
    if phidget1065 == True:
        posdataPub = rospy.Publisher("position_data", PosMsg)
    rospy.spin()
Пример #10
0
def AttachMotorControl(databasepath, serialNumber):
	def onAttachHandler(event):
		logString = "MotorControl Attached " + str(event.device.getSerialNum())
		#print(logString)
		DisplayAttachedDeviceInfo(event.device)

	def onDetachHandler(event):
		logString = "MotorControl Detached " + str(event.device.getSerialNum())
		#print(logString)
		DisplayDetachedDeviceInfo(event.device)

		event.device.closePhidget()

	def onErrorHandler(event):
		logString = "MotorControl Error " + str(event.device.getSerialNum()) + ", Error: " + event.description
		print(logString)
		DisplayErrorDeviceInfo(event.device)
		
	def onServerConnectHandler(event):
		logString = "MotorControl Server Connect " + str(event.device.getSerialNum())
		#print(logString)

	def onServerDisconnectHandler(event):
		logString = "MotorControl Server Disconnect " + str(event.device.getSerialNum())
		#print(logString)

	def backEMFUpdateHandler(event):
		logString = "MotorControl BackEMF Update"
		#print(logString)

		try:
			conn = sqlite3.connect(databasepath)

			#conn.execute("INSERT INTO MOTORCONTROL_BACKEMFUPDATE VALUES(NULL, DateTime('now'), ?, ?, ?)",
					#(event.device.getSerialNum(), event.index, event.voltage))

			conn.commit()
			conn.close()
		except sqlite3.Error as e:
			print "An error occurred:", e.args[0]

	def currentChangeHandler(event):
		logString = "MotorControl Current Change"
		#print(logString)

		try:
			conn = sqlite3.connect(databasepath)

			#conn.execute("INSERT INTO MOTORCONTROL_CURRENTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
					#(event.device.getSerialNum(), event.index, event.current))

			conn.commit()
			conn.close()
		except sqlite3.Error as e:
			print "An error occurred:", e.args[0]

	def currentUpdateHandler(event):
		logString = "MotorControl Current Update"
		#print(logString)

		try:
			conn = sqlite3.connect(databasepath)

			#conn.execute("INSERT INTO MOTORCONTROL_CURRENTUPDATE VALUES(NULL, DateTime('now'), ?, ?, ?)",
					#(event.device.getSerialNum(), event.index, event.current))

			conn.commit()
			conn.close()
		except sqlite3.Error as e:
			print "An error occurred:", e.args[0]

	def inputChangeHandler(event):
		logString = "MotorControl Input Change"
		#print(logString)

		try:
			conn = sqlite3.connect(databasepath)

			#conn.execute("INSERT INTO MOTORCONTROL_INPUTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
					#(event.device.getSerialNum(), event.index, event.state))

			conn.commit()
			conn.close()
		except sqlite3.Error as e:
			print "An error occurred:", e.args[0]

	def positionChangeHandler(event):
		logString = "MotorControl Position Change"
		#print(logString)

		try:
			conn = sqlite3.connect(databasepath)

			#conn.execute("INSERT INTO MOTORCONTROL_POSITIONCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
					#(event.device.getSerialNum(), event.index, event.current))

			conn.commit()
			conn.close()
		except sqlite3.Error as e:
			print "An error occurred:", e.args[0]

	def positionUpdateHandler(event):
		logString = "MotorControl Position Update"
		#print(logString)

		try:
			conn = sqlite3.connect(databasepath)

			#conn.execute("INSERT INTO MOTORCONTROL_POSITIONUPDATE VALUES(NULL, DateTime('now'), ?, ?, ?)",
					#(event.device.getSerialNum(), event.index, event.position))

			conn.commit()
			conn.close()
		except sqlite3.Error as e:
			print "An error occurred:", e.args[0]

	def sensorUpdateHandler(event):
		logString = "MotorControl Sensor Update"
		#print(logString)

		try:
			conn = sqlite3.connect(databasepath)

			#conn.execute("INSERT INTO MOTORCONTROL_SENSORUPDATE VALUES(NULL, DateTime('now'), ?, ?, ?)",
					#(event.device.getSerialNum(), event.index, event.value))

			conn.commit()
			conn.close()
		except sqlite3.Error as e:
			print "An error occurred:", e.args[0]

	def velocityChangeHandler(event):
		logString = "MotorControl Velocity Change"
		#print(logString)

		try:
			conn = sqlite3.connect(databasepath)

			conn.execute("INSERT INTO MOTORCONTROL_VELOCITYCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
					(event.device.getSerialNum(), event.index, event.velocity))

			conn.commit()
			conn.close()
		except sqlite3.Error as e:
			print "An error occurred:", e.args[0]

	try:
		p = MotorControl()

		p.setOnAttachHandler(onAttachHandler)
		p.setOnDetachHandler(onDetachHandler)
		p.setOnErrorhandler(onErrorHandler)
		p.setOnServerConnectHandler(onServerConnectHandler)
		p.setOnServerDisconnectHandler(onServerDisconnectHandler)

		p.setOnBackEMFUpdateHandler (backEMFUpdateHandler)
		p.setOnCurrentChangeHandler (currentChangeHandler)
		p.setOnCurrentUpdateHandler (currentUpdateHandler)
		p.setOnInputChangeHandler   (inputChangeHandler)
		p.setOnPositionChangeHandler(positionChangeHandler)
		p.setOnPositionUpdateHandler(positionUpdateHandler)
		p.setOnSensorUpdateHandler  (sensorUpdateHandler)
		p.setOnVelocityChangeHandler(velocityChangeHandler)

		p.openPhidget(serialNumber)

	except PhidgetException as e:
		print("Phidget Exception %i: %s" % (e.code, e.details))
		print("Exiting...")
		exit(1)