# Wandering robot FSM example

Description: Writing FSM's for a wandering robot

Keywords: FSM, decision_making_examples

Tutorial Level: BEGINNER

## Running the example

`roslaunch decision_making_examples fsm_wandering.launch`

### Overview

In this tutorial we will cover creation of a simple wandering algorithm using a finite-state machine as depicted below. The source code described here can be found in decision_making_examples packages (src/Wandering.cpp).

### The FSM

```   1 FSM(Wandering)
2 {
3     FSM_STATES
4     {
5         Drive,
6         Turn,
7         Pause
8     }
9     FSM_START(Turn)
10     FSM_BGN
11     {
12         FSM_STATE(Turn)
13         {
15
16             FSM_TRANSITIONS
17             {
18                 FSM_ON_EVENT(/TURN_TIMEOUT, FSM_NEXT(Drive))
19                 FSM_ON_EVENT(/PAUSE, FSM_NEXT(Pause))
20             }
21         }
22         FSM_STATE(Drive)
23         {
25
26             FSM_TRANSITIONS
27             {
28                 FSM_ON_EVENT(/DRIVE_TIMEOUT, FSM_NEXT(Turn))
29                 FSM_ON_EVENT(/OBSTACLE, FSM_NEXT(Turn))
30                 FSM_ON_EVENT(/PAUSE, FSM_NEXT(Pause))
31             }
32         }
33         FSM_STATE(Pause)
34         {
36
37             FSM_TRANSITIONS
38             {
39                 FSM_ON_EVENT(/RESUME, FSM_NEXT(Turn))
40             }
41         }
42     }
43     FSM_END
44 }
```

### Generated SCXML

```   1 <?xml version="1.0"?>
2 <scxml>
3    <state name="Wandering" initialstate="Turn" id="/Wandering">
4       <state name="Turn" id="/Wandering/Turn">
6          <transition event="/TURN_TIMEOUT" target="/Wandering/Drive"></transition>
7          <transition event="/PAUSE" target="/Wandering/Pause"></transition>
8       </state>
9       <state name="Drive" id="/Wandering/Drive">
11          <transition event="/DRIVE_TIMEOUT" target="/Wandering/Turn"></transition>
12          <transition event="/OBSTACLE" target="/Wandering/Turn"></transition>
13          <transition event="/PAUSE" target="/Wandering/Pause"></transition>
14       </state>
15       <state name="Pause" id="/Wandering/Pause">
17          <transition event="/RESUME" target="/Wandering/Turn"></transition>
18       </state>
19    </state>
20 </scxml>
```

As you can see, we use three states: Drive, Turn and Pause. For each state we implement corresponding task: driveTask(), turnTask() and pauseTask().

Drive task tells the robot to drive straight for a random amount of time. Note that we use preemptive wait, because we can receive an /OBSTACLE event which tells the robot must stop.

```   1 decision_making::TaskResult driveTask(string name, const FSMCallContext& context, EventQueue& eventQueue) {
2     ROS_INFO("Driving...");
3
4     double timeToDriveMs = _randomizer.uniformInteger(4000, 8000);
5
6     geometry_msgs::Twist forwardMessage;
7     forwardMessage.linear.x = 2;
8     _velocityPublisher.publish(forwardMessage);
9
10     /**
11      * Preemptive wait
12      */
13     for (int i = 0; i < 100; ++i) {
14         if (eventQueue.isTerminated()) {
15             ROS_INFO("Obstacle!");
17         }
18
20     }
21
22     eventQueue.riseEvent("/DRIVE_TIMEOUT");
24 }
```

This task turns the robot in random direction. Here we don't have to use preemptive wait, because turn state can be completed with a /TIMEOUT event.

```   1 decision_making::TaskResult turnTask(string name, const FSMCallContext& context, EventQueue& eventQueue) {
2     ROS_INFO("Turning...");
3
4     bool turnRight = _randomizer.uniformInteger(0, 1);
5     int timeToTurnMs = _randomizer.uniformInteger(2000, 4000);
6
7     geometry_msgs::Twist turnMessage;
8     turnMessage.angular.z = 2 * (turnRight ? 1 : -1);
9     _velocityPublisher.publish(turnMessage);
10
12
13     eventQueue.riseEvent("/TURN_TIMEOUT");
15 }
```

Here we just say the robot to stop.

```   1 decision_making::TaskResult pauseTask(string name, const FSMCallContext& context, EventQueue& eventQueue) {
2     ROS_INFO("Pausing...");
3
4     geometry_msgs::Twist stopMessage;
5     _velocityPublisher.publish(stopMessage);
6
8 }
```

As you may have noticed, in the state machine we call for tasks Turn, Drive and Pause, but the actual names of task methods are turnTask, driveTask and pauseTask, so we need to bind task name to actual method:

```   1     LocalTasks::registrate("Drive", driveTask);
```

### Events

#### /OBSTACLE

TurnTask and DriveTask raise TIMEOUT event upon completion, but the /OBSTACLE event must come from an external source.

The external source in our case, is the onLaserScanMessage callback:

```   1 void onLaserScanMessage(const sensor_msgs::LaserScan::Ptr laserScanMessage, RosEventQueue* eventQueue) {
2     double frontRange = laserScanMessage->ranges[laserScanMessage->ranges.size() / 2];
3
4     if (frontRange < 0.5) {
5         eventQueue->riseEvent("/OBSTACLE");
6     }
7 }
```

This is just an example of how you can raise the /OBSTACLE event, of cause you have to implement a better algorithm in real wandering.

#### /PAUSE and /RESUME

You can raise these two events by publishing a string to events topic:

`rostopic pub /decision_making/wandering/events std_msgs/String "data: '/PAUSE'"`

or

`rostopic pub /decision_making/wandering/events std_msgs/String "data: '/RESUME'"`

Wiki: decision_making_examples/Tutorials/FSM_Wandering (last edited 2014-01-28 11:33:13 by TommySM)