Note: This tutorial assumes that you have completed the previous tutorials: decision_making/Tutorials/FSM(C++).

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.

Dual Task FSM Example

Description: Dual Task 2-state FSM

Keywords: FSM, decision_making_examples

Tutorial Level: BEGINNER

Contents

Running the example

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).

Wandering state machine

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

   1 FSM(DualExample)
   2 {
   3         FSM_STATES 
   4         {
   5                 A, B
   6         }
   7         FSM_START(A);
   8         FSM_BGN
   9         {
  10                 FSM_STATE(A)
  11                 {
  12                         FSM_TRANSITIONS
  13                         {
  14                                 FSM_ON_EVENT("/b", FSM_NEXT(B));
  15                                 FSM_ON_EVENT("/a", FSM_NEXT(A));
  16                         }
  17                 }
  18 
  19                 FSM_STATE(B)
  20                 {
  21                         FSM_CALL_TASK(Bend)
  22                         FSM_CALL_TASK(Touch)
  23                         FSM_TRANSITIONS
  24                         {
  25                                 FSM_ON_EVENT("/a", FSM_NEXT(A));
  26                                 FSM_ON_EVENT("/b", FSM_NEXT(B));
  27                         }
  28                 }
  29         }
  30         FSM_END
  31 }

Generated SCXML

   1 <?xml version="1.0"?>
   2 <scxml>
   3    <state name="DualExample" initialstate="A" id="/DualExample">
   4       <state name="A" id="/DualExample/A">
   5          <transition event="/b" target="/DualExample/B"></transition>
   6          <transition event="/a" target="/DualExample/A"></transition>
   7       </state>
   8       <state name="B" id="/DualExample/B">
   9          <parallel>
  10              <invoke name="TASK[Bend]" id="/DualExample/B/Bend" />
  11              <invoke name="TASK[Touch]" id="/DualExample/B/Touch" />
  12          </parallel>
  13          <transition event="/a" target="/DualExample/A"></transition>
  14       </state>
  15    </state>
  16 </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:

   1 decision_making::TaskResult bendTask(string name, const FSMCallContext& context, EventQueue& eventQueue){
   2         ROS_INFO("Bending...");
   3 
   4         while(!preemptiveWait(1000, eventQueue)){
   5                 ROS_INFO("Bending Over");
   6 
   7         }
   8         return decision_making::TaskResult::SUCCESS();
   9 }
  10 
  11 decision_making::TaskResult touchTask(string name, const FSMCallContext& context, EventQueue& eventQueue){
  12         ROS_INFO("Touching...");
  13 
  14         while(!preemptiveWait(1000, eventQueue)){
  15                 ROS_INFO("Touching Toes");
  16         }
  17         return decision_making::TaskResult::SUCCESS();
  18 }

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:

   1     LocalTasks::registrate("Bend", bendTask);
   2     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:

   1         while(!preemptiveWait(1000, eventQueue)){
   2                 ROS_INFO("Bending Over");
   3         }

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

   1 bool preemptiveWait(double ms, decision_making::EventQueue& queue) {
   2     for (int i = 0; i < 100 && !queue.isTerminated(); i++)
   3         boost::this_thread::sleep(boost::posix_time::milliseconds(ms / 100.0));
   4 
   5     return queue.isTerminated();
   6 }

/a & /b

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

   1 void publishRandomEvent(ros::Publisher& RandEventPublisher) {
   2           static random_numbers::RandomNumberGenerator randomizer;
   3           std_msgs::String event;
   4           //a condition that will come in once in a while...
   5           if((0 == randomizer.uniformInteger(1,100) % 3) && (0 == randomizer.uniformInteger(1,100) % 5)){
   6                   event.data = "/b";
   7           }
   8           else{
   9                   event.data = "/a";
  10           }
  11           //cout<< event.data<< endl;
  12           RandEventPublisher.publish(event);
  13       //so the transition will be visible...
  14       boost::this_thread::sleep(boost::posix_time::milliseconds(300));
  15 }

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

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

ROS 2 Documentation

Wiki

Page

User