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'