EXPROBLAB_Assignment #1

Code Documentation

Introduction

The assignment is the realization of an algorithm to make the robot move in an environment and have a surveillance behaviour to choose the way to move. The invironment is composed of locations that are connected in a certain way depending on the choice of the user. The goal is to build a Finite State Machine (FSM), with the use of SMACH, that allows the robot to choose the behaviour to adopt depending on the situation. The robot is also provided a battery, periodically checked, that needs to be recharged after some time of using.
The locations are divided into:

• room: location with one door;
• corridor: location with at least two door.

The entities that connect two locations are called doors and the entity that moves in the environment is the robot, called Robot1.

Folder organization

This repository contains a ROS package named EXPROBLAB_Assignment1 that includes the following resources.

• CMakeLists.txt: File to configure this package.
• package.xml: File to configure this package.
• setup.py: File to import python modules from the utilities folder into the files in the script folder.
• launch/: Contains the configuration to launch this package.
  • assignment.launch
• msg/: It contains the message exchanged through ROS topics.
  • Point.msg: It is the message representing a 2D point.
• action/: It contains the definition of each action server used by this software.
  • Plan.action: It defines the goal, feedback and results concerning motion planning.
  • Control.action: It defines the goal, feedback and results concerning motion controlling.
• scripts/: It contains the implementation of each software components.
  • speech.py: It is a dummy implementation of the speech-based commands detection algorithm.
  • gesture.py: It is a dummy implementation of the gesture-based commands detection algorithm.
  • robot_state.py: It implements the robot state including: current position, and battery level.
  • planner.py: It is a dummy implementation of a motion planner.
  • controller.py: It is a dummy implementation of a motion controller.
• utilities/EXPROBLAB_Assignment1/: It contains auxiliary python files, which are exploited by the files in the scripts folder.
  • name_mapper.py: It contains the name of each node, topic, server, actions and parameters used in this architecture, if any.
• docs/: It contains the HTML documentation of the package.
• topology/: It contains the starting ontology used in the package which is modified in the initial state to build the new environment for the assignment.
  • topological_map.owl: topological map from which the environment is built.
• images/: It contains the diagrams and images shown in this README file.

Software Architecture

The main software, the one of the Finite State Machine is composed of four states:

• Init State;
• Reasoner State;
• Move Random State;
• Recharge State.

This structure can be seen by the following image:

Where the Move Random State represents a sub finite state machine, which means that it is composed in turn of other states, in particular:

• Plan Path To Location State;
• Go To Location To Visit State.

The graph above is taken from the automatic SMACH viewer and by purpose it does not include all the transitions of the states (to make it more readable). In order to evaluate the entire graph correctly and see all the transitions clearly we provided one by drawing it and highlighting the transitions in a better way.

In order to retrieve the automatic viewer from ros

rosrun smach_viewer smach_viewer.py

The hand-drawn graph can be seen in the following image:

Here all the transitions can be clearly read and it is highlighted also the sub finite state machine used with the two inner states in details.

Temporal diagram

Here there is the temporal diagram of the component interaction:

It was put all the component of the architecture and it was highlighted the battery low case. This is a trivial case, which is always checked. It in fact make the system to return to the FSM immediately and to recharge the robot, according to the actual conditions the robot is into.
The other components are active always but they do something only when the FSM, so the main program, ask them to do. In this way the interaction is short and do no interfer with the normal flow of the program.

Software components

It follows the details of the software components used in the program, which are available in the scripts folder.

The FSM Node

The FSM node implements the behaviour of the machine. It is used to make the robot move with a certain behaviour inside the environment. It communicates mainly with the aRMRO server to which it sents query to ask for information about the robot actual positiopn, the possible locations that can be reach, the currently urgent locations for example. Then, from this server it receives the result of the query which are basically a serires of list of objects or some boolean returns.
The other components it communicates with are the planner and the controller. To these servers, it sends some requests and it receives a result that is later analyzed and used in the program. In particular:

• with the planner:

  • request: Point type with a target and a goal position for the path to be computed;
  • response: Point [] list with the via points the robot will have to follow;

• with the controller:

  • request: Point [] list with the via points the robot will have to follow;
  • response: status of the server which explains when it has finished or not to simulate the movement of the robot through the via points.

The planner Node

The planner node implements an action server called motion/planner. This is done by the means of the SimpleActionServer class based on the Plan action message. This action server requires a start and a target position passed as two fields of the goal.
Given the goal parameters this component return a plan as a list of via_points, which are computed by spacing linearly the distance between the two x and y coordinates of the points. The number of via_points can be modified thanks to the parameter in the name_mapper.py file.
When a new via_points is generated, the updated plan is provided as feedback. When all the via_points have been generated, the plan is provided as results.

The controller Node

The controller node implements an action server named motion/controller. This is done by the means of the SimpleActionServer class based on the Control action message. This action server requires the plan given as a list of via_points by the planner.
Given the plan the controller iterates for each planned via_points and waits to simulate the time spent to move the robot.
Each time a via_point is reached the a feedback is provided. When the last via_point is reached, the action service provides a result by propagating the current robot position.

The program starts in the Init State which initializes the ontology (the environmemt). Then this state is no longer executed. The program passes to the Reasoner State which reasons the changes:

• the actual robot position;
• the timestamps representing the last time a location has been visited by the robot.

The FSM enter now the sub-FSM, which first computes a path from the actual location of the robot to a reachable location and then moves the robot to that location.
The loop of Reasoner - Plan Path - Go To Location is repeated in an infinite loop until the robot gets a stimulus:

• battery low: which means that the robot needs to be recharged. To do so, the robot is firstly driven to the dedicated location and then is recharged. At the end it starts again the loop;
• urgent location: after some time a location is not visited by the robot it becomes urgent and if the robot can reach it by its actual position it has to go there.

The code is also composed of an Helper which is a class containing all the shared variables and the mutex to allow the FSM states access them correctly. This class also manages the initialization and the share of the clinents for the respective servers.

In the figure above there is the presentation of how the components used in the software interact among them. In particular:

• armor server: it is the server used to provide the computations related to the ontology. It communicates through queries that are sent from the Finite State Machine to the server and this in turn gives a result then analyzed in the FSM.
• planner: it is responsible of computing the path and communicates through the Plan Action sent. It returns a plan which is the result of the server.
• controller: it is responsible of moving the robot along the path and communicates through the Control Action sent. It updates the actual position of the robot through the via points thanks to the server result.

For the complete documentation of the aRMOR serve check its github repository.

Installation and Running Procedure

Run by roslaunch file

In order to run the program by the roslaunch file we first need to install the xterm tool by following these steps:

sudo apt-get update
sudo apt-get -y install xterm

Now to run the code we just type these on a terminal:

roslaunch EXPROBLAB_Assignment1 assignment.launch

Run each single node manually

In order to run the code without using the roslaunch:

roscore &

# Terminal A: aRMOR Server
rosrun armor execute it.emarolab.armor.ARMORMainService

# Terminal B: Planner Server
rosrun EXPROBLAB_Assignment1 planner.py

# Terminal C: Controller Server
rosrun EXPROBLAB_Assignment1 controller.py

# Terminal D: FSM node
rosrun EXPROBLAB_Assignment1 fsm.py

Video

The video below represents a part of the program during its normal execution. As the video starts there are four sections in the window:

• top-left: it is the aRMOR Server window and it is used to make the query and communicate with the FSM program;
• top-right: it is the FSM node. Here the program shows its log messages and it is the one that has to be read to better understand the program execution.
  It shows us which state the FSM is into, what it is doing into ithe state and the changing action that provides the pass from one state to another;
• bottom-left: it is the planner server that computes the path from the start postion to the target one and publish the result;
• bottom-right: it is the controller server that simualtes the movement of the robot by changing its position through the points of the path.

Here there is the gif:

