# 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")