\documentclass[11pt,twoside,openright,a4paper]{report}
\usepackage[DEM,newLogo,final]{uaThesis} %acrescentar "final" para remover o "Documento Provisorio"

\def\ThesisYear{2015}
\usepackage{graphicx}
\usepackage{caption}
\usepackage{subcaption}
\graphicspath{{images/}}
\usepackage[hyphens]{url} %breaks urls with hypen
\usepackage{gensymb}
\usepackage{listings} % Include the listings-package for code snippets
%\usepackage{url}
% optional packages
%\usepackage[portuguese]{babel}
\usepackage{url}
\usepackage{hyperref}
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{xspace}% used by \sigla
\usepackage{epstopdf} % server para colocar imagens em formato eps
\usepackage{pgfplots} %graficos
\renewcommand{\bibname}{References}%troca bibliography para references
\usepackage{xcolor}
% optional (comment to use default)s
%   depth of the table of contents
%     1 ... chapther and sections
%     2 ... chapters, sections, and subsections
%     3 ... chapters, sections, subsections, and subsubsections
\setcounter{tocdepth}{3}

% optional (comment to used default)
%   horizontal line to separate floats (figures and tables) from text
\def\topfigrule{\kern 7.8pt \hrule width\textwidth\kern -8.2pt\relax}
\def\dblfigrule{\kern 7.8pt \hrule width\textwidth\kern -8.2pt\relax}
\def\botfigrule{\kern -7.8pt \hrule width\textwidth\kern 8.2pt\relax}

% custom macros (could also be defined using \newcommand)
\def\I{\mathtt{i}}         % one possible way to represent $\sqrt{-1}$
\def\Exp#1{e^{2\pi\I #1}}  % argument inside braces, i.e., "{}"
\def\EXP#1.{e^{2\pi\I #1}} % argument finishes when a full stop is encountered, i.e., "."
\def\sigla{\LaTeX\xspace}  % use as "blabla \sigla blabla (no need to do "blabla \sigla\ blabla"

