SOFTWARE ARCHITECTURE FOR AN AUTONOMOUS CAR SIMULATION USINGROS, MORSE & A QT BASED SOFTWARE FOR CONTROL AND MONITORING

Tiberio Ferreira∗, Olmer Garcıa †∗, Janito Vaqueiro∗

∗Faculdade de Engenharia Mecanica da Universidade Estadual de CampinasRua Mendeleyev, 200Campinas,SP,Brasil

†Ph.D Scholarship PEC/PG CAPES/CNPq-Brazil.

Emails: tiberiusferreira@gmail.com, olmerg@fem.unicamp.br, janito@fem.unicamp.br

Abstract— Cars capable of driving in urban environments autonomously are today one of the most challengingtopics in the intelligent transportation systems (ITS) field. This paper presents the architecture and softwareused in the autonomous car simulation used in the VILMA (Intelligent Vehicle of the Autonomous MobilityLaboratory) project at UNICAMP. It focus on describing each program used in the project and the architecturewhich integrates them. The programs used are MORSE, ROS and a custom program based on the QT framework.Finally, the article presents how some autonomous functions like the control of orientation and velocity of thevehicle are implemented inside the architecture.

Abstract— Carros capazes de dirigir em ambientes urbanos autonomamente sao hoje um dos topicos maisdesafiadores no campo de sistemas inteligentes de transporte (ITS). Este artigo apresenta a arquitetura e softwareusados no simulador de carro autonomo do projeto VILMA (Veıculo Inteligente do Laboratorio de MobilidadeAutonoma) da UNICAMP. Este se foca em descrever cada programa usado no projeto, e a arquitetura que osintegra. Os programas usados sao o MORSE, o ROS e um programa customizado criado a partir da frameworkQT. Finalmente, o artigo apresenta como algumas funcoes autonomas como o controle de orientacao e velocidadedo veıculo estao implementadas dentro da arquitetura.

Keywords— Autonomous robotics, MORSE, ROS, Vehicle.

1 INTRODUCTION

Autonomous cars play an important role in cur-rent robotics and artificial intelligence research.The development of driverless cars started in thelate ’70s and ’80s. Ernst Dickmann’s MercedesBenz achieved a travel velocity of 100 km/h onrestricted highways without traffic (Dickmannset al., 1994). In the DARPA Grand Challenge2005, autonomous cars drove off-road on desertterrain, several of them reaching the finish line(Thrun et al., 2006). DARPA’s Urban Challengeof 2007 demonstrated that intelligent cars are ableto handle urban scenarios and situations with sim-ulated traffic (Urmson et al., 2007). Lately, au-tonomous cars have been driving through realworld traffic for testing purposes in urban and ru-ral areas alike (Thrun, 2010).

In mid-2008, interested in robotics and ve-hicular safety, the Laboratory for AutonomousMobility (LMA) was formed in the DMC-FEMat UNICAMP. Taking into account the model of(Siegwart et al., 2011), the three main lines ofstudy are: Perception, Navigation and Control.According to (Thrun et al., 2006), these systemscan be organized into six major functional groups:Interface, Sensors, Perception, Control, VehicleInterface and User Interface. The figure 1 showshow these blocks interact and for this article theenvironment and the vehicle will be replaced bya model in a simulator. The algorithm of theblocks of speed and path controllers are tackledin this article. It is important to note that even

though this article focuses on MORSE as it’s sim-ulator, there are other simulators worth noting forrobotic applications. Two of simulators tried bythe project were V-REP (Freese et al., 2010) andGazebo (Koenig and Howard, 2004).

Figure 1: Architecture of the autonomous car ofLMA

The paper is presented in four section. Thefirst one presents the implementation of theMORSE simulator and the Robot Operating Sys-tem (ROS) as well as the data exchange waysbetween them to the necessities of the project.The second one explains the back end and frontend design of the software developed. The third

one explains the algorithms and theory of the au-tonomous function of the software and presentsresults obtained. Finally, some conclusions andfutures work are presented.

2 Simulator integration

The software architecture of the simulator is basedon two softwares: ROS (Robotics Operating Sys-tem) (Quigley et al., 2009) and the MORSEsimulator (Echeverria et al., 2011) (Lemaignanet al., 2012). Both will be introduced below aswell as the way they exchange information withthe supervisory software.

