56 lines
2.0 KiB
Python
56 lines
2.0 KiB
Python
import os
|
|
import logging
|
|
logging.basicConfig(level='INFO')
|
|
|
|
class StateMachineHandler:
|
|
def __init__(self) -> None:
|
|
self.state_machine_file = "/Users/max/Desktop/THEA_CODE/Control/Jetson/tests/MACHINE_STATE"
|
|
# self.state_machine_file = "/home/pi/MACHINE_STATE"
|
|
self.__MACHINE_STATES = {
|
|
"IDLE": 0,
|
|
"NAVIGATING_TO_POINT_GPS": 1,
|
|
"NAVIGATING_TO_POINT_VISION": 2,
|
|
"MANIPULATING_CARGO": 3,
|
|
"READY_FOR_NEXT_STAGE": 4,
|
|
"RC_CONTROL": 5,
|
|
"PROXIMITY_STOP": 6,
|
|
"ESTOP": 7,
|
|
"OTHER_ERROR": 8
|
|
}
|
|
self.init_file()
|
|
|
|
def get_state(self):
|
|
if os.path.isfile(self.state_machine_file):
|
|
with open(self.state_machine_file, 'r') as smfile:
|
|
state = int(smfile.read())
|
|
return state, list(self.__MACHINE_STATES.keys())[list(self.__MACHINE_STATES.values()).index(int(state))]
|
|
|
|
def init_file(self, force=False):
|
|
if not os.path.isfile(self.state_machine_file) or force:
|
|
logging.info(f"Creating new state file, force flag is {force}")
|
|
with open(self.state_machine_file, 'w+') as smfile:
|
|
smfile.write(str(self.__MACHINE_STATES["IDLE"]))
|
|
else:
|
|
logging.info("State file already exists and force flag is absent, leaving file as is")
|
|
|
|
def set_state(self, state):
|
|
if self.check_state_validity():
|
|
with open(self.state_machine_file, 'w') as smfile:
|
|
smfile.write(str(self.__MACHINE_STATES.get(state, 8)))
|
|
|
|
def check_state_validity(self):
|
|
_, state = self.get_state()
|
|
if state == "ESTOP" or state == "OTHER_ERROR":
|
|
logging.critical(f"Machine state is {state}! Cannot perform operation")
|
|
return False
|
|
return True
|
|
|
|
sm = StateMachineHandler()
|
|
print(sm.get_state())
|
|
sm.set_state("RC_CONTROL")
|
|
print(sm.get_state())
|
|
sm.set_state("ESTOP")
|
|
print(sm.get_state())
|
|
sm.set_state("NAVIGATING_TO_POINT_VISION")
|
|
print(sm.get_state())
|