department of computing science - umeå university · for the project, a software framework...

Design of a general framework forsynchronizing behaviors in a complexrobot

Peter Hohnloser

Peter HohnloserSpring 2012Degree Project: Bachelor of Science in Computing Science, 15 hpSupervisor: Ola RingdahlExaminer: Pedher JohanssonBachelor of Science Programme in Computing Science, 180 hp

Abstract

This thesis describes a general framework for synchronizing behaviors in a complex robot, usinga Finite State Machine. The framework is developed in C++ and with the robotic frameworkROS. It will be used for the EU funded research project CROPS for developing a fruit-harvestingrobot. The thesis also focuses on how to connect a robot behavior to a state that makespreemptive multitasking possible. One important thing about connecting a behavior to a state iswhich kind of communication to use; publish-subscribe, request-reply, or a goal-feedback-resultcommunication. These communications can be used by two different state interfaces. Anotherimportant point regarding connecting a behavior to a state is the definition of state transitions.The state transitions are defined in a text file in yaml format. There are also three differentways of implementing state transactions presented. Passing data by ROS messages, by ROSparameter server and by saving and loading data in and from the Finite State Machine. Theframework has been successfully implemented in CROPS and is able to control a robot arm.

Acknowledgements

The work presented in this report was partly funded by the European Commission in the 7thFramework Program (CROPS GA no 246252). I would like to thank my supervisor at UmeaUniversity, Ola Ringdahl. Another great thanks goes to Emma who was supportive and helpfulduring the work with this project, and Thomas Hellstrom who gave me the opportunity to dothis project.

iii

Contents

1 Introduction 11.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2 Purpose and Goals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41.3 Thesis outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2 Aspects of the general framework 52.1 Finite State Machine . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.2 ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.3 Multitasking and Preemption . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3 Implementation of the general framework 113.1 Framework structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113.2 Finite State Machine . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

3.2.1 Transition file . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.3 State . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

3.3.1 State using ROS Topics . . . . . . . . . . . . . . . . . . . . . . . . . 153.3.2 State using ROS Services . . . . . . . . . . . . . . . . . . . . . . . . . 163.3.3 SimpleActionState . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173.3.4 Transactions between states . . . . . . . . . . . . . . . . . . . . . . . 18

4 Summary and conclusions 194.1 Design of the Finite State Machine . . . . . . . . . . . . . . . . . . . . . . . 194.2 How to enable preemptive multitasking . . . . . . . . . . . . . . . . . . . . . 194.3 State transactions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

5 Future Development 215.1 State Diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215.2 Hierarchical State Machine . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215.3 Using other robotic frameworks . . . . . . . . . . . . . . . . . . . . . . . . . 21

Bibliography 23

v

vi CONTENTS

Chapter 1

Introduction

Today’s robots can be found in a range of areas, for example hospitals, homes, and factories. Arobot is a mechanical or virtual intelligence device that can perform different kinds of tasks. Thetasks can be performed automatically or controlled by a remote control. In the future, accordingto Siciliano & Khatib [19], robots will be part of everyday life similar to how personal computersare nowadays.

Research on agricultural robots began in the 1980’s by the development of semi-autonomousrobotic systems for harvesting apples, peaches and citrus. Japan developed the first harvestingrobot for tomatoes which was fully autonomous in 1985. Later on, harvesting robots weredeveloped for cucumbers, melons, cherry tomatoes and cherries (Wouter Bac, personal commu-nication, April 26, 2012).

CROPS is a recent project for the development of autonomous harvesting robots. CROPSis a EU funded research project in which fourteen international universities and companies areinvolved. The four year long project started in October 2010. During the 48 months

”CROPS will develop scientific know-how for a highly configurable, modular andclever carrier platform that includes modular parallel manipulators and intelligenttools” [5]

The tools can be easily installed onto the carrier and easily adapted to new tasks. Furthermore,the project will develop a number of demonstrators, for example a robot that is able to harvestdifferent kinds of fruit.

For the project, a software framework with a number of important features is needed. Ex-amples of such features are good documentation, distributed computing, hardware abstraction,timestamping, built-in simulator, and logging of data to file. A framework that provides suchservices is ROS [3].

ROS is short for Robot Operating System. Windows, Linux, and Mac OS are examples ofcomputer operating systems, but ROS is another kind of operating system. It is a structuredcommunication layer on top of an operating system and it can also be seen as a framework thatsimplifies the development of a robotic software in C++ and Python. Similar robot frameworksare Player [23], YARP [10], Orocos [20], CARMEN [21], Orca [17], MOOS [16] and MicrosoftRobotics Studio [13].

The creators of ROS do not claim that ROS is a good solution for developing all types ofrobot software [18]. They developed ROS specifically to be used on larger service robots, and