2.1 MORSE - The Simulator

The project architecture uses MORSE as the sim-ulator framework in which the car is developed.MORSE is derived from Blender’s Game Engineand so Blender can be used to create vehicle mod-els to be used inside MORSE. It allows for variousparameters to be set in order to have a realisticsimulation, such as mass, gravity center, suspen-sion damping, suspension stiffness and roll influ-ence. As Blender uses python as it’s scripting lan-guage, scripting is easy and fast. It’s based ona game engine and because of that it has varioustools for debugging, optimization and real time re-alistic physics based on the widely known Bulletphysics library (Coumans et al., 2013). MORSEand Blender are both open source, which makes itpossible to add any missing critical feature for theproject. MORSE also has many common sensorsbuilt into it. Most sensors used in UNICAMP’sVILMA car were readily available from MORSE’slibrary, such as GPS, stereo cameras and inertialmeasuring unit. MORSE also provides data fromthe Blender Game Engine itself which can be usedto build any sensor necessary by getting the orig-inal accurate data and adding some kind of noise,such as Gaussian noise.

The model of the car in the Blender GameEngine(BGE) is based on scripts. There is a carsetup script which sets the car wheels’ and sus-pension parameters which are: stiffness, damp-ing, compression, roll influence, tire grip and sus-pension height. Once the setup is complete, theBGE constantly executes scripts which change thecar steering, engine power and brake according tovalues stored in variables at runtime. These vari-ables are the ones which allow external programsto communicate with the car and control him be-cause they are accessible to external applicationthrough MORSE’s socket interface. This way, ifan external application sends a new steering an-gle, all MORSE does is rewrite the variable corre-sponding to it at runtime and the scripts take careof actually changing the vehicle physical state. Inaddition the BGE also listens to keyboard events.

This way the vehicle can also be controlled usingthe keyboard.

2.2 ROS - The Middle-ware and Toolbox

ROS acts as a middle-ware and toolbox. ROSprovides multiple libraries which are commonlyused in robotic problems. Some of those usedin the VILMA project include PID implemen-tations, communication interfaces between pro-grams with it’s own publisher-subscriber system,callback queues, clocks and timers. In additionit also provides multiple graphical user interfacestools to aid development. Some of those toolsare used in the VILMA project to view real-timedetails of messages being published to other pro-grams, to interpret sensor information such as acloud of points (typical for LIDAR sensors) andto process video stream.

2.3 Data exchange between the softwares

The interaction with MORSE is done throughROS Topics and sockets. ROS collects data fromthe simulation and makes it available to exter-nal programs using it’s topic-subscriber interface.Sockets are used to send commands to the car in-side the simulator. Sockets are used instead ofROS topics because the interface to do so us-ing sockets was already developed by USP/SCand used in their CaRINA project (Fernandeset al., 2014). Also, using sockets demonstrateshow different middle-wares can be used togetherwithout problems as seen in the Figure 2.

Figure 2: Simplified simulation architecture.

3 Software used for Control andMonitoring

A software to serve as base to the development ofthe control and monitoring algorithms was devel-oped using the QT framework (Ward, 2000).

3.1 front-end Design

The front-end shown in Figure 3 makes use ofthe QT framework, because of that it is portable

across multiple platforms, mainly Windows, Linuxand OSX. Being open source it could be ported tovirtually any platform.

Figure 3: Front-end currently in use in theVILMA project at UNICAMP.

The tools available in the front-end are dis-playing of data from the sensors in real time, dis-playing of the current value being sent to the caractuators and plotting data in real time from val-ues received by the sensors in a separate windowas show in Figure 12. This plotting tool allowsfor different plots to be on a single window at thesame time. In the VILMA project this is used byfirst plotting a desired trajectory and then plot-ting the car position every 250ms. This way thealgorithms controlling the vehicle can be testedand it’s results validated graphically.

The frequency of update of the values dis-played on the screen such as position, orientationand velocity, are controlled internally by a timerwhich when triggered queries each sensor module,gets an updated value and updates the displayedimage.

3.2 back-end

