def execute(self, userdata):
rospy.loginfo(_namespace + 'On Ready State')
self.stateMsg.state = self.stateMsg.READY
status_handler.publishRoverState(self.stateMsg)
self.gotWayPoint = status_handler.gotWayPoint
if self.gotWayPoint == True:
status_handler.checkAllSensors() #Check All Sensors For Once ##TODO: Criticize if it is neccesary
self.allSensorsWorking = status_handler.allSensorsWorking
if self.allSensorsWorking == True:
rospy.loginfo(_namespace + "Got new waypoint. Switching to GPS state.")
self.timeoutCounter = 0
return 'TO_GPS'
else:
return 'FAIL'
else:
self.timeoutCounter += 1
if self.timeoutCounter == self.readyTimeout:
rospy.loginfo("Waited too long for waypoint ...")
self.timeoutCounter = 0
return 'FAIL'
rospy.sleep(0.1)
return 'REPEAT'
def execute(self, userdata): rospy.loginfo(_namespace + 'On APPROACH State') self.stateMsg.state = self.stateMsg.APPROACH status_handler.publishRoverState(self.stateMsg) self.first_artag = status_handler.first_artag self.second_artag = status_handler.second_artag self.doneApproach = status_handler.doneApproach if self.doneApproach == True: #Kodlari degistirince fake olmasi lazim :))) rospy.loginfo(_namespace + "Approaching...........") self.timeoutCounter = 0 return 'SUCCESS' self.timeoutCounter += 1 if self.timeoutCounter == self.approachTimeout: rospy.loginfo(_namespace + "Artag is still not detected, get your shit together.") self.timeoutCounter = 0 return 'RETURN' if (self.first_artag == True and self.second_artag == False) or (self.first_artag == False and self.second_artag == True): rospy.loginfo(_namespace + "Only artag is found, BACK FIND_APPROACH") self.timeoutCounter = 0 return 'BACK' rospy.sleep(0.1) return 'REPEAT'
def execute(self, userdata):
rospy.loginfo(_namespace + 'On Initialise state')
self.stateMsg.state = self.stateMsg.INITIALISE
status_handler.publishRoverState(self.stateMsg)
status_handler.publishRoverSC(status_handler.sc)
#print(self.scMsg)
self.gpsWorking = status_handler.gpsWorking # Necessary parameters to go to READY state
self.imuWorking = status_handler.imuWorking
self.encoderWorking = status_handler.encoderWorking
if self.gpsWorking == True and self.imuWorking == True and self.encoderWorking == True: # Check all necessary parameters.
rospy.loginfo(_namespace + "All localization sensors are working good...")
self.timeoutCounter = 0
return 'SUCCESS' ## TODO : Criticize if encoder is critical here.
elif self.gpsWorking == True and self.imuWorking == True and self.encoderWorking != True: # Check all necessary parameters except encoder
rospy.loginfo(_namespace + "All localization sensors are working good except encoder...")
self.timeoutCounter = 0
return 'SUCCESS'
else: # Count timeout.
self.timeoutCounter += 1
if self.timeoutCounter == self.initaliseTimeout: # If Timeout counter has reached the limit.
self.timeoutCounter = 0
return 'FAIL'
#print("Gps working {}\nImu working {}\n\n".format(self.gpsWorking,self.imuWorking))
rospy.sleep(0.1)
return 'REPEAT'
def execute(self, userdata):
rospy.loginfo(_namespace + 'On Find Artag State')
self.stateMsg.state = self.stateMsg.FIND_ARTAG
status_handler.publishRoverState(self.stateMsg)
self.artagDetected = status_handler.artagDetected
self.first_artag = status_handler.first_artag
self.second_artag = status_handler.second_artag
self.goBack = status_handler.goBack
#print(str(self.artagDetected))
#if self.artagDetected == True:
if(status_handler.sc >= 4):
if self.first_artag == True and self.second_artag == True:
rospy.loginfo(_namespace + "Artag has detected, moving to APPROACH state")
self.timeoutCounter = 0
print("ALL OF ARTAG ARE FOUND!!!!! ")
#self.artagDetected = False #??
return 'GO_APPROACH'
else:
self.timeoutCounter += 1
if(status_handler.sc == 3):
#rospy.loginfo(_namespace + "1.if de ......................................")
if(self.first_artag == True):
#rospy.loginfo(_namespace + "2.if de ......................................")
rospy.loginfo(_namespace + "Artag has detected, moving to REACH_ARTAG state")
self.timeoutCounter = 0
#self.artagDetected = False #??
return 'SUCCESS'
else:
rospy.loginfo("goback in icinde")
self.timeoutCounter += 1
time.sleep(1)
return 'REPEAT'
if self.timeoutCounter == self.findArtagTimeout:
rospy.loginfo(_namespace + "Artag is still not detected, get your shit together.")
self.timeoutCounter = 0
return 'RETURN'
rospy.sleep(0.1)
return 'REPEAT'
def execute(self, userdata):
rospy.loginfo(_namespace + "On Reach GPS State")
self.stateMsg.state = self.stateMsg.REACH_GPS
status_handler.publishRoverState(self.stateMsg)
self.gpsReached = status_handler.gpsReached
self.artagDetected = status_handler.artagDetected
self.movementAttribute = status_handler.movementAttribute
if self.movementAttribute == 0: #muhtemelen test case
if self.gpsReached == True:
rospy.loginfo(_namespace +
"Reached to GPS, moving to FIND_ARTAG state.")
return 'SUCCESS' #muhtemelen test case
elif self.movementAttribute == 1:
if status_handler.sc == 1 or status_handler.sc == 2:
if self.gpsReached == True:
rospy.loginfo(
_namespace +
"Reached to Gps, moving to DEINITIALISE state.")
return 'SUCCES12'
elif self.gpsReached == False and self.artagDetected == True:
rospy.loginfo(
_namespace +
"Detected the ARTAG, moving to REACH_ARTAG state.")
return 'IMAGE_INTERRUPT'
elif self.gpsReached == True and self.artagDetected == False:
rospy.loginfo(_namespace +
"Reached to Gps, moving to FIND_ARTAG state.")
return 'SUCCESS'
elif self.gpsReached == True and self.artagDetected == True:
rospy.loginfo(
_namespace +
"Reached to Gps, Detected the ARTAG, moving to REACH_ARTAG state."
)
return 'IMAGE_INTERRUPT'
rospy.sleep(0.1)
return 'REPEAT'
def execute(self, userdata): rospy.loginfo(_namespace + 'On Reach Artag State') self.stateMsg.state = self.stateMsg.REACH_ARTAG status_handler.publishRoverState(self.stateMsg) self.first_artag = status_handler.first_artag self.second_artag = status_handler.second_artag self.artagReached = status_handler.artagReached self.goBack = status_handler.goBack if status_handler.sc == 3 : if self.artagReached == True: rospy.loginfo(_namespace + "Reached The Artag !!") self.timeoutCounter = 0 self.artagReached = False #eklendi return 'SUCCESS' else: self.timeoutCounter += 1 if self.timeoutCounter == self.reachArtagTimeout: rospy.loginfo(_namespace + "Artag is still not reached, get your shit together.") status_handler.checkAllSensors() #Check All Sensors For Once ##TODO: Criticize if it is neccesary self.allSensorsWorking = status_handler.allSensorsWorking if self.allSensorsWorking == True: self.timeoutCounter = 0 return 'REPEAT' #RETURN else: self.timeoutCounter = 0 return 'FAIL' print(str(self.goBack)) '''if self.goBack == True: rospy.loginfo(_namespace + "Going back to last waypoint and searching for the ball.") self.timeoutCounter = 0 return 'BACK' ''' ## !! TO DO : send waypoint here !! rospy.sleep(0.1) return 'REPEAT'
def execute(self, userdata): rospy.loginfo(_namespace + "On PASS_GATE State") self.stateMsg.state = self.stateMsg.PASS_GATE status_handler.publishRoverState(self.stateMsg) self.control_dir = status_handler.control_dir self.passComplete = status_handler.passComplete #fake '''if self.passComplete == True: #fake rospy.loginfo(_namespace + "Pass through the gate, moving to DEINITIALISE state.") return 'SUCCESS' ''' if self.control_dir == False: rospy.loginfo(_namespace + " Pass through the gate, moving to DEINITIALISE state.") self.timeoutCounter = 0 return 'SUCCESS' rospy.sleep(0.1) return 'REPEAT_APPROACH'
def execute(self, userdata):
rospy.loginfo(_namespace + 'On APPROACH State')
self.stateMsg.state = self.stateMsg.APPROACH
status_handler.publishRoverState(self.stateMsg)
if self.doneApproach == True: #Kodlari degistirince fake olmasi lazim :)))
rospy.loginfo(_namespace + "Artag has detected, moving to DEINITIALISE state")
self.timeoutCounter = 0
return 'SUCCESS'
self.timeoutCounter += 1
if self.timeoutCounter == self.approachTimeout:
rospy.loginfo(_namespace + "Artag is still not detected, get your shit together.")
self.timeoutCounter = 0
return 'RETURN'
rospy.sleep(0.1)
return 'REPEAT'