1

2 Chapter 1. Introduction

the framework had the following design goals:

– Peer-to-peer

– Tools-based

– Multi-lingual

– Thin

– Free and Open-Source

These design goals helped while developing the software framework in CROPS. ROS providesbasic functionality for implementing nodes, messages, topics and services [18].

1.1 BackgroundThe research in CROPS aims at developing a robot that is able to harvest different kinds of fruit.The project members at Umea University are responsible for developing a system that controlsand synchronizes the top-level functionality of the robot. This is achieved by a novel hybridrobot architecture illustrated in Figure in 1.1 [12]. A Finite State Machine is a core component

Figure 1.1: This figure shows a novel hybrid robot architecture with a Finite State Machine asis core component. This robot architecture is used in the CROPS project.

in this architecture and is responsible for sequencing of the required actions performed by therobot. As an example a sweet-pepper harvesting robot can be specified in the following way:

1.1. Background 3

A simple example of an algorithm is specified:1. Initialize hardware and software.

2. Wait until user presses start.

3. Move the robot arm to a known position.

4. Take an image of a sweet-pepper plant.

(a) identify and calculate the position for all visible pepper fruits in the image.

5. For each fruit in the image, do:

(a) move the robot arm to the fruit.

(b) grip the fruit and remove it from the plant.

(c) move to a fruit box.

(d) put the fruit in the box.

6. If no fruits are visible (or if all has been picked), move the robot to a new location.

A complete algorithm should of course also handle errors, such as if no fruits are visiblein the image, or the fruit has been dropped after pick up. A more complete algorithm can beillustrated as a flow diagram, as exemplified in Figure 1.2. This flow diagram can be transformedinto a state diagram, as shown in Figure 1.3 (slightly simplified).

Figure 1.2: A simplified flow diagram for the functionality of a fruit harvesting robot in theCROPS project.

4 Chapter 1. Introduction

Figure 1.3: Transformed state diagram from the flow diagram shown in Figure 1.2.

1.2 Purpose and GoalsThe purpose of this thesis is to design a Finite State Machine that can be used for synchronizingbehaviors in a complex robot, as described in the previous section. The Finite State Machineshould handle these two aspects: sequencing of behaviors and parallel behaviors, useful in forexample error handling. The Finite State Machine is developed and implemented in C++ usingthe robotics operating system ROS [18]. In addition to the development and implementation ofthe Finite State Machine, the following questions will be answered:

– How can a robot behavior be connected to a State such that preemptive multitasking be-comes possible?

– How can transitions1 be defined?

– How can transactions2 between states be implemented?

1.3 Thesis outlineChapter 2, Aspects of the general framework, gives a theoretical view of what is needed todesign a general framework as a Finite State Machine with ROS. Chapter 3 describes how ageneral framework is implemented and how to use it. In chapter 4 conclusion are presented.The last chapter, describes possible future development.

1Mapping state outcome to the next state2Passing data

Chapter 2

Aspects of the general framework

This chapter provides the theoretical part of designing and implementing a general frameworkfor synchronizing behaviors in a complex robot. As written in Section 1.2, the frameworkdescribed in this thesis is designed as a Finite State Machine (FSM) and therefore a sectionabout FSMs can be found in this chapter. The section ROS describes how a robot applicationcan be crated by the Robot Operating System mentioned above. The next section explainsmultitasking and preemption which is needed by the robot framework because it has to handleparallel robot behaviors.

2.1 Finite State MachineThe definition of a FSM is :

“A finite state machine is a device, or a model of a device, which has a finite numberof states it can be in at any given time and can operate on input to either maketransitions from one state to another or to cause an output or action to take place. Afinite state machine can only be in one state at any moment in time.” [7]

In other words it is a mechanism for specifying what a program should do at a given time [15].A FSM breaks a program down into a series of steps, called states. Each state performs its owndefined task [14]. A state is a configuration of information in a program. Hellstrom & Ringdahl[12] describe that a state in a FSM for a robot can be associated with a robot behavior. Forexample move arm can be a robot behavior and a state. These states can for example be shownin a state diagram, which makes it easier for a developer to follow the program flow. Accordingto Bukovics [8] a FSM is not defined as a fixed sequences of steps, but as a set of states whereeach state has transitions to other states. State transitions are triggered by external or internalinput that is raised by the application. This leads to that the exact sequence of state transitionsis controlled externally [8].

The FSM knows which state function to call by mapping the defined state transitions [14].The new state triggered by another state or action depends on which FSM its in [15]. When astate causes a transition to a new state, the new state is allowed to execute. It is however possiblethat a state transition goes to the same state again [14]. According to Bukovics [8] transitionsin FSMs normally go from state to state, so a FSM does not need a specific start or end point.A transition can however be defined to perform a task. This can be done by hard coding or byreading a definition from a file.

5

6 Chapter 2. Aspects of the general framework

Buckland [7] means that a FSM is easy and quick to implement in many ways. Consideringthat all implemented FSMs work almost the same way [15] the way to implement it dependsmore on how the FSM is supposed to work than which type it is. FSMs are used in manysituations since they are so flexible and intuitive [7]. They can for example be used in trafficlights, micro processors, and light switches. Thanks to the many possibilities to implementa FSM it can be used in many systems where it gives a good overall system description [15].Another advantage of using a FSM is that the machines follow hard-coded rules, and no artificialintelligence has to be involved [7].

A good way to start implementing a FSM in object-oriented programming is with a designpattern called State [7]. The design pattern makes sure a state has its own function which makesit easier to read, than for example a huge single switch statement [14], which is another way ofimplementing a FSM.

In robotics there is normally one of two typical kinds of FSMs in use. The first one is anormal FSM described above, and the other one is a Hierarchical State Machine. The differencebetween them is that a Hierarchical State Machine has a recursive structure, which means thateach state in the machine can be a FSM in itself [22].

2.2 ROSThe information in this section is based on the ROS wiki page [3]. Robot Operating System(ROS) is a framework that adds the ROS client libraries and tools to an existing computer oper-ating system such us Linux or Mac OS. The major components and tasks of ROS are:

– Hardware abstraction

– Device driver

– Process communication with passing of messages

– Package management system

– Client libraries

– Implementation of commonly-used functionality

A collection of code that eases the job for the ROS developer is called client libraries. Theseclient libraries general task is to let the programmer write ROS nodes, publish and subscribe totopics, write and call services and use the Parameter Server. The client libraries are provided inthe programming languages C++, Python and Lisp.

In larger robot systems it is possible to run ROS applications on more than one computer,by connecting them via Ethernet, since ROS is a peer to peer network application. ROS im-plements different kinds of communications; synchronous communication via service, asyn-chronous communication via writing and reading messages to a topic, or by saving data to aparameter server.

The ROS file system concepts are organized in stacks, packages and files. See an examplein Figure 2.1.

The example shows some files and directories. The directories actionlib and bfl are ROSpackages, and the directory common is a ROS stack. Each package has a manifest.xml file, thatdescribes the dependencies between packages and also contains a description of the package.A ROS stack has a similar file, called stack.xml. It describes the dependencies between stacks

2.2. ROS 7

Figure 2.1: This figure from the ROS wiki [3] shows the ROS File-system concepts which isorganized in stacks, packages and files.

and also contains a stack description. A ROS package is the lowest level of ROS softwareorganization and can include ROS nodes, a ROS-independent library, a dataset, configurationfiles, a third-party piece of software or anything else that logically constitutes a useful module.A ROS stack is a higher level library with a number of ROS packages.

A ROS node is a process, which is the result of running an executable file in the ROS pack-age. A robot software application contains many ROS nodes, e.g. a node for path calculation, anode for moving the robot arm, one node for preforming localization, another node for movingthe robot and so on. These nodes can communicate with each other by publishing and subscrib-ing messages to topics, by using services (request-response), actionlib (goal-feedback-result),or parameter server.

A topic is a named communication channel where ROS nodes can publish messages, andother nodes can subscribe to them. Topics have anonymous communication semantic, whichmeans that the nodes are not aware of each other. Every topic has a specified type, dependingon the message type.

ROS Nodes can also exchange messages using a request-response model called ROS service.It works as follows: a node sends a request message and waits for the response message fromanother node.

ROS provides a third way for node communication and the ROS Documentation calls itactionlib. It is basically the same as the service communication. The difference between thosetwo is that the actionlib can cancel executing tasks and get updates on processes of ongoingtasks. The actionlib communication is build as a server-client model. The Action client andAction server communicates by topic as shown in Figure 2.2. The ROS topic messages are:

goal - message to send a new goal to the server

cancel - message to send “cancel the ongoing requests” to servers


status - message to notify clients on the current state of every goal in the system.

feedback - message the server sends to the client to get feedback for a goal.

result - message sent by the server to the client when the goal is completed.

Figure 2.2: This figure from the ROS wiki [3] shows how an Action Client and Action Servercommunicates by topics between each other.

Figure 2.3 shows how a user’s code can interact with an Action Client or an Action Server. BothAction Client and Action Server are objects where the user code calls a function. The user codecan also get a response from the Action Client/Action Server object by callback functions.

Figure 2.3: This figure from the ROS wiki [3] shows how user’s code interact with an ActionClient or an Action Server.

The last ROS node communication is saving data at a parameter server. The parameterserver in ROS is a central server to which all ROS nodes can get access. At runtime the nodescan use the parameter server for saving and retrieving data. The data types that can be stored atthe parameter server are :

