%---------------------------------------------------------------------
%
%                          Capítulo 7
%
%---------------------------------------------------------------------

\chapter{Implementación}

\begin{FraseCelebre}
\begin{Frase}
Nunca te fíes de un ordenador que no puedas tirar por la ventana.
\end{Frase}
\begin{Fuente}
Steve Wozniak.
\end{Fuente}
\end{FraseCelebre}



%-------------------------------------------------------------------
\section{Introducción}
\label{cap7:sec:introduccion}
%-------------------------------------------------------------------
Cuando se habla de un programa computacional es sabido que contiene archivos y funciones para poder funcionar correctamente, es por esto que a continuación se dará a conocer los distintos archivos involucrados y funciones creadas para llevar a cabo con éxito la simulación del caso estudiado con anterioridad. Los tres archivos de coódigo principales que están guardados en el \verb|Package Try_1| son:

\subsection{Main}
Para el caso de este trabajo se utilzó el nombre \verb|Try_1.cpp| como nombre de archivo principal. Este contiene todas las funciones a utilizar por cada robot para su correcto funcionamiento.
\subsection{Listener}
Archivo que contiene las instrucciones para inicializar el protocolo de comunicación \verb|Publicador/Subscriptor| que utiliza ROS y así poder hacer que el robot \verb|"escuche"| instrucciones desde el usuario y \verb|"hable"| su estado para ser observado por el usuario.
\subsection{Comandos}
Archivo encargado de procesar los mensajes realizados por el usuario o robot y así poder paramentrizar, mover y conocer al robot que se desee.


%-------------------------------------------------------------------
\section{Estado de los Robots}
\label{cap7:sec:estado-robots}
%-------------------------------------------------------------------
Para tener un conocimiento de cada robot involucrado en la simulación se crearon funciones que mostrasen el estado de todas las características relevantes del robot en particular, a través de un tópico con la sintaxis \verb|nombreRobot_m| al cual se le debe realizar un echo\footnote{comando para la impresión de un texto en pantalla. Es utilizado en las terminales de los sistemas operativos como Unix, GNU/Linux, o MS-DOS; dentro de pequeños programas llamados scripts; y en ciertos lenguajes de programación tales como PHP.} para \verb|"escuchar"| el estado del robot en cuestión.
\begin{verbatim}
Ejemplo para REA:
rostopic list						comando ver topicos iniciados 
rostopic echo -c /REA_m				comando escuchar caracteristicas REA
\end{verbatim}
Las funciones que se utilizan para el funcionamiento de lo antes expuesto son:
\subsection{Load}
Esta función se encarga de cargar e inicializar los robots del archivo xml \verb|world|, además deja un aviso en la consola del robot iniciado correctamente y su respectivo nombre y también un registro en las variables globales. Esto se genera en el mismo orden del URDF del world.
\subsection{OnUpdate}
Función que es inicializada en Load y llamada cuando se inicia la simulación y utiliza el tiempo de simulacioón para actualizarse. Además va actualizando el \verb|estado| del robot para poder ser visualizado en una terminal y realiza la llamada a la función IARobot. El \verb|estado| incluye los siguientes datos que pueden ser observados en pantalla:
\begin{itemize}
\item Id del robot.
\item Nombre del robot
\item Ángulo de las uniones.
\item Velocidad de las uniones.
\item Posición en x del robot
\item Posición en y del robot
\item Modulo de la posicion con respecto al \REB
\item Rssi relativo al \REB
\item Accion realizada por el robot
\end{itemize}

\subsection{buscar}
Función encargada de encontrar todas las uniones del robots.
\subsection{pintar}
Función que muestra por terminal el listado de las uniones del robot.
\subsection{cargarUniones}
Función encargada de establecer las uniones del robot con valores por defecto.


%-------------------------------------------------------------------
\section{Cantidad de Robots Router Autónomos}
\label{cap7:sec:cantidad-RRA}
%-------------------------------------------------------------------
El programa creado se inicia desde un archivo *.launcher que está asociado a un *.world el cual es un archivo XML con etiquetas bien definidas por la comunidad de URDF, es decir, que un archivo antes modificado es leído para realizar la simulación. Es por esto que, en este apartado, se dará a conocer la manera de adicionar \RRA, las variables globales a utilizar, el codigo de \IA que utiliza cada robot intrínsecamente y la manera de auto-gestionar un mensaje enviado desde la estructura de ROS, la cual es entre subscriptor y publicador.
\newpage
\subsection{Adicionar RRA}
\begin{figure}[h]
	\centering
		\includegraphics[width=1\textwidth]{Imagenes/Vectorial/estructura_world}
	\caption{Estructura de un archivo *.world.}
\label{fig:world}
\end{figure}

\figura{Vectorial/estructura_robot}{width=1\textwidth}{fig:estrobot}%
	{Estructura de model de un robot.}%
	
Para poder adicionar un \RRA se debe conocer la estructura general de un *.world la cual esta representada por la Figura~\ref{fig:world}. En esta estructura el código URDF de los robots va uno a continuación del otro, y para tener un orden en el programa se deberá incluir primero el Robot Estación Base , seguido de Robot Explorador Autónomo y finalmente los Robot Router Autónomo, dando un número ascendente al final del nombre RRA, comenzando desde el 1. Luego se debe incluir el sistema operativo o plugins a utilizar, que para efectos del código entregado en este trabajo, lleva por nombre \verb|name = "actuador"| y el \verb|filename ="libTry_1.so"|, que se encuentra en el directorio \verb|devel/lib| y se coloca al final de cada URDF agregado como se muestra en la Figura~\ref{fig:estrobot}. 

\subsection{Variables Globales Auxiliares}
Debido a que el sistema operativo creado para los robots es el mismo en cada uno de ellos(\verb|libTry_1.so|), se deben utilizar variables auxiliares debidamente indexadas y que se puedan guardar en cada momento para su uso en las funciones creadas.
\begin{itemize}
\item \verb|int conexiones| : constante utilizada para llevar el conteo de cuantos robots se inicializan.
\item \verb|vector<int> id_robot| : vector que contiene el id del modelo del robot iniciado.
\item \verb|vector<std::string> accion_robot| :Variable auxiliar para evitar un movimiento de retroceso consecutivo en la desición de acción mediante la \IA implementada.
\item \verb|vector<std::string> robots_names| : almacena en orden el nombre de los robots iniciados REB->REA->RRA1->...->RRAx.
\item \verb|vector<double> modulo_robots| : vector que almacena el módulo de la posicion (x, y) de cada robot de la cadena y es calculado con la siguiente formula:$$ modulo_{robot[i]} = \sqrt{x^2+y^2} $$
\item \verb|vector<std::string> tipo_robot| : vector que contiene el tipo de robot a analizar para seleccionar la carpeta de instrucciones correspondientes a dicho robot:
\begin{itemize}
\item REB
\item REA
\item RRA
\end{itemize}
\item \verb|vector<double> vec_x| : vector que almacena el valor de la posición en x de cada robot.
\item \verb|vector<double> vec_y| : vector que contiene el valor de la posición en y de cada robot.
\item \verb|vector<double> vec_dif| : vector que almacena la diferencia de RSSI entre el robot superior(sucesor) y robot inferior (antecesor) según la fórmula:$$ vec_{dif_i} = RSSI_{sup} - RSSI_{inf} $$
\item \verb|vector<double> vec_sup| : vector que contiene la RSSi actual del robot superior(sucesor) de cada robot.
\item \verb|vector<double> vec_inf| : vector que contiene la RSSi actual del robot inferior(antecesor) de cada robot.
\item \verb|vector<int> vec_Minvc| : vector que almacena el valor del indicador $Min_{vc}$ de cada robot.
\item \verb|vector<int> vec_Maxvl| : vector que almacena el valor del indicador $Max_{vl}$ de cada robot.
\item \verb|vector<int> vec_MinDif| : vector que almacena el valor del indicador $Min_{Difa}$ de cada robot.
\item \verb|vector<int> vec_SS| : vector que almacena el valor del indicador $SS_{(k)}$ de cada robot.
\item \verb|vector<int> vec_Acc| : vector que almacena el valor del indicador $Acc$ de cada robot.
\item \verb|vector<int> vec_Accr| : vector que almacena el valor del indicador $Acc_{r}$ de cada robot.
\item \verb|vector<double> R| : vector que almacena el valor dela Recompensa de cada robot.
\item \verb|double Robot_sucesor| : varible que almacena el valor del módulo del robot sucesor de cada uno de los robots y se reutiliza al ser global. Es calculado con la siguiente fórmula:
$$ Robot_{sucesor} = modulo_{robots[i-1]} - modulo_{robots[i]} $$
\item \verb|double Robot_antecesor| : varible que almacena el valor del módulo del robot antecesor de cada uno de los robots y se reutiliza al ser global. Es calculado con la siguiente fórmula:
$$ Robot_{antecesor} = modulo_{robots[i]} - modulo_{robots[i+1]}$$
\item \verb|double Q [10][2][10]| : matriz para almacenar los valores de Q de cada robot.
\item \verb|int AccRelativa [8][8]| : matriz que contiene los valores para tomar una acción relativa entre Orientación del robot y el valor de la Acción correspondiente a la matriz Q.
\item \verb|vector<int> contador| : variable utilizada para realizar el envío de una instrucción y va aumentado cada vez que se cumple un ciclo de simulación del programa.
\end{itemize}
\subsection{IARobot}
Función encargada de:
\begin{itemize}
\item Actualizar los vectores globales con los datos correspondientes de cada robot.
\item Utilizar las funciones de calcularRSSI, Accion y enviarRos cuando se necesite según la lógica planteada a través de funciones \verb|if| y \verb|for|.
\end{itemize} 
\subsection{enviarRos}
Con esta función se puede publicar una orden el mismo robot con el formato de comunicación de ROS, recibiendo un \verb|publicador| y un \verb|mensaje|.

%-------------------------------------------------------------------
\section{Inteligencia Artificial}
\label{cap7:sec:IA}
%-------------------------------------------------------------------
La inteligencia de los robots simulados depende del tipo de robot pero, para simplificar el código y los archivos involucrados, se utiliza un mismo código que reacciona distinto en cada tipo de robot. Sin embargo, existen funciones comunes para poder llevar a cabo una decisión que, en este, caso es la de calcularRSSI.
\subsection{calcularRSSI}
Toda \IA necesita un parámetro de entrada, como estimulación, para tomar una decisión, en este caso es el valor RSSI. Para simplificar la creación del programa de simulación el cálculo de RSSI se hace utilizando el módulo de la ubicación de los robots y luego se resta un perdida asociada a la distancia entre el receptor y emisor de la señal de comunicación. Esto se ve reflejado en la ecuación:$$ RSSI_{relativo} = Potencia_{transmisi\'on} - Perdida_{Gaussiana}  $$ y la perdida Gaussiana se calcula con la siguiente ecuación~\footnote{https://www.gaussianwaves.com/2013/09/log-distance-path-loss-or-log-normal-shadowing-model/}:$$10* gamma * log_{10}(m\'odulo_{robot}/1_{punto referencia}) + e^{-1*rand()^2}(Ruido Gaussiano)$$ 

\subsection{Accion}

Esta función se encarga de escojer el archivo Ax que se debe realizar el \RRA según la \IA seleccionada y que para este caso es Q-Learning. Para esto se siguieron los estudios de Palma que tiene las siguientes variables:
\begin{itemize}
\item $Dif_{n}$ :Diferencia de RSSI entre el robot sucesor y el robot antecesor.
\item $Dif_{a}$ :Módulo de $Dif_{n}$ calculado de la ecuación:$\sqrt{(RSSI_{sup}^2+RSSI_{inf}^2)}$
\item $Dif_{d}$ :Diferencia digital RSSI donde +1 significa más cercano al robot superior y -1 nás cercano al robot inferior
\item $Max_{vl}$ :Maximizar vínvulo lejano.
$$MAX_{vl}=1 ; SI (RSSI_{lejano}(k) - RSSI_{lejano}(k-1)  < 0)$$
$$MAX_{vl}=0 ; SI (RSSI_{lejano}(k) - RSSI_{lejano}(k-1)  > 0)$$

\item $Min_{vc}$ :Minimizar vínculo cercano.
$$MIN_{vc}=1 ; SI (RSSI_{cercano}(k) - RSSI_{cercano}(k-1)  < 0)$$
$$MIN_{vc}=0 ; SI (RSSI_{cercano}(k) - RSSI_{cercano}(k-1)  > 0)$$
\newpage
Algoritmo de obtención de Maxvl y Minvc:
\begin{lstlisting}[language=Python]
if(RSSI_sup > RSSI_inf)
	if(vec_sup[i]-vec_sup_a[i]<0)
		Minvc = 1
	else
		Minvc = 0
		
	if(vec_inf[i]-vec_inf_a[i]>0)
		Maxvl=1;
	else
		Maxvl=0
else
	if(vec_inf[i]-vec_inf_a[i]<0)
		Minvc = 1
	else
		Minvc = 0

	if(vec_sup[i]-vec_sup_a[i]>0)
		Maxvl=1
	else
		Maxvl=0
		

\end{lstlisting}

\item $Min_{Dif}$ Minimizar diferencia de RSSI entre el robot sucesor y el robot antecesor.

$$MIN_{Dif}=1 ; SI (Dif_a(k) - Dif_a(k-1)  < 0)$$
$$MIN_{Dif}=0 ; SI (Dif_a(k) - Dif_a(k-1)  > 0)$$

\item $R_{(Min_{Dif},Max_{vl},Min_{vc})}$ : Recompensa con valor 1 para estado SM1 y con valor -1 para los estados SM2 al SM8. según la tabla~\ref{cap7:tab:cumplimiento-objetivos}
\begin{table}[h]
\caption{Cumplimiento de objetivos
   \label{cap7:tab:cumplimiento-objetivos}}
\
\footnotesize
\centering
\begin{tabular}{|l|c|c|c|}
\hline
 Estado & $MIN_{Dif}$ & $MIN_{vc}$ & $MAX_{vl}$ \\
\hline
SM1 & 1 & 1 & 1 \\
SM2 & 1 & 1 & 0 \\
SM3 & 1 & 0 & 1 \\
SM4 & 1 & 0 & 0 \\
SM5 & 0 & 1 & 1 \\
SM6 & 0 & 1 & 0 \\
SM7 & 0 & 0 & 1 \\
SM8 & 0 & 0 & 0 \\
\hline
\end{tabular}

\end{table}

\end{itemize}
\newpage
\subsection{getDeseado}
\begin{table}[h]
\caption{Accion deseada v/s Orientacion robot
   \label{cap7:tab:getdeseado}}
\
\footnotesize
\centering
\begin{tabular}{|c|c|c|c|c|c|c|c|c|}
\hline
 $Orientación_{Robot}$ & $A_{1}$ & $A_{2}$ & $A_{3}$ & $A_{4}$ & $A_{5}$ & $A_{6}$ & $A_{7}$ & $A_{8}$ \\
\hline
$A_{1}$ & $A_{1}$ & $A_{2}$ & $A_{3}$ & $A_{4}$ & $A_{5}$ & $A_{6}$ & $A_{7}$ & $A_{8}$ \\
$A_{2}$ & $A_{8}$ & $A_{1}$ & $A_{2}$ & $A_{3}$ & $A_{4}$ & $A_{5}$ & $A_{6}$ & $A_{7}$ \\
$A_{3}$ & $A_{7}$ & $A_{8}$ & $A_{1}$ & $A_{2}$ & $A_{3}$ & $A_{4}$ & $A_{5}$ & $A_{6}$ \\
$A_{4}$ & $A_{6}$ & $A_{7}$ & $A_{8}$ & $A_{1}$ & $A_{2}$ & $A_{3}$ & $A_{4}$ & $A_{5}$ \\
$A_{5}$ & $A_{5}$ & $A_{6}$ & $A_{7}$ & $A_{8}$ & $A_{1}$ & $A_{2}$ & $A_{3}$ & $A_{4}$ \\
$A_{6}$ & $A_{4}$ & $A_{5}$ & $A_{6}$ & $A_{7}$ & $A_{8}$ & $A_{1}$ & $A_{2}$ & $A_{3}$ \\
$A_{7}$ & $A_{3}$ & $A_{4}$ & $A_{5}$ & $A_{6}$ & $A_{7}$ & $A_{8}$ & $A_{1}$ & $A_{2}$ \\
$A_{8}$ & $A_{2}$ & $A_{3}$ & $A_{4}$ & $A_{5}$ & $A_{6}$ & $A_{7}$ & $A_{8}$ & $A_{1}$ \\
\hline
\end{tabular}
\
\end{table}
Debido a que en el algoritmo original analizado se utilizó un Robot Omnidireccional para los \RRA y en la simulación creada en este trabajo se utiliza un Robot No Omnidireccional, se debe utilizar esta función que identifica la orientación del robot y realiza un movimiento acorde a la selección de la \IA , es decir, se tiene una acción deseada y se busca obtener la accion del robot según su orientación que sea equivalente a la acción deseada según la Tabla~\ref{cap7:tab:getdeseado}
%-------------------------------------------------------------------
\section{Trayectoria del Robot Explorador Autónomo}
\label{cap7:sec:trayectoria-REA}
%-------------------------------------------------------------------
Para poder manejar las distintas partes de los robots involucrados en la simulación se definieron algunas funciones que ayudan a controlar cada parámetro, así como velocidad, ángulo, PID\footnote{valores de controlador proporcional, integral y derivativo de un motor de corriente continua} entre otros. Ademas se estandarizó la carpeta de las intrucciones a utilizar y los movimientos de los tipos de robots utilizados. Las funciones que nos ayudan a realizar esta tarea se describirán a continuación.
\subsection{procesar}
Esta función que se encuentra en el archivo \verb|Comandos.cpp| está encargada de recibir el mensaje enviado por el usuario o desde el mismo robot a través del protocolo de comunicación de ROS(mediante la publicación a un tópico), utilizando la función \verb|Switch/Case| obteniendo como parametros:
\begin{itemize}
\item d : se utiliza para mostrar en el \verb|"estado"| del robot las partes del robot.
\item p : se utiliza para cambiar los parámetros de velocidad de las partes del robot.
\item m : se utiliza para mover una parte del robot a un ángulo específico.
\item s : sirve para provocar un tiempo de espera de las instrucciones del robot, medido en milisegundos y así conseguir crear una trayectoria al robot.
\item e : se utiliza para abrir un archivo con instrucciones definidas por el usuario y tiene por defecto la carpeta \verb|/Robot/src/Try_1/instrucciones/|, por lo que sólo necesita el tipo de robot y el nombre del archivo como se ejemplifica a continuación:
\begin{verbatim}
En general:
tipoRobot/archivo

Ejemplo
RRA/A0
\end{verbatim}
\end{itemize}
\subsection{mover}
Esta función fue creada para mover un Joint a un ángulo específico medido en radianes utilizando la siguiente ecuación:$$Error = |Angulo_{actual} - Angulo_{deseado}| $$ con el Error calculado se actualiza el PID del Joint el cual tiene por defecto las siguientes constantes:
\begin{itemize}
\item Acción proporcional : p = 2
\item Acción Integral : i = 1 
\item Acción derivativa : d = 1 
\item Limite superior Integral : imax = 0
\item Limite inferior Integral : imin = 0 
\item Valor maximo de salida : cmdMax = Velocidad del URDF 
\item Valor minimo de salida : cmdMin = -1 * Velocidad del URDF 
\end{itemize}

\subsection{parametrizar}%https://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/classgazebo_1_1common_1_1PID.html
Es la función encargada de establecer la Velocidad de una union del robot medida en \verb|[metros/segundos]| y cambiar la velocidad máxima del PID del robot. La sintaxis es:
\begin{verbatim}
Para setear velocidad en general:
p nombre_exacto_union_robot V velocidad_deseada tiempoAccion
Ejemplo:
p create::left_wheel V 0 1

Para modificar la Velocidad Maxima En general:
p nombre_exacto_union_robot VM velocidadMaxima_deseada 0
Ejemplo:
p create::left_wheel VM 2 0
\end{verbatim}
\subsection{Trayectoria zigzag}
Gracias a las funciones creadas y mencionadas anteriormente, se utilizó un archivo llamado \verb|zigzag| con las instrucciones de la Tabla~\ref{cap7:tab:zigzag} para que el \REA  tenga una trayectoria en forma de zigzag y así poder analizar la \IA utilizada por los \RRAs.
\begin{table}[h]
\caption{Contenido Archivo zigzag
   \label{cap7:tab:zigzag}}
\
\footnotesize
\centering
\begin{tabular}{|c|l|l|}
\hline
 Par\'ametro & Instrucci\'on & Explicaci\'on \\
\hline
e & REA/A3 & movimiento A3\\
s & 9000 & espera 9 segundos\\
e & REA/A1 & movimiento A1\\
s & 40000 & espera 40 segundos\\
e & REA/A7 & movimiento A7\\
s & 18000 & espera 18 segundos\\
e & REA/A1 & movimiento A1\\
s & 40000 & espera 40 segundos\\
e & REA/A3 & movimiento A3\\
s & 18000 & espera 18 segundos\\
e & REA/A1 & movimiento A1\\
s & 40000 & espera 40 segundos\\
e & REA/A7 & movimiento A7\\
s & 18000 & espera 18 segundos\\
e & REA/A1 & movimiento A1\\
s & 40000 & espera 40 segundos\\
e & REA/A0 & movimiento A0\\

\hline
\end{tabular}
\end{table}

\subsection{Trayectoria L Derecha}

Gracias a las funciones creadas y mencionadas anteriormente, se utilizó un archivo llamado \verb|Lder| con las instrucciones de la Tabla~\ref{cap7:tab:Lder} para que el \REA  tenga una trayectoria en forma de diagonal derecha y así poder analizar la \IA utilizada por los \RRAs.
\begin{table}[h]
\caption{Contenido Archivo diader
   \label{cap7:tab:Lder}}
\
\footnotesize
\centering
\begin{tabular}{|c|l|l|}
\hline
 Par\'ametro & Instrucci\'on & Explicaci\'on \\
\hline
e & REA/A1 & avanzar\\
s & 120000 & espera 120 segundos\\
e & REA/A3 & girar 90º\\
s & 51200 & espera 51.2 segundos\\
e & REA/A1 & avanzar\\
s & 120000 & espera 120 segundos\\
e & REA/A0 & detener\\
\hline
\end{tabular}

\end{table}

%-------------------------------------------------------------------
\section{Iniciar Simulación}
\label{cap7:sec:iniciar-simulacion}
%-------------------------------------------------------------------
Para poder comenzar a utilizar el programa creado se debe configurar las referencias de ROS desde la carpeta correspondiente al codigo a utilizar. En este caso y para ejemplificar se utilizó la carpeta denominada \verb|"Robot"|, la cual contiene las carpetas creadas por \verb|carkin_make|, \verb|"build"|, \verb|"src"| y \verb|"devel"|. La carpeta devel contiene un archivo de configuracion llamado \verb|"setup.sh"| el cual se debe iniciar de la siguiente forma:
\begin{verbatim}
cd Robot/
source devel/setup.sh
\end{verbatim}
Esto da paso a poder utilizar todas las funciones de ROS desde la consola para su funcionamiento y se debe realizar en cada una de las ventanas de consolas que se quiera utilizar.
Luego se debe iniciar la simulacion con el comando ros "roslaunch" seguido del package(\verb|Try_1|) creado que contiene el codigo fuente a utilizar y finalmente el archivo *.launcher que contiene el *.world a utilizar.
\begin{verbatim}
roslaunch Try_1 Try_3.launcher
\end{verbatim}

% Variable local para emacs, para  que encuentre el fichero maestro de
% compilación y funcionen mejor algunas teclas rápidas de AucTeX
%%%
%%% Local Variables:
%%% mode: latex
%%% TeX-master: "../Tesis.tex"
%%% End:
