the real worldThere are 4 fundamental classes of ways to program robots:

Deliberative: think, then act Use available sensory information and knowledge to reason about what action to do next. Reasoning typically involves planning, requiring a search in the set of possible state-action sequences and their outcomes. Sense-plan-act: combine sensory information in a model of the world, then use a planner to find a path in such model, then execute the plan
Reactive: do not think, react Couples sensory inputs and the effector outputs without any kind of reasoning. Doesn’t require us to maintain a model of the world, rule-based methods are used. Rapid real-time responses in form of pre-programmed condition-action rules. Good for unstructured worlds.
Hybrid: think and act at the same time Real-time response while also reasoning. Contain two components: reactive and deliberative part Both need to interact to produce a coherent output. Reactive usually overrides the deliberative when an immediate event needs to be handled.
Behaviour-based: think the way you act Distributed interacting modules called behaviours, each receives inputs from sensors and other behaviours, and provides outputs to actuators and other behaviours. No centralised world representation.

A Finite State Machine is an abstract model of computation consisting of:

A set of states
A set of possible inputs to the machine
A transition function that maps one state to another state

Formally, a state machine is a five-tuple $(S, Σ, s_{0}, δ, F)$ :

$S$ is a finite non-empty set of states
$Σ$ is the input alphabet / set of symbols
$s_{0}$ is the initial state, $s_{0} \in S$
$d e l t a$ is the state-transition function $δ : S \times Σ \to S$
$F$ is the set of final states, $F \subseteq S$

SMACH

Creating a state machine with SMACH:

import smach
 
sm = smach.StateMachine(outcomes=['outcome4', 'outcome5'])
 
# states are added by  calling the static  method
with sm:
	smach.StateMachine.add('FOO', Foo(), transitions={
		'outcome1': 'BAR',
		'outcome2': 'outcome4'
	})
	
	smach.StateMachine.add('BAR', Bar(), transitions={
		'outcome2': 'FOO'
	})
 
class Bar(smach.State):
	def __init__(self):
		smach.State.__init__(self, outcomes=['outcome2'])
	
	def execute(self, userdata):
		rospy.loginfo('Executing state BAR')
		return 'outcome2'
 
class Foo(smach.State):
	def __init__(self):
		smach.State.__init__(self,
			outcomes=['outcome1', 'outcome2'])
		self.counter = 0
	
	def execute(self, userdata):
		rospy.loginfo('Executing state FOO')
		if self.counter < 3:
			self.counter += 1
			return 'outcome1'
		else:
			return 'outcome2'
 
# we can execute it by running execute()
outcome = sm.execute()

Inter-state communication

States sometimes need to pass data to other states or state machines, so execution can be performed. For example, one state detects an object and sends the class name so another state can speak the object name.

In SMACH, this is called the “userdata”. States have input and output data. States define input keys (expected and needed to run) and output keys.

class Foo(smach.State):
	def __init__(self, outcomes=['outcome1', 'outcome2'],
		input_keys=['foo_input'], output_keys=['foo_output'])
	
	def execute(self, userdata):
		# Do something with userdata
		if userdata.foo_input == 1:
			return 'outcome1'
		else:
			userdata.foo_output = 3
			return 'outcome2'

The state machine can also have inputs and outputs, and can remap data from and to state.

sm_top = smach.StateMachine(outcomes=['outcome4','outcome5'],
	input_keys=['sm_input'], output_keys=['sm_output'])
 
with sm_top:
	smach.StateMachine.add('FOO', Foo(),
		transitions={'outcome1':'BAR', 'outcome2':'outcome4'},
		remapping={'foo_input':'sm_input', 'foo_output':'sm_data'})
	
	smach.StateMachine.add('BAR', Bar(),
		transitions={'outcome2':'FOO'},
		remapping={'bar_input':'sm_data', 'bar_output1':'sm_output'})

Special SMACH States

There are a lot of special pre-implemented states that we can use.

Callback State

Pre-implemented state that calls a callback, simplifies definition of states.

from smach import CBState
 
@smach.cb_interface(input_keys=['q'],
					output_keys=['xyz'],
					outcomes=['foo'])
def my_cb(ud, x, y, z):
	ud.xyz = ud.q + x + y + z
	return 'foo'
 
with sm:
	(...)
	StateMachine.add('MY_CB', CBState(my_cb,
									cb_args=[10],
									cb_kwargs={'z':2,'y':3}),
					{'foo':'OTHER_STATE'})

SimpleActionState

Pre-implemented state that calls an actionlib action. User defines the goal and it automatically calls the actionlib when needed.

# import it
from smach_ros import SimpleActionState
 
# empty goal message
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
	smach.StateMachine.add('TRIGGER_GRIPPER',
		SimpleActionState('action_server_name',
							GripperAction),
		transitions={'succeeded':'APPROACH_PLUG'})
 
# fixed goal message
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
	gripper_goal = Pr2GripperCommandGoal()
	gripper_goal.command.position = 0.07
	gripper_goal.command.max_effort = 99999
	StateMachine.add('TRIGGER_GRIPPER',
		SimpleActionState('action_server_namespace',
						GripperAction,
						goal=gripper_goal),
		transitions={'succeeded':'APPROACH_PLUG'})
 
# create goal from userdata
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
	StateMachine.add('TRIGGER_GRIPPER',
		SimpleActionState('action_server_namespace',
						GripperAction,
						goal_slots=['max_effort', 'position']),
		transitions={'succeeded':'APPROACH_PLUG'},
		remapping={'max_effort':'user_data_max',
					'position':'user_data_position'})
 
# with a goal callback
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
	def gripper_goal_cb(userdata, goal):
		gripper_goal = GripperGoal()
		gripper_goal.position.x = 2.0
		gripper_goal.max_effort = userdata.gripper_input
		return gripper_goal
		
	StateMachine.add('TRIGGER_GRIPPER',
		SimpleActionState('action_server_namespace',
						GripperAction,
						goal_cb=gripper_goal_cb,
						input_keys=['gripper_input'])
		transitions={'succeeded':'APPROACH_PLUG'},
		remapping={'gripper_input':'userdata_input'})
 
# setting the result to userdata
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
	StateMachine.add('TRIGGER_GRIPPER',
		SimpleActionState('action_server_namespace',
						GripperAction,
						result_slots=['max_effort', 'position']),
		transitions={'succeeded':'APPROACH_PLUG'},
		remapping={'max_effort':'user_data_max',
					'position':'user_data_position'})
 
# using result in a callback
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
	def gripper_result_cb(userdata, status, result):
		if status == GoalStatus.SUCCEEDED:
			userdata.gripper_output = result.num_iterations
		return 'my_outcome'
	
	StateMachine.add('TRIGGER_GRIPPER',
		SimpleActionState('action_server_namespace',
						GripperAction,
						result_cb=gripper_result_cb,
						output_keys=['gripper_output'])
		transitions={'succeeded':'APPROACH_PLUG'},
		remapping={'gripper_output':'userdata_output'})

ServiceState

Similarly to Actions, we can automatically call a service. A pre-implemented state that calls a ROS service.

# import it
from smach_ros import ServiceState
 
# empty request
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
	smach.StateMachine.add('TRIGGER_GRIPPER',
		ServiceState('service_name',
					GripperSrv),
		transitions={'succeeded':'APPROACH_PLUG'})
 
# fixed request
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
	smach.StateMachine.add('TRIGGER_GRIPPER',
		ServiceState('service_name',
					GripperSrv,
					request = GripperSrv(9.0)),
		transitions={'succeeded':'APPROACH_PLUG'})
 
# getting request from previous state (from its userdata)
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
	smach.StateMachine.add('TRIGGER_GRIPPER',
		ServiceState('service_name',
					GripperSrv,
					request_slots = ['max_effort', 'position']),
		transitions={'succeeded':'APPROACH_PLUG'})
 
# create request in a callback
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
	@smach.cb_interface(input_keys=['gripper_input'])
	def gripper_request_cb(userdata, request):
		gripper_request = GripperSrv().Request
		gripper_request.position.x = 2.0
		gripper_request.max_effort = userdata.gripper_input
		return gripper_request
	
	smach.StateMachine.add('TRIGGER_GRIPPER',
		ServiceState('service_name',
					GripperSrv,
					request_cb = gripper_request_cb,
					# and return response as userdata!
					response_slots=['result'],
					input_keys = ['gripper_input']),
		transitions={'succeeded':'APPROACH_PLUG'})
 
# process service response in a callback
sm = StateMachine(['succeeded','aborted','preempted'])
with sm:
	def gripper_response_cb(userdata, result):
		userdata.gripper_output = result.num_iterations
		return 'succeeded'
	
	smach.StateMachine.add('TRIGGER_GRIPPER',
		ServiceState('service_name',
					GripperSrv,:01G8BMQZT1P5QENS92XBB64SZV:
					response_cb = gripper_response_cb,
					output_keys = ['gripper_output']),
		transitions={'succeeded':'APPROACH_PLUG'})

Containers

Containers that execute states in some specific manner. For example, the StateMachine, Concurrence, Sequence and Iterator.

Concurrence Container

Container executes all the states at the same time, concurrently. We can specify the policy to determine the outcome of the container in two ways, using an outcome map or a callback.

from smach import Concurrence
 
cc = Concurrence(outcomes = ['outcome1', 'outcome2'],
	default_outcome = 'outcome1',
	input_keys = ['sm_input'],
	output_keys = ['sm_output'],
	outcome_map = {'succeeded':{'FOO':'succeeded',
								'BAR':'outcome2'},
				   'outcome3':{'FOO':'outcome2'}})
 
with cc:
	Concurrence.add('FOO', Foo())
	Concurrence.add('BAR', Bar())
 
# instead of a callback map, we can use two callbacks
# check docs for this

Hierarchical State Machines

We can also specify state machines in order state machines:

sm_top = smach.StateMachine(outcomes=['outcome5'])
 
with sm_top:
	sm_sub = smach.StateMachine(outcomes=['outcome4'])
	
	with sm_sub:
		pass
	
	smach.StateMachine.add('SUB', sm_sub,
		transitions={'outcome4':'outcome5'})

Visualising State Machines

We can visual state machines by running an introspection server:

sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
 
outcome = sm.execute()
 
rospy.spin()
sis.stop()

Then view it:

rosrun smach_viewer smach_viewer.py

Archive Notes

Explorer

Week 9. State Machines