– 32-bit integers

– booleans

– strings

– doubles

– iso8601 dates

– lists

– base64-encoded binary data

2.3. Multitasking and Preemption 9

2.3 Multitasking and PreemptionMultitasking is a widely used term and an important part in computer systems. Haldar & Ar-avind [11] defines a multitasking system as a system that is able to run many processes at thesame time. For a robot for example, this could mean that an arm and a camera can functionsimultaneously. Modern computers are often capable of multitasking [4].

Systems which are able to execute many processes concurrently need to have a control mech-anism that communicates and synchronizes processes [11]. An example of a framework thatmanages parallel executions is ROS [12].

According to Haldar & Aravind, one system performance improvement is to run many pro-cesses at the same time. This also improves the utilization of the system resources. Manyprograms can be loaded at the same time in the memory to be able to run at the same time.For example program A is actively running in the foreground and program B is working in thebackground. To accomplish running many processes at the same time, the operating systemschedules the processes’ run-time on the CPU [6].

A way to improve system performance is to use preemptive multitasking. The definition ofpreemptive multitasking, according to Haldar & Aravind [11], is that a process is removed fromthe CPU and replaced by another one. Preemptive is said to be just a term that is placed in frontof multitasking system to point out the idea that a computer is very much able to execute manytasks at once [4].

The author of the website “Operativsystem i persondator” (Operating System in personalcomputer) [6] defines a preemptive multitasking system as a system that interrupts now andthen to check the status of the currently running programs and can then terminate a programthat has failed, without slowing down the other operations. An advantage of using preemptivemultitasking is that preemption can happen any time in a preemptive system [11].

Chapter 3

Implementation of the generalframework

This chapter describes the general framework developed in this thesis and how to create it. Italso explains how to create a FSM, using code examples. In addition it shows how to createdifferent kinds of states, and a way to pass data between states by saving and loading data to andfrom the FSM.

3.1 Framework structureThe general framework is as mentioned implemented as a FSM with the state design pattern.Figure 3.1 shows the state design pattern as a UML-diagram. It shows the class objects Finite

Figure 3.1: This figure shows the state design pattern that is used for implementing the FSM asa UML-diagram.

State Machine, State, Locate Fruits, Select Fruit and Pick Fruit. It also describes the structural

11

12 Chapter 3. Implementation of the general framework

relationship between these objects. The state design pattern provides a solution for a goodrelationship between the objects. The objects Locate Fruits, Select Fruit and Pick Fruit are stateimplementations that inherit from the abstract class State and these objects are saved in a hashmap in the object Finite State Machine. An important method they inherit from the abstractclass State is execute. The execute method takes a hash map pointer as parameter andreturns a string. This string is the outcome of a state and is needed in the Finite State Machineobject to map to the next state. Through the parameter (the hash map pointer) it is possible topass data between states by saving and loading data to and from the hash map.

The outcomes from each object that inherits from the abstract State object are saved inthe Finite State Machine object using a hash map for each state. As written before, all statesare saved in a hash map. The transition between states happens by mapping these hash maps.The transition is saved in the Finite State Machine by parsing a yaml [9] file (described insection3.2.1). Before executing the FSM it checks if all transitions work, otherwise the FiniteState Machine exits with an error message.

The Finite State Machine has two other important functions: changeState and pause.The changeState function is a callback function for a ROS topic communication. This func-tion allows to change states from other ROS nodes at runtime, e.g by the error handler. This willalso stop the moving parts, e.g the arm. The function pause uses ROS service communicationto pause and unpause the FSM and the current state.

The Finite State Machine is a ROS node, which makes it possible to use a ROS tool to checkhow ROS nodes communicate with the Finite State Machine through a state interface.

There are two different state interfaces implemented: State and SimpleActionState (see Sec-tions 3.3.1, 3.3.2 and 3.3.3 respectively). These two interfaces makes it possible to create ROScommunication between the Finite State Machine and other ROS nodes (robot behaviors).

3.2 Finite State MachineThe Finite State Machine (FSM) is defined by mapping state outcomes to the next state (i.e.what should be done when a state returns a certain outcome). This is done in a transition file(.yaml) [9] as described in Section3.2.1.

The first thing you have to do to implement a FSM is calling the constructor with a stringarray of outcomes and the length of the array:

/∗ ∗∗ C a l l i n g t h e c o n s t r u c t o r wi th a s t r i n g a r r a y and t h e l e n g h t∗ of t h e a r r a y∗ /

string sm_outcome[] = {"Exit"};StateMachine sm(sm_outcome, 1);

After you created the FSM object, define transitions between states by adding an yaml [9]file to the FSM. Then define the state objects to use, and add them to the FSM (sm.add):

