# STATE TRANSITION Initialisation
fsm.AddTransition("to_IDLE", Transition("IDLE"))
fsm.AddTransition("to_ARM_TO_OBJ", Transition("ARM_TO_OBJ"))
fsm.AddTransition("to_SEARCH_OBJ", Transition("SEARCH_OBJ"))
fsm.AddTransition("to_ALIGN_CAMERA", Transition("ALIGN_CAMERA"))
fsm.AddTransition("to_WAIT", Transition("WAIT"))
fsm.AddTransition("to_ALIGN_BLOCK", Transition("ALIGN_BLOCK"))
fsm.AddTransition("to_APPROACH", Transition("APPROACH"))
fsm.AddTransition("to_PICK_UP", Transition("PICK_UP"))
fsm.AddTransition("to_VERIFY", Transition("VERIFY"))
fsm.AddTransition("to_LOCATE_BIN", Transition("LOCATE_BIN"))
fsm.AddTransition("to_ALIGN_BIN", Transition("ALIGN_BIN"))
fsm.AddTransition("to_APPROACH_BIN", Transition("APPROACH_BIN"))
fsm.AddTransition("to_DROPPED", Transition("DROPPED"))
fsm.SetState("IDLE")
fsm.ToTransition("to_IDLE")
rospy.loginfo("Initialising ArmNode")
r = rospy.Rate(RATE)
while not rospy.is_shutdown():
fsm.Execute()
rArm.CurrentStatePub()
rArm.JointPub()
r.sleep()
except rospy.ROSInterruptException:
rospy.loginfo("Interrupt Exception! Shutting down")
