## page was copied from smach/Tutorials/State Preemption Implementation ## page was renamed from smach/Tutorials/Preemption of State Machines ## page was renamed from smach/Tutorials/Writing Custom Trivial State Classes ## For instruction on writing tutorials ## http://www.ros.org/wiki/WritingTutorials #################################### ##FILL ME IN #################################### ## for a custom note with links: ## note = ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0= ## descriptive title for the tutorial ## title = State Preemption Implementation ## multi-line description to be displayed in search ## description = This tutorial shows how to implement simple flag-based preemption in SMACH states. ## the next tutorial description (optional) ## next = ## links to next tutorial (optional) ## next.0.link= ## next.1.link= ## what level user is this tutorial for ## level= AdvancedCategory ## keywords = #################################### <> <> Sometimes, no state or container type will not satisfy your needs. Before you decide this, make sure you have examined all the provided state and container types. There are three key features that need to be considered when defining a new type of SMACH state: * Interface declaration * Execution implementation * Preemption implementation If you just want to copy and paste some code to modify for a state you can start with this very simple state: {{{#!python import roslib; roslib.load_manifest('smach') import rospy from smach import State class FibState(State): def __init__(self, n): """Constructor.""" State.__init__(self, outcomes = ['done','preempted'], input_keys = [], output_keys = ['fib_result']) self.n = n def execute(self,ud): """Calculate the nth fibonacci number.""" f = 0 f1 = 0 f2 = 1 for i in range(self.n): # Check for preempt if self.preempt_requested(): self.service_preempt() return 'preempted' # Calculate the next number f = f1 + f2 f1 = f2 f2 = f # Store a tuple containing the result in userdata ud.fib_result = (self.n, f) return 'done' def request_preempt(self): """Overload the preempt request method just to spew an error.""" State.request_preempt(self) rospy.logwarn("Preempted!") }}} === SMACH Interface === SMACH containers interact with their contained states through state outcomes and userdata keys. The outcomes need to be declared in order to be able to be bound to targets in a container, and the userdata keys need to be declared in order to trace the dataflow in the system. This explicit declaration allows us to catch errors on construction of a SMACH tree instead as well as while we're executing it. === Execution Implementation === When a state is entered by the execution of some container, its {{{execute()}}} method is called. This is a method that blocks and returns some (registered) outcome identifier (string). Once this method exits, the state should be considered dormant, or inactive. If a certain state corresponds to a long-running task, it should be brought up in a concurrence or some other parallel container, instead of spinning off a thread and exiting quickly. === Preemption Implementation === In simple cases like the example above, preemption can be done simply by checking the method {{{preempt_requested()}}}. If this cannot be done, the {{{request_preempt()}}} method can be overloaded in this new state class. Preempts will usually come in on a separate thread, and this method should not block longer than it takes to notify any child threads / processes to preempt. ## AUTOGENERATED DO NOT DELETE ## TutorialCategory ## FILL IN THE STACK TUTORIAL CATEGORY HERE ## AdvancedSMACHCategory