/ / add t r a n s i t i o n f i l e t o t h e f i n i t e s t a t e machinesm.addTransitionFile("transfile.yaml");

/ / De f in e s t a t e o b j e c t sFruitLocState fs;MoveToFruitState mtf;TransportFruitState trf;

3.2. Finite State Machine 13

PickOrderState pos;

/ / add s t a t e s t o t h e f i n i t e s t a t e machinesm.add("Locate_fruits", &fs);sm.add("Pick_fruit", &mtf);sm.add("Fruit_to_basket", &trf);sm.add("Select_fruit", &pos);

Start the Finite State Machine by calling the execute method with the state in which the FSMshould starts the execution.

/ / s t a r t t h e f i n i t e s t a t e machine i n L o c a t e f r u i tstate sm.execute("Locate_fruit");

3.2.1 Transition fileThe transition file defines the transitions between states (which state should be called for eachoutcome of a state). This text file uses yaml [9] format, similar to XML [1]. The advantage touse a text file is the ease of defining a FSM without having to write and compile code (exceptthan for adding new states as described above). For each state, three things have to be specified:

– State label - This is the variable name of the state used in the FSM (sm.add above).

– Descriptive name - A short description of what the robot will do when in this state. Thisis shown in the main GUI under current state.

– Transitions - Here the mapping between outcome and next state is done with the syntaxoutcome : next state. Outcome is defined in each state (see Sections 3.3.2 & 3.3.3).next state must be the state label of a state (i.e. not the descriptive name).

Below is an example of a transition file defining four states. The resulting Finite State Machineis illustrated in Figure 3.2.

− s t a t e :s t a t e l a b e l : L o c a t e f r u i t sd e s c r i p t i v e n am e : L o c i l i z i n g f r u i t st r a n s i t i o n s :No f r u i t : L o c a t e f r u i t sFound f r u i t : S e l e c t f r u i t

− s t a t e :s t a t e l a b e l : S e l e c t f r u i td e s c r i p t i v e n am e : S e l e c t i n g f r u i t to p i c kt r a n s i t i o n s :

F r u i t s e l e c t e d : P i c k f r u i tA l l f r u i t s p i c k ed : E x i t

− s t a t e :s t a t e l a b e l : P i c k f r u i td e s c r i p t i v e n am e : Moving arm to f r u i tt r a n s i t i o n s :


Moving to f r u i t : P i c k f r u i tGoal : F r u i t t o b a s k e t

− s t a t e :s t a t e l a b e l : F r u i t t o b a s k e td e s c r i p t i v e n am e : T r an spo r t i n g f r u i t to ba ske tt r a n s i t i o n s :

Transpo r t f r u i t : F r u i t t o b a s k e tBasket : S e l e c t f r u i t

Figure 3.2: This figure shows the resulting Finite State Machine from the definition above.

3.3 StateThe FSM executes several times per second and repeatedly checks if conditions are met tochange state. Each state performs a task, usually by calling a ROS node, as shown in Figure 3.3,and at each time step returns an outcome (e.g. “moving” or “done”).

There are two different kinds of state interfaces; State and SimpleActionState. States canuse ROS Topics (publish-subscribe) or ROS Services (request-reply) to communicate with theROS node. In the later case, the state sends a request and receives a response from the ROSnode connected to it. A drawback with services is that it uses blocking calls, meaning that themain control program will loose control while the state (or more precise the node connected tothe state) is executed. For tasks that takes significant time SimpleActionState should be used,which utilize ROS actionlib (goal-feedback-result). This kind of state sends a goal (e.g. a goalcoordinate for the arm) to the ROS node and immediately returns control to the FSM. In eachtime step you can get periodic feedback on the progress (e.g. distance to the goal) from the

3.3. State 15

Figure 3.3: The relationship between States and ROS Nodes.

node. When the task is finished, the state will get the result of the task (e.g. if the arm reachedthe goal or not).

3.3.1 State using ROS TopicsWriting and reading from a topic is the easiest ROS communication. Creating a state that canuse this ROS communication is also quite easy. The example in this section shows a state thatshould wait in its state until the user starts the system (i.e. do not begin to move the arm beforethe user says OK)

The first step that we have to do is creating a subscriber in the constructor:

sub_ = nh_.subscribe("sm_ready_msg", 10, &ReadyState::readyMsgCallback, this);

The next step is to create a function that the subscriber can use as a callback function(readyMsgCallback):

void ReadyState::readyMsgCallback(const std_msgs::String::ConstPtr& msg) {ROS_INFO("I heard: [%s]", msg->data.c_str());

if (msg->data == "Start state machine") {/ / The u s e r has r e q u e s t e d autonomous c o n t r o l . L e t s go !

selection = "Start auto";}else if (msg->data == "Start manual control")

/ / The u s e r has r e q u e s t e d manual c o n t r o lselection = "Start manual control";

else/ / L e t s c o n t i n u e t o w a i t


selection = "Not ready";}