\def\AddVMargin#1{\setbox0=\hbox{#1}%
                  \dimen0=\ht0\advance\dimen0 by 2pt\ht0=\dimen0%
                  \dimen0=\dp0\advance\dimen0 by 2pt\dp0=\dimen0%
                  \box0}   % add extra vertical space above and below the argument (#1)
\def\Header#1#2{\setbox1=\hbox{#1}\setbox2=\hbox{#2}%
           \ifdim\wd1>\wd2\dimen0=\wd1\else\dimen0=\wd2\fi%
           \AddVMargin{\parbox{\dimen0}{\centering #1\\#2}}} % put #1 on top #2


\begin{document}

%
% Cover page (use only one of the first two \TitlePage)
%

% First alternative, with a figure
\TitlePage
  %\GRID  % for debugging ONLY
  \HEADER{\BAR\FIG{\includegraphics[height=60mm]{uaLogoNew}}} % the \FIG{} is optional
         {\ThesisYear}
  \TITLE{Ivan \newline dos Santos Ferreira}
        {Integration of the planning, global localization and path execution in autonomous cars}
  \TITLE{}
  		{\newline}  
  \TITLE{}
  	    {Integra\c c\~ao do planeamento, localiza\c c\~ao global e execu\c c\~ao de caminhos na navega\c c\~ao de autom\' oveis aut\' onomos}
\EndTitlePage
\titlepage\ \endtitlepage % empty page

%% Second alternative, with a citation
%\TitlePage
%  %\GRID  % for debugging ONLY
%  \HEADER{\BAR\FIG{\begin{minipage}{50mm} % no more than 120mm
%          ``I'm King of the world.''
%           \begin{flushright}
%            --- Jack Nicholson
%           \end{flushright}
%          \end{minipage}}}
%         {\ThesisYear}
%  \TITLE{Ivan \newline Ferreira}
%        {Integrated manager of navigation missions in autonomous cars}
%\EndTitlePage
%\titlepage\ \endtitlepage % empty page


%
% Initial thesis pages
%
\TitlePage
  \HEADER{}{\ThesisYear}
  \TITLE{Ivan \newline dos Santos Ferreira}
        {Integration of the planning, global localization and path execution in autonomous cars}
  \TITLE{}
  		{\newline}  
  \TITLE{}
  	    {Integra\c c\~ao do planeamento, localiza\c c\~ao global e execu\c c\~ao de caminhos na navega\c c\~ao de autom\' oveis aut\' onomos}
  \vspace*{15mm}
  \TEXT{}
       {Disserta\c c\~ao apresentada \`a Universidade de Aveiro para cumprimento dos requesitos
        necess\'arios \`a obten\c c\~ao do grau de Mestre em Engenharia de Automa\c c\~ao Industrial, realizada sob a orienta\c c\~ao
        cient\'\i fica de V\'itor Manuel Ferreira dos Santos, Professor Associado do Departamento de Engenharia Mec\^anica da Universidade de Aveiro e de Manuel Bernardo Salvador Cunha, Professor Auxiliar do Departamento de Eletr\'onica, Telecomunica\c c\~oes e Inform\'atica da Universidade de Aveiro}
\EndTitlePage
\titlepage\ \endtitlepage % empty page

\TitlePage
  \vspace*{55mm}
  \TEXT{\textbf{o j\'uri~/~the jury\newline}}
       {}
  \TEXT{presidente~/~president}
       {\textbf{Prof. Doutor Pedro Nicolau Faria da Fonseca}\newline {\small
        Professor Auxiliar da Universidade de Aveiro}}
  \vspace*{5mm}
  \TEXT{vogais~/~examiners committee}
       {\textbf{Prof. Doutor Carlos Fernando Couceiro de Sousa Neves}\newline {\small
        Professor Coordenador do Instituto Polit\'ecnico de Leiria - Escola Superior de Tecnologia e Gest\~ao}}
  \vspace*{5mm}
  \TEXT{}
       {\textbf{Prof. Doutor V\'itor Manuel Ferreira dos Santos}\newline {\small
        Professor Associado da Universidade de Aveiro (Orientador)}}
%  \vspace*{5mm}
%  \TEXT{}
%       {\textbf{Prof. Doutor Manuel Bernardo Salvador Cunha}\newline {\small
%        Professor Auxiliar da Universidade de Aveiro (Co-orientador)}}
\EndTitlePage
\titlepage\ \endtitlepage % empty page

\TitlePage
  \vspace*{55mm}
  \TEXT{\textbf{agradecimentos~/\newline acknowledgements}}
       {\'E com muito gosto que aproveito esta oportunidade para agradecer ao Prof. Doutor V\'itor Manuel Ferreira  dos Santos pela disponibilidade e apoio, pelo conhecimento transmitido e por me motivar ao longo deste tempo. } 
\TEXT{}   
       {Queria tamb\'em agradecer ao Prof. Doutor Manuel Bernardo Salvador Cunha pela sua disponibilidade e conselhos.}
\TEXT{}
       {Agrade\c co tamb\'em a todos os membros do laborat\'orio que me ajudaram durante este tempo e pela boa disposi\c c\~ao que traziam para o laborat\'orio. Um especial obrigado ao Jorge Almeida pelo apoio, paci\^encia e conhecimento transmitido.}
  \TEXT{}
       {Aos amigos e colegas de curso pela boa disposi\c c\~ao, companheirismo, pela ajuda em tempos mais dif\'iceis, e pelo enorme gosto que tem por estas mat\'erias.}
       \TEXT{}
       {Aos meus pais um especial agradecimento pelo enorme apoio que me tem dado ao longo dos tempos, pelo esfor\c co e pela dedica\c c\~ao que tiveram para que eu conseguisse concretizar os meus objectivos. Sem voc\^es nada disto seria poss\'ivel.}
\EndTitlePage
\titlepage\ \endtitlepage % empty page

\TitlePage
  \vspace*{55mm}
  \TEXT{\textbf{Palavras-chave}}
  		{Navega\c c\~ao, Planeamento, GPS, ROS, C++, PHP, JavaScript, Html, SQL} \hfill \\
  \TEXT{\textbf{Resumo}}
  {O Atlascar \'e um projeto desenvolvido no laborat\'orio de automa\c c\~ao e rob\'otica, no departamento de Engenharia Mec\^anica da Universidade de Aveiro. Este ve\'iculo \'e utilizado para investiga\c c\~ao em condu\c c\~ao aut\'onoma e sistemas avan\c cados de apoio ao condutor.}
  \TEXT{}
       {O objetivo desta disserta\c c\~ao \'e desenvolver um sistema de navega\c c\~ao para planeamento de rotas. Este sistema inclui uma interface gr\'afica que permite ao utilizador selecionar um destino, com pontos de passagem interm\'edios se assim for pretendido. A aplica\c c\~ao deve permitir monitorizar em tempo real o ve\'iculo e indicar as manobras que devem ser realizadas ao longo do percurso. Novo \textit{hardware} ter\'a de ser instalado e configurado, nomeadamente um novo servidor e recetor GPS. Isto requer uma interven\c c\~ao na configura\c c\~ao atual do Atlascar.}
  \TEXT{}
       {O sistema est\'a dividido em tr\^es partes: Planeador, Navegador e Interface Gr\'afica. O m\'odulo do planeador \'e respons\'avel por calcular os percurso e as manobras a realizar. O navegador \'e respons\'avel pelo processamento dos dados vindos do planeador. Estabelece uma comunica\c c\~ao com uma base de dados de onde vai ler e escrever valores. Este m\'odulo tamb\'em sera respons\'avel por ler os valores vindos do GPS. A interface gr\'afica permite ao utilizador selecionar o destino pretendido, o que \'e feito clicando num mapa. Este m\'odulo tamb\'em permite monitorizar a localiza\c c\~ao do ve\'iculo, ver o percurso que ir\'a ser realizado e apresenta a instru\c c\~ao a realizar. Este m\'odulo tamb\'em estabelece uma comunica\c c\~ao com uma base de dados. Este m\'odulo \'e opcional porque o sistema tamb\'em funciona sem uma interface visual, esta simplesmente permite ao utilizador faz\^e-lo de forma interactiva e tamb\'em permite monitoriza\c c\~ao. Caso o utilizador escolha um percurso errado durante a viagem, o sistema recalcula a miss\~ao e apresenta um novo caminho.}
\EndTitlePage
\titlepage\ \endtitlepage % empty page

\TitlePage
  \vspace*{55mm}
  \TEXT{\textbf{Keywords}}
  		{Navigation, Planning, GPS, ROS, C++, PHP, JavaScript, Html, SQL} \hfill \\
  \TEXT{\textbf{Abstract}}
       {Atlascar is a project developed in the Automation and Robotics laboratory at the Mechanical Department of the University of Aveiro, Portugal. It is used for research in autonomous navigation and for advanced driver assistance systems.} 
       \TEXT{} 
{The aim of this thesis is to develop a navigation system for path planning, which includes a graphical interface that allows the user to input a final destination, with possible via points if needed. The application should be able to track the vehicle in real-time and output sequences of high level maneuvers along the mission. New hardware is also to be installed and configured, namely a new server and a new GPS receiver, which requires an intervention on the previous setup of the Atlascar.}
\TEXT{}
{The whole system is divided into three parts: Planner, Navigator and User interface. The planner module will be responsible for calculating the path and the instructions. The navigator module will be responsible for processing the data from the planner module ; communicating with a database, read and write values and acquiring data from the GPS. The user interface allows the user to input the desired destination by clicking on a map, this module also allows the user to: monitor the current location of the vehicle; view the path to the destination and it shows the instruction that needs to be done. This module also establishes a communication with the database. This module is optional because the system also works without a user interface, it simply allows the user to do it interactively and it allows motorization. If a wrong decision is made and the user follows a wrong path, the system will recalculate the mission and show a new path.}
\EndTitlePage
\titlepage\ \endtitlepage % empty page


%
% Tables of contents, of figures, ...
%

\pagenumbering{roman}
\tableofcontents

\cleardoublepage
\listoffigures

\cleardoublepage
\listoftables


% The chapters (usually written using the isolatin font encoding ...)

\cleardoublepage
\pagenumbering{arabic}

\chapter{Introduction}

Navigation has been used for a long time, for example by using maps and a compass. We "navigate" ourselves everyday to our destination. During the navigation process it might happen that we enter an unknown location and we probably might get lost. What now ? Well, this is where the commercial GPS (Global positioning system) navigation devices come into action. They help us getting from point A to point B without getting lost.
%, or at least that's what we expect.
Nowadays, we use commercial navigation solutions on our smartphones and cars to plan our journey or to get more information about a specific place.
%N�o me parece que fa�a sentido incluir isto aqui
 If we add to the GPS technology a digital map, we can create a precise navigation solution. For example, in our car,  we use the navigation system to set up a destination on the map and it will generate a route so that we can drive there based on its instructions. 
 %end nonsense
 
 Vehicles play an important role in modern society.
 %, and we use them everyday in cars, trains, planes, etc. 
But, as society evolves and grows, more and more vehicles enter our roads, which generates more traffic and consequently more accidents. This could be reduced by a more organized traffic, or in a near future, by intelligent autonomous vehicles.
 Times are changing and with the evolution of autonomous cars, in a few years (about year 2020 ) \cite{Baptiste} we will simply enter in our car, tell it where we want to go and the only thing we will need to do is to sit back and enjoy the ride. This could reduce the accidents because the vehicles will possibly communicate with each other using the vehicle-to-vehicle technology. 
 This type of communication allows cars to broadcast their position, speed, steering-wheel position, brake status, and other data to other vehicles within a few hundred meters. The other cars can use such information to build a detailed picture of what's unfolding around them, revealing trouble that even the most careful and alert driver, or the best sensor system, would miss or fail to anticipate \cite{WillKn}.

\section{Context and Motivation}
Atlascar, figure \ref{fig: atlascar_full}, is a project developed in the Automation and Robotics division at the Mechanical Department of University of Aveiro, Portugal. Project Atlascar 1 is a real car (Ford Escort SW) that is used for research in autonomous navigation and ADAS (Advanced driver assistance systems). The general philosophy has been to enrich the car with different kinds of sensors, to account for different types of perception and cover for redundancy, and thus allowing different types of research in data fusion and interpretation for the future developments in this project. The purpose of the vehicle transcends the mere massive collection of data, which it can do very well, but also uses that data to create models for enhanced perception and data fusion \cite{Santos2010}. It is equipped with several state of the art hardware. Much software has already been developed, but it is missing a navigation software for path and mission planning.  We can split the developments in navigation into two parts: Local navigation and Global navigation. Local navigation is related to obstacle detection and avoidance, lane detection, target tracking, maneuvers, among others. In this area several solutions have already been developed. On the other hand, we have Global navigation, which is related to mission planning and localization. Here, little developments have been made in the context of the Atlascar project, and there is the need for an infrastructure to merge the local and global navigation components. 
%In the market, we can find some commercial solutions for mission planning, for example GPS navigation devices or the ArduPilot mission planner for autonomous land vehicles. But none of these solution fulfils our needs because we need to communicate with the vehicle hardware. Car manufacturers, companies and some universities already have solutions for this problem, but they are private. 
Atlascar already features technologies that allows it to be partially autonomous. It currently features technologies previously developed and some examples are: road detection \cite{RicardoM}, automatic gearbox \cite{Sergio2014} and throttle \cite{jramal}, automatic parking \cite{JO} and pedestrian detection \cite{PedroBatistae} \cite{De2014} among many others. The work developed in this dissertation will contribute to get one step closer to the final goal, which is a fully autonomous car. There is also the need to reorganize the components, cables and its labels in the car because during these years of modifications it has become very disorganized.
\begin{figure}
\includegraphics[width=0.9\textwidth]{atlascar_full.eps}
\caption{Atlascar}
\label{fig: atlascar_full}
\end{figure}

\section{Objectives}
%Novo
The main objectives of this work are:
\begin{itemize}
  \item Installation of the new hardware for the Atlascar, namely: a more powerful server and a high precision GPS;
  \item Development of a graphical interface that allows choosing the start and destination coordinates in an interactive way; the application should also allow the user to monitor in real-time the steps of the mission;
  \item Development of a module to execute the mission planning that uses high level maneuvers or decisions that need to be executed during the mission (e.g. follow road, change direction, turn in the next crossing, turn around, stop/park, etc.);
  \item In a more advanced phase, this module should be able to select and execute the necessary maneuvers.
\end{itemize}

%%MEU
%Currently Atlascar1 doesn't have any solution for mission planning and managing, therefore a graphical user interface that allows the user to input a start and final destination, with possible via points if needed, needs to be built. The application should be able to track the vehicle in real-time and output sequences of high level maneuvers (e.g., turn right, turn left, etc.) that need be executed during the whole mission. In a more advanced phase, this module should be able to communicate with the vehicle hardware and trigger the respective maneuvers.
%
%New hardware must also be installed such as: a more powerful server and a high precision GPS receiver. This will require an intervention on the current infrastructure, because the new server requires much more space and some modifications to the actual equipment will be required. A place for the GPS receiver needs to be found and a support for the antenna needs to be made. 

\section{Structure of the document}
This thesis is divided into six chapters and the appendix section.
% Starting with chapter one, an introduction to the problem is made and explains what already exists and what needs to be done. In chapter two the state of the art is presented where technologies related to autonomous cars are presented. Also definitions related to localization and navigation are explained. Chapter three will present the current infrastructure and the modifications done on the car. The software infrastructure that will be used is also explained. The new hardware is also shown. Now in chapter four the solution found to solve the problem is explained and all the software developed is explained in detail. Chapter five shows the experiments made and the obtained results will be analysed. And finally chapter six are the conclusions and the future work that needs to be done.
\begin{description}
  \item[Chapter One] This chapter presents an introduction to the problem and explains what already exists and what needs to be done;
  \item[Chapter Two] This chapter presents the state of the art where technologies related to autonomous cars are presented. Definitions related to localization and navigation are also explained;
  \item[Chapter Three] This chapter presents the current infrastructure and the modifications done on the car. The software infrastructure that will be used is also explained. The new hardware is also shown;
  \item[Chapter Four] This chapter presents the solution found to solve the problem and explains all the software developed in detail;
  \item[Chapter Five] This chapter presents the experiments made and the obtained results will be analysed;
  \item[Chapter Six] This chapter presents the conclusions and the future work that needs to be done.
  \item[Appendix] The appendix contains all the instructions to install and use the software. Contains explanations on how to execute the developed system.
\end{description}

\chapter{State of the art}
The main topics that are relevant for this thesis are localization, planning and routing. This chapter will focus on these topics.
\section{Autonomous Cars}
Autonomous Cars is a topic that is getting more and more popular. Numerous researches have been made in this area by universities, companies and vehicle manufacturers. Research on this field started long time ago. On figure \ref{fig: auto_evo} we can find the evolution of what has been done, and what will possibly happen in this field. 

\begin{figure}[h]
\includegraphics[width=\textwidth]{car_timeline.jpg}
\caption{Autonomous Cars Timeline \cite{IEEE2012}}
\label{fig: auto_evo}
\end{figure}

In a few years we will see fully autonomous cars on the roads but for now there are still several issues related to security (where 99\% is not enough)  and navigation that need to be solved. There are already some vehicles that drive autonomously, for example the cars from Google, but its not yet a commercial solution. Nowadays, there are already semi-autonomous cars on the roads and the main technologies are \cite{McBride2008}:

\begin{itemize}
\item Anti-lock brake systems(ABS), imminent collision warning; 
\item Cruise control, urban cruise control(UCC) able to recognize stop signs and traffic signals); 
\item Lane departure warning (LDW), lane keeping assistance (LKA);
\item Traction control, electronic stability control (ESC), active suspension;
\item Pre-crash sensing with occupant protection equipment (airbags, seat belts);
\item Blind spot detection, pedestrian detection, parking assistance, night vision;
\item Driver drowsiness and distraction monitoring;
\item Vehicle to vehicle and infrastructure integration (VII / V-V), intelligent vehicle highway systems (IVHS);
\item Total accident avoidance / autonomous vehicle control;
\end{itemize} 

More features are coming soon and according to \cite{Barker} the timeline for autonomous cars and security features are the following: 

\begin{itemize}
\item[2015] 
\item "Super cruise":autonomous steering, braking and lane guidance at speed;
\item Autonomous throttle, gear shifting, and unoccupied self-parking;
\item[2018] 
\item Release of Google's autonomous car technology.
\item[2020] 
\item Volvo expects accident-free cars and GM, Audi, Nissan and BMW expect fully autonomous driverless cars.
\end{itemize}

It is expected that in 2040 almost 75\% of the cars will drive autonomously \cite{IEEE2012}. 

With the implementation of autonomous cars, we could possibly reduce accidents, improve productivity because we could work while we drive and have more organized traffic.

\subsection{GNSS}
GNSS (Global Navigation Satellite System) is used to describe the collection of satellite positioning systems that are now operating or planned \cite{Jeffrey2010}.

\begin{description}
\item [GPS (United States)] \hfill \\
GPS was the first GNSS system and, as of today, it is the only one that is fully operational. GPS was launched in the late 1970's by the United States Department of Defense. It now uses a constellation of between 24 and 32 satellites, and provides global coverage.
\item[GLONASS (Russia)] \hfill \\
GLONASS is operated by the Russian government. The full GLONASS constellation will consist of 24 satellites. Global coverage is provided since 2011.
\item[Galileo (European Union)] \hfill \\
Thirty satellites are planned with the first being launched in 2006. The full constellation will not be
complete for several years.
\item[Compass (China)] \hfill \\
Compass will be the Chinese navigation satellite system. The system will consist of 35 satellites. A
regional service is provided since 2011 then the service will be extended to provide global coverage in the years 2015-2020.
\end{description}

A more detailed explanation on how GNSS works can be found in: \cite{Jeffrey2010}.
\subsection{Localization} 
Localization provides estimates of the location, attitude, velocity and acceleration of the vehicle with respect to some fixed coordinate system.\\

\textbf{Geographic Coordinate System} \\
This is the type of coordinates that is used in this thesis and consists of using measures of latitude and longitude to determine location. The earth globe is divided into lines as represented on figure \ref{fig: latlon_map}.

The ones belonging to the latitude are parallel to the equator line. Positive numbers represent the northern hemisphere, negative numbers represent the southern hemisphere and the equator represents 0 degrees of latitude, while both poles represent 90 degrees. All other points range between 0{\degree} to 90{\degree} north or 0{\degree} to 90{\degree} south. Lines of longitude run east and west parallel to the prime meridian. Positive numbers represent the eastern hemisphere, negative numbers represent the western hemisphere, and the prime meridian represents 0 degrees of longitude. All other points range between 0{\degree} to 180{\degree} east and 0{\degree} to 180{\degree} west. Lines of longitude are not parallel. The closer they are to the poles, the shorter the distance between them \cite{Incident}.
 
\begin{figure}[h]
\includegraphics[width=\textwidth]{latitudelongitude.jpg}
\caption{Latitude-Longitude Map \cite{Geographyworldonline}}
\label{fig: latlon_map}
\end{figure}

According to \cite{Incident} there are three primary ways of describing locations using latitude and longitude coordinates:
\begin{enumerate}
\item Degrees Minutes Seconds (ddd{\degree} mm' ss.s") \\
This is the most common format that is used on maps. (e.g. 43{\degree} 23' 45" , 71{\degree} 8'36")
\item Degrees Decimal Minutes (ddd{\degree} mm.mmmm') \\
This format is used by aircraft guidance systems: (e.g. 43{\degree} 23.75' Latitude , 71{\degree} 8.6' Longitude )
\item Decimal Degrees (ddd.dddd{\degree}) \\
This is used by the American National Weather Service as well as based mapping systems. This is the type of coordinates that will be used. (e.g. 43.395833{\degree} Latitude, 71.143333{\degree} Longitude)
\end{enumerate}

%60 seconds (60") = one minute (1') \\
%60 minutes (60') = one degree (1{\degree})
%Falar da precisao ? 1� = 111.32km no equador

\textbf{GPS}\\
A common GPS is capable of accuracies of the order of 10m. Differential GPS is capable of accuracies of better than 0.5m, and real-time kinematic GPS is capable of accuracies of around 2cm. GPS systems can fail in a number of different ways. Most common failure modes involve obstruction of line-of-site to satellites, multipath from foliage or terrain geometry, and active jamming from other RF (Radio Frequency) sources \cite{Durrant-whyte2001}. \\

\textbf{INS}\\ 
An INS (Inertial Navigation System) uses information from an IMU (Inertial Movement Unit) to compute accurate position over time. Inertial sensors have a major advantage in being non-radiating, non-jammable sensors which do not rely on any external information to provide estimates of position, attitude and body rates. An IMU has enough precision to allow vehicles to run without any external navigation updates for several minutes. This is often enough to overcome intermittent jamming of GPS or slow acquisition of other landmark data (for example). The use of INS in Autonomous Vehicle applications is now a standard practice \cite{Durrant-whyte2001}.

Novatel's SPAN-IGM-A1 unit which will be used in this work, combines GPS with INS to create a precise navigation solution.
An example is illustrated in figure \ref{fig:gnss-ins}. We can observe that if we would only use a GNSS solution and drive for example trough tall buildings or inside a tunnel we would eventually lose signal. In addition using only an INS is not enough because after a while it is required to adjust the output. So, by combining both technologies we obtain a precise navigation system that overcomes the limitations of the individual technologies.
%NEW IMAGE
\begin{figure}[h]
\centering
\includegraphics[scale=0.3]{gnss-ins}
\caption{Combined GNSS/INS \cite{NovatelGNSS/INS}}
\label{fig:gnss-ins}
\end{figure}
%END IMAGE

Two examples of GPS/ INSS integration architectures that can be implemented are \cite{Bevly2010}:
\begin{description}
\item [Loose Coupling] \hfill \\
A loosely coupled GPS/INS integration routine combines the inertial measurements with position and velocity measurements calculated by the GPS receiver. Feedback to the inertial processor calibrates the IMU to remove effects from biases, scale factors, and/or misalignment. \hfill \\
\item[Close Coupling] \hfill \\
A closely coupled GPS/INS integration routine combines the inertial measurements with range information to the GPS satellites provided by the GPS receiver. Feedback to the inertial processor is retained, but there is no feedback to the GPS receiver. Range information includes a user to satellite distance and user to satellite velocity.
\end{description}

\section{Navigation} 
There are five basic forms of navigation \cite{Andrews2007}:
\begin{enumerate}
\item \textit{Pilotage}, which essentially relies on recognizing landmarks to know where we are and how we are oriented.
\item \textit{Dead reckoning}, which relies on knowing where we started from, plus some form of heading information and some estimate of speed.
\item \textit{Celestial navigation}, using time and the angles between local vertical and known celestial objects (e.g., sun, moon, planets, stars) to estimate orientation, latitude, and longitude.
\item \textit{Radio navigation},which relies on radiofrequency sources with known locations (including global navigation satellite systems satellites).
\item \textit{Inertial navigation},which relies on knowing your initial position, velocity, and attitude and thereafter measuring your attitude rates and accelerations. It is the only form of navigation that does not rely on external references.
\end{enumerate}

Navigation is concerned with the acquisition of, and response to, external sensed information. The navigation function takes input from sensors observing the operational environment. It must use this information to create an internal representation of the environment that can subsequently be used in the execution of a mission.

\section{Routing}
Mission and task planning functionally generates trajectories, behaviours or way points for the system as a whole.  It has no direct links with either sensory input or controller output. However, it clearly must use an understanding of these, in conjunction with prior maps and defined mission objectives, to produce appropriate navigation commands \cite{Durrant-whyte2001}.

There are differences between Route Planning and Path Planning \cite{Lope}:
\begin{description}
\item [Route planning]
This topic focuses on determining the route between two geographical locations, or how the vehicle will traverse from its present location to a destination, which could be very far away.
\item [Path planning]
Also known as Motion planning. This topic discusses the creation of a series of path segments that describe the path directly in front of (or behind) the vehicle.
\end{description}

There are a lot of map providers, which provide a web interface to calculate a route between two points (with intermediate points, if desired). Some of them also provide an API (Application Programming Interface) that can be used to develop specific mapping software based on the API (or APIs) we choose. Some of the most common providers are shown in table \ref{table: routing soft}. Figure \ref{fig:grapphopperweb}, \ref{fig:mapquestweb}, \ref{fig:mapboxweb} and \ref{fig:NokiaHereWeb} show the website from a few providers.

\begin{table}[h]
\centering
\caption{Routing Software comparison}
\label{table: routing soft}
\begin{tabular}{l|l|l}
Provider &  API & Open Source  \\ \hline
Graphhopper & Yes & Yes \\ %\hline
OpenStreetMap & Yes (3rd Party) & Yes \\ %\hline
Google Maps & Yes & No \\ %\hline
MapQuest & Yes & No \\ %\hline
Mapbox & Yes & No \\ %\hline
Nokia Here & Yes & No \\ %\hline
Yahoo! Maps & Yes & No \\ %\hline
Bing Maps & Yes & No \\ %\hline
ViaMichelin & Yes & No
\end{tabular}
\end{table}

\begin{figure}
 \centering
    \includegraphics[width=0.85\linewidth]{graphopper} 
    \caption{Routing software provided by Graphhopper \cite{Gra2015}}
    \label{fig:grapphopperweb} 
\end{figure}
\begin{figure}
\centering
    \includegraphics[width=0.85\linewidth]{mapquestapi} 
    \caption{Routing software provided by Mapquest \cite{Map}}
    \label{fig:mapquestweb} 
\end{figure}
\begin{figure}
\centering
    \includegraphics[width=0.85\linewidth]{mapbox} 
    \caption{Routing software provided by Mapbox \cite{Mapbox}}
    \label{fig:mapboxweb} 
\end{figure}
\begin{figure}
\centering
    \includegraphics[width=0.85\linewidth]{nokiahere} 
    \caption{Routing software provided by Nokia Here \cite{Nokia}}
    \label{fig:NokiaHereWeb} 
\end{figure}

There is also software that allows to work offline (e.g. OpenSourceRoutingMachine, pgRouting, OpenTripPlaner), which has advantages and disadvantages. We can benefit from these solutions in case we don't have internet access in the car or if we frequently drive through areas where the internet access is poor. We also don't rely on the website that might be unavailable when we need it. But, of course, there are also disadvantages; for example, the speed of the service depends on the hardware and it is necessary to manually download the maps, to perform an update for the maps. For example the data size for a map of the whole planet is about 29GB, Europe about 16GB and Portugal, only 108MB. The maps can be obtained from Geofabrik  \cite{Geofabrik} and OpenStreetMaps \cite{OpenStr2015} . \\

\textbf{OpenSourceRoutingMachine (OSRM)}
This project can be split into two parts: Server(backend) and Webpage(frontend). It is written in C++, very fast and consumes a low amount of resources. The map data originates from the OpenStreetMap Project.

\textbf{pgRouting}
pgRouting is an extension to the PostGIS/PostgreSQL geospatial database and adds routing functionality. It is mainly based on SQL (Structured Query Language) and no turn restrictions are available.

\textbf{OpenTripPlaner (OTP)}
OpenTripPlaner runs on a Java web server. It bundles a REST Api and a WebFrontend. Needs more resources than OSRM and is also slower in calculating routes. 

Comparing these three services, OSRM was chosen because it fulfils the needs of this project, very fast, the maps can easily be updated, it works offline and is written in C++.



%\begin{figure} 
%  \begin{subfigure}[b]{0.5\linewidth}
%    \centering
%    \includegraphics[width=0.85\linewidth]{graphopper} 
%    \caption{Graphhopper \cite{Gra2015}}
%    \label{fig:grapphopperweb} 
%    \vspace{4ex}
%  \end{subfigure}%% 
%  \begin{subfigure}[b]{0.5\linewidth}
%    \centering
%    \includegraphics[width=0.85\linewidth]{mapquestapi} 
%    \caption{Mapquest \cite{Map}}
%    \label{fig:mapquestweb} 
%    \vspace{4ex}
%  \end{subfigure} 
%  \begin{subfigure}[b]{0.5\linewidth}
%    \centering
%    \includegraphics[width=0.85\linewidth]{mapbox} 
%    \caption{Mapbox \cite{Mapbox}}
%    \label{fig:mapboxweb} 
%  \end{subfigure}%%
%  \begin{subfigure}[b]{0.5\linewidth}
%    \centering
%    \includegraphics[width=0.85\linewidth]{nokiahere} 
%    \caption{Nokia Here \cite{Nokia}}
%    \label{fig:NokiaHereWeb} 
%  \end{subfigure} 
%  \caption{Illustration of various routing software providers}
%  \label{fig:webpagemapproviders} 
%\end{figure}

\chapter{Experimental Infrastructure}

This chapter describes the car and the related interventions, the new equipment that has been installed and some preliminary tests that have been made.
%\section{Robot Operating System} \section{Atlascar}
%ROS is an open-source, meta-operating system for your robot. It provides the services you would expect from an operating system, including hardware abstraction, low-level device control, implementation of commonly-used functionality, message-passing between processes, and package management. It also provides tools and libraries for obtaining, building, writing, and running code across multiple computers.
%
%The ROS runtime "graph" is a peer-to-peer network of processes (potentially distributed across machines) that are loosely coupled using the ROS communication infrastructure. ROS implements several different styles of communication, including synchronous RPC-style communication over services, asynchronous streaming of data over topics, and storage of data on a Parameter Server. 
%
%The primary goal of ROS is to support code reuse in robotics research and development. ROS is a distributed framework of processes (aka Nodes) that enables executables to be individually designed and loosely coupled at runtime. These processes can be grouped into Packages and Stacks, which can be easily shared and distributed. ROS also supports a federated system of code Repositories that enable collaboration to be distributed as well. This design, from the filesystem level to the community level, enables independent decisions about development and implementation, but all can be brought together with ROS infrastructure tools. \cite{ROSa}
\section{Atlascar}
Currently the vehicle is equipped with several equipment and software that allows performing different tasks related to autonomous driving. Some equipment is shown in figure \ref{fig: atlasfront_tag}. As mentioned in chapter 1 some examples of the existing technologies are:
\begin{itemize}
\item road detection \cite{RicardoM}
\item target detection with laser \cite{O}
\item pedestrian detection \cite{PedroBatistae}, \cite{De2014}
\item driver monitoring \cite{AndreJoaoLopesOliveira}
\item vehicle monitoring and remote control \cite{TiagoNunesdaRocha}
\item automatic gearbox \cite{Sergio2014}
\item automatic throttle \cite{jramal}
\item automatic parking \cite{JO}
\end{itemize}

\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{atlasfront_tag.eps}
\caption{Some equipment featured in the Atlascar}
\label{fig: atlasfront_tag}
\end{figure}

The equipment aboard the Atlascar can be divided into four areas, namely:
%\begin{itemize}
%\item power generation
%\item perception/vision
%\item sensors
%\item data processing and management
%\end{itemize}
\begin{description}
  \item[Power generation] \hfill \\
   The Atlascar has a more powerful alternator to provide more alternate current, an inverter to change the direct current (DC) to alternate current (AC), a secondary battery with a power and current indicator to store the energy, an Uninterruptible Power Supply and a power distribution panelboard that contains two power sources (24V and 12V) and a PLC (Programmable Logic Controller) for turning the sensors on and off. 
 
  \item[Perception/vision] \hfill \\
  Atlascar is equipped with a 3D camera, stereo camera, two planar 2D laser sensors, foveal vision system and a 3D laser.
 
  \item[Sensors and accessory tools] \hfill \\
   Atlascar has two PLC for turning the sensors on and off, an inertial movement unit, a GPS, an encoder to measure the speed, a unit that allows remote monitoring and several selectors that allow commuting between manual and automatic mode for brakes, steering, clutch, gearbox, handbrake and engine.
  
  \item[Data processing and management] \hfill \\
  The Atlascar has a powerful server and two network switches.
\end{description}

% In the power generation area it features: a more powerful alternator to provide more alternate current, an inverter to change the direct current (DC) to alternate current (AC), a secondary battery with a power and current indicator to store the energy, a Uninterruptible Power Supply and power distribution panelboard that contains two power sources (24V and 12V) and a PLC (Programmable Logic Controller) for turning the sensors on and off. 
% In the perception/vision are it features: a 3D camera, stereo camera, two planar sensors, 2D laser, Infrared (IR) laser, foveal vision system and a 3D laser.
% In the sensor area it features two PLC for turning the sensors on and off, an inertial movement, a GPS, an encoder to measure the speed, a unit that allows a remote monitoring and several selectors that allows to commute between manual and automatic mode for brakes, steering, clutch, gearbox, handbrake and engine.
%In the data processing and management area it features: a server and two switches.

\section{Atlascar Hardware}
An intervention on the hardware level was required because new hardware had to be installed, namely a new Server and the new GPS along with its antenna. The new server was necessary because the car required more processing power. In addition, the new GPS was required for this thesis because it is much more accurate and has more features than the old unit, as it will be explained later. This new GPS is very important for this work because in the future, when the car hopefully drives by itself, it will need a reliable equipment that is very accurate and that works better in places where the old unit might not perform so well. However, as the new server is much bigger, the old trunk layout was not appropriate, figure \ref{fig:old_layout} and \ref{fig:oldlayout}. A new layout, figure \ref{fig:new_layout} and \ref{fig:newlayout}, was designed so that all components would fit correctly and to guarantee that the server has enough space to get a good air flow. The PLC board had to be moved and a new place for the battery, Switch and the new GPS receiver had to be found. It was also ensured that all components are not loose and don't collide with each other during travel, therefore, supports made of styrofoam where placed between the components. 

\begin{figure}[h]
  \begin{subfigure}[b]{0.5\linewidth}
    \centering
   % \includegraphics[width=0.85\linewidth]{graphopper} 
   \includegraphics[width=0.85\linewidth]{old_layout} 
    \caption{Old Schematic}
    \label{fig:old_layout}
    \vspace{4ex}
  \end{subfigure}%% 
  \begin{subfigure}[b]{0.5\linewidth}
    \centering
    \includegraphics[width=0.85\linewidth]{oldlayout} 
    \caption{Old Picture}
    \label{fig:oldlayout}
    \vspace{4ex}
  \end{subfigure} 
  \begin{subfigure}[b]{0.5\linewidth}
    \centering
    %\includegraphics[width=0.85\linewidth]{mapbox} 
	\includegraphics[width=0.85\linewidth]{new_layout} 
    \caption{New Schematic}
    \label{fig:new_layout}
  \end{subfigure}%%
  \begin{subfigure}[b]{0.5\linewidth}
    \centering
    \includegraphics[width=0.85\linewidth]{newlayout} 
    \caption{New Picture}
    \label{fig:newlayout}
  \end{subfigure} 
  \caption{Atlascar Trunk Layout Schematics and Pictures}
  \label{fig:oldnewlayout} 
\end{figure}
%\begin{figure}
%\centering
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=.95\textwidth]{oldlayout}
%  \caption{Old} 
%  \label{fig: oldlayout}
%\end{subfigure}%
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=.95\textwidth]{newlayout} 
%  \caption{New}
%  \label{fig:newlayout}
%\end{subfigure}
%\caption{Layout}
%\label{fig:oldnewlayout}
%\end{figure}
A new support has been installed on the roof of the car to hold the external GPS antenna, figure \ref{fig: antenasup}, therefore an old support already installed on the car was moved to a new place and adapted so that it can properly hold the antenna.

\begin{figure}[h]
\centering
\includegraphics[width=0.9\textwidth]{atlascar_antena.eps}
\caption{GPS Antenna and Support}
\label{fig: antenasup}
\end{figure}

An old case was removed and replaced by a new one. This was necessary because the old case was too big and heavy, and in its interior there was only an Arduino, figure \ref{fig: closedbox} and \ref{fig:openbox}. The new case is much smaller and lighter, figure \ref{fig:newclosedandopenbox}. Some unnecessary cables where also removed from the car, and a few cables that had no identification got one. 

\begin{figure}[h]
\centering
\begin{subfigure}{.5\textwidth}
  \centering
  \includegraphics[width=.9\textwidth]{oldboxclosed}
  \caption{Closed} 
  \label{fig: closedbox}
\end{subfigure}%
\begin{subfigure}{.5\textwidth}
  \centering
  \includegraphics[width=.9\textwidth]{oldboxopen}
  \caption{Opened}
  \label{fig:openbox}
\end{subfigure}
\caption{Old box for the odometry control unit}
\label{fig:closedandopenbox}
\end{figure}

%%image here !
\begin{figure}[h]
\centering
\begin{subfigure}{.5\textwidth}
  \centering
  \includegraphics[width=.9\textwidth]{newclosedbox}
  \caption{Closed} 
  \label{fig: newclosedbox}
\end{subfigure}%
\begin{subfigure}{.5\textwidth}
  \centering
  \includegraphics[width=.9\textwidth]{newboxopen}
  \caption{Opened}
  \label{fig:newopenbox}
\end{subfigure}
\caption{New box for the odometry control unit}
\label{fig:newclosedandopenbox}
\end{figure}

The throttle was switched from automatic mode, figure \ref{fig: autoaccel} to manual, figure \ref{fig:manaccel}. This is done by opening the case identified as number "1" and removing the cable. The cable which comes from the motor controlled by the arduino, identified as number "3" and connects to number "2" is also removed. To change to manual acceleration the cable that was removed from number "1" is placed on the location marked as "2".

\begin{figure}[h]
\centering
\begin{subfigure}{.5\textwidth}
  \centering
  \includegraphics[width=.95\textwidth]{autoaccel}
  \caption{Automatic} 
  \label{fig: autoaccel}
\end{subfigure}%
\begin{subfigure}{.5\textwidth}
  \centering
  \includegraphics[width=.95\textwidth]{manaccel} 
  \caption{Manual}
  \label{fig:manaccel}
\end{subfigure}
\caption{Throttle modes}
\label{fig:automanaccel}
\end{figure}

The new server that has been installed is much heavier than the previous one and this causes the chassis to hit the sidewalk when trying to park it next to the Mechanical Department. So two ramps, figure \ref{fig: ramp}, where made in order to prevent this from happening.

\begin{figure}[h]
\centering
\includegraphics[width=0.9\textwidth]{ramp.jpg}
\caption{Ramp that helps to avoid the collision of the atlascar's chassis with the sidewalk}
\label{fig: ramp}
\end{figure}

\subsection{GNSS}
In order to provide a highly accurate localization, a new GNSS unit was installed. The equipment used is the Novatel SPAN-IGM-A1 unit, figure \ref{fig: subNovatel}. It combines a high precision Global Navigation Satellite System (GNSS) with an Inertial Measurement Unit (IMU). It can be configured to work as a GNSS+INS engine or as a standalone IMU sensor. This unit will be responsible for determining the vehicle localization.
Table \ref{tab:gpsComp} compares the old unit, figure \ref{fig:sub353} with the new one, figure \ref{fig: subNovatel}. 
%Estas referencias nao fazem sentido ???? Ou preciso de indicar onde fui buscar os dados das tabelas ?
%\cite{USGlobal}
%\cite{ShengJayAu}

\begin{figure}[h]
\centering
\begin{subfigure}{.5\textwidth}
  \centering
  \includegraphics[width=.7\textwidth]{Novatel-SPAN-IGM-A1}
  \caption{Novatel-SPAN-IGM-A1 \cite{NovatelSIA1}}
  \label{fig: subNovatel}
\end{subfigure}%
\begin{subfigure}{.5\textwidth}
  \centering
  \includegraphics[width=.6\textwidth]{gps-353-bu}
  \caption{GPS-353-BU \cite{USGlobalSatInc.}}
  \label{fig:sub353}
\end{subfigure}
\caption{New and old GPS units}
\label{fig:NewOldGps}
\end{figure}

\begin{table}[h]
\centering
\caption{Comparison of the new GPS that will be installed and the old unit that will be removed}
\begin{tabular}{l|l|l}
 & Novatel-SPAN-IGM-A1  & GPS-353-BU \\ \hline 
RMS & Single point L1/L2 - 1.2m, DGPS - 0.4m  &  5m WAAS enabled, \\
~ & ~ & 10m WAAS disabled \\ \hline
Data Rates & GNSS measurement \& position - 20Hz, \\ & IMU measurement \& solution 200Hz  & 1Hz  \\ \hline
IMU &  Yes & No    \\ \hline
COM Ports & 1xUSB, 1xRS-232, 1xCAN Port & 1xUSB
\end{tabular}
\label{tab:gpsComp}
\end{table}

To establish a communication with this device was not a trivial task. Not only because this device doesn't work as a plug-and-play unit, but also because it needs to be configured before using it. Finding a good ROS (Robot Operating System) package for this unit required some time because there are very few packages for this device. And the ones that exist are unclear to install and don't work as expected. But at the end the \textit{novatel} package from GAVLab was successfully installed. Even with this package installed the communication did not work. This had to do with the USB (Universal Serial Bus) connection that wasn't being recognized by the operating system. The solution was found on the Novatel website \cite{Novatel}, the procedures to fix this issue are described on Appendix \ref{App:AppendixA}.4. Finally it was possible to communicate with the GPS. 
To test the precision of the new equipment, it was placed outside through a window of the Automation and Robotics laboratory \ref{fig:gpstestcondition} and using ROS, the coordinates where logged during ten hours into a ROS bag. Figure \ref{fig: latgraph} and \ref{fig: longraph} show the Latitude and Longitude values that were recorded during the ten hours. 

\begin{figure}[h]
\centering
\begin{subfigure}{.5\textwidth}
  \centering
  \includegraphics[width=.9\textwidth]{gpsoutside}
  \label{fig: gpsout}
\end{subfigure}%
\begin{subfigure}{.5\textwidth}
  \centering
  \includegraphics[width=.9\textwidth]{gpsinside}
  \label{fig:gpsin}
\end{subfigure}
\caption{Test conditions of the new GPS Antenna}
\label{fig:gpstestcondition}
\end{figure}

\begin{figure}
\centering
\begin{tikzpicture}[]
\begin{axis}[
  width=12cm,
  height=7cm,
  xlabel=Time (min),
  ylabel=latitude variation around $40.62975\degree$,
  ymajorgrids,
  enlargelimits=false,
]
\addplot[no markers,blue] table[y=latitude, y expr=\thisrowno{0}-40.62975, x=Tempo(min), col sep=comma] {gpsvalues.csv}; 
\end{axis}
\end{tikzpicture}
\caption{Latitude Graph of the GPS test measures}
\label{fig: latgraph}
\end{figure}

\begin{figure}
\centering
\begin{tikzpicture}[]
\begin{axis}[
  width=12cm,
  height=7cm,
  xlabel=Time (min),
  ylabel=longitude variation around $-8.65813\degree$,
  ymajorgrids,
  enlargelimits=false,
]
\addplot[no markers,red] table[y=longitude, y expr=\thisrowno{1}+8.65813, x=Tempo(min), col sep=comma] {gpsvalues.csv}; 
\end{axis}
\end{tikzpicture}
\caption{Longitude Graph of the GPS test measures}
\label{fig: longraph}
\end{figure}

%\begin{figure}
%\includegraphics[width=\textwidth]{LatitudeGraph.eps}
%\caption{Latitude Graph of the GPS test measures}
%\label{fig: latgraph}
%\end{figure}
%
%\begin{figure}
%\includegraphics[width=\textwidth]{LongitudeGraph.eps}
%\caption{Longitude Graph of the GPS test measures}
%\label{fig: longraph}
%\end{figure}

 With all the data processed a standard deviation was calculated, the result was 0.00001406. Accordingly to \cite{Osborne2013} the radius of the equator is: 6,371,008 meters. So the circumference of the earth will be 40,030,173 meters. The equator is divided into 360 degrees of latitude, so dividing the circumference of the earth by 360 degrees of latitude the result will be that each degree of latitude corresponds to 111,195 km. Table \ref{table: degreesprecision} illustrates the precision based on the decimal places.  So with the standard deviation that was obtained the precision that could be obtained was 1.57 meters. This precision was achieved between two buildings and not on an open field. The precision on an open field would possibly be better because the signals from the satellites would not be obstructed by buildings.
 %On open field the precision would be better. (old)
 
 \begin{table}[h]
 \centering
\caption{Decimal degrees precision}
    \begin{tabular}{l|l|l}
    decimal places & decimal degrees & distance  \\ \hline
    0              & 1               & 111.92 km \\
    1              & 0.1             & 11.192 km \\
    2              & 0.01            & 1.1192 km \\
    3              & 0.001           & 111.92 m  \\
    4              & 0.0001          & 11.192 m  \\
    5              & 0.00001         & 1.1192 m  \\
    6              & 0.000001        & 111.92 mm \\
    7              & 0.0000001       & 11.192 mm \\
    8              & 0.00000001      & 1.1192 mm \\
    \end{tabular}
    \label{table: degreesprecision}
\end{table}
 
%delete 
% By calculating the great circle distance between a random logged coordinate and a logged coordinate with the addition %of the standard deviation results in a difference of two meters. Calculations where made online \cite{AdamSchneider}. 
  
\subsection{Server}
This new server will be responsible for processing all the information that comes from multiple sensors and to seamlessly run the required software. This new server is more powerful than the older one, and a comparison between the old server, figure \ref{fig:subHpOld}, and the new server, figure \ref{fig: subHpNew}, can be found in table \ref{table: server comp} \cite{HP2015}.

\begin{figure}[h]
\centering
\begin{subfigure}{.5\textwidth}
  \centering
  \includegraphics[width=.7\textwidth]{hp-server}
  \caption{HP ProLiant ML350p Gen8 \cite{He2014}}
  \label{fig: subHpNew}
\end{subfigure}%
\begin{subfigure}{.5\textwidth}
  \centering
  \includegraphics[width=.7\textwidth]{hp-m9782pt}
  \caption{HP Pavilion Elite m9782pt \cite{HP}}
  \label{fig:subHpOld}
\end{subfigure}
\caption{New server that will be installed on the car and the old server that will be removed}
\label{fig:NewOldServer}
\end{figure}

\begin{table}[h]
\centering
\caption{New and old server comparison}
\begin{tabular}{l|l|l}
        & HP ProLinant ML350p Gen8  & HP Pavilion Elite m9782pt \\ \hline
Processor & Intel Xeon E5-2640 v2 (2.0Ghz / 8-core) &  Intel i7 920 (2.66Ghz / 4-core)\\ \hline
Memory    & 16Gb & 4Gb  \\ \hline
Storage   &  1TB (10000rpm) &  640Gb (7200rpm) \\ \hline
\end{tabular}
\label{table: server comp}
\end{table}

Since the server was completely new, Ubuntu 14.04 was installed, the repository of the laboratory (LarTkV5) was set up and all software required for this thesis was installed.
%\iffalse
%\subsection{UPS}
%In case of power failure, this unit will guarantee backup power for a short while. A comparison between the new UPS \ref{fig: subUpsNew} and the old one \ref{fig:subUpsOld} can be found in table \ref{table: ups comp}. \cite{Hewlett}
%
%\begin{figure}[h]
%\centering
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=.65\textwidth]{hp-ups}
%  \caption{HP UPS R/T3000 G2}
%  \label{fig: subUpsNew}
%\end{subfigure}%
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=.85\textwidth]{mge-ups}
%  \caption{MGE UPS S3000}
%  \label{fig:subUpsOld}
%\end{subfigure}
%\caption{New and old UPS}
%\label{fig:NewOldUps}
%\end{figure}
%
%\begin{table}[h]
%\centering
%\caption{UPS comparison}
%\begin{tabular}{l|l|l}
% & HP UPS R/T3000 G2 & MGE UPS S3000 \\ \hline
%Power Protection & 3300 VA & 3300 VA \\ \hline
%Battery Runtime (100\% load) & 4minutes (No ERM) 45min (one ERM) & ?? 
%\end{tabular}
%\label{table: ups comp}
%\end{table}
%\fi
\chapter{Methods and Programming}
This chapter describes how the system works and what was implemented. This concerns the several components and their interconnections and outputs.

%\section{Experimental infrastructure}
\section{Proposed Architecture}
The main goal was to create an application that would calculate the route between two points and output the maneuvers that needed to be executed along it. The application should have a visual interface that would allow monitoring the car, choose a destination and show the maneuvers that needs to be executed. The idea was to split this problem into three modules, one that calculates the route, one to interface with the GPS and other modules and a third module that would be the user interface. In order to save time and have a precise solution for calculating a route, an open source software has been chosen. For the user interface a solution based on a webpage was chosen. The initial idea for the relationship between these modules is shown on figure \ref{fig: init_idea}, where WEB is the user interface; Navigator is the module responsible for communicating with the WEB and the Planner module; Planner is the module responsible for calculating the route. An example of what the application could be is represented on figure \ref{fig: sample_inst}, where several waypoints are placed on the road and each of them has a specific maneuver. 

\begin{figure}
\centering
\includegraphics[width=0.9\textwidth,height=\textheight,keepaspectratio]{initialidea.eps}
\caption{An initial solution to solve the problem was to split it into three modules}
\label{fig: init_idea}
\end{figure}

\begin{figure}
\centering
\includegraphics[width=0.9\textwidth,height=\textheight,keepaspectratio]{sample_inst.png}
\caption{Application Example of possible solution, in which each waypoint, from the start till the end has an instruction associated}
\label{fig: sample_inst}
\end{figure}
\hfill \\
The main framework used  to develop the modules is ROS, and with it a client-server architecture that enables communication between two nodes only when needed, in this case the Navigator and the Planner modules. The main programming language used was C++. The OSRM-backend calculates the requested route, this program was installed on the LAR (Laborat\' orio de Automa\c c\~ao e Rob\' otica) server and can be used by anyone. The planner module will be responsible to communicate with this backend, it works based on web requests. Curl (Client URL Request Library) is also used to do a web request and gather the response, in this case in form of a JSON (JavaScript Object Notation) file.
To store all the information for each mission, the database that is used is PostgreSQL 9.3. The web server used to host all the web modules is Apache2. PHP5 is used to communicate with the database and access all the relevant information. A package from GAVLab, called \textit{novatel}, is  used to establish a communication with the GPS receiver.
%Everything was built from scratch because no software has been done on this level at the laboratory. 
The problem was split into three main parts: 
\begin{description}
 \item[Mission planner - Server based service] \hfill \\
responsible for communicating with the OSRM-backend; processes the response and returns a route based on the given start and destination coordinates.
 \item[Navigation manager - Client] \hfill \\
will process the user requests, write the waypoints, instructions, current position, closest waypoint and user destination to a Database, getting the coordinates from the GPS receiver; 
\item[Web page - Visual Interface]\hfill \\
reads the data that was written by the client from the database and presents the waypoints, the current and next instructions and the current position on the web page. The user can select the desired destination by clicking on the map. It also allows to track the vehicles movement.
\end{description}

The proposed architecture and the interaction between all modules and components is shown on figure \ref{fig: sys_work_final}
 \begin{figure}
\includegraphics[width=0.9\textwidth]{system_workflow_final.eps}
\caption{Interaction between all modules and components}
\label{fig: sys_work_final}
\end{figure}

\subsection{Mission planner}
This module will be the server and will process the requests from the client and returns a response. The current position and the desired destination coordinates are sent, and what is returned are the waypoints and all instructions. A custom ROS service has been implemented to send and receive this data. The client chooses a destination and this information is then sent to the server. When a request arrives, the server builds a specific URL (Uniform Resource Locator) that using curl will be sent to the OSRM-backend which, in case of a successful request, responds with a JSON containing all the data for the trip, an example of a response is shown in figure \ref{fig: osrm-response}.
 
\begin{figure}
\includegraphics[width=\textwidth]{jsonFile.png}
\caption{Osrm-backend Response to a route request}
\label{fig: osrm-response}
\end{figure}

This data needs then to be processed in order to retrieve only the desired information, namely waypoints and instructions. This was done using the \textit{JSONCpp library}, obtained from \cite{Lepilleur}. But the waypoints come in and encoded format (Google Encoded Polyline), an example can be found in table \ref{tab: Polyline}, and therefore an algorithm to decode this information had to be written (Appendix \ref{App:AppendixC}). 

\begin{table}
\centering
 \caption{Polyline Example}
 \label{tab: Polyline}
 \begin{tabular}{l|l}
 Encoded Polyline & Decoded Polyline \\ \hline
 \url{ez\~vFvwys@o@\~@NRTVU} & 40.63155, -8.65676 \\
 ~ & 40.63179, -8.65708 \\
 ~ & 40.63171, -8.65718 \\
 ~ & 40.63160, -8.65730 \\
 ~ & 40.63171, -8.65745 \\
 \end{tabular}
\end{table}

Luckily decoding this format is quite common and the Google company provides the instructions \cite{GoogleDevelopers}, unfortunately no algorithm in C++ was found, so an algorithm written in JavaScript \cite{MAPQUEST} had to be adapted to C++. After successfully decoding the waypoints, they had to be matched with the respective instruction. After this process, this module returns the waypoints and the instructions that need to be done during the entire trip. Figure \ref{fig: missionplan_list} shows an example of the waypoints and the instructions that are returned by the this module. 
\begin{figure}
\centering
\includegraphics[width=\textwidth]{missionplanner3X.png}
\caption{Mission Planner response showing a list of waypoints and the respective instruction}
\label{fig: missionplan_list}
\end{figure}
\hfill \\
\hfill \\ The following eighteen maneuvers are available: 
%list of manuevers 
\begin{itemize}
\item NoTurn
\item GoStraight
\item TurnSlightRight
\item TurnRight
\item TurnSharpRight
\item UTurn
\item TurnSharpLeft
\item TurnLeft
\item TurnSlightLeft
\item ReachViaLocation
\item HeadOn
\item EnterRoundAbout
\item LeaveRoundAbout
\item StayOnRoundAbout
\item StartAtEndOfStreet
\item ReachedYourDestination
\item EnterAgainstAllowedDirection
\item LeaveAgainstAllowedDirection
\end{itemize}
%end list of maneuvers


An example of the mission planning workflow is illustrated in figure \ref{fig: mission_plan_work}.

\begin{figure}
\centering
\includegraphics[width=\textwidth,height=0.7\textheight,keepaspectratio]{mission_planning_workflow.eps}
\caption{Mission planner workflow, illustrating all the steps since the request until a response is sent}
\label{fig: mission_plan_work}
\end{figure}

\subsection{Navigation manager}
This module will act as the client. It establishes a communication with the GPS receiver and communicates with a database. It subscribes to the messages that are being published by the GPS topic. When the navigation manager module starts, it establishes a communication with the database and, in case of a successful connection, it proceeds else the program stops and lets the user know that there was an error. After successfully establishing a connection it will delete all the data in all tables. A small menu in the console appears and waits for the user to select the desired destination from a predefined list of destinations or, by using the webpage, to choose a custom destination. The current position and the destination coordinates are then sent to the server as a request. After successfully obtaining the response, the destination coordinates, waypoints and instructions are written to the database. After this step the waypoints received need to be matched with the coordinates from the GPS. The GPS coordinates are also written to the database. Using the Haversine formula (\ref{eq: Haversine formula}) \cite{HaverFormula}, the distance between two pairs of latitude and longitude coordinates can be calculated.

\begin{description}
 \item d : is the distance between the two points (along a great circle of the sphere),
 \item r : is the radius of the sphere (the mean radius of the Earth is used, which is 6371 km),
 \item[$\phi_{2},\phi_{1}$] : latitude of point 1 and latitude of point 2 (in radians), 
 \item[$\lambda_{2},\lambda_{1}$] : longitude of point 1 and longitude of point 2 (in radians).
 \end{description}

\begin{equation} \label{eq: Haversine formula} 
d=2r\arcsin\left (  \sqrt{\sin ^{2}\left ( \frac{\phi _{2}-\phi_{1}}{2}\right )+\cos (\phi _{1}) \cos (\phi _{2})\sin ^{2}\left (  \frac{\lambda _{2}-\lambda _{1}}{2}\right )}\right )
\end{equation} 

 The Haversine formula is used to calculate the distance between all waypoints from the list and the current GPS coordinates. The waypoint with the shortest distance is then chosen, and the instruction associated with it is presented. The procedure is similar for the next instructions to perform, but instead of choosing the closest waypoint it chooses the second closest waypoint. These two items are then written to the database. This module also monitors the movement of the car and guarantees that, in case it goes out of the planned route, a new route based from the current position to its previous destination is generated. This is done by comparing the two closest waypoints and, in case they both increase, an event is triggered and a new request is made to the server.
 
An example of the workflow is illustrated in the figure \ref{fig: navi_man_work}.

\begin{figure}
\centering
\includegraphics[width=\textwidth,height=0.8\textheight,keepaspectratio]{navi_manager_workflow.eps}
\caption{Navigation manager workflow, illustrating all the steps and decisions that are taken by this module}
\label{fig: navi_man_work}
\end{figure}


\subsubsection{Database}
The database system used is PostgreSQL 9.3. After installing PostgresSQL 9.3 it was necessary to create a user and a password. After that, it is necessary to create a database; it was named $"atlas{\_}navigation"$. Finally all five tables are created.
The database contains five tables that will be used to store the data from the Navigation manager module and from the webpage. In the first table the waypoints and the instructions are stored. On the second table the current position acquired from the GPS receiver is stored. Table three contains the closest waypoint. The forth table contains the destination coordinates, and the last table contains the next instruction. The structure of the database is represented in figure \ref{fig: db_structure}.

\begin{figure}
\centering
\includegraphics[width=0.9\textwidth,height=\textheight,keepaspectratio]{db_structure.eps}
\caption{Database Structure. There are five tables with the mission information}
\label{fig: db_structure}
\end{figure}


The connection to the database is made using a localhost connection, as the database is stored in the computer aboard the Atlascar. A graphical interface, pgAdmin III, is also available to monitor the databases (figure \ref{fig: pgAdmin}). 
\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{pgAdmin.png}
\caption{pgAdmin III interface that allows monitoring several databases and its content}
\label{fig: pgAdmin}
\end{figure}

\subsection{Web page}
The Webpage, figure \ref{fig: webpage_final}, allows the user to choose the desired destination by clicking on the map and pressing the "submit" button. It presents to the user a visual interface of the route (the waypoints are drawn on google maps), the location of the car and the current and next maneuver that needs to be executed. It runs on a local Apache2 server. The server contains the webpage layout (.html)(Hypertext Markup Language), two javascript files (.js) and a PHP (Hypertext Preprocessor) file (.php). 

\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{finalWebpageNew.png}
\caption{Webpage that provides a user interface and allows choosing a destination}
\label{fig: webpage_final}
\end{figure}

%INUTIL
%And example of the web page structure is visible on figure \ref{fig: webpage_structure}.
%\begin{figure}
%\includegraphics[width=\textwidth,height=\textheight,keepaspectratio]{webpage_relation.png}
%\caption{Webpage Structure}
%\label{fig: webpage_structure}
%\end{figure}
%
The workflow of the webpage is described in figure \ref{fig: webpage_workflow}.

\begin{figure}
\includegraphics[width=\textwidth,height=\textheight,keepaspectratio]{webpage_workflow.eps}
\caption{Webpage Workflow illustrating the steps and decisions that are taken by the user interface}
\label{fig: webpage_workflow}
\end{figure}

\subsubsection{Html}
This file contains the layout of the webpage which was designed using the Bootstrap framework. The user is able to choose a custom destination by clicking on the map. The coordinates are then saved and when the user presses the $"submit"$ button they are sent to the .php and stored in the database. The destination can also be chosen from the console after executing the $"mission\_planning"$ launch file. Using the google Maps API (Application Program Interface) the location of the car using a custom icon and also all the waypoints are drawn on the map. The current position, the maneuver that needs to be executed and the next maneuver to be executed is shown on the page. Every time the car approaches to the closest waypoint, this will change its default icon to a custom one.
 
\subsubsection{PHP}
It was necessary to install \textit{php5} and the \textit{php5-pgsql} package, which provides a module to establish connections to a PostgreSQL database directly from PHP scripts. By using PHP we establish then a connection to the $"atlas\_navigation"$ database (listing \ref{php_connection}). 

% Set your language (you can change the language for each code-block optionally)
\lstset{language=PHP, 
	commentstyle = \color{gray},
    extendedchars = \true,
    inputencoding = utf8x,
    keepspaces = true,
    showstringspaces=false,
    keywordstyle = \bfseries}  
% Start your code-block
\begin{lstlisting}[frame=single,caption=Database connection,label=php_connection] 
//Connect to database
  $dbconn = pg_connect("host=localhost port=5432 
  dbname=atlas_navigation user=atlas 
  password=atlascar")or die(
  'Could not connect: ' . pg_last_error());
\end{lstlisting} 

By using specific queries the data is retrieved from the tables. Listing \ref{php_query} shows an example query that retrieves everything inside the $"waypoint\_table"$.

% Start your code-block
\begin{lstlisting}[frame=single,caption=Database query,label=php_query] 
//Performing SQL query
$query = 'SELECT * FROM waypoint_table';
\end{lstlisting} 

After reading the data from the databse, the waypoints, instructions, closest waypoint and current position are processed and encoded in a JSON format. This JSON is then accessible to the Javascript. This module is also responsible for writing the desired destination coordinates to the database.

\subsubsection{JavaScript}
A Javascript program named $"main.js"$ requests the JSON object that has been built by the PHP. After obtaining the JSON, the script accesses the information inside it, namely the waypoints, instructions, GPS coordinates and the closest waypoint. This is done using functions from the jQuery library. The data obtained from the JSON is then processed and shown on the webpage. Every two seconds the PHP is called to check for new data and update the results.
A second Javascript named $"button.js"$ is responsible for sending the desired destination coordinates to the PHP. This happens when the user presses the $"submit"$ button. After successfully sending the destination a message appears informing the user that the destination has been sent, this information fades out after five seconds. 

\chapter{Experiments and results}
%This chapter will describe all the experiments made to the system. The results obtained are shown and explained 

%\section{Testing}
Several tests have been made to the software and to the hardware. First the software was tested in the laboratory, by doing simulations. After this, the software was tested in different paths, from city to open field. A video for every path was recorded for further analysis and all maneuvers were tested during these procedures. Invalid destinations where also tested and in case an unreachable location is chosen, the software returns the path to closest reachable location. The GPS receiver was also tested to see where it loses signal. This happens, for example, inside buildings (e.g. garage or underground parks) but only if the receiver is still trying to acquire coordinates. Because after this it even works in the garage. 
The system overall works well. But it might happen that it  misses the current instruction to do, due to the fact that there was multiple waypoints close to each other. Even if it fails the current instruction to do, the next instruction to do was already shown so the user is aware of what he needs to do.
\section{Fine Tune}
The tests were made with different values to check if the car went out of route and to determine the next instruction that needs to be executed. First the system was tested with a value of 100m, this would allow the vehicle to move away 100m from the next waypoint it was supposed to go. If the vehicle moves away from the next waypoint above 100m the route would be recalculated. But this didn't work as well as expected so the distance was reduced to 75m which works much better. 

The refresh rate for the webpage was also reduced from three seconds to two seconds, this allows a more accurate observation of the results, yet not perfect because the ideal scenario would be the same refresh rate as the publish rate of the GPS receiver. Since the GPS was set to publish coordinates each 0.25 seconds, some information is lost. This could be improved in the future, because there was not enough time to find, learn and adapt the new solution.
\section{Missions}
During the missions, figure \ref{fig: mission1} and \ref{fig: mission2}, a rosbag has been created to record the coordinates for further analysis. This coordinates have then been plotted on the map. 

\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{mission1oricalcX.png}
\caption{First mission to test the system. The route was from point A to B. The original route (red) was ignored at a certain point and a new route was generated (blue) from A1 to B1.}
\label{fig: mission1}
\end{figure}

\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{mission2fullX.png}
\caption{Second mission to test the system. Following the original route.}
\label{fig: mission2}
\end{figure}

On the first mission the original plan was to go from point A to B (red route) and test several maneuvers. But during this mission the original plan was ignored to test if the system generates a new route within 75m. This new route (blue route) has been successfully generated (from point A1 to B1) as shown in figure \ref{fig: altroute1}. The distance was not 75m as expected, instead the distance obtained was 80m, which is still quite good.

The second mission was also randomly chosen but this time without ignoring the original plan.

\begin{figure}[h]
\centering
\includegraphics[width=0.9\textwidth]{mission1gapX.png}
\caption{Alternative Route generated (blue) when the original route was ignored.}
\label{fig: altroute1}
\end{figure}

The GPS works very well in urban areas and even short tunnels are not a problem. A good example can be found in figure \ref{fig: goodtrack} which shows the recorded coordinates plotted on a map. Although there was a situation in both missions where the GPS coordinates where not so good, this happened in an area with a high density of trees. In this situation the GPS still works, but since its signal is being blocked, without loss if signal, the coordinates vary a bit and the result shown in figure \ref{fig: badtrack} is not so good.

\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{mission2goodX.png}
\caption{Good GPS Coordinates Recording}
\label{fig: goodtrack}
\end{figure}

\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{mission1woodX.png}
\caption{Bad GPS coordinates recording, due to the high density of trees.}
\label{fig: badtrack}
\end{figure}

During these missions two different videos for both missions where recorded. One video contains the recordings from the webpage and the other one which was made with a camera contains the recordings from path made during the mission. These videos where used for further processing. 
\section{Instructions}
On figure \ref{fig:m1doubleturn} the current instruction is to turn left and the next instruction is turn right, which corresponds to the reality as shown by the camera. 
%% map   ||  % video  
%\begin{figure}
%\centering
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=1.0\textwidth]{m1doubleturnmapX.png}
%  
% % \caption{Mission 1}
% % \label{fig: mission1full}
%\end{subfigure}%
%\begin{subfigure}{.5\textwidth}
%  \centering
% \includegraphics[width=0.9\textwidth]{m1doubleturncam.png}
% % \caption{Mission 2}
% % \label{fig:mission2full}
%\end{subfigure}
%\caption{Right and left turn}
%\label{fig:m1doubleturn}
%\end{figure}
\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{m1doubleturn.png}
\caption{Instruction to turn first left and then right}
\label{fig:m1doubleturn}
\end{figure}

Another example of an instruction is shown in figure \ref{fig:m2sright} where the user is informed to turn slight right.
%% map   ||  % video  
%\begin{figure}
%\centering
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=\textwidth]{m2turnsrightmapX.png}
%%  \caption{Interface}
%%  \label{fig: currInstPage}
%\end{subfigure}%
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=.9\textwidth]{m2turnsrightcam.png}
%%  \caption{Video}
%%  \label{fig:currInstVideo}
%\end{subfigure}
%\caption{Slight right turn}
%\label{fig:m2sright}
%\end{figure}
\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{m2turnsright.png}
\caption{Slight right turn}
\label{fig:m2sright}
\end{figure}

When there is no instruction it means to go on/follow the road. There is also an instruction that might appear which tells the user to go straight and don't turn as shown in figure \ref{fig:m2gostraight}. 
%% map   ||  % video  
%\begin{figure}
%\centering
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=\textwidth]{m2gostraightmapX.png}
%%  \caption{Interface}
%%  \label{fig: currInstPage}
%\end{subfigure}%
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=.9\textwidth]{m2gostraightcam.png}
%%  \caption{Video}
%%  \label{fig:currInstVideo}
%\end{subfigure}
%\caption{Follow Road}
%\label{fig:m2gostraight}
%\end{figure}
\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{m2gostraight.png}
\caption{Instruction to follow road and ignore all exits}
\label{fig:m2gostraight}
\end{figure}

The instructions overall are quite good. But some instructions in roundabouts can be a bit more confusing for the user as shown in figure \ref{fig:m1roundfail}. In this case the goal was to go back, so the third exit after entering the roundabout needs to be taken. This is not so clear with the instruction given, which says "Enter Roundabout and turn right". But the instruction is not wrong and it appears on the correct exit. 

%% map   ||  % video  
%\begin{figure}
%\centering
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=\textwidth]{m1roundfailmap.png}
%%  \caption{Interface}
%%  \label{fig: currInstPage}
%\end{subfigure}%
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=.9\textwidth]{m1roundfailcam.png}
%%  \caption{Video}
%%  \label{fig:currInstVideo}
%\end{subfigure}
%\caption{Confusing instruction}
%\label{fig:m1roundfail}
%\end{figure}
\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{m1roundturnfail.png}
\caption{Confusing instruction to take the third exit at the roundabout}
\label{fig:m1roundfail}
\end{figure}

A less confusing instruction for a roundabout is shown in figure \ref{fig:m1roundgood}. In this case the goal is to take the third exit, but the exit that leads to the hospital doesn't count, so the second exit needs to be taken. The instruction says "Enter Roundabout and turn slight right". If the goal is to take the first exit in a roundabout the instruction would be "TurnRight" so "Enter Roundabout and turn slight right" means to take the second exit and "Enter Roundabout and turn right" indicates to take the third exit.
%
%% map   ||  % video  
%\begin{figure}
%\centering
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=\textwidth]{m1roundgoodmapX.png}
%%  \caption{Interface}
%%  \label{fig: currInstPage}
%\end{subfigure}%
%\begin{subfigure}{.5\textwidth}
%  \centering
%  \includegraphics[width=.9\textwidth]{m1roundgoodcam.png}
%%  \caption{Video}
%%  \label{fig:currInstVideo}
%\end{subfigure}
%\caption{Roundabout instruction}
%\label{fig:m1roundgood}
%\end{figure}
\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{m1roundgood.png}
\caption{Roundabout instruction to take the second exit}
\label{fig:m1roundgood}
\end{figure}

\chapter{Conclusions and future work}
\section{Conclusion}
The objectives for this thesis were to install the new hardware for the Atlascar, namely: a more powerful server and a high precision GPS; Developing a graphical interface that would allow to choose the start and destination coordinates in an interactive way, this application should also allow the user to monitor in real-time the steps of the mission; Developing a module to execute the mission planning that uses high level maneuvers or decisions that need to be executed during the mission. And finally, in an more advanced phase, this module should be able to select and execute the necessary maneuvers. This work contributed to the need the Atlascar had of a solution for mission planning and managing. The developments of this work contributed to fill the need for a mission planning and managing solution that would allow the car to become more autonomous.

%The objectives proposed for this thesis were successfully achieved, tested and documented. An application to track the vehicle in real-time and output sequences of high level maneuvers (e.g., turn right, turn left, etc.) that need to be executed during the whole mission was successfully built. Installation of the new hardware, interfacing with the GPS and redesigning the trunk layout was also done. 

The objectives proposed for this thesis were successfully achieved, tested and documented. An application to track the vehicle in real-time and output sequences of high level maneuvers (e.g., turn right, turn left, etc.) that need to be executed during the whole mission was successfully built. This was done using an open source routing planner to calculate the path between two locations, a high-precision GPS to determine the locations of the vehicle, a webpage to serve as a user interface and a database to store the data. All these solutions had to be linked to work in a ROS environment. The installation of the new hardware (server and GPS) was also done by redesigning the trunk layout and with some interventions on the equipment. 
Interfacing with the GPS was also achieved thanks to a ROS package created by the GPS Vehicle Dynamics Laboratory from the Auburn University (GAVLab).

The system works well and provides a user-friendly interface. Establishing a communication on linux with the GPS was complicated but, in the end, everything worked well. As expected, it is a very precise GPS and works very well in areas where a cheaper GPS could possibly fail. A solution to communicate, write and read from a database via C++ was also found and implemented, since the \textit{database{\_}interface} package available in the LAR repository doesn't work. It was not possible to install the package due to the errors it produced, also, including the header files did not work and caused a lot of errors. The backend provided by OSRM works very well and saved a lot of time that otherwise would be spent in writing algorithms for calculating the best way between two points. The instructions that are provided by this software are simple and easy to understand, except for the ones related to roundabouts where the instructions can be confusing. This needs to be fixed in future works. This software was later installed on the server that is located in the laboratory. This service is now available for everyone to use, at the university or at home. In case there is a power failure or if the servers need to be rebooted, the backend needs to be launched again which is carefully explained in appendix \ref{App:AppendixA}. The software infrastructure is working well and is ready for upgrades, improvements or modifications.

During the tests, there was an error that occurred several times and which precludes the correct operation of the system. With the help of both advisors the problem was found. It had to do with the power DC inverter that was causing interference in the USB ports. The problem was not deeply investigated but the new UPS (Uninterruptible Power Supply) that was bought apparently could solve this issue. The temporary solution found is to turn off the inverter and work only with the power from the UPS which lasts around 40 minutes.

\section{Future work}
Despite the fact that the system is working well, it still has a few things that could be improved. For future work:
\begin{itemize}
  \item it is necessary to make this module communicate with the vehicle hardware and trigger the respective maneuvers;
 
  \item a new solution for the visual interface could be implemented to update the values without having to refresh the page every time;
 
  \item the server lacks a firewire card which needs to be installed in order to make the cameras work. The firewire card that needs to be installed is currently installed on the old server which is located in the laboratory;
  
  \item possibly finding a better way to install the software for the GPS since it was installed without creating a catkin package for it;
  
  \item the launcher file for the GPS could be improved so that it searches automatically for the USB port where the device is plugged in;
  
  \item install the new UPS to solve the interference problem;
  
  \item create a script for the LAR server that automatically launches the osrm-backend software.
  
\end{itemize} 
    

%Este contem links ****************************
\bibliographystyle{plainnat} % nao esta ordenado
%\bibliographystyle{unsrtnat} %e esta ordenado
%\bibliographystyle{abbrvnat} %inutil
%Este � bom , mas nao tem links ...************
%\bibliographystyle{ieeetr}

%MAU esta todo trocado*************************
%\bibliographystyle{unsrt}
%\bibliographystyle{plain}
%\bibliographystyle{abbrv}
%\bibliographystyle{alpha}
\bibliography{library}
\cleardoublepage

\appendix 
\chapter{General Instructions}\label{App:AppendixA}

%\textbf{Qt Installation}
\section{Qt Installation}
An example can also be found under the following link:\hfill \\
\url{http://sysads.co.uk/2014/05/install-qt-5-3-ubuntu-14-04/}

Go to the qt website, then choose downloads and select "Qt Online Installer for Linux 64-bit (29 MB)"   

After downloading open the console and "cd" into the location of the downloaded file. Next do: \hfill \\
\path{chmod +x qt-unified-linux-x64-2.0.2-1-online.run}
and then:\hfill \\
\path{./qt-unified-linux-x64-2.0.2-1-online.run}

Install the program.

To execute the program open the console and type:\\
\path{cd /home/<user>/Qt/Tools/QtCreator/bin/}

For example: \hfill \\
\path{cd /home/atlas/Qt/Tools/QtCreator/bin/}

After that type:\hfill \\
./qtcreator

In case you don't do the opening this way, it will cause errors when compiling programs that uses ROS.
\hfill \\


%************************************
\section{Postgresql 9.3 Installation}
%\textbf{POSTGRESQL 9.3 INSTALLATION}

An example can also be found under the following link:\hfill \\
\url{http://ubuntuhandbook.org/index.php/2014/02/install-postgresql-ubuntu-14-04/} \hfill \\

To get install PostgreSQL on Ubuntu 14.04, do the following steps:
\begin{enumerate}
  \item Create and edit the PostgreSQL repository by running the command below: \hfill \\
  \path{sudo vi /etc/apt/sources.list.d/pgdg.list}

Press I on keyboard and add the below line into the file: \hfill \\
    \path{deb http://apt.postgresql.org/pub/repos/apt/ trusty-pgdg main}

Press Esc on keyboard and followed by :wq to save the file.

  \item Download \& import the repository key: \hfill \\
  \path{wget --quiet -O - https://www.postgresql.org/media/keys/ACCC4CF8.asc | sudo apt-key add -}
  
  \item Update your system: \hfill \\
  \path{sudo apt-get update && sudo apt-get upgrade}
  
  \item Now you're able to install PostgreSQL via below command: \hfill \\
  \path{sudo apt-get install postgresql-9.3 pgadmin3}
\end{enumerate}


%\textbf{Configuring the Database}
\subsection{Configuring the Database}
An example can also be found under the following link:\hfill \\
\url{https://wixelhq.com/blog/how-to-install-postgresql-on-ubuntu-remote-access}

%The configurations where made with my name. 
%First set up your new Linux User Accounts
%
%The accounts you will set up here will be what you will be using to connect to your database. The connection strings you use in your applications will contain the usernames and passwords for these accounts.
%
\begin{enumerate}
	\item First, we'll switch to our postgres super user account: "sudo su - postgres" \hfill \\ 
You are now in the postgres user account session, next we'll log into PostgreSQL itself using the psql CLI.

	\item Now lets create a new role ( New Database user) \hfill \\
	type: \path{createuser --interactive} \hfill \\

	Then answer the upcoming questions like this: \hfill \\
	Name of role: atlas \hfill \\
	new role will be a superuser?: n \hfill \\
	new role will be allowed to create databases?: y \hfill \\
	new role will be allowed to create more new roles?: y \hfill \\
	\item Next, we will create a database with the name atlas{\_}navigation \hfill \\
	createdb \path{<database name>}

	\path{<database name>} will be "atlas{\_}navigation"
	\item Last thing here would be to change ownership of the newly created database to your new role: \hfill \\
	
Inside the console type : "psql" 

The command to change a owner of a database is: ALTER DATABASE \path{<database name>} OWNER TO \path{<role name>}; 

 ALTER DATABASE atlas{\_}navigation OWNER TO atlas; 

The user you just created has no password so you can't connect remotely using it. We'll need to set a password for this account before proceeding:

The command to set a password for a database is : ALTER USER \path{<username>} WITH PASSWORD '\path{<new password>}';

 ALTER USER atlas WITH PASSWORD 'atlascar';

Now enter \textbackslash q and then "exit" to logout from postres user.
%check \q
\end{enumerate}

\subsection{Connect to your Database Remotely}

There are several ways to connect to your database server, here are the most common options:
\begin{enumerate}
\item Connecting to PostgreSQL using a connection string \hfill \\
\path{postgresql://<username>:<password>@<server>:5432/<database>}

\item Connecting to PostgreSQL via the Command Line \hfill \\
psql -h \path{<server>} -U \path{<username>} -d \path{<database>} -W

The -W tells psql that you will be entering a password.

In this case we will use option 2:

psql -h localhost -U atlas -d atlas{\_}navigation -W
\end{enumerate}

Now that you are in the postgresql command line, here are some useful commands you can use:

\begin{description}
  \item[\textbackslash password]  Change your password
  \item[\textbackslash q] Exit psql
  \item[\textbackslash l ] List all databases accessible by the current account
  \item[\textbackslash du ] List roles
  \item[\textbackslash c \path{<db name>}] Connect to a database
  \item[\textbackslash dt  ]  List all tables in a connected database
  \item[\textbackslash d \path{<table>} ] List columns in the selected table
  \item[\textbackslash conninfo   ] Show information about your current connection
\end{description}
 
Next we want to create the following tables, type:

CREATE TABLE waypoint{\_}table (waypoint{\_}id serial PRIMARY KEY, way{\_}lat numeric, way{\_}lon numeric, instruction varchar);

CREATE TABLE closest{\_}waypoint{\_}table (closest{\_}id serial PRIMARY KEY, closest{\_}index numeric);

CREATE TABLE gps{\_}coord{\_}table (gps{\_}coord{\_}id serial PRIMARY KEY, gps{\_}lat numeric, gps{\_}lon numeric);

CREATE TABLE destination{\_}table (dest{\_}id serial PRIMARY KEY, dest{\_}lat numeric, dest{\_}lon numeric);

CREATE TABLE next{\_}inst{\_}table (nextinst{\_}id serial PRIMARY KEY, next{\_}inst varchar); \hfill \\

POSTGRES should now be set up. Type \textbackslash q to exit psql command line \hfill \\

You can use now pgAdmin3 as a GUI to watch your database. Just press"connect" fill in the fields like this:
\begin{description}
\item[Name:] atlas
\item[Host:] localhost
\item[Port:] 5432
\item[Service:] 
\item[Maintenance DB:] postgres
\item[Username:] atlas
\item[Password:] atlascar
\end{description}

And now press OK. Now you are connected to the atlas database, you can press the "+" to expand \path{Atlas->Databases->atlas_navigation->Schemas->public->Tables} . 

To disconnect right-click "atlas(localhost:5432)" and choose "Disconnect/Connect Server" \hfill \\

%*********************************/*****************/*********************************

%\textbf{Installing PHP5 and Apache on Ubuntu}
\section{Installing PHP5 and Apache on Ubuntu}
Further examples can be found under the following links: \hfill	\\
\url{http://www.howtogeek.com/howto/ubuntu/installing-php5-and-apache-on-ubuntu/} \hfill \\
\url{http://php.net/manual/en/pgsql.installation.php} \hfill \\		

From a command shell, you will run the following commands:

    sudo apt-get install apache2

    sudo apt-get install php5

    sudo apt-get install libapache2-mod-php5

    sudo apt-get install php5-pgsql

    sudo /etc/init.d/apache2 restart \hfill \\

After successfully installing these packages create an empty file named "index.php" edit and paste this inside

\path{<?php}
\path{print_r(phpinfo());}
\path{?>}

Copy this file to \path{/var/www/html} you will need sudo permissions so do: "sudo cp index.php /var/www/html"

Test apache2 by opening a new page in your browser and type "localhost"
Test php by opening a new page in your browser and type "localhost/index.php"

To start and stop apache2 server type in console: sudo service apache2 start or stop \hfill \\

%**************************************************************************************

%\textbf{Gps Configurations}
\section{GPS Configuration}
As described by novatel under the following link: \url{http://www.novatel.com/support/known-solutions/oem4-and-oemv-usb-driver-for-linux/} \hfill \\

In order to make the gps usb port be recognized by your computer it is necessary to create a new rule in the system. \hfill \\

sudo nano /etc/udev/rules.d/z90{\_}novatel.rules

place this inside: \hfill \\

SUBSYSTEM=="usb", SYSFS{idProduct}=="0100", SYSFS{idVendor}=="09d7", 
PROGRAM="/sbin/modprobe usbserial vendor=0x09d7 product=0x0100" \hfill \\

BUS=="usb", SYSFS{idProduct}=="0100", SYSFS{idVendor}=="09d7", 
SYSFS{product}=="NovAtel GPS Receiver", SYSFS{manufacturer}=="NovAtel 
Inc.", SYMLINK+="gps\%n" \hfill \\

Save file. S \hfill \\

You will also need to add the computer \path{<user>} to the dialout group.
 
Open terminal and type :

"cd /etc/" \hfill \\
"more group" \hfill \\
Now check if dialout has no user added , or if the current user is added. If no user is added it should be like this: "dialout:x:20:" \hfill \\

Now in the terminal type:
"sudo gedit group" \hfill \\

Edit the file so that you add your user to the dialout group: \hfill \\		
"dialout:x:20:\path{<user>}" \hfill \\		

Save file. \hfill \\

Log out from current session and enter again. \hfill \\	
Voil\'a it should work now. Communication can be tested with CuteCom . \hfill \\

%**************************************************************************************
%\textbf{How to install osrm-backend} \hfill \\
\section{How to install osrm-backend}
Tutorials available at: \hfill \\

\url{http://dogeo.fr/osrm-installation/} \hfill \\
\url{https://www.digitalocean.com/community/tutorials/how-to-set-up-an-osrm-server-on-ubuntu-14-04} \hfill \\
\url{https://github.com/Project-OSRM/osrm-backend/wiki/Building-on-Ubuntu} (1st) (the dependencies where not enough aparently ... , use the ones from digitalocean ) \hfill \\
\url{https://github.com/Project-OSRM/osrm-backend/wiki/Building\%20OSRM} (2nd) \hfill \\
\url{https://github.com/Project-OSRM/osrm-backend/wiki/Running-OSRM}(3rd) \hfill \\


This tutorial will guide you to the process of installing osrm-backend on your machine:

\begin{enumerate}

\item Enter on your Ubuntu account ( make sure you have sudo permissions because you will need them ) 

\item Install dependencies 

sudo apt-get install build-essential git cmake pkg-config \
libbz2-dev libstxxl-dev libstxxl-doc libstxxl1 libxml2-dev \
libzip-dev libboost-all-dev lua5.1 liblua5.1-0-dev libluabind-dev libluajit-5.1-dev libtbb-dev

if you get an error on step 3 at cmake try these additional dependencies:

sudo apt-get install libboost-all-dev libtbb-dev liblua5.2-dev libluabind-dev libstxxl-dev libxml2 libxml2-dev libosmpbf-dev libbz2-dev libprotobuf-dev

\item now do these steps: 

git clone \url{https://github.com/Project-OSRM/osrm-backend.git} \hfill \\
cd osrm-backend \hfill \\
mkdir -p build \hfill \\
cd build \hfill \\
cmake ..   ( if you get an error here its because of the dependencies, read step 2 ) \hfill \\
make

\item Now download the maps that you want, in this case we will use the ones from Portugal. Go to \url{http://download.geofabrik.de/europe.html} and select Portugal from the list and download the .osm.pbf file. Copy this file into the "build" folder. 

\item Now inside the build folder do these two steps: 

ln -s osrm-backend/profiles/car.lua profile.lua \hfill \\
ln -s osrm-backend/profiles/lib

\item now do ./osrm-extract portugal-latest.osrm.pbf  

\item wait... 

\item now do ./osrm-prepare portugal-latest.osrm 

\item wait ... 

\item After completing these two steps lets change the ip. Do ./osrm-routed --ip=computerIp 

\item Now lets test the installation  and start the service, do  ./osrm-routed portgual-latest.osrm 

\item It should be working fine if you don't see any errors. You can test the system for example with this query :

\url{http://193.137.172.18:5000/viaroute?loc=40.631599,-8.657289&loc=40.637814,-8.658555&instructions=true}

you should get a similar message from figure \ref{fig: osrm-response} 

%{"hint_data":{"locations":["Tz0CAOrwDAAFdQAAHQAAAAIAAAAAAAAAzgEAAKeqDgAAAAAAPf1rAmPme_8AABEA","ChYBAMwaAQCBEwAABgAAAA4AAAAAAAAAsQQAACngAQAAAAAAghVsAnLhe_8AABEA"],"checksum":1046632732},"route_name":["Avenida Jo�o Jacinto Magalh�es","Rua Calouste Gulbenkian"],"via_indices":[0,51],"found_alternative":false,"route_summary":{"end_point":"Rua da Pega","start_point":"Avenida Jo�o Jacinto Magalh�es","total_time":163,"total_distance":1543},"via_points":[[40.631611,-8.657309],[40.637825,-8.658574]],"route_instructions":[["10","Avenida Jo�o Jacinto Magalh�es",538,0,65,"537m","SW",229,1],["2","",42,15,5,"41m","E",96,1],["11-3","",49,24,4,"49m","NW",322,1],["1","Rua Calouste Gulbenkian",320,28,25,"320m","NW",315,1],["3","Rua dos Santos M�rtires",155,39,18,"155m","NE",42,1],["7","Cais dos Santos M�rtires",63,42,7,"62m","NW",310,1],["7","Rua Doutor Querubim Guimar�es",35,43,4,"34m","SW",221,1],["5","Rua Doutor Querubim Guimar�es",35,44,4,"34m","NE",41,1],["3","Cais dos Santos M�rtires",63,45,7,"62m","SE",130,1],["3","Rua dos Santos M�rtires",155,46,20,"155m","SW",229,1],["1","Rua da Pega",6,49,1,"5m","SW",211,1],["15","",0,50,0,"0m","N",0]],"route_geometry":"yr}nlAxxkoOkYff@{QxYgHxHeJ|F}M~DcIj@mMw@eOkGsLsLmg@as@aQsWkVs]uAuC{@eDoBuJhAs]X_FFwE}@}CeB}EmD{DaGoCkDxBiBfByFbGyCvEwDdFg@hB}GzJsIxLoG|IsM~Q{CrEqJrMa]|e@uIvLyDdGoKnXoCdHuFkHoI{Mwf@_|@_Vhb@tMvOuMwO~Uib@vf@~{@nIzMtFjHzAfA","status_message":"Found route between points","status":0}

You can see on the console, where the service is running, from where the request came from ( ip and browser ) and what was requested.

Note: To shutdown the system simply do CTRL+C  and to start it again do step 11. This way it will only work during this session if you logout this won't work anymore. 
In order to make this work even after you logout, you need to add "\& disown".

To do so, first make sure you are in the "build" folder ( cd osrm-backend/build ) then do: \hfill \\
./osrm-routed portgual-latest.osrm \& disown 

\item Enjoy !

\end{enumerate}

%------------------------------------------------------------------------------------------------------------
%------------------------------------***************************---------------------------------------------
%------------------------------------------------------------------------------------------------------------
\section{How to use the program}
%How to use the program: \hfill \\

After having everything launched , on the client console you can select one of the predefined options ( 1 to 8 ) or select a custom destination by clicking on the map. If you wish to select a custom destination don't press 8 now! First open the browser and type localhost. Then click on the destination you want to go, you will see a red marker on that location. After this, press "send location" and confirm the message that will pop-up. Now you can go back to the console and press 8. You are ready to start the mission. To select a different mission, do CTRL+C on the client console and launch it again. Repeat the steps.

\hfill \\

%**************************************************************************************

\textbf{How to execute the software step-by-step}

There are two ways to do this, either by launching the components one by one.
\begin{enumerate}
\item sudo service apache2 start
\item open webpage (type in browser : localhost, wait until the map loads before proceeding to the next step. Refresh page if necessary )
\item roscore
\item roslaunch novatel novatel{\_}for{\_}psr.launch
%\item ./osrm-routed portugal-map.osrm (possibly not necessary if this module runs on the LAR server)
\item rosrun mission{\_}planning mission{\_}planning{\_}node
\item rosrun mission{\_}planning mission{\_}planning{\_}client
\item select an option from the console and you are ready to go, results appear on the webpage
\end{enumerate}

\textbf{Or} by launching the launch file, this will save you some time.
\begin{enumerate}

\item sudo service apache2 start
\item open webpage (type in browser : localhost )
\item roslaunch mission{\_}planning mission{\_}planning.launch
\item select an option from the console and you are ready to go, results will appear on the webpage. If you want to select a destination from the map, click on the desired location ( a marker will be placed ) and the press the "submit" button. After this step go to the console and type "8". 
\end{enumerate}

NOTE: To choose another route simply press CTRL+C on the console and launch it again (roslaunch mission{\_}planning mission{\_}planning.launch) . \hfill \\

If you get and error on step 3 which has to do with the GPS, it happend because the GPS was not found at the port specified in the launchfile. To solve this go to \path{workingcopies/lar5/src/sensors/novatel_span/novatel-master/launch} and edit novatel{\_}for{\_}psr.lauch. Look out for param name="port" value="/dev/ttyUSB3" and change to USB port to the one that the GPS is connected to.

You can change the publish rate of the GPS coordinates by editing the novatel{\_}for{\_}psr.lauch file and changing the value of the GPS default log period. param name="gps{\_}default{\_}logs{\_}period" value="0.25"

%1st roscore
%2nd roslaunch novatel novatel_for_psr.launch
%3rd ./osrm-routed portugal-map.osrm (possibly not necessary if this module runs on the LAR server)
%4th sudo service apache2 start
%5th rosrun mission_planning mission_planning_node
%6th rosrun mission_planning mission_planning_client
%7th open webpage (type in browser : localhost )
%8th select an option from the console and your are ready to go, results appear on the webpage
%
%NOTE: To choose another route simply press CTRL+C on the "mission{\_}planning{\_}client" console and launch it again %(rosrun mission{\_}planning mission{\_}planning{\_}client)

%****
%1st catkin_make on server
%
%sudo apt-get install gpsd  
%sudo apt-get install gpsd-clients
%sudo apt-get install ros-indigo-gps-umd  //Ir a pasta sensor/novatel{\_}span/other_libs/gps{\_}test  e alterar a cmakelist para nao procurar este pacote gpsd{\_}client
%
%entrar na pasta gps{\_}test e criar a pasta "include" e dentro dessa pasta criar a pasta "gps" !

%************************************************************************************
%\hfill \\
%\textbf{How to install the novatel package}
\section{How to install the novatel package}
This is only necessary if, for any reason all data stored in the LAR server is lost. Or another operative system that is not based on linux is chosen.

To install this package follow the instructions on GAVLab official Git webpage \url{https://github.com/GAVLab/novatel} (visited on 09/09/2015)

%***************************************************************************************

\chapter{Connection Diagram}\label{App:AppendixB}
\begin{figure}
\centering
\includegraphics[width=0.9\textwidth]{connection_diagram.eps}
%\caption{GPS Antenna and Support}
\label{fig: connectiondiagram}
\end{figure}

\chapter{Algorithm to decode Google Polyline}\label{App:AppendixC}
%\lstset { %
%    language=C++
%    %backgroundcolor=\color{black!5}, % set backgroundcolor
%  %  basicstyle=\footnotesize,% basic font setting
%} % Set your language (you can change the language for each code-block optionally)
%
%\begin{listing}
%for (int i=0; i<iterations;i++)
%{
%do something
%}
%\end{listing}
% Set your language (you can change the language for each code-block optionally)
%\lstset{language=C++}  
\lstdefinestyle{customc}{
  belowcaptionskip=1\baselineskip,
  breaklines=true,
  frame=L,
  xleftmargin=\parindent,
  language=C,
  showstringspaces=false,
  basicstyle=\footnotesize\ttfamily,
  keywordstyle=\bfseries\color{green!40!black},
  commentstyle=\itshape\color{purple!40!black},
  identifierstyle=\color{blue},
  stringstyle=\color{orange},
}
\lstset{escapechar=@,style=customc}
% Start your code-block
\begin{lstlisting}
vector<double>Decompress(string encodedPoints, int precision){

    int len = encodedPoints.length();
    int index =0;
    double lat =0;
    double lng = 0;
    vector<double> array;

    while (index < len) {

        int b;
        int shift =0;
        int result =0;
        do {
            b = (int)encodedPoints[index++] - 63; //gets ascii value of the char
            //result |= (b & 0x1f) << shift;
            result |= (b & 31) << shift;
            shift +=5;
        } while (b>=32);
        int dlat= ((result & 1) ? ~(result >> 1) : (result >> 1));
        lat+=dlat;
        shift =0;
        result =0;
        do {
            b = (int)encodedPoints[index++]- 63; //gets ascii value of the char
            result |= (b & 31) << shift;
            shift +=5;
        } while (b>=32);
        int dlng = ((result & 1) ? ~(result >> 1) : (result >> 1));
        lng+=dlng;

        array.push_back(lat*pow(10,-precision));
        array.push_back(lng*pow(10,-precision));
    }
    return array;
}
\end{lstlisting} 

\end{document}