The current architecture of the back-end is highlymodular and is by design easy to adapt to anyproject using ROS or any other middleware. Foreach sensor there is a module which receives datafrom ROS, but since each module has it’s ownclass other middlewares can be used without anymodification of the program architecture. Themodules communicate with each other using func-tions of the type get and set. Therefore the inter-nals of each class are not important to the othermodules as long as the API is kept the same.

The current modules in use in the project are:

GPS Collects data from the car GPS.

IMU Collects data from the car IMU.

MainWindow Responsible for the main appli-cation window. Generates requests to each

sensor module and updates the screen accord-ingly. Also receives user input and sends mes-sages to the appropriate module.

MorseReceiver Module responsable for receiv-ing messages from the MORSE simulator us-ing ROS as middleware. Among some of themessages are the car exact speed and posi-tion.

MorseTransmiter Module responsible for send-ing messages to the MORSE simulator usingsockets.

VilmaSelfDriver Module responsible for the au-tonomous functions of the vehicle.

Figure 4: Modules communication scheme.

4 Control of the vehicle

This section explain the implementation of theclass VilmaSelfDriver. This class controls the ve-hicle according to the instructions given in theuser interface. It supports controlling the carspeed by giving a new value or taking the currentvehicle speed as input. It also allows the vehicleto follow pre-determined trajectories described bya series of waypoints. These waypoints can be en-tered manually in a table in the user interface orloaded from a pre-recorded text file.

4.1 Trajectory

Trajectories can be taken by recording the car po-sition at regular time intervals. The one shownin Figure 12 was taken at a 250ms interval. Eventhough the user interface does not yet support aneasy way to record the vehicle position, the back-end support is already in place and in a futurepatch there will be support for it in the user in-terface. However, trajectories could also be gen-erated using maps and could contain hard turnsor a small number of points.

In order to solve these problems two func-tion which can be accessed by the user inter-face were created. The first one, accessed by the

Smooth Trajectory button, takes the trajectorypoints from the table and tries to make the hardturns smoother. For that, the algorithm utilizestwo criteria, minimizing the quadratic differencefrom a new point’s position to it’s older positionand minimizing the distance between consecutivepoints. While the first condition ensures fidelityto the original points, the second one turns thetrajectory into a smoother one. How strongly onecondition is obeyed in detriment of the other canbe changed, this way the algorithm is very versa-tile. The results can be compared in Figures 5 and6. The algorithm used to smooth the trajectory

Figure 5: Before Smoothing Algorithm.

Figure 6: After Smoothing Algorithm

is the following:

Data: X,ε,α,βResult: YY 0 = X;while EQM > ε do

EQM ← 0;for i := 2 to n− 1 do

tmp← Y ki

Y ki ← Y k−1

i + α(X − Y k−1);

Y ki ← Y k

i + β(Y ki+1 + Y k

i−1 − 2Y ki

);

EQM ← EQM +∣∣tmp− Y k

i

∣∣;end

endAlgorithm 1: Gradient algorithm to smooththe path

Where ε is the tolerance used as a stoppingpoint in the iteration, the value used in the projectis 0.000001. X are the points corresponding to thepath before being ran by algorithm, Y the pointsafter being ran by the algorithm. α is the termwhich controls the fidelity to the original points,the higher the closer the resulting points are tothe original points. In the project it is set to 0.5. β is the term which controls the smoothness ofthe new trajectory, the higher the closer the pointswill be. In the project the value of β is 0.1 . Also,n is the number o points of a trajectory.

Another problem that occurs is lack of pointsto create a proper trajectory. In order to solvethis problem there is an algorithm, accessible bythe user interface, which interpolates linearly be-tween points in order to create a better defined

trajectory. The result can be viewed in Figures 7,and 8.

Figure 7: In red the trajectory before IncreasePoint Count Algorithm and in purple after theIncrease Point Count Algorithm

Figure 8: After Increase Point Count Algorithmand Smoothing Algorithm

4.2 Path Controller