The last step done in the execute function:

std::string ReadyState::execute(std::map<std::string, boost::any> * data) {

std::string trans = selection;selection = "Not ready";return trans;

}

3.3.2 State using ROS ServicesWhen using this kind of state, it is important to make sure that the state is not blocked by a callto a ROS node. If the node takes long time to execute it may be better to use SimpleActionState(see section3.3.3). Another solution can be to use threads, enabling the state to make severalcalls to the node and check if it is finished while the node can continue its computations withoutinterruptions.

The example in this section shows how a node retrieves a list of fruit coordinates by sendinga service request to the ROS node fruit localization that performs image analysis and computa-tions of fruit coordinates.

First we have to create a service client in the constructor:

fruit_clt_ = nh_.serviceClient<crops_msgs::Fruits_req> ("fruit_request");

The rest is done in the execute method:

/∗ ∗∗ C r e a t e a s e r v i c e message . In t h i s c a s e we send an empty∗ message . I f you want t o send a p a r a m e t e r t o t h e node you∗ s h o u l d do some th ing l i k e∗ /

srv.request.foo = bar;crops_msgs::Fruits_req srv;

/∗ ∗∗ Send a r e q u e s t t o t h e s e r v e r ( F r u i t L o c a l i z a t i o n ) . Th i s i s∗ where t h e s t a t e machine may hang i f t h e r e p l y t a k e s t ime :∗ /

if (fruit_clt_.call(srv)){/ / Th i s i s t h e r e p l y :fruit_message_ = srv.response.fruit_list;

/∗ ∗∗ We can save t h e d a t a t o t h e s t a t e machine so o t h e r s t a t e s∗ can use i t :∗ /

3.3. State 17

(*data)["fruit"] = fruit_message_;/∗ ∗∗ The d a t a i s i d e n t i f i e d by [ ” f r u i t ” ] and i s used t o l o a d i t∗ from t h e s t a t e machine i n a n o t h e r s t a t e∗ /

/∗ ∗∗ r e t u r n t h e outcome of t h e s t a t e ( used by t h e s t a t e machine∗ t o d e t e r m i n e t h e n e x t s t a t e )∗ /return "Found_fruit";

}else{/∗ ∗∗ The s e r v e r d i d n o t r e s p o n d . Le t t h e s t a t e machine d e t e r m i n e∗ what t o do n e x t ( by r e t u r n i n g a n o t h e r outcome of t h e s t a t e )∗ /ROS_ERROR("Failed to call service Fruits_req");return "No_fruit_detected";

}

3.3.3 SimpleActionState

Below is an example of a state that requires actionlib since the execution takes long time . Thisstate is responsible for moving the arm towards the fruit.

First we define the type of message sent by the state in the constructor:

MoveToFruitState::MoveToFruitState() :SimpleActionState("/move_box_arm_action")

What should be done in the state is defined in the execute method. The first time the stateis run, the coordinates to the fruit we are supposed to pick next (determined previously bypickOrderState and saved as [”fruit loc”]) is loaded from the Finite State Machine:

if (first_){

goal_ = boost::any_cast<crops_msgs::moveArmGoal>((*data)["fruit_loc"]);

first_ = false;}

The action client (in this case the node controlling the arm) can be in one of seven differentstates (not to be confused with states in the FSM): preempted, recalled, rejected, pending, active,lost, or succeeded. It’s possible to define what the state should do in each of these cases, butnormally we only care about if the task has succeeded or not, or if it has preempted (e.g. thearm could not reach the goal pose):


actionlib::SimpleClientGoalState current_state_ = ac_.getState();

switch (current_state_.state_){

case actionlib::SimpleClientGoalState::SUCCEEDED:/ / r e s e t f i r s t so we g e t new c o o r d i n a t e s n e x t t ime

first_ = true;outcome = "Goal";break;

case actionlib::SimpleClientGoalState::PREEMPTED:ROS_WARN("#GUI Arm could not reach the fruit");outcome = "Out of reach";break;

default: / / t h i s c o v e r s a l l f i v e o t h e r a c t i o n s t a t e soutcome = "Moving to fruit";break;

}return outcome; / / r e t u r n t h e s t a t e outcome

3.3.4 Transactions between statesIt is possible to pass data between states by saving and loading the data in the FSM. The datapassing is possible because the FSM sends the same variable-memory by passing the samepointer to each state in the execute-function:

execute(std::map<std::string, boost::any> * data)

