def resetVariables(self): self.landmarkVars = dict([('inView', False),('visibleLandmark', -1),('landmarkX', 0), ('distance', -1), ('targetReached', False), ]) self.ballVars = dict([('visible', False),('location', ballLocation()),('captured', False), ('colorIndex', 0)]) #Initialize lifter servoPublisher = rospy.Publisher('/hoverboard/PWMRaw/', PWMRaw) liftMiddle = PWMRaw(channel=5, pwm=4) servoPublisher.publish(liftMiddle)
def __init__(self): self.debug = float(rospy.get_param('~debug', '0')) self.state = 0 self.ballLocation = ballLocation() self.ballVisible = False self.ballCaptured = False self.previousColor= None self.currentColor = 'yellow' self.targetColor = None rospy.Subscriber('/ballLocation',ballLocation,self.getBallLocation) rospy.Subscriber('ballVisible',ballVisible, self.isBallVisible) rospy.Subscriber('/rangeInfo', Range, self.isBallCaptured) rospy.Timer(rospy.Duration(.1), self.stateMachine) print("Ball Initialized")
def __init__(self): self.debug = float(rospy.get_param('~debug', '0')) self.color1 = rospy.get_param('~color1', 'blue') self.color2 = rospy.get_param('~color2', 'red') #Used as a boolean and loop iterator. We need to publish to thresh/high and low #multiple times for the new color to actually get set self.ballColor1Set = 0 self.ballColor2Set = 0 self.landmarkForColor1 = float(rospy.get_param('~landmark1', '1')) self.landmarkForColor2 = float(rospy.get_param('~landmark2', '2')) self.currentLandmark = self.landmarkForColor1 self.detectionPublisher = rospy.Publisher('/detectionSwitch',DetectionSwitcher) self.detectionSwitch = DetectionSwitcher() self.landmarkX = 0 #Used when we're transitioning between states self.haltMove = Movement() self.haltMove.theta = 0 self.haltMove.x = 0 self.haltMove.y = 0 self.haltMove.modType = 'Add' self.ballLocation = ballLocation() self.targetReached = False self.state = 0 self.prevState = 0 #Used to print debug messages when state changes self.isBallCaptured = False #Uses the IR range finders to see if the ball is in the prong self.isBallInView = False #Uses the camera to see if a ball is visible in the scene self.isLandmarkInView = False #Uses camera to see if landmark is visible in the scene self.distanceToLandmark = 300 self.currentVisibleLMCode = -2 self.switchedToSecond = 0 rospy.Timer(rospy.Duration(.1), self.stateBasedMove) #Updates movement based on current state rospy.Subscriber('/rangeInfo', Range, self.checkForBallCaptured) #Checks for ball in prong via IR sensors rospy.Subscriber('ballVisible', ballVisible, self.checkForBallVisibility) rospy.Subscriber('/ballLocation',ballLocation,self.getBallLocation) rospy.Subscriber('landmarkLocation', landmarkLocation, self.getDistanceToLandmark) rospy.Subscriber('landmarkVisible', landmarkVisible, self.checkForLandmark) #Checks for landmark
def __init__(self): self.debug = float(rospy.get_param('~debug', '0')) #Updates state by publishing different move messages rospy.Timer(rospy.Duration(.1), self.updateState) #Initialize vision arbitrator publisher = rospy.Publisher('/detectionSwitch', DetectionSwitcher) switcher = DetectionSwitcher() switcher.state = 2 for i in xrange(0,25): switcher.header.stamp = rospy.Time.now() publisher.publish(switcher) self.colors = ['blue', 'purple', 'green', 'orange'] #Maps colors to landmark numbers self.colorDict = dict() self.colorDict['blue'] = int(rospy.get_param('~blueLandmark', '2')) self.colorDict['green'] = int(rospy.get_param('~greenLandmark', '3')) self.colorDict['yellow'] = int(rospy.get_param('~yellowLandmark', '4')) self.colorDict['purple'] = int(rospy.get_param('~purpleLandmark', '5')) self.colorDict['orange'] = int(rospy.get_param('~orangeLandmark', '6')) self.switchedState = True #State vars for the substate machines for landmark and ball collection self.landmarkVars = dict([('inView', False),('visibleLandmark', -1),('landmarkX', 0), ('distance', -1), ('targetReached', False)]) self.ballVars = dict([('visible', False),('location', ballLocation()),('captured', False), ('colorIndex', 0)]) self.state = 0 #Ball Subscriptions rospy.Subscriber('/ballLocation',ballLocation,self.getBallLocation) rospy.Subscriber('ballVisible',ballVisible, self.isBallVisible) rospy.Subscriber('/rangeInfo', Range, self.isBallCaptured) #Landmark subscrptions rospy.Subscriber('landmarkVisible',landmarkVisible, self.checkForLandmark) rospy.Subscriber('landmarkLocation', landmarkLocation, self.getDistanceToLandmark) #Initialize lifter servoPublisher = rospy.Publisher('/hoverboard/PWMRaw/', PWMRaw) liftMiddle = PWMRaw(channel=5, pwm=4) servoPublisher.publish(liftMiddle)