def stateMachine(self, timerStuff):
publisher = rospy.Publisher('ballCollectionMovement', Movement)
moveMessage = None
rest = PWMRaw(channel=5, pwm=0)
servoPublisher = rospy.Publisher('/hoverboard/PWMRaw/', PWMRaw)
one = PWMRaw(channel=5, pwm=2)
#detectionPublisher = rospy.Publisher('/detectionSwitch', DetectionSwitcher)
#detectionSwitch = DetectionSwitcher()
#detectionSwitch.state = 2
#detectionPublisher.publish(detectionSwitch)
if (self.state == 0): #Find a ball of any color
#detectionSwitch.state = 2
#detectionPublisher.publish(detectionSwitch)
for i in xrange(0,numPublishTimes):
servoPublisher.publish(one)
moveMessage = self.detectBall()
elif (self.state == 1): #Move towards the ball
for i in xrange(0,numPublishTimes):
servoPublisher.publish(rest)
#detectionPublisher.publish(detectionSwitch)
moveMessage = self.moveToBall()
elif (self.state == 2): #Collect the ball in the mechanism
#servoPublisher.publish(rest)
#detectionPublisher.publish(detectionSwitch)
moveMessage = self.collectBall()
else: #Reset to initial state
self.publishColorToBallPy()
moveMessage = self.stop()
self.ballVisible = False
self.ballCaptured = False
self.currentColor = 'yellow'
self.targetColor = None
self.state = 0
donePublisher = rospy.Publisher('currentBallState', DetectionSwitcher) #Reuse of existing message type
dState = DetectionSwitcher()
dState.state = 0
donePublisher.publish(dState)
publisher.publish(moveMessage)
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)
def stateMachine(self):
publisher = rospy.publisher('landmarkDetection',Movement)
moveMessage = None
if (self.state == 0): #Look for the landmark
moveMessage = self.lookForLandmark(self.landmark1)
elif (self.state == 1): #Move towards landmark
moveMessage = self.moveTowardsLandmark
else: #Reset to initial state
moveMessage = self.move()
self.currentLandmark = -2
self.currentVisibleLMCode = -2
self.isLandmarkInView = False
self.landmarkLocation = None
self.distanceToLandmark = 1000
self.targetReached = False
self.landMarkX = None
self.state = 0
donePublisher = rospy.publisher('currentBallState', DetectionSwitcher) #Reuse of existing message type
dState = DetectionSwitcher()
dState.state = 1
donePublisher.publish(dState)
publisher.publish(moveMessage)
def publishColorToBallPy(self):
#Publish the collect ball color
color = DetectionSwitcher()
if self.currentColor == 'orange':
color.state = 1
elif self.currentColor == 'purple':
color.state = 2
elif self.currentColor == 'yellow':
color.state = 3
elif self.currentColor == 'green':
color.state = 4
elif self.currentColor == 'blue':
color.state = 5
publisher = rospy.Publisher('foundBallColor', DetectionSwitcher)
publisher.publish(color)
def updateState(self,timerShit):
#Controls the overall state of the node
numPublishTimes = 10
cameraPublisher = rospy.Publisher('/detectionSwitch', DetectionSwitcher)
switcher = DetectionSwitcher()
#Publishes for the lift
servoPublisher = rospy.Publisher('/hoverboard/PWMRaw/', PWMRaw)
liftDown = PWMRaw(channel=5, pwm=0)
liftMiddle = PWMRaw(channel=5, pwm=4)
movePublisher = rospy.Publisher('/ballCleanerOut', Movement)
moveMessage = None
if self.state == 0:
servoPublisher.publish(liftMiddle)
moveMessage = self.detectBall()
switcher.state = 2
elif self.state == 1:
if self.switchedState:
self.switchedState = False
self.debugPrint('Moving to Ball')
servoPublisher.publish(liftDown)
rospy.sleep(2.0)
moveMessage = self.moveToBall()
switcher.state = 2
elif self.state == 2:
if self.switchedState:
self.switchedState = False
self.debugPrint('Loading Ball')
moveMessage = self.collectBall()
switcher.state = 2
elif self.state == 3:
string = 'Looking for landmark %d'%(self.colorDict[self.getCurrentColor()])
if self.switchedState:
self.switchedState = False
self.debugPrint(string)
moveMessage = self.lookForLandmark(self.colorDict[self.getCurrentColor()])
switcher.state = 1
elif self.state == 4:
if self.switchedState:
self.switchedState = False
self.debugPrint('Moving Towards Landmark')
moveMessage = self.moveTowardsLandmark()
switcher.state = 1
elif self.state == 5:
if self.switchedState:
self.switchedState = False
self.debugPrint('Firing')
moveMessage = self.launch()
switcher.state = 1
elif self.state == 6:
if self.switchedState:
self.switchedState = False
self.debugPrint('Stop Firing')
moveMessage = self.stopLaunching()
switcher.state = 1
elif self.state == 7: #Resets state machine back to default settings
self.resetVariables()
switcher.state = 2
self.state = 0
self.switchedState = True
moveMessage = self.stop()
else:
print('Well that shouldn\'t have happened. State is now: ' + str(self.state))
switcher.header.stamp =rospy.Time.now()
cameraPublisher.publish(switcher)
movePublisher.publish(moveMessage)