The video begins with the starting of all the points, then the FSM enters in the PlanPathToPostion so the planner publishes the result. Then it goes in the GoToLocationToVisit state and controller server moves the robot. In the new location the robot waits for some instants, defined as a parameter in the file as 5 seconds, and then the FSM reasons the changes in the ontology through the reasoner state: where it is and which lcoation it can now reach.
After that, it plans again and starts moving randomly again.
The robot has also to reach some urgent locations that in the video are the possible rooms Rn that are shown as destination while it is in the corridors. Once visited the robot return to the corridor and goes on with its behaviour.
Just as a video simulation, the autonomy of the battery is set to 15 seconds and the urgent parameter for the location to 5 seconds.

Surveillance Policy adopted

When the robot’s battery is not low, it should move among locations with this policy:

• it has to stay mainly in the corridors: E, C1 and C2 in this case;
• if a reachable location has not been visited for some times, which means that it has become urgent, then is has to go there immediately.

In the video the urgent location is the X one. The robot then move to the corridor again since it is the only location reachable from that room in this particular ontology.
The FSM is then brought to execute the Recharge state, so the robot is firstly driven to this location, E and then it is recharged (as it can be read from the comments in the video).
Just as a video demonstration the battery is set to need a recharge after 15 seconds of execution so that the user can see how the robot behaves in case of battery low.
The normal autonomy of the battery is set to one minute of program execution, after which the robot has to recharge to go on with its surveillance behaviour.

Working Hypothesis and Environment

Environment

The ontology that we initialized in this assignment is the following:

The environment used and initialized is supposed to be consinstent with the real one, so that the reasoner can always find a consinstent ontology to work on.
Moreover, it is also assumed that all corridors, E, C1 and C2 are connected together. In this way the robot is able to perform its surveillance policy correctly.
Also for more difficult environment, so for bigger ontologies it is assumed that all the corridors are connected together and at least with the recharging location.
It was also assumed that each location is associated to a spicific coordinate point composed of x and y value. These are the values that are used in the planner to compute the path. The values of the coordinates are taken from the name_mapper.py file and are also shown in the figure above. Of course it is not taken into account the actual dimension of the environment, but they are just used to have a reference for the path computation.

The urgent parameter that allows the location to have the priority to be visited is set to 15 seconds. All the location that have not been visited for more than 15 seconds become urgent and have the priority. Of course, if the robot has not enough battery the priority is taken by the recharging room E that is the one to be reached as soon as possible.

Robot

The robot starts in the Recharging Room which is in this case the E one. From here it move randomly, which means that it checks the reachable location from its actual one and then chooses randomly among the list of possibilities.
The robot is also assumed to have an autonomy of 60 seconds, after which it needs to be recharged. It is assumed that the robot needs to reach the proper location before being recharged and also this reaching transition is performed randomly at the beginning when the robot cannot reach the recharging location; as soon as it has the possibility to reach it, then the choice is imposed to this one.
The robot has an autonomy battery of 60 seconds after which it has to be recharged before continuing with its normal behaviour.

System Features

In order to build the system it was opted to build a robust code. In fact, when the robot has a low battery level its only goal is to reach the E room: as it was more than an urgent location. This means that the robot could move around for a high amount of time if it did not detect the recharing room immediately, but this behaviour would not affect neither the surveillance problem of the robot nor the physical constraints: the robot could not be teletransported from a room to another so need to have the physical possibility to reach that new location.

System Limitation

One of the possible limitation of the code is the possibility of having the robot moving randomly for a long time before reaching the recharging room when needed which could be tricky from a physical point of view since the robot could not have all that amount of battery left. Howeverm this approach is preferred with respect to another one since it is robuster and respects the physics behind the robot movement.

Possible technical improvements

A possible improvement could be the possibility to make the robot remember a path from each location to the recharging room so that in case of low battery it can reach the location without having to choose randomly and thus reducing the time wasting due to the random choice.
Moreover, if the robot had to compute the path it would have to compute it by checking all the possible paths and take the most optimal one thus avoiding loosing time with not useful movements and not have to waste important battery resource.

Authors and Contacts

Matteo Maragliano (S4636216)
S4636216@studenti.unige.it