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 Initialise state')
self.stateMsg.state = self.stateMsg.INITIALISE
status_handler.publishRoverState(self.stateMsg)
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'
rospy.sleep(0.1)
return 'REPEAT'
def execute(self, userdata):
rospy.loginfo(_namespace + 'On Find Image State')
self.stateMsg.state = self.stateMsg.FIND_IMAGE
status_handler.publishRoverState(self.stateMsg)
self.imageDetected = status_handler.imageDetected
print(str(self.imageDetected))
if self.imageDetected == True:
rospy.loginfo(_namespace +
"Ball has detected, moving to REACH_IMAGE state")
self.timeoutCounter = 0
self.imageDetected = False #??
return 'SUCCESS'
else:
self.timeoutCounter += 1
if self.timeoutCounter == self.findImageTimeout:
rospy.loginfo(
_namespace +
"Ball 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 Image State')
self.stateMsg.state = self.stateMsg.REACH_IMAGE
status_handler.publishRoverState(self.stateMsg)
self.imageReached = status_handler.imageReached
self.goBack = status_handler.goBack
if self.imageReached == True:
rospy.loginfo(_namespace + "Reached The Ball !!")
self.timeoutCounter = 0
self.imageReached = False #eklendi
return 'SUCCESS'
else:
self.timeoutCounter += 1
if self.timeoutCounter == self.reachImageTimeout:
rospy.loginfo(_namespace +
"Ball 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 Reach GPS State")
self.stateMsg.state = self.stateMsg.REACH_GPS
status_handler.publishRoverState(self.stateMsg)
self.gpsReached = status_handler.gpsReached
self.imageDetected = status_handler.imageDetected
self.movementAttribute = status_handler.movementAttribute
if self.movementAttribute == 0:
if self.gpsReached == True:
rospy.loginfo(_namespace +
"Reached to GPS, moving to FIND_IMAGE state.")
return 'SUCCESS'
elif self.movementAttribute == 1:
if self.gpsReached == False and self.imageDetected == True:
rospy.loginfo(
_namespace +
"Detected the Ball, moving to REACH_IMAGE state.")
return 'IMAGE_INTERRUPT'
elif self.gpsReached == True and self.imageDetected == False:
rospy.loginfo(_namespace +
"Reached to Gps, moving to FIND_IMAGE state.")
return 'SUCCESS'
elif self.gpsReached == True and self.imageDetected == True:
rospy.loginfo(
_namespace +
"Reached to Gps, Detected the Ball, moving to REACH_IMAGE state."
)
return 'SUCCESS'
rospy.sleep(0.1)
return 'REPEAT'