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 DEINITIALISE state') status_handler.sc += 1 status_handler.publishRoverSC(status_handler.sc) print("stage:", status_handler.sc) status_handler.deinitialise() rospy.sleep(1) return 'SUCCESS'