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'