This makes it possible to save and load data to and from the variable. The variable is a hashmap, which is a container that stores a combination of a key and data. The key for saving andloading data to map in the FSM is a text string. An example for saving data:

/ / t h e v a r i a b l e t h a t w i l l be saved i n t h e s t a t e machinevector<int> picking_order;

/ / t h i s shows how t o save d a t a i n t h e s t a t e machine(*data)["picking_order"] = picking_order;

picking order is the text string key to which the vector is saved. To load the saved data fromthe FSM:

/ / Get t h e p i c k i n g o r d e r from t h e s t a t e machinepicking_order = boost::any_cast<vector<int> >((*data)["

picking_order"]);

With the key picking order you get the data from the map in the Finite State Machine. Thefunction any cast<vector<int>> makes sure that you get the right type of variable that hasbeen saved in FSM.

Chapter 4

Summary and conclusions

This chapter explains the reasoning behind the design and implementation of the state machine.Furthermore, it describes what has to be kept in mind connecting a state to a behavior, and prosand cons between three different kinds of state transaction

4.1 Design of the Finite State MachineAs mentioned in Section 3.1, the Finite State Machine is implemented with the state designpattern as Buckland suggested [7]. The reason to implement it this way is that it is easy to createa new state without changing the code in the FSM. The only thing that has to be done to create anew state object is to inherit from the abstract class State. The state design pattern also providesthe possibility that states in the FSM can have different algorithms.

The FSM executes about 100 times per second and checks repeatedly if the state conditionshave changed. This behavior of the FSM has been developed because the FSM should handleparallel execution of states/behaviors and according to the definition of an FSM, only one statecan be active at the time. Thats is the reason for developing the FSM as a preemptive multitask-ing system, that checks the status of a program/state periodically. To enable the FSM to handlemultitasking, the state transitions have to be defined so that they allow the FSM to change fromstate to state while checking for the status. It is possible to define the state transitions in a FSMby reading the definitions of transitions from a file, or by hard coding them in the FSM programcode. The problem with how to define transitions in a FSM was solved by reading the state tran-sitions from a text file, in yaml [1] format. A benefit with this is that it is an easy way to define aFSM without having to write and re-compile code. Another benefit of reading from a text file isthat it is easy to switch between two different FSMs, for example one FSM for simulation andone for running the real application. Using hard coded transitions, the developer would have tocreate two different FSMs in this case. Using a text file means that you can just load differentfiles depending on what application you are currently running.

4.2 How to enable preemptive multitaskingRobot behaviors are implemented as one or more ROS nodes as shown in Figure 3.3. This figurealso shows that ROS nodes/behaviors can communicate with the FSM when a state interface, asshown in Chapter 3.3, but the behaviors run independently of the FSM. There are two differentkinds of behavior that are run separately or in parallel; computing and acting.

19

20 Chapter 4. Summary and conclusions

As mentioned above, the FSM uses the state interfaces to check if the behaviors havechanged or not, and what it should do at each timestep. This is important, because the robot’sbehaviors can be interrupted at any time, for example by a human through a graphical interface,or by the error handler.

When you create a state interface there are things you have to think about. These thingscould be which kind of behavior you have to communicate with? Is this behavior an actingor a computing behavior? How long does this behavior run? These questions are importantfor your choice of ROS communication. An example could be the behavior moving the robotarm, which is an acting behavior and takes some time to execute. Therefore it is better to usethe SimpleActionState communication rather than a state using service communication. Thereason why SimpleActionState is better is quite simple - the state using service communicationis blocking communication. There is a way to avoid that by using threads. But then it becomesmore complicated, and by using actionlib you get information about the progress of the action(e.g. how far the arm is to the goal). Getting this continuous feedback is a huge advantage ofactionlib. Yet another advantage is that it is possible to stop the execution anytime. Since theSimpleActionState communication is already implemented it is better to use this communicationmode.

4.3 State transactionsTransactions between states can be implemented in many ways. In ROS there are two possibleways: sending ROS messages between states and saving/retrieving data to and from a parameterserver. As ROS is a distributed system, both ways can be network based. A huge disadvantagewith the parameter server is that it can only save a few data types. Another disadvantage withthe parameter server is that it is not designed for high-performance, it is best used for static,non-binary data such as configuration parameters. A disadvantage with ROS messages could bethat the message has to be of the same data type as the data that is to be passed to another state.This is a minor drawback when much data of different types has to be passed between states.Thus, to be able to save and load all kinds of data types another way of state transactions hadto be implemented for this project. That way is described in details in Section3.3.4. The factthat this way is not network based is not an issue for this project, since states are the only oneswith access to the saved data, and states are always running on the same computer. A minordisadvantage with this way of implementing transactions is that each key can only be used foraccessing one saved data item, an issue easily solved by using different keys.

Chapter 5

Future Development

This chapter describes possible future development, such as automatically creating a state dia-gram from the transition file, and adding code to make it possible to create a hierarchical statemachine.

5.1 State DiagramOne plan for the future in the CROPS Project is to automatically create a state diagram fromthe FSM. The state diagram will help the developers to see if the FSM has the correct statesand transitions defined . This state diagram could be created as a dot file from Graphviz [2].Graphviz is an open source graph visualization software. The features of Graphviz is to takedescriptions of graphs in a simple text language, and make diagrams in useful formats, such asimages, PDF or Postscript for inclusion in other document.

5.2 Hierarchical State MachineAnother plan for the future, but not needed in the CROPS Project, is to change the code so that itis possible to create a Finite State Machine with a recursive structure, also known as HierarchicalState Machine. By using this, more complex robot behaviors can be created by using a task levelarchitecture. For example a robot that vacuums the floor, avodis obstacles, and at the same timecheck the state of the battery.

5.3 Using other robotic frameworksThis is not needed for the CROPS Project, but is another plan for the future, to create moreinterfaces/abstract classes so that it is easier to create the general framework for synchronizingbehaviors in a complex robot. The other robotic frameworks could be Player [23], YARP [10],Orocos [20] and Orca [17]. This feature would make it possible to use the general frameworkfor more robots.

21

22 Chapter 5. Future Development

Bibliography

[1] Extensible markup language (xml). http://www.w3.org/XML/. Accessed May 28, 2012.

[2] Graphviz - graph visualization software. http://www.graphviz.org/. Accessed May 16,2012.

[3] Ros documentation. http://www.ros.org/. Accessed April 1, 2012.

[4] Unix. http://www.ibiblio.org/team/intro/unix/what.html. Accessed April 16, 2012.

[5] Clever robots for crops. http://crops-robots.eu/, 2010. Accessed April 4, 2012.

[6] Per Akesson. Operativsystem i persondator. http://www.abc.se/∼pa/data/os.htm, 1997.Accessed April 16, 2012.

[7] Mat Buckland. Programming Game AI by Example. Jones & Bartlett Publishers, 1 edition,September 2004.

[8] B. Bukovics. Pro WF: Windows Workflow in .NET 4. Apress Series. Apress, 2010.

[9] Clark C. Evans. The official yaml web site. http://yaml.org/. Accessed May 28, 2012.

[10] Paul Fitzpatrick, Giorgio Metta, and Lorenzo Natale. Towards long-lived robot genes.Robotics and Autonomous Systems, 56:29 – 45, 2008.

[11] Sibsankar Haldar and Alex A. Aravind. Operating Systems. Pearson Education, 2010.

[12] Thomas Hellstrom and Ola Ringdahl. A software framework for agricultural and forestryrobotics. In First RHEA International Conference on Robotics and associated High-technologies and Equipment for Agriculture, 2012. Accepted for publication.

[13] J. Jackson. Microsoft robotics studio: A technical introduction. IEEE Robotics & Automa-tion Magazine, 14(4):82–87, December 2007.

[14] David Lafreniere. State machine design in c++. http://www.drdobbs.com/cpp/184401236,2000. Accessed April 16, 2012.

[15] Robin R. Murphy. Introduction to AI Robotics. MIT Press, 2000.

[16] Paul Newman. Introduction to programming with moos. Communications, (November):1–34, 2009.

[17] A. Oreback. A Component Framework for Autonomous Mobile Robots. PhD thesis, RoyalInstitute of Technology, ECS, Stockholm, Sweden, Nov. 2004.

23

24 BIBLIOGRAPHY

[18] Morgan Quigley, Brian Gerkey, Ken Conley, Josh Faust, Tully Foote, Jeremy Leibs, EricBerger, Rob Wheeler, and Andrew Ng. Ros: an open-source robot operating system. 2010.

[19] Bruno Siciliano and Oussama Khatib, editors. Handbook of Robotics. Springer; Har/Dvdredition, 2008.

[20] Peter Soetens. A Software Framework for Real-Time and Distributed Robot and MachineControl. PhD thesis, Department of Mechanical Engineering, Katholieke Universiteit Leu-ven, Belgium, May 2006.

[21] Sebastian Thrun, Dieter Fox, Wolfram Burgard, and Frank Dellaert. Robust monte carlolocalization for mobile robots. Artificial Intelligence, 128(1-2):99–141, 2001.

[22] Ethan Tria-Thompson. Tekkotsu: A rapid developmant framework for robotics. Master’sthesis, Carnegie Mellon University, 2004.

[23] Richard Vaughan. Massively multi-robot simulation in stage. Swarm Intelligence, 2:189–208, 2008.

department of computing science - umeå university · for the project, a software framework...

Documents