62 lines
1.4 KiB
Python
62 lines
1.4 KiB
Python
"""
|
|
Finite State Machine (FSM) Module
|
|
Python equivalent of the C++ FSM system
|
|
"""
|
|
from abc import ABC, abstractmethod
|
|
from enum import Enum
|
|
|
|
from common.joystick import ControlFlag
|
|
from common.robot_data import RobotData
|
|
|
|
|
|
class FSMStateName(Enum):
|
|
"""FSM状态枚举"""
|
|
STOP = 0 # 停止状态
|
|
ZERO = 1 # 零位状态
|
|
WALKAMP = 2 # WALKAMP策略状态
|
|
MYPOLICY = 3 # 自定义策略状态
|
|
XSIMRUN = 4 # 更贴近sim2sim的xSIM run状态
|
|
|
|
class FSMState(ABC):
|
|
"""FSM状态抽象基类"""
|
|
|
|
def __init__(self, robot_data: RobotData):
|
|
self.robot_data_ = robot_data
|
|
|
|
@abstractmethod
|
|
def on_enter(self):
|
|
"""进入状态时的行为"""
|
|
pass
|
|
|
|
@abstractmethod
|
|
def run(self, flag: ControlFlag):
|
|
"""运行状态的正常行为"""
|
|
pass
|
|
|
|
@abstractmethod
|
|
def on_exit(self):
|
|
"""退出状态时的行为"""
|
|
pass
|
|
|
|
@abstractmethod
|
|
def check_transition(self, flag: ControlFlag) -> FSMStateName:
|
|
"""检查状态转换"""
|
|
pass
|
|
|
|
class RobotFSM(ABC):
|
|
"""机器人FSM抽象基类"""
|
|
|
|
def __init__(self, robot_data: RobotData):
|
|
self.robot_data_ = robot_data
|
|
# self.disable_joints_ = False
|
|
|
|
@abstractmethod
|
|
def run_fsm(self, flag: ControlFlag):
|
|
"""运行FSM"""
|
|
pass
|
|
|
|
@abstractmethod
|
|
def get_current_state(self) -> FSMStateName:
|
|
"""获取当前FSM状态"""
|
|
pass
|