The path controller is implemented as a functionwhich takes as input the first point (X,Y) of thetrajectory and calculates the required turn anglewhich the front wheels require to get to that point.In order to do so, the algorithm subtracts the carcoordinates from the desired coordinates, result-ing in coordinates in respect to the car center, acoordinate system with the car at the origin. Afterthis, desired car angle is obtained by calculatingthe tangent of the angle formed by the two newcoordinates in respect to the car center. Finally,the resulting wheel angle is obtained by taking ac-count the current car orientation and subtractingit from the desired car angle. When a point isreached it is behind the vehicle. The algorithmdetects it and proceeds to the next point of thetrajectory. The algorithm gets the vehicle currentangle from the IMU or MORSE’s report of the ve-hicle’s exact orientation. The vehicle’s position isobtained from the GPS sensor or MORSE’s exact(X,Y,Z) report. Checking if the point is behindthe car or not is done by evaluating if the abso-lute value of the angle required to turn is greaterthan 0.6 rad. If it is, the point is behind the ve-

hicle. A proportional controller is used to controlthe wheel’s angle. The algorithm used is the fol-lowing:

1: procedure Turn-Wheels(X,Y,CarX,CarY,CarZAngle)

2: newX ← X − CarX3: newY ← Y − CarY4: ang ← atan(newX/newY)5: ang ←ModAng(ang)6: if ang ≥ 0.6 or ang ≤ −0.6 then

start over with next pointend

7: return ang8: end procedure

Figure 9: Algorithm to determine the wheel anglein order to get to a given point.

4.3 Speed Controller

The speed controller is implemented as a func-tion which takes as input the desired speed, thecar current speed and the amount of power be-ing sent to the wheels. It utilizes a PID (Propor-tional, Derivative, Integrator) controller in orderto control the amount of power sent to the wheels.The PID gains are dynamically adjusted based onthe difference between the desired speed and thecurrent speed (Delta) and the current wheel de-flection.

Figure 10: PID parameters

There is also a limit on the maximum allowedchange of power sent to the wheels per second.This limit is imposed in order to prevent suddenaccelerations and decelerations. The PID con-troller is predominantly proportional if the Deltais bigger than one meter per second and predomi-nantly integral if the Delta is smaller than one.There is also a limit on the power sent to thewheels at any given time based on the desiredspeed. This is used to prevent large overshooting.

This approach allows a steady accelerationuntil the speed is not close to the target speed.When the speed is close to the target, the integralcontroller takes over and allows for a smootherconvergence to the target speed. If at any giventime the Delta switches from lower than one tobigger than one, the controller has it’s parame-ters reset and is set to mostly proportional again.This usually happens when there is disturbancesuch as the vehicle taking a hard turn. Resetting

the parameters allows for a reliable response fromthe controller because it cleans the residual inte-gral value stored from before the disturbance. Thealgorithm also tries to compensate for the varia-tions of velocity when the car is turning by addinga term proportional to the current front wheel an-gle in the PID’s parameters.

4.4 Results

The speed controller performance can be seen inFigure 11. The vehicle currently takes about 15seconds to get to the desired speed, but the ac-celeration can be increased or lowered by chang-ing the proportional PID parameter. The threegraphs are very close in form to one another, butit can be noticed that as the desired speed in-creases, the lower the overshoot is. This happensbecause the maximum power sent to the wheelsis implemented to scale linearly with the desiredspeed. However, the resistance faced by the ve-hicle to accelerate scales quadratically. In a fu-ture development this issue will be addressed andwhen the algorithm is implemented in the real car,experimental data will be used to determine themaximum output to a given desired speed.

Figure 11: PID control performance for targetspeed of 5[m/s], 10[m/s] and 15[m/s] in a straightline.

The results using the path controller can beseen in Figure 12 which shows the planned pathand the actual path.

The results of the path controller are shownin Figure 12. For a reasonably high density ofpoints the algorithm provides very good resultswith error in the order of centimeters even whencomplicated trajectories previously recorded areused. Although the algorithm provides good re-sults, it has it’s drawbacks, such as not takinginto account the wheel turn speed and modify-ing the angle of the wheels at each point used inthe trajectory leading to continuous small changeswhich can cause discomfort and mechanical com-ponents can suffer fatigue. One solution is to in-crease the number of points of the trajectory untilthe changes in wheel angle become so small that it

seems as it is a continuous transition. The prob-lem with this solution is that the algorithm mustbe run at a high frequency in order to keep upwith the large number of points and also the carposition information must also be updated at thesame rate or higher. Which means the positionestimation algorithms would also need to run at ahigh frequency.

