from FSM import *
from ArmNode_1_5 import *
# ============================================================================
# ============================================================================
if __name__ == "__main__":
try:
rospy.loginfo("Initialising FSM")
# Initialise Finite State Machine
fsm = FSM()
# Initialise ArmNode
rArm = ArmNode(fsm)
# STATE Initialisation
fsm.AddState("IDLE", IDLE(fsm, rArm))
fsm.AddState("ARM_TO_OBJ", ARM_TO_OBJ(fsm, rArm))
fsm.AddState("SEARCH_OBJ", SEARCH_OBJ(fsm, rArm))
fsm.AddState("ALIGN_CAMERA", ALIGN_CAMERA(fsm, rArm))
fsm.AddState("WAIT", WAIT(fsm, rArm))
fsm.AddState("ALIGN_BLOCK", ALIGN_BLOCK(fsm, rArm))
fsm.AddState("APPROACH", APPROACH(fsm, rArm))
fsm.AddState("PICK_UP", PICK_UP(fsm, rArm))
fsm.AddState("VERIFY", VERIFY(fsm, rArm))
fsm.AddState("LOCATE_BIN", LOCATE_BIN(fsm, rArm))
fsm.AddState("ALIGN_BIN", ALIGN_BIN(fsm, rArm))
fsm.AddState("APPROACH_BIN", APPROACH_BIN(fsm, rArm))
fsm.AddState("DROPPED", DROPPED(fsm, rArm))
# STATE TRANSITION Initialisation
fsm.AddTransition("to_IDLE", Transition("IDLE"))
