|Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.|
State Preemption ImplementationDescription: This tutorial shows how to implement simple flag-based preemption in SMACH states.
Tutorial Level: ADVANCED
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:
1 import roslib; roslib.load_manifest('smach') 2 import rospy 3 4 from smach import State 5 6 class FibState(State): 7 def __init__(self, n): 8 """Constructor.""" 9 State.__init__(self, 10 outcomes = ['done','preempted'], 11 input_keys = , 12 output_keys = ['fib_result']) 13 self.n = n 14 15 def execute(self,ud): 16 """Calculate the nth fibonacci number.""" 17 f = 0 18 f1 = 0 19 f2 = 1 20 for i in range(self.n): 21 # Check for preempt 22 if self.preempt_requested(): 23 self.service_preempt() 24 return 'preempted' 25 # Calculate the next number 26 f = f1 + f2 27 f1 = f2 28 f2 = f 29 30 # Store a tuple containing the result in userdata 31 ud.fib_result = (self.n, f) 32 33 return 'done' 34 35 def request_preempt(self): 36 """Overload the preempt request method just to spew an error.""" 37 State.request_preempt(self) 38 rospy.logwarn("Preempted!")
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.
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.
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.