Figure 12: Car position in blue and desired tra-jectory in red.

5 CONCLUSION

Because of the modular architecture used in thisproject it can be easily adapted to other projects.Having MORSE as a module makes it possibleto exchange the simulator for the real car with-out big changes. Since the path controller makesno assumptions about the car it is expected towork properly with all ground vehicles moving ina plane. The speed controller is based on PID con-trols with variable parameters, so other projectscan reuse the code only changing the parametersin order to match the different vehicle dynamics.MORSE supports many vehicles, including flyingvehicles and aquatic. This way, MORSE and allthe work reported here can be a starting point toother projects.

Perception using vision to implement a lo-cal path planning is the next step in the project.This way the vehicle will be able to plan it’s ownpaths knowing what happening in the environ-ment. Also planned are refinements to the cur-rent path following and speed control algorithms.All the work done so far is hosted publicly atwww.github.com/tiberiusferreira/VilmaProject.

ACKNOWLEDGMENT

This work has been cofounded by CAPES and byCNPq Brazil. Part of the code used in MORSEsimulator is based in CaRINA PROJECT of theLaboratory Mobile Robotics from USP/SC.

References

Coumans, E. et al. (2013). Bullet physics library,Open source: bulletphysics. org .

Dickmanns, E. D., Behringer, R., Dickmanns,D., Hildebrandt, T., Maurer, M., Thomanek,F. and Schiehlen, J. (1994). The seeingpassenger car’vamors-p’, Intelligent Vehicles’94 Symposium, Proceedings of the, IEEE,pp. 68–73.

Echeverria, G., Lassabe, N., Degroote, A. andLemaignan, S. (2011). Modular open robotssimulation engine: Morse, Robotics and Au-tomation (ICRA), 2011 IEEE InternationalConference on, IEEE, pp. 46–51.

Fernandes, L. C., Souza, J. R., Pessin, G., Shin-zato, P. Y., Sales, D., Mendes, C., Prado, M.,Klaser, R., Magalhaes, A. C., Hata, A. et al.(2014). Carina intelligent robotic car: Archi-tectural design and applications, Journal ofSystems Architecture 60(4): 372–392.

Freese, M., Singh, S., Ozaki, F. and Matsuhira, N.(2010). Virtual robot experimentation plat-form v-rep: a versatile 3d robot simulator,Simulation, modeling, and programming forautonomous robots, Springer, pp. 51–62.

Koenig, N. and Howard, A. (2004). Design anduse paradigms for gazebo, an open-sourcemulti-robot simulator, Intelligent Robots andSystems, 2004.(IROS 2004). Proceedings.2004 IEEE/RSJ International Conferenceon, Vol. 3, IEEE, pp. 2149–2154.

Lemaignan, S., Echeverria, G., Karg, M.,Mainprice, J., Kirsch, A. and Alami, R.(2012). Human-robot interaction in themorse simulator, Proceedings of the seventhannual ACM/IEEE international conferenceon Human-Robot Interaction, ACM, pp. 181–182.

Quigley, M., Conley, K., Gerkey, B., Faust, J.,Foote, T., Leibs, J., Wheeler, R. and Ng,A. Y. (2009). Ros: an open-source robotoperating system, ICRA workshop on opensource software, Vol. 3, p. 5.

Siegwart, R., Nourbakhsh, I. R. and Scaramuzza,D. (2011). Introduction to autonomous mo-bile robots, second edn, The MIT Press.

Thrun, S. (2010). What we’re driving at, TheOfficial Google Blog .

Thrun, S., Montemerlo, M., Dahlkamp, H.,Stavens, D., Aron, A., Diebel, J., Fong,P., Gale, J., Halpenny, M., Hoffmann, G.et al. (2006). Stanley: The robot that wonthe darpa grand challenge, Journal of fieldRobotics 23(9): 661–692.

Urmson, C., Bagnell, J. A., Baker, C. R., Hebert,M., Kelly, A., Rajkumar, R., Rybski, P. E.,Scherer, S., Simmons, R., Singh, S. et al.(2007). Tartan racing: A multi-modal ap-proach to the darpa urban challenge.

Ward, P. (2000). QT Programming for Linux andWindows, Prentice Hall PTR, Upper SaddleRiver, NJ, USA.