## 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=[[decision_making/Tutorials/FSM(C++)]]
## descriptive title for the tutorial
## title = Dual Task FSM Example
## multi-line description to be displayed in search 
## description = Dual Task 2-state FSM
## 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= BeginnerCategory
## keywords = FSM, decision_making_examples
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== Running the example ==
{{{
roslaunch decision_making_examples fsm_dualtask.launch
}}}

=== Overview ===
In this tutorial we will cover the creation of a simple two-state finite state machine with one state running two tasks as depicted below. The source code described here can be found in decision_making_examples packages (src/DualTask.cpp).

{{attachment:Dual.png|Wandering state machine|width=450, height=450}}

Notice FSM State Definitions, and usage of FSM Transitions, Event Handling and Task Calls, all complying with FSM C++ syntax explained in the previous tutorial:

=== The FSM ===

{{{
#!cplusplus

FSM(DualExample)
{
	FSM_STATES 
	{
		A, B
	}
	FSM_START(A);
	FSM_BGN
	{
		FSM_STATE(A)
		{
			FSM_TRANSITIONS
			{
				FSM_ON_EVENT("/b", FSM_NEXT(B));
				FSM_ON_EVENT("/a", FSM_NEXT(A));
			}
		}

		FSM_STATE(B)
		{
			FSM_CALL_TASK(Bend)
		        FSM_CALL_TASK(Touch)
			FSM_TRANSITIONS
			{
				FSM_ON_EVENT("/a", FSM_NEXT(A));
		    	        FSM_ON_EVENT("/b", FSM_NEXT(B));
			}
		}
	}
	FSM_END
}

}}}

=== Generated SCXML ===

{{{
#!xml
<?xml version="1.0"?>
<scxml>
   <state name="DualExample" initialstate="A" id="/DualExample">
      <state name="A" id="/DualExample/A">
         <transition event="/b" target="/DualExample/B"></transition>
         <transition event="/a" target="/DualExample/A"></transition>
      </state>
      <state name="B" id="/DualExample/B">
         <parallel>
             <invoke name="TASK[Bend]" id="/DualExample/B/Bend" />
             <invoke name="TASK[Touch]" id="/DualExample/B/Touch" />
         </parallel>
         <transition event="/a" target="/DualExample/A"></transition>
      </state>
   </state>
</scxml>

}}}

=== Tasks ===

As shown, we use two states A and B,  and B contains two simple tasks, that will only give us a print-out for examples sake.  the tasks are bendTask() and touchTask() and the implementations are as follows:

{{{
#!cplusplus
decision_making::TaskResult bendTask(string name, const FSMCallContext& context, EventQueue& eventQueue){
	ROS_INFO("Bending...");

	while(!preemptiveWait(1000, eventQueue)){
		ROS_INFO("Bending Over");

	}
	return decision_making::TaskResult::SUCCESS();
}

decision_making::TaskResult touchTask(string name, const FSMCallContext& context, EventQueue& eventQueue){
	ROS_INFO("Touching...");

	while(!preemptiveWait(1000, eventQueue)){
		ROS_INFO("Touching Toes");
	}
	return decision_making::TaskResult::SUCCESS();
}
}}}

==== Tasks registration ====
As you may have noticed, in the state machine we call for tasks ''Touch'' and ''Bend'', but the actual names of task methods are ''bendTask'' and ''touchTask'', so we need to bind task names to actual methods:

{{{
#!cplusplus
    LocalTasks::registrate("Bend", bendTask);
    LocalTasks::registrate("Touch", touchTask);
}}}

=== Events ===
Here is one of the key components of the system - ''Preemptive Waiting'' - In general, our tasks can raise events internally(depends on user code), in this example our tasks will run as long as they don't receive an event from an ''External'' source, using preemptive waiting.

a reminder of the Bend Task while loop:
{{{
#!cplusplus
	while(!preemptiveWait(1000, eventQueue)){
		ROS_INFO("Bending Over");
	}
}}}

And here is the implementation, using the FSM Condition isTerminated():

{{{
#!cplusplus
bool preemptiveWait(double ms, decision_making::EventQueue& queue) {
    for (int i = 0; i < 100 && !queue.isTerminated(); i++)
        boost::this_thread::sleep(boost::posix_time::milliseconds(ms / 100.0));

    return queue.isTerminated();
}

}}}


==== /a & /b ====

We check for an external event that will originate from src/DualTaskEvents.cpp:

{{{
#!cplusplus
void publishRandomEvent(ros::Publisher& RandEventPublisher) {
	  static random_numbers::RandomNumberGenerator randomizer;
	  std_msgs::String event;
	  //a condition that will come in once in a while...
	  if((0 == randomizer.uniformInteger(1,100) % 3) && (0 == randomizer.uniformInteger(1,100) % 5)){
		  event.data = "/b";
	  }
	  else{
		  event.data = "/a";
	  }
	  //cout<< event.data<< endl;
	  RandEventPublisher.publish(event);
      //so the transition will be visible...
      boost::this_thread::sleep(boost::posix_time::milliseconds(300));
}

}}}

And the publisher advertises to a topic followed by the event queue, So the publish() method serves as an event raiser for external sources:

{{{
#!cplusplus

 ros::Publisher statePublisher = ros::NodeHandle().advertise<std_msgs::String>("/decision_making/fsm_dualtask/events", 1, false);

}}}





## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE