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)
