P oyec o Fin de Ca e a
Ingenie ía de Telecomunicación
Fo ma o de Publicación de la Escuela Técnica
Supe io de Ingenie ía
Au o : F. Ja ie Payán Some
Tu o : Juan José Mu illo Fuen es
Dep. Teo ía de la Señal y Comunicaciones
Escuela Técnica Supe io de Ingenie ía
Uni e sidad de Se illa
Se illa, 2013
T abajo Fin de Más e
Ingenie ía Elec ónica, Robó ica y Au omá ica
E icien obo coope a ion using MTSP
Au o : Jesús Caballe o Rod íguez
Tu o es: Begoña C. A ue Ullés y Aníbal Olle o Ba u one
Dp o. Teo ía de Sis emas y Au omá ica
Escuela Técnica Supe io de Ingenie ía
Uni e sidad de Se illa
Se illa, 2019
T abajo Fin de Más e
Ingenie ía Elec ónica, Robó ica y Au omá ica
E icien obo coope a ion using MTSP
Au o :
Jesús Caballe o Rod íguez
Tu o es:
Begoña C. A ue Ullés y Aníbal Olle o Ba u one
P o eso Ti ula
Dp o. Teo ía de Sis emas y Au omá ica
Escuela Técnica Supe io de Ingenie ía
Uni e sidad de Se illa
Se illa, 2019
T abajo Fin de Más e : E icien obo coope a ion using MTSP
Au o : Jesús Caballe o Rod íguez
Tu o es: Begoña C. A ue Ullés y Aníbal Olle o Ba u one
El ibunal nomb ado pa a juzga el abajo a iba indicado, compues o po los siguien es p o eso es:
P esiden e:
Vocal/es:
Sec e a io:
acue dan o o ga le la cali icación de:
El Sec e a io del T ibunal
Fecha:

Resumen
El
obje i o p incipal de es e abajo consis e en implemen a una solución del denominado P oblema del
Viaje o (TSP) en el eco ido de un UAV simulado en el en o no de ROS (Robo ic Ope a ing Sys em).
En dicho p oblema, concebi emos al UAV que eco e una se ie de waypoin s de su eco ido como el iaje o
que se plan ea eco e una se ie de ciudades, op imizando la dis ancia eco ida en el iaje.
Con ayuda de dicha analogía, supond emos que el iaje o pa e de una ciudad o igen y p e ende llega
a una ciudad des ino pasando an solo una ez po las ciudades in e medias del eco ido. Pos e io men e,
abo da emos la a ian e del MTSP (Mul iple T a eling Salesman P oblem) en la que se conside an a ios
iaje os que deben llega a sus espec i os des inos, pa iendo del mismo o igen y epa iéndose en e ellos
las ciudades in e medias del eco ido con el in de ob ene una solución acep able conjun amen e.
III
1.3 Me odología 3
La solución del P oblema del Viaje o basada en nues a p opues a, que se explica á a con inuación, puede
pe mi i acili a una supues a misión de inspección de UAVs, en la que sea necesa io aba ca un e i o io de
ex ensión conside able dadas coo denadas de pun os del eco ido. Po ejemplo, en un en o no de placas
sola es se ía in e esan e usa es a écnica pa a decidi el o den de inspección y a amien o de de ec os.
En el apa ado co espondien e, una ez que se haya expues o y esuel o el p oblema del iaje o, se
explica á de qué mane a se incluye la lib e ía del MTSP desa ollada en es e ejemplo de UAV.
Me odología
Se ha lle ado a cabo la p og amación necesa ia pa a esol e el p oblema, an o del TSP como del MTSP,
empleando el lenguaje de p og amación C.
Se han empleado nue as he amien as de p og amación, con espec o a la solución p opues a en MATLAB
del algo i mo, que simpli ican su esolución. Además de es a simplicidad, o a en aja asociada al empleo de
es as nue as he amien as consis e en la sencillez con la que se puede implemen a el código en la lib e ía de
ROS.
Se explica á pos e io men e en qué consis en dichas écnicas de p og amación y en qué medida mejo a el
algo i mo con espec o al caso sin e izado an e io men e en MATLAB.
Es uc u a de la memo ia
En los p óximos apa ados, se a a á de explica con de enimien o en qué consis e cada uno de los dos
p oblemas de op imización p esen ados an e io men e, el TSP y el MTSP.
Comenzando po el TSP, se explica á en qué consis e, de qué mane a se puede plan ea el p oblema de la
mane a más simple y, una ez acla ado el plan eamien o gene al del p oblema, se eco da á a g andes asgos
cómo se esol ió en p oblema empleando el lenguaje de p og amación MATLAB.
Una ez hecho es o, se p opond á la nue a solución empleada en el en o no de ROS, empleando el lenguaje
de p og amación C.
Pos e io men e indaga emos en el MTSP: in oduci emos b e emen e el p oblema, acla a emos a iables
in oducidas en el código del TSP elacionadas con es e nue o p oblema, y p esen a emos las a ian es del
MTSP que se han conside ado. Habla emos, en p ime luga , de la solución usada con Ma lab pa a compa a la
pos e io men e con la solución hecha en el en o no de ROS.
Una ez se hayan explicado cada una de las a ian es, qué se quie e consegui y cómo que emos log a lo,
p esen a emos las
clases usadas pa a esol e an o TSP como MTSP
en ROS con el empleo del lenguaje
C.
T as es o, se explica á de qué mane a se implemen a la solucion de es e p oblema de op imización en el
uelo de un UAV
(Vehículo Aé eo no T ipulado) en una simulación en Gazebo, que o ma pa e del en o no
de ROS.
Pa a conclui , se habla á de
posibles mejo as
en el código y en la me odología empleada pa a mejo a
las soluciones p opues as del p oblema. También se compa a á con o os mé odos que pueden esol e el
p oblema, algo que se i á de pun o de pa ida pa a discu i an o las en ajas como los incon enien es del
mé odo p opues o.

Desc ipción del sis ema y algo i mia: TSP
In oducción
A pesa de que la idea de eco e di e sas ciudades has a llega a un des ino puede pa ece bas an e in ui i a
pa a el se humano, cuando es una máquina la enca gada de da el esul ado más óp imo den o de sus
posibilidades no es algo i ial.
Si u iésemos, además de un solo des ino y un solo comienzo, an sólo dos ciudades in e medias po las
que pasa , no esul a complicado da la solución óp ima del eco ido mínimo; hay muy pocas posibilidades
de u a espe ando las p emisas necesa ias (pa i de un o igen, eco e las ciudades in e medias, y llega
a un des ino). Incluso con es ciudades in e medias puede esul a un p oblema ela i amen e sencillo a
esol e po el se humano, si se pe sigue el óp imo en nues o eco ido.
Sin emba go, ¿qué hace cuando se nos p esen a un núme o ela i amen e ele ado de ciudades in e medias?
¿Y si engo 9 o 10 ciudades in e medias? No es en absolu o discu ible que una pe sona p opo cione una
buena solución an e es e p oblema, aunque dado el eno me núme o de posibilidades que aho a exis en pa a
plani ica una u a, una máquina pod ía da una solución más op ima que la p opo cionada po la in uición
humana, haciendo uso de los algo i mos ap opiados.
5
6Capí ulo 2. Desc ipción del sis ema y algo i mia: TSP
Resumen del mé odo an e io (MATLAB)
En la implemen ación que se lle ó a cabo en MATLAB, conside ábamos un g a o que ilus aba los posibles
eco idos que el iaje o podía lle a a cabo desde la ciudad de o igen has a el des ino.
Los nodos que con o maban dicho g a o almacenaban in o mación bas an e ele an e en cuan o al eco ido.
P incipalmen e, p opo cionaban in o mación ace ca de la dis ancia o al eco ida has a la ciudad ac ual
del iaje, nos decía po qué ciudades habíamos iajado an e io men e (y en qué o den) y en la ciudad en la
que el iaje o se encon aba en ese momen o. Además, cada nodo del g a o almacenaba más in o mación
elacionada con las posibles u as que quedaban po explo a en el g a o pa a ob ene más soluciones del
p oblema de op imización.
Sin emba go, en cuan o a las ansiciones en e nodos, es as no gua daban ningún ipo de in o mación; an
sólo apa ecían en el g a o del iaje pa a ilus a la conexión en e a ios nodos o es ados del á bol.
Po ejemplo, una ep esen ación del g a o pa a es ciudades in e medias podía se la siguien e:
1
2
5
11
17
6
12
18
3
7
13
19
8
14
20
4
9
15
21
10
16
22
Figu a 2.1 G a o de es ado pa a es ciudades in e medias.
CO
C1
C2
C3
CF
C3
C2
CF
C2
C1
C3
CF
C3
C1
CF
C3
C1
C2
CF
C2
C1
CF
Figu a 2.2 G a o de es ado pa a es ciudades in e medias; ciudades.
En es a úl ima ep esen ación, se mues a el g a o de es ados con el nomb e de la ciudad asociada a cada
nodo.
Po ejemplo, el nodo pad e del á bol (CO) almacena oda la in o mación asociada a ese pun o del iaje; el
iaje o ha pasado po la ciudad o igen y puede i a las ciudades C1,C2, o C3.
Toda es a in o mación, como se comen ó, asociada a los nodos. De es a mene a, se ob iene una base de
da os conside able con es uc u as de nodos, en las que se gua da oda la in o mación.
2.3 Mé odo de implemen ación en ROS: empleo de clases 7
Po o a pa e, los hijos de un nodo pad e siemp e apa ecen o denados de meno a mayo dis ancia en el
iaje en e ciudades, de mane a que el iaje CO-C1 es el que almacena meno dis ancia.
Una posible solución del g a o se pod ía ep esen a de es a o a o ma:
1
2
5
11
17
6
12
18
3
7
13
19
8
14
20
4
9
15
21
10
16
22
Figu a 2.3 Posible solución del g a o pa a 3 ciudades in e medias.
En conc e o, la úl ima solución mos ada en imagen ep esen a el eco ido del iaje o pasando a la ciudad
más ce cana desde la que de encuen a en ese momen o.
Es o no implica que la mejo solución sea la ama
si uada más a la izquie da del g a o, pe o sí que suela p opo ciona una buena solución.
A con inuación, a pa i del p óximo apa ado, se explica á cómo se ha e o mulado el p oblema pa a
ob ene soluciones con una complejidad a ni el de p og amación mucho meno .
Incluso, se ba aja la posibilidad de se i se del pa alelismo en e p ocesos que b indan los hilos o subp o-
cesos, pa a pode ob ene más soluciones de es e p oblema de p og amación en menos iempo.
Mé odo de implemen ación en ROS: empleo de clases
Pa a ees uc u a el p oblema desde ce o, como se acaba de comen a en el apa ado an e io , es necesa io
adop a un en oque dis in o al que se ha lle ado a cabo has a aho a pa a esol e es e p oblema de op imización.
En conc e o, a pa i de aho a nos se i emos de la p og amación o ien ada a obje os pa a concebi dis in os
ipos de a iables con in o mación que se complemen e en e sí; odo es o pa a pode esol e es e p oblema
de una mane a mucho más li iana.
Po ejemplo, a pa i de aho a emplea emos obje os de la
clase Node
que no end án po qué almacena
oda la in o mación del eco ido que ha lle ado a cabo el iaje o has a ese momen o (como se hacía
an e io men e).
En luga de ello, la
clase B anch
de ini á un ipo de obje o que se á capaz de ob ene in o mación ace ca
del eco ido comple o del iaje o, si iéndose a su ez de la clase Node.
También emplea emos una
clase
llamada
Pa h
que almacena á la dis ancia en e ciudades del eco ido,
de mane a que los nodos no almacenen más in o mación de la necesa ia (que es algo que ocu ía en la
implemen ación con Ma lab).
En los p óximos apa ados, se explica á con de enimien o el come ido de cada una de las clases de inidas
y qué papel juga án cuando a emos de abo da el p oblema comple o.
Toda la implemen ación de clases se lle a á a cabo, además de pa a disminui la complejidad de p o-
g amación, pa a p escindi de una base de da os de un amaño conside able en la que se almacene oda la
in o mación, como se hacía en el caso explicado en el apa ado an e io con MATLAB.
In oducción a las clases de inidas
Un buen ejemplo pa a ilus a el ipo de obje os que se maneja á a ni el de p og amación lo podemos encon a
en la es uc u a que p esen a un á bol no mal y co ien e.
8Capí ulo 2. Desc ipción del sis ema y algo i mia: TSP
¿Po qué azón? Nues o obje i o p incipal es ob ene la solución que haga que la u a del iaje o sea la de
meno eco ido posible o, al menos, la de meno dis ancia en el iaje conside ando el es o de soluciones
ob enidas.
Si conside ásemos el á bol mencionado an e io men e, pod íamos imagina que la ciudad de inicio del
iaje se co esponde con el onco del á bol, mien as que el ex emo de odas y cada una de las amas del
á bol se co esponden con las soluciones al p oblema del iaje o. Pod íamos en ende las bi u caciones en
las amas del á bol como las ciudades in e medias, a pa i de las cuales podemos iaja a o as ciudades.
Figu a 2.4 Algo i mo en clases: símil con un á bol.
Si nos in e esase ob ene las mejo es soluciones, posiblemen e la mejo opción se ía busca la ami icación
más co a que pa a del onco del á bol y que llegue has a el ex emo inal de una ama.
Vemos que es amos ma e ializando el p oblema que a amos de esol e en un obje o del mundo eal, que
se encuen a en la na u aleza: un á bol.
Es e á bol es á o mado po lo que pod ían se obje os con ca ac e ís icas dis in as. Po una pa e, podemos
conside a las bi u caciones en e las amas, así como las amas que an uniendo bi u caciones en e sí.
Teniendo en cuen a las bi u caciones y las secciones de las amas en el á bol, pod ían de ini se a su ez
amas más la gas que pa an desde el onco y lleguen ce ca del ex emo inal, el cual se puede de ini como
ciudad de des ino.
Todas es as pa es del á bol (así como el p opio á bol) se pueden de ini como obje os en un con ex o de
p og amación. A con inuación, habla emos de las clases que de inen a dichos obje os y del ipo de in o mación
que ienen asociada.

2.3 Mé odo de implemen ación en ROS: empleo de clases 9
Ci y
En el á bol mencionado, podemos conside a las bi u caciones como pun os en los que cambia el eco ido
de una ama hacia o a dis in a, pa a ob ene soluciones dis in as a su ez. En es os pun os de in lexión es a á
la in o mación asociada a las ciudades po las que el iaje o i á pasando al lle a a cabo su eco ido.
Po ello, en p ime luga , podemos de ini la
clase Ci y
que con enga las coo denadas de las ciudades
conside adas en odo el eco ido del iaje o, así como un iden i icado uní oco que pe mi a dis ingui las del
es o de ciudades que se ienen en cuen a en el iaje.
De aho a en adelan e, ilus a emos median e diag amas UML (Uni ied Modeling Language Diag am
[
13
]) las clases que de ini án a los obje os que se emplea án en nues o p og ama. Haciendo alusión a dicho
diag ama, el de la clase Ci y end ía es a o ma:
Ci y
+ id : in
+ ci y : ec o < loa >
+ ill : oid
+ ge -s ing-da a : ec o <s ing>
Figu a 2.5 Diag ama UML: Clase Ci y.
En el diag ama mos ado, podemos ap ecia que hay es secciones di ididas ho izon almen e y que de inen
conjun amen e la clase Ci y, con la que se pod án de ini los obje os que se án las ciudades.
En las es di isiones, espec i amen e, apa ece in o mación ace ca de:
1. Nomb e de la clase. (Ci y)
2. A ibu os de la clase Ci y ( alo es almacenados).
3. Mé odos de la clase ( unciones que pe mi en ope a sob e los a ibu os de la clase).
En cuan o a la ma ca que apa ece an es de nomb e del a ibu o o mé odo, nos da á in o mación ace ca de
la isibilidad del a ibu o o mé odo al que es é haciendo e e encia ("+" signi ica público, mien as que "-"
signi ica p i ado).
A ni el de código, la clase Ci y queda ía de inida de es a mane a en lenguaje C, en el anexo [A.1].
Node
Aunque en el subapa ado an e io hayamos de inido las ciudades que o ma án pa e del eco ido del iaje o,
es a in o mación no es su icien e pa a de ini de mane a uní oca las bi u caciones en el á bol que p esen amos
an e io men e.
Dicho de o a mane a: necesi amos un iden i icado que dis inga una misma ciudad po la que se ha pasado
en u as di e en es del algo i mo. Pa a ello, conside a emos y de ini emos una nue a clase en la que se
almacena án an o la ciudad del eco ido como su iden i icado en en á bol. El iden i icado , a ni el de
p og amación, lo de ini emos como un en e o.
A es a nue a clase la llama emos
clase Node
. En el siguien e esquema, a amos de ilus a median e un
diag ama UML la de inición que acabamos de apo a :
Como conside a emos un g a o de es ados, igual que hicimos en el caso del algo i mo esuel o con
MATLAB, ambién decla a emos un a ibu o que almacene el ni el que ocupa el nodo den o del g a o. Es o
se ha á pa a acili a la implemen ación de la clase B anch (que p esen a emos más adelan e).
Con espec o a los mé odos de la clase, hemos de inido los siguien es:
10 Capí ulo 2. Desc ipción del sis ema y algo i mia: TSP
Node
+ label : in
+ le el : in
+ ci y : Ci y
+ Node:
+ ill: oid
+ con e -elemen : oid
Figu a 2.6 Diag ama UML: Clase Node.
1. Node: Cons uc o de la clase.
2. ill: Dados los a ibu os como a gumen os de en ada, de ine el nodo.
3. con e -elemen : Con ie e un elemen o a la clase Node. Más a de explica emos la clase Elemen .
A ni el de código, los mé odos de la
clase Node
se han de inido de la siguien e mane a en el anexo [A.2].
Pa h
Los obje os de inidos con la
clase Node
ep esen aban las bi u caciones en el á bol, y almacenaban las
coo denadas de las ciudades del eco ido del iaje o.
Aho a, de ini emos la unión en e dichas bi u caciones; lo que se án po ciones de la ama del á bol que
ep esen an los caminos que unen las bi u caciones (los nodos).
Pa a ello, da emos nomb e a una nue a clase; la
clase Pa h
. En p ime luga , mos a emos el diag ama UML
que ilus a esquemá icamen e an o los a ibu os como los mé odos de la clase. Pos e io men e, explica emos
b e emen e en qué consis e cada campo.
Pa h
+ o igin-id: in
+ des iny-id: in
+ dis ance: loa
+ Pa h:
+ ill: oid
Figu a 2.7 Diag ama UML: Clase Pa h.
•ATRIBUTOS:
1. o igin-id:
En e o que almacena á el id de o igen del obje o de inido. Pos e io men e, asocia emos
es e iden i icado al iden i icado del nodo del que pa e es e camino.
2. des iny-id:
En e o que almacena á el id de des ino del obje o de inido. Pos e io men e, asocia e-
mos es e iden i icado al iden i icado del nodo al que llega es e camino.
3. dis ance: Almacena la dis ancia eco ida en es e camino.
•MÉTODOS:
1. Pa h: Cons uc o de la clase.
2. ill: Dados los a ibu os como a gumen o de en ada, de ine el obje o de la clase Pa h.
A ni el de código, los mé odos de la
clase Pa h
se han de inido de la siguien e mane a, en el anexo [A.3].
2.3 Mé odo de implemen ación en ROS: empleo de clases 11
B anch
Necesi amos c ea un ipo de obje o nue o que con enga an o obje os ipo
Node
como obje os ipo
Pa h
, pa a
sabe po qué ciudades ha pasado el iaje o, el o den en que las ha eco ido y la dis ancia que ha acumulado
has a ese momen o.
Necesi a emos obje os de la clase
Node
pa a conoce las dos p ime as ca ac e ís icas de la ama, mien as
que la dis ancia o al de las amas la ob end emos acumulando las dis ancias de los muchos obje os de la
clase Pa h.
De es a mane a, se de ini á la clase
B anch
, que end á una asociación con la clase
Elemen s
, la cual
explica emos en el p óximo apa ado.
El diag ama UML de la clase B anch p esen a es a o ma:
B anch
+ elemen s : deque <Elemen >
+ las -elemen : in ec o < loa >
+ B anch :
+ ge -ci ies : ec o <Ci y>
+ ge - ou e : ec o <Node>
+ add-elemen : oid
+ ge - o al-dis ance : loa
+ ge - o al-dis ance-des iny-id : loa
+ sea ch-node : bool
+ sea ch-pa h : bool
Figu a 2.8 Diag ama UML: Clase B anch.
•ATRIBUTOS:
1. elemen s:
Se a a de una cola que almacena los elemen os que con o ma la ama. Reseña emos
que los elemen os pueden se o bien obje os de la clase Node, u obje os de la clase Pa h.
2. las -elemen : Puede oma es alo es "NODE", "PATH" o "-1", si no se encuen a de inido.
•MÉTODOS:
1. B anch:
Cons uc o de la clase: inicializa "las -elemen " a "-1". Es deci , c eamos una ama
que aún no iene elemen os.
2. ge -ci ies:
De uel e un ec o con las ciudades de la ama que es amos conside ando. Los
elemen os del ec o (las ciudades) es án o denados desde el inicio del eco ido del iaje o has a
la úl ima ciudad de la ama.
3. ge - ou e:
De uel e un ec o con los nodos del eco ido de la ama que es amos conside ando,
has a llega al nodo de la ama que se oma como a gumen o de en ada de es e mé odo.
4. add-elemen :
Inse a un elemen o al inal de la ama. Cuando indaguemos en el código de es e
mé odo, se explica án más de alles.
5. ge - o al-dis ance:
De uel e la dis ancia o al de la ama, acumulando las dis ancias de los
elemen os que son Pa h.
6. ge - o al-dis ance-des iny-id:
De uel e la dis ancia de la ama has a llega a un obje o
Pa h
con el des iny-id especi icado como a gumen o de en ada del mé odo.
7. sea ch-node:
Nos dice si, en la ama conside ada, se encuen a el nodo (obje o de la clase Node)
que se oma como a gumen o de en ada del mé odo.
8. sea ch-pa h:
Nos dice si, en la ama conside ada, se encuen a el camino (obje o de la clase
Pa h) que se oma como a gumen o de en ada del mé odo.
12 Capí ulo 2. Desc ipción del sis ema y algo i mia: TSP
El código de la cabece a de es a clase B anch iene el siguien e aspec o, en el anexo [A.4].
Hay que señala que el amaño de las dis in as amas que se de inen en el algo i mo no end án el mismo
amaño necesa iamen e. Es o signi ica que, en cie as ocasiones, se pueden lle a a cabo inse ciones de
dis in as amas en e ellas, con el in de con o ma una sola ama quu aba que el eco ido comple o del
iaje o, desde la ciudad de inicio has a la ciudad de des ino.
Desc ipción del sis ema y algo i mia: MTSP
In oducción
Una ez esuel o el p oblema del TSP, a a emos de complica lo un poco conside ando, en es a ocasión,
a ios iaje os que pa en de una misma ciudad de inicio, se epa en las ciudades in e medias de inidas en el
espacio de una mane a u o a (en unción de los casos que se conside en, explicados más adelan e) y, además,
a an de llega cada uno a su espec i a ciudad des ino.
A es a a ian e del TSP la denominamos MTSP (Mul iple T a eling Salesman P oblem).
Se a a á en p o undidad la mane a en la que hemos abo dado el p oblema con los concep os de clase
in oducidos en apa ados an e io es, pa a implemen a el algo i mo en ROS.
Debido a que la idea empleada es simila al caso del MTSP esuel o con MATLAB, se explica á la nue a
clase
M sp
pa a esol e es a a ian e, y de qué mane a maneja los obje os del ipo
WholeG aph
que
ep esen an los g a os de cada iaje o.
MTSP: a ian es conside adas
Se conside a án es a ian es pa a esol e el Mul iple T a ele Salesman P oblem, de inidas en unción del
ipo que end án las ciudades in e medias en el p oblema.
Las ciudades in e medias pod án se :
•Ciudades Res ingidas: aquellas en las que se sabe de an emano qué iaje o debe pasa po ella.
•Ciudades Auxilia es
: aquellas que no ienen un iaje o asignado de an emano pa a que pase po ellas.
Es es e ipo de ciudad la que ha á que el algo i mo del MTSP deba ba aja qué iaje o debe pasa po
qué ciudad in e media.
A con inuación se explican b e emen e cada uno de los casos conside ados.
Sólo ciudades es ingidas
Todas las ciudades in e medias del p oblema son es ingidas y, po an o, cada iaje o ya iene asignadas
las ciudades po las que debe pasa has a llega a su des ino. Resol e es e p oblema equi ale a esol e
n
p oblemas del iaje o simples (TSP); uno pa a cada iaje o del MTSP conside ado.
Sólo ciudades auxilia es
En es a a ian e, odas las ciudades deben epa iese en e los dis in os iaje os que de inen el MTSP.
En unción del núme o o al de iaje os (suponemos que es
n
), se ha á un epa o equi a i o del núme o de
ciudades auxilia es del p oblema (
aux
) en e los iaje os. En caso de que la di isión
aux
n
no enga es o, no
hab á incon enien e y cada iaje o end á el mismo núme o de ciudades auxilia es asignadas que cualquie
o o. Si, po el con a io, el es o es mayo a 0, epa i emos dicho alo de mane a equi a i a en e los
iaje os, sin mos a especial p e e encia en el epa o.
Una ez asignado el núme o de ciudades in e medias a cada iaje o, se esol e á el p oblema del TSP pa a
cada iaje o sucesi amen e.
19

20 Capí ulo 3. Desc ipción del sis ema y algo i mia: MTSP
¿De qué mane a? El p ime iaje o conside a á
aux1
ciudades auxilia es
de en e odas las ciudades
in e medias del MTSP
. El siguien e iaje o, conside a á
aux2
ciudades auxilia es solo que, a di e encia
del caso an e io ,
no conside a las ciudades in e medias ya asignadas a an e io es iaje os (p ime
iaje o, en es e caso)
. Se p ocede á de es a mane a, esol iendo el TSP pa a cada iaje o has a llega al
úl imo iaje o, que se en en e al p oblema del iaje o simple en el cual
su núme o de ciudades auxilia es
asignado coincide con el núme o de ciudades in e medias sob an es (las que no se asigna on a iaje os
an e io es en el epa o). Todo lo explicado se ap ecia mucho mejo en el código, en el cual se de ine una
lis a con el núme o o al de ciudades auxilia es de la que se an qui ando aquellas que se an asignando a
iaje os an e io es.
Una ez que se ob enga un esul ado con la dis ancia op imizada de cada iaje o, se obse a á cual es el
iaje o con mayo dis ancia eco ida, y se asigna á al iaje o con meno dis ancia en el eco ido esa ciudad
que e asó al o o iaje o.
Se ol e á a esol e el algo i mo con es a nue a asignación, e i emos p ocediendo de es a mane a
i e a i amen e has a alcanza un pun o en el cual no se p oduzcan más mejo as.
Ciudades es ingidas y auxilia es
En es e caso, se conside an an o ciudades es ingidas pa a cada iaje o como ciudades auxilia es a epa i .
El epa o de las ciudades auxilia es se lle a á a cabo de la mane a que se explicó en el apa ado an e io .
Mé odo de implemen ación en ROS: empleo de clases
Tal y como se hizo en el apa ado en el que se explicó el TSP simple, a con inuación se explica á de qué
mane a se ha implemen ado en el lenguaje C la solución de es e p oblema de op imización ecu iendo al
empleo de clases.
Tan solo conside amos una clase, a la que se ha llamado
M sp
. En el p óximo subapa ado, explica emos
su signi icado, sus a ibu os asociados y los mé odos que ealizan modi icaciones en los obje os de es a clase.
Todas las explicaciones se án in oduc o ias, ya que se indaga á con mayo p o oundidad en las secciones
del código en un apa ado pos e io de la memo ia.
M sp
En el anexo [A.8] se p esen a el código de la cabece a de la clase
M sp
con el a ibu o y los mé odos que
de inen la clase.
•ATRIBUTOS:
1. min-dis ance:
Vec o en el que se almacena án las dis ancias mínimas ob enidas pa a cada iaje o.
El o den de las componen es se co esponde á po el o den de los iaje os de inidos inicialmen e
en el código.
•MÉTODOS:
1. sol e-only- es:
Tomando las ciudades in e medias y las ciudades o igen y des ino del p oblema,
además de o os pa áme os que se explica án en el siguien e apa ado, se a a de esol e n
p oblemas TSP. La a iable
es-ids
almacena una se ie de ec o es, donde cada uno de ellos
almacena los ids de las ciudades po los que debe pasa un iaje o de e minado.
2. sol e-only-aux:
También oma los mismos a gumen os de en ada que el mé odo an e io , eempla-
zando
es-ids
po
aux-ids
. El nue o a gumen o del mé odo hace alusión a los ids de las ciudades a
epa i . Nó ese que es a a iable es un ec o con los ids, mien as que
es-ids
e a un ec o de
ec o es en e os, ya que la
componen e iésima
se co espondía con el
iaje o iésimo
a esol e .
En es e p opio mé odo se hace el epa o, po lo que no exis e una co espondencia inicial en e
iaje o e iden i icado es.
3. sol e-aux-and- es:
Se mezclan ambos mé odos an e io es pa a esol e el p oblema conjun o. Una
ez que se asignan ciudades auxilia es con los ids almacenados en
es-ids
, me ol ido de dicha
a iable y ealizo la asignación con
aux-ids
de la misma mane a en la que se p ocedió en el mé odo
M sp::sol e-only-aux.
3.3 Mé odo de implemen ación en ROS: empleo de clases 21
4. sol e- sp:
Se ecu e a es e mé odo den o del código de los es an e io es. De ine un ec o de
iaje os (obje os de la clase
WholeG aph
). A cada componen e de es e ec o (a cada iaje o) se
le asignan ciudades auxilia es y/o ciudades es ingidas pa a esol e el p oblema del iaje o con
ellas. Una ez hecho es o, se uelca en el ec o
e u n-ci ies-ids
los ids de las ciudades po las
que ha iajado cada iaje o pa a ob ene su mejo solución, conside ando inicialmen e odos los
iden i icado es de las ciudades auxilia es po las que se puede pasa (aux-ids-lis ).
Fiche o p incipal
Código
En el anexo [A.9] se incluye el código del iche o p incipal de ex ensión ".cpp", en el cual se ha incluido la
lib e ía del MTSP pa a esol e el p oblema del iaje o, con el p ocedimien o explicado has a aho a.
En él, se de alla de qué mane a de ha lle ado a cabo la decla ación de las ciudades que se conside an en el
p oblema del iaje o. Así mismo, se de alla en el código asociado al hilo
sp- h ead
la decla ación de los
ids asociados a las ciudades auxilia es y a las ciudades es ingidas, omados en base a odas las ciudades
decla adas p e iamen e. Pos e io men e, se esuel e el algo i mo con el mé odo ap opiado den o de la
clase
M sp.
Visualización RVIZ
En es e caso conside a emos un solo hilo, median e el cual se a a de esol e el p oblema del MTSP con
es iaje os y únicamen e ciudades es ingidas. Como se señaló an e io men e, equi ale a esol e es TSP
po sepa ado.
Se de ine un nue o mé odo en la
clase M sp
, con el cual se de uel en los ids de las ciudades po las que
pasa cada iaje o. A dicho mé odo lo hemos llamado e u n-ci -ids.
En el anexo [A.10] se de inen, a ni el de código, las ciudades conside adas en es a isualización en RVIZ.
Po o a pa e, en el anexo [A.11] se especi ican las líneas de código con las cuales se de uel en los ids
mencionados an e io men e po la clase M sp.
Ya que enemos de inidas las ciudades con sus coo denadas y sus espec i os iden i icado es al comienzo
de es e iche o p incipal, y que además conocemos los ids del esul ado del algo i mo ( olcados en la a iable
de - ack-poin s), podemos isualiza en RVIZ ácilmen e las ciudades po las que pasa cada iaje o.
Po un lado, el código que se enca ga de ep esen a en RVIZ el esul ado del algo i mo se ía el mos ado
en el anexo [A.12].
El esul ado, en es e caso, queda ía de la siguien e mane a:
23
24 Capí ulo 4. Fiche o p incipal
Figu a 4.1 Resul ado en RVIZ con es iaje os.
1,2.3 3,2
-7,2
7,-9
-2,9
5,5
5,-5
-9,5
0,1.1
Figu a 4.2 Coo denadas de las ciudades. Resul ados RVIZ.
Se ap ecia la ep esen ación de cada una de las u as del iaje o en un colo dis in o.
En la segunda imagen, se especi ican las coo denadas de cada ciudad y las u as de los es iaje os, en
colo es di e en es. El pun o g is se co esponde con la ciudad de o igen del iaje (común pa a odos los

4.2 Visualización RVIZ 25
iaje os), mien as que los pun os neg os ep esen an las ciudades de des ino de cada una de las u as.
Po simplicidad en la ep esen ación del esul ado, se ha supues o que abajamos en dos dimensiones,
asignando un alo nulo a la e ce a coo denada de las ciudades.
Resul ados de los algo i mos
A con inuación, se mos a án ejemplos que se han pues o en el código p incipal an o pa a TSP como pa a
MTSP.
En el caso del TSP, se mos a án dos ejemplos: uno en el que se esuel e el P oblema del Viaje o con un
solo hilo de ejecución, y o o caso en el que se esuel e el mismo p oblema con cua o hilos de ejecución.
Po o a pa e, abo da emos el caso del MTSP con un solo hilo de ejecución. Se mos a án los esul ados
de e minal pa a los es casos: MTSP esuel o solo con ciudades es ingidas, solo con ciudades auxilia es, o
con ambas ciudades.
Además, pa a odos los casos, se enseña á po e minal la salida que se ob iene con los esul ados más
signi ica i os.
TSP
En el apa ado an e io , en el que se mos aba el código p incipal, apa ecía una sección de código en la que
se de inían como ec o es las ciudades po las que pasa ía el iaje o (o a ios iaje os, en el caso de que nos
en en aśemos al MTSP).
A lo la go de odo el apa ado del TSP, conside a emos las mismas ciudades de o igen y de des ino,
así como las sie e ciudades in e medias que se de allan a con inuación en el código. De dichas ciudades
in e medias, an solo usa emos algunas de ellas pa a esol e el P oblema del Viaje o, asignando los ids de
ciudades es ingidas.
Todas las ciudades conside adas se de inen de la o ma especii acada en el anexo [A.13]:
A pesa de no comen a lo an e io men e, se de ine un hilo en el código en el cual hay un obje o de la clase
MTPS con el cual se esuel e el p oblema de op imización. Se especi ican, en e o os pa áme os, ciudades
es ingidas del p oblema, núme o de i e aciones del algo i mo, e c.
El código que de ine el hilo se encuen a en el anexo [A.14].
Señalemos que el mu ex que oma el mé odo
M sp::sol e-only- es
como a gumen o de en ada se emplea
pa a que la salida po pan alla de los esul ados de los dis in os hilos no se mezclen, y se puedan e po
sepa ado en el e minal a la salida.
Tsp con un solo hilo (G aph)
Veamos la salida po e minal en el caso de que, con los ids de las ciudades es ingidas especi icados
an e io men e, se esuel a el TSP simple.
En lo e e en e al código, las líneas especi icadas en el anexo [A.15] ienen asociadas la salida po e minala
an e io .
Comen emos los esul ados más ele an es:
•
El iempo de ejecución del algo i mo es de unos 12 milisegundos. Más adelan e, compa a emos con el
caso mul ihilo pa a ap ecia mejo la mejo a que se e á pos e io men e.
•
Ya que hemos conside ado cua o ciudades in e medias en el p oblema, se obse a a la salida po
e minal el núme o de posibles amas: 4! =24.
27
28 Capí ulo 5. Resul ados de los algo i mos
Figu a 5.1 TSP: un solo hilo.
•
Hemos especi icado, como núme o deseado de soluciones, 500. Es deci , que el algo i mo debe á se
capaz de ob ene 500 amas del g a o que se es á conside ando. Si hay menos soluciones, como ocu e
en es e caso, se ob end á el máximo núme o de soluciones posible (24 en es e caso).
•
Se mues an las ciudades en el o den en el que el iaje o las eco e. La p ime a ciudad que se mues a
es el o igen, mien as que la úl ima es el des ino. Se an mos ando a la de echa de cada ciudad la
dis ancia que se acumula con espec o la ciudad an e io , en unidades.
•
Después, se mues a la dis ancia asociada a la mejo solución encon ada, el id de la ama con dicha
solución, y po úl imo el núme o de ciudades in e medias conside adas en el p oblema.
Con el in de isualiza con mayo cla idad la solución, a con inuación se mues a el á bol de nodos que
ep esen a la solución que se conside a.
1
2
6
1
1
1
1
1
1
1
13
1
1
1
1
5
1
1
1
1
6
3
1
1
1
1
2
1
1
1
1
7
1
1
1
1
8
4
1
1
1
1
3
1
1
1
1
9
1
1
1
1
10
5
1
1
1
1
4
1
1
1
1
11
1
1
1
1
12
Figu a 5.2 Solución del TSP en el caso de un solo hilo.
Los nodos que apa ecen en el esquema an e io con o man las amas que de inen el á bol de nodos has a
ese momen o. Apa ece des acada la ama que iene asociada la solución mos ada po e minal an e io men e
(la mejo de odas). En iole a, apa ecen nume adas las amas comple as del g a o, en el o den en el que se
c ean has a llega a la solución inal. Aunque el á bol se siga desa ollando has a ob ene odas las soluciones,
se ha mos ado el esquema an e io con el in de ilus a de qué mane a se a desglosando has a llega a
nues a solución. De hecho, en azul es án nume ados los nodos que se han desglosado has a ese momen o.
Mé odos implemen ados en el algo i mo
A lo la go de es e apa ado, i emos p esen ando los mé odos asociados a cada una de las clases con las que se
ha implemen ado el P oblema del Viaje o en el lenguaje de p og amación C.
Lle a emos a cabo una desc ipción más exhaus i a del código que la que hemos hecho en los apa ados
an e io es, en los cuales an sólo p esen ábamos las clases y mencionábamos los mé odos que se de inían.
Clase "Ci y"
ill
La sección de código co espondien e apa ece e e enciada en el anexo [A.21].
Con es e mé odo, asignamos alo es a los a ibu os de la clase Ci y.
ge -s ing-da a
La sección de código co espondien e apa ece e e enciada en el anexo [A.22].
De uel e una cadena con dos componen es; en la p ime a de ellas, el id de la ciudad conside ada, mies as
que en el segundo elemen o apa ecen las coo denadas de la ciudad en cues ión.
Clase "Node"
ill
La sección de código co espondien e apa ece e e enciada en el anexo [A.23].
Con es e mé odo, asignamos alo es a los a ibu os de la clase Node.
con e -elemen
La sección de código co espondien e apa ece e e enciada en el anexo [A.24].
Dado un obje o del ipo
Elemen
como a gumen o del mé odo, olcamos en los a ibu os del nodo
conside ado los campos del elemen o de en ada.
Es e mé odo se enca ga de inicializa un nodo a pa i de un obje o del ipo Elemen .
Clase "Pa h"
ill
La sección de código co espondien e apa ece e e enciada en el anexo [A.25].
Con es e mé odo, asignamos alo es a los a ibu os de la clase Node.
35

36 Capí ulo 6. Mé odos implemen ados en el algo i mo
Clase "B anch"
add-elemen
La sección de código co espondien e apa ece e e enciada en el anexo [A.26].
Añade un elemen o a la ama conside ada. Es deci , olcamos en el componen e úl imo del a ibu o
elemen s el elemen o a añadi , conside ando lo siguien e:
•El p ime elemen o de elemen s (la ama conside ada) siemp e debe se un nodo.
•
Los elemen os añadidos han de se del ipo opues o al del úl imo elemen o añadido al a ibu o. Es
deci , elemen os del ipo Node y del ipo Pa h deben i al e nándose con o me se añaden a la ama.
ge - o al-dis ance
La sección de código co espondien e apa ece e e enciada en el anexo [A.27].
Reco e odos los elemen os de la ama conside ada, con el in de acumula las dis ancias de cada uno de
los caminos que ha omado el iaje o. Pa a ello, en el bucle de i e ación, solo se conside an los elemen os
que se co esponden con caminos.
ge - o al-dis ance-des iny-id
La sección de código co espondien e apa ece e e enciada en el anexo [A.28].
De uel e la dis ancia acumulada en los caminos de la ama,
has a llega al id especi icado como a gu-
men o del mé odo
. Hay que señala que se empiezan a acumula dis ancias de los iajes desde la ciudad
o igen, has a llega al elemen o de la clase Node con el id especi icado.
ge -ci ies
La sección de código co espondien e apa ece e e enciada en el anexo [A.29].
De uel e un ec o de ipo
Ci y
con las ciudades po las que el iaje o ha pasado, de la ama conside ada.
Pa a log a lo, se conside an los elemen os Node y se oma su a ibu o asociado a la clase Ci y.
sea ch-node
La sección de código co espondien e apa ece e e enciada en el anexo [A.30].
De uel e
ue
o
alse
en unción de si el nodo que oma el mé odo como en ada se encuen a en la ama
conside ada. Es e mé odo esul a bas an e ú il si se desea encon a un nodo de e minado en el á bol comple o;
bas a con aplica lo a odas las amas que almacene el á bol.
sea ch-pa h
La sección de código co espondien e apa ece e e enciada en el anexo [A.31].
De uel e
ue
o
alse
en unción de si el camino que oma el mé odo como en ada se encuen a en
la ama conside ada. Como comen amos pa a el mé odo an e io , es e mé odo esul a bas an e ú il si se
desea encon a un camino de e minado en el á bol comple o; bas a con aplica lo a odas las amas que se
encuen en almacenadas en el á bol.
ge - ou e
La sección de código co espondien e apa ece e e enciada en el anexo [A.32].
De uel e un ec o con los nodos de la ama que o man pa e de la u a
has a llega al nodo n, que es el
a gumen o de en ada del mé odo
. En es e mé odo, se emplea
Node::con e -elemen
pa a con e i los
elemen os nodos de la ama a obje os de la clase Node.
Clase "G aph"
G aph (cons uc o )
La sección de código co espondien e apa ece e e enciada en el anexo [A.33].
Se explica á po o den las secciones de código empleadas pa a de ini el cons uc o :
6.5 Clase "G aph" 37
•
Almacenamos en
ci ies
(a ibu o de la clase G aph) odas las ciudades del iaje comple o del iaje o,
incluyendo o igen y des ino.
•Se inicializa la base de da os con el nodo pad e.
•Se inializan los pun e os del iaje o y de desglose ( a ele yb eak-down, espec i amen e).
•Se c ea la p ime a ama del g a o.
•Se ejecu a el algo i mo p incipal en es e mismo cons uc o (main-algo i hm).
explo e-and-o de
La sección de código co espondien e apa ece e e enciada en el anexo [A.34].
Po sencillez, se i án mencionando las secciones del código que se p esen a, con su núme o de línea
co espondien e, pa a hace alusión a cada una de las pa es que se aya comen ando a con inuación:
•Líneas 2-8: Se de inen, en es e o den:
– es: Salida que almacena á los nodos y los caminos ob enidos en es e mé odo.
– ou e:
Vec o que almacena los nodos po los que se ha pasado has a llega has a donde se
encuen a el iaje o en es e momen o.
–
El es o de a iables se i án comen ando pos e io men e, con o me se ayan empleando en el
mé odo.
•Líneas 10-13
: Se de ine una lis a en la que se uelcan las ciudades almacenadas en el g a o. Se emplea
una lis a como a iable local de es e mé odo po mayo acilidad en elimina elemen os de e minados
du an e su uso.
•Líneas 15-17
: En la lis a de ciudades que se acaba de de ini (
lis -o -ci ies
), desca amos aquellas po
las que el iaje o ya ha pasado.
•Líneas 21-29
: Se c ean an o nodos como caminos (almacenados, espec i amen e, en
nodes-c ea ed
ypa hs-c ea ed) con la in o mación asociada a la lis a de ciudades de inida an e io men e.
•Líneas 33-50
: Mien as quede algún nodo o camino almacenado en sus espec i as lis as (
nodes-
c ea ed ypa hs-c ea ed), se lle an a cabo las siguien es ins ucciones:
–
Buscamos el camino de la lis a con la dis ancia mínima, de en e odos los caminos c eados. Lo
almacenamos en el i e ado pa h-i .
–
Pos e io men e, se busca el nodo que enga la misma e ique a que la e ique a o igen del camino
almacenado en pa h-i .
–
Añadimos a
nodes- igh
y
pa hs- igh
los obje os o denados po meno dis ancia. Eliminamos
de las lis as deso denadas los obje os que acabamos de añadi a las lis as o denadas, con el in
de no ol e a conside a los en pos e io es i e aciones del bucle while.
•Líneas 52-54: De ol emos es, con los caminos y nodos o denados.
dis ance-be ween-nodes
La sección de código co espondien e apa ece e e enciada en el anexo [A.35].
Dados dos nodos a la en ada, se calcula y de uel e a la salida el módulo de la dis ancia en e sus ciudades
almacenadas.
a el-nea es -ci y
La sección de código co espondien e apa ece e e enciada en el anexo [A.36].
Po sencillez, se i án mencionando las secciones del código que se p esen a, con su núme o de línea
co espondien e, pa a hace alusión a cada una de las pa es que se aya comen ando a con inuación:
•Línea 3
: Se de inen dos lis as en las que se uelcan los nodos y caminos ob enidos del desglose del
nodo del iaje o, ealizado con el mé odo G aph::explo e-and-o de .
•Línea 8: Añadimos el nodo iaje o a la ama del g a o (su a ibu o b anches).
38 Capí ulo 6. Mé odos implemen ados en el algo i mo
•Líneas 11-13
: Buscamos el mínimo camino que pa a del nodo en el que se encuen a el iaje o (al
que apun a a ele ) y lo almacenamos en p-i .
•Línea 16: Añadimos dicho camino a la ama del g a o.
•Líneas 19-20
: Buscamos el nodo con el mismo id que el a ibu o
id-des iny
del camino mínimo que
hemos encon ado an e io men e, y lo gua damos en el i e ado n-i .
•Líneas 24-25
: Se ealiza una búsqueda en la base de da os de nodos del g a o (
nodes
). Buscamos el
nodo almacenado en
n-i
, y modi icamos la di ección de memo ia del pun e o
a ele
po la de es e
nue o nodo.
•Líneas 28: Añadimos a la ama del g a o es e úl imo nodo.
A modo de esumen, obse amos que con es e mé odo ac ualizamos la di ección de memo ia del pun e o
iaje o con o me se a iajando a la siguien e ciudad. Además, se an inse ando en la ama del g a o los
elemen os co espondien es que an de iniendo el iaje has a el momen o.
Cabe señala que, ya que empleamos el mé odo
B anch::add-elemen
, en caso de que que amos inse a
el nodo del iaje o de la siguien e i e ación una ez que acabamos de inse a el mismo nodo en la i e ación
an e io , no pod emos hace lo, y el siguien e elemen o inse ado en la ama se á el p óximo camino.
e esh-g aph-da a
La sección de código co espondien e apa ece e e enciada en el anexo [A.37].
En es e mé odo, se lle an a cabo las siguien es modi icaciones en el á bol:
•
Se modi ica la di ección de memo ia del pun e o
mo e-bd
en unción de si decidimos hace lo o no
du an e la ejecución de
G aph::main-algo i hm
. Una ez que epliquemos el algo i mo p incipal en
es e mismo apa ado, se comp ende á mejo en qué caso cambiamos el pun e o de desglose.
•Ac ualizamos la di ección de memo ia del pun e o del iaje o, a ele .
•
C eamos una nue a ama en el á bol, con el in de llena sus elemen os en las i e aciones pos e io es
de G aph::main-algo i hm.
•Rese eamos las bases de da os, an o pa hs como nodes.
•Inse amos el p ime nodo en la base de da os; se á el nodo al que apun a el pun e o del iaje o.
•Tan o b eak-down como a ele apun an a dicho nodo que se ha inse ado en la base de da os.
emo e-b anches-elemen s
La sección de código co espondien e apa ece e e enciada en el anexo [A.38].
Es e mé odo se aplica jus o después de
G aph::explo e-and-o de
. Su come ido p incipal consis e en
elimina de los elemen os de en ada (nodos y caminos) aquellos obje os que ya o men pa e de o as amas
c eadas has a aho a.
¿Po qué es impo an e es e mé odo? Po que odas las amas nue as que se ayan c eando deben se
dis in as a las c eadas an e io men e, ya que que amos buscando soluciones nue as cons an emen e.
En el caso de que oda ía no se hayan c eado amas an e io men e, es e mé odo no modi ica de mane a
alguna el a ibu o que almacena las amas del g a o; b anches.
Po sencillez, se i án mencionando las secciones del código que se p esen a, con su núme o de línea
co espondien e, pa a hace alusión a cada una de las pa es que se aya comen ando a con inuación:
•Líneas 11-15: Se eliminan los nodos que o men pa e de o as amas an e io es del g a o.
•Líneas 18-22: Se eliminan los caminos que o men pa e de o as amas an e io es del g a o.
•Líneas 25-27: Se ac ualiza la base de da os de los nodos.
•Líneas 30-32: Se ac ualiza la base de da os de los caminos.
•Líneas 34: De uel o los nodos y los caminos almacenados en la a iable local e -elemen s.
6.5 Clase "G aph" 39
b eak-down-son
La sección de código co espondien e apa ece e e enciada en el anexo [A.39].
Con es e mé odo, de ol emos el iden i icado adecuado pa a segui e ique ando a los nodos que se segui án
c eando en el g a o.
En p ime luga , buscamos la ama en la que se encuen e el nodo al que apun a
b eak-down
. Una ez que
la hayamos encon ado, gua damos en el i e ado
e-i
el elemen o que se co esponde con el nodo al que
apun a el pun e o de desglose.
Hay que ene en cuen a que, conside ando el o den en el que se han ido de ieniendo las amas del g a o,
se asegu a que es e mé odo de ol e á el hijo del nodo de desglose con el camino de la meno dis ancia.
En caso de que no se haya encon ado el nodo con la di ección de
b eak-down
, el mé odo de uel e un
-1
.
c ea e-b anch
La sección de código co espondien e apa ece e e enciada en el anexo [A.40].
Llegamos al mé odo que c ea las amas del á bol. Analiza emos paso a paso las secciones del código y
comen a emos el come ido de las a iables más impo an es que se decla an al comienzo.
An es de ello, es impo an e esal a lo siguien e. Las amas del g a o almacenadas en b anches
no ienen
po qué aba ca odo el eco ido del iaje o.
El p ime nodo de la p ime a ama almacena á la ciudad de o igen del iaje, y el úl imo nodo de dicha
ama almacena á la ciudad de des ino. Sin emba go, con o me amos c eando mas y más amas, su longi ud
disminuye, ya que el p ime nodo de las mismas ienen asignado un ni el más p o undo en el g a o.
Una imagen que in en a ilus a es a explicación puede ene la siguien e o ma:
1
2
5
11
17
6
12
18
3
7
13
19
8
14
20
4
9
15
21
10
16
22
Figu a 6.1 Sub amas en el caso de es ciudades in e medias.
En cuan o a la imagen an e io , las es amas azules que pa en del nodo pad e del g a o son las es
p ime as amas c eadas en el g a o. Es amos desglosando el NODO 1, po lo que es amos ob eniendo amas
que pa en de dicho nodo has a la ciudad de des ino (nodos 17,19 y 21, espec i amen e).
Una ez desglosado el NODO 1, a amos de hace lo mismo con el NODO 2. Como la p ime a ama pasa
po el NODO 2, cuando a emos de desglosa dicho nodo an solo ob end emos una ama. Dicha ama end á
como nodo inal el NODO 18, y pa i á del NODO 2. Es una de las amas que apa ece ep esen ada en ojo,
más co a que las es an e io es.
En el caso en el que nos encon emos an e un g a o con más ciudades in e medias, los desgloses de los
nodos i án asociados con una c eación de amas cada ez más co as. Es o se aduce en una meno ca ga de
in o mación almacenada po el p og ama (lo cual es una en aja).
Comencemos a abo da el código que se p esen a a con inuación:
•El código se compone po un bucle do-while
, suje o a una condición que depende del cumplimien o
de una exp esión booleana, llamada
c
. Es a condición es a á de inida de una mane a u o a según se
abo de el p oblema del TSP simple o el p oblema con múl iples iaje os y múl iples ciudades des ino;
el MTSP. En conc e o, depende á de una a iable ex e na a es e mé odo, h eshold.
40 Capí ulo 6. Mé odos implemen ados en el algo i mo
Dicha a iable de ine el ni el lími e has a el cual que emos que las amas del g a o almacenen elemen os.
Dicho de o a mane a, con
h eshold
decidimos cuán as ciudades in e medias que emos inclui en el
p oblema.
En unción de es o, hay una condición i -else den o de es e bucle que dis ingue dos casos:
–
Añado la ciudad de des ino a la ama y la doy po inalizada (
líneas 11-24
). En es e caso, el
iaje o apun a al nodo con el ni el deno ado po h eshold.
–
Se an c eando las amas en el caso de que el iaje o no haya llegado a
h eshold
(
líneas 25-42
).
Dicho es o comen a emos ambas condiciones.
•Líneas 11-24:
1.
C eamos el nodo
n
que almacene la ciudad des ino del iaje o; lo incluimos en la base de da os
nodes y hacemos que el pun e o a ele apun e a dicho nodo de la base de da os.
2.
Del mismo modo, c eamos el camino
p
que c ea el amo que iene como
des iny-id
la e ique a
asociada al nue o nodo del iaje o.
3. Añadimos el camino py el nodo nal la ama ac ual.
4. De inimos la condición c: el bucle while se ejecu a mien as el núme o de elemen os de la
ama ac ual sea meno al amaño deseado de la ama, eniéndose en cuen a desde dónde se
empieza el desglose (b eak-down) y el alo de h eshold.
•Líneas 25-42:
1.
En p ime luga , e escamos la a iable
global-node-id
en unción del nodo al que apun e
b eak-
down
. En el caso de que apun e al nodo del iaje o, el
global-node-id
con el que se e ique an
los nue os nodos oma el alo del id del hijo de
b eak-down
, como se comen ó en el mé odo
G aph::b eak-down-son
. En o o caso, se oma el id con el que se e ique ó al úl imo nodo pa a
segui e ique ando al es o.
8
2
5
8
5
6
2
2
3
2
2
2
2
2
2
4
2
2
2
2
2
2
Figu a 6.2 Nume ación de amas: iaje o y desglose en dis in os nodos.
En la igu a an e io , se ilus a el caso en el cual el pun e o de desglose (ma ca oja) iene una
di ección dis in a a la del pun e o del iaje o (ma ca azul). Po ello, a la ho a de hace el p óximo
desglose, se oma á el úl imo iden i icado que se ha asignado a un nodo en la base de da os, en
es e caso el núme o 6 (ma cado en ojo). De es a mane a, el nodo ac ual al que apun a el iaje o se
nume a con un 7 y se sigue inc emen ando el indicado has a comple a la ama ac ual.
Hay que señala que en el g a o ep esen ado an solo se han c eado los nodos nume ados (y los
caminos que unen los nodos exis en es en e sí). Aquellos que no apa ecen somb eados y que es án
nume ados, se co esponden con los nodos que
solo pe enecen a la base de da os de la ama
ac ual
. Po o o lado, los nodos que apa ecen somb eados pe enecen a la ama ac ual (y ambién a
los de la base de da os, que se c ea on an es). Pa a los caminos, se aplica el mismo azonamien o.

6.5 Clase "G aph" 41
88
2
5
7
8
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
2
Figu a 6.3 Nume ación de amas: iaje o y desglose en el mismo nodo.
En la segunda imagen del g a o, se con empla el caso en el que el iaje o y el desglose apun an al
mismo nodo. Po ello, se oma como úl imo iden i icado aquel que se co esponde con el p ime
nodo hijo del nodo de desglose ac ual. Se p ocede de es a mane a po que hay que c ea una nue a
base de da os, ya que se empieza a c ea una nue a ama que pa e desde el nodo pad e del g a o. De
es a mane a, se nume an los nodos del segundo ni el de mane a idén ica con espec o a la imagen
an e io , y los nodos adquie en un iden i icado uní oco en el g a o.
2.
C eo caminos y nodos desglosando el nodo del iaje o ac ual. En caso de que no ob enga elemen os
en
elemen s-c ea ed
, signi ica que que he llegado al inal de la ama y que debo cambia la di ección
del pun e o de desglose b eak-down.
3.
Ac ualizamos el id de nume ación de los nodos, en unción de la conside ación p e ia que se ha
lle ado a cabo con espec o a los hijos de b eak-down.
4. De inimos la condición c: el bucle while se ejecu a mien as el núme o de elemen os de la
ama ac ual sea meno al amaño deseado de la ama, eniéndose en cuen a desde dónde se
empieza el desglose (b eak-down).
mo e-b eak-down
La sección de código co espondien e apa ece e e enciada en el anexo [A.41].
Ac ualizamos el alo del pun e o de desglose; apun a á al nodo con su iden i icado una unidad mayo
que el nodo de desglose ac ual.
p in - esul s
No es necesa io mos a código; imp ime los alo es más signi ica i os que esul an de ejecu a el algo i mo
del TSP.
ge - o al-dis ance
La sección de código co espondien e apa ece e e enciada en el anexo [A.42].
De uel e la dis ancia o al de la ama del g a o que enga el iden i icado b anch-id.
Comen a emos las secciones más eseñables del código:
•Líneas 7-13
: Se de inen los condiciones booleanas
mino
y
equal
, las cuales se emplea án más
adelan e en el p opio código.
1. Líneas 7-10
: En caso de que el lími e
h eshold
no ome el alo del úl imo ni el del g a o, se
de ine las condiciones
equal
y
mino
como e dade a siemp e y cuando el núme o de elemen os
de la ama ac ual sea meno al deseado conside ando el h eshold.
2. Líneas 11-13
: En caso de que no se conside e
h eshold
(y, po an o, su alo coincidiese con el
úl imo ni el del g a o) se de inen mino yequal sin conside a se el alo del h eshold.
42 Capí ulo 6. Mé odos implemen ados en el algo i mo
•Líneas 19-32
: En caso de que la ama enga elemen os y de que se cumpla
mino
, añadimos al
acumulado e -dis ance la dis ancia de la ama ac ual.
Pos e io men e, a a emos de busca en odas las amas del g a o has a encon a una ama con un
camino
con el des iny-id que sea igual al o igin-id del p ime camino de la úl ima ama conside-
ada; de id b anch-id-aux.
Con la siguien e imagen, a a emos de ilus a cómo que emos ob ene la dis ancia o al de la ama,
pa iendo del nodo des ino has a llega al nodo o igen, buscando amas in e medias con coincidencias
de id.
1
2
5
11
17
6
12
18
3
7
13
19
8
14
20
4
9
15
21
10
16
22
Figu a 6.4 Elemen os de una ama comple a del g a o.
Una ez que lleguemos a una ama en la que su p ime nodo sea el nodo pad e del g a o, signi ica que
ya enemos la dis ancia o al del iaje y que podemos de ol e la dis ancia o al acumulada.
•Líneas 35-38
: Si se cumple equal, signi ica que la ama en la que es amo con iene los nodos que
almacenan, espec i amen e, la ciudad o igen y la ciudad des ino.
•Líneas 41-42
: En caso de que la ama es é acía (sin elemen os), de uel o un 0 en la dis ancia o al
acumulada.
ge - ou e
La sección de código co espondien e apa ece e e enciada en el anexo [A.43].
Como se comen ó en apa ados an e io es con meno de alle, con
G aph::ge - ou e
p e endemos de ol e
un ec o con los nodos que con o man la u a del iaje o has a llega al nodo
n
, que es la en ada del mé odo
conside ado.
Básicamen e, buscamos en las amas que con o man el g a o de mane a simila a como hacíamos en
G aph::ge - o al-dis ance
, solo que aho a no conside amos los caminos de las amas, sino los nodos. Dichos
nodos se an añadiendo al ec o que de ol emos a la salida; ou -.
main-algo i hm
La sección de código co espondien e apa ece e e enciada en el anexo [A.44].
Algo i mo p incipal, que implemen a la mayo ía de los mé odos explicados has a el momen o.
Analicemos las líneas más impo an es del código:
•Línea 12
: Damos alo a
begin
, pa a con abiliza el iempo de ejecución del algo i mo pos e io men e.
•Líneas 15-17
: De inimos el
ac o ial- alue
, que nos da in o mación ace ca del núme o de posibles
soluciones del p oblema del iaje o. Lo hemos llamado de es a mane a aunque ealmen e no sea un
ac o ial;
ac o ial- alue oma el alo del ac o ial del núme o de ciudades in e medias consi-
de adas en el p oblema, que no iene po qué coincidi con el núme o de ciudades in e medias
o ales
. La dis inción en e ambas conside aciones la lle a a cabo la a iable
h eshold
, al y como se
con empla en las líneas de código.
6.6 Clase "WholeG aph" 43
•Líneas 20-25
: Bucle enca gado de la c eación de una ama en el g a o. También se uelca en
dis ances
el alo de la dis ancia o al asociada a la ama asociada,
conside ando la explicación que se dio con
espec o al uncionamien o de G aph::ge - o al-dis ance
. El bucle
do-while
sigue c eando amas
mien as el núme o de amas c eadas no haya alcanzado el núme o deseado, o mien as dicho
núme o no haya llegado al núme o de soluciones posibles del p oblema.
•Líneas 31-35
: Po un lado, se almacena en
-i
el núme o de componen e del ec o
dis ances
con la
dis ancia mínima conseguida y, po an o, asociado a la mejo solución.
Po o o lado, almaceno en
n
el nodo de la ama asociada a la mejo solución, y ob engo la u a
comple a del iaje o usando B anch::ge - ou e.
ou e-ou pu
La sección de código co espondien e apa ece e e enciada en el anexo [A.45].
Se enca ga de de ol e la u a de nodos que con o ma la solución del p oblema del iaje o. Se almacena
en el a ibu o denominado ou e-ou .
min-dis ance-ou pu
La sección de código co espondien e apa ece e e enciada en el anexo [A.46].
De uel e la dis ancia mínima asociada a la mejo solución del algo i mo.
Tan o es e mé odo como el del apa ado an e io se implemen a on con el in de emplea se en la clase
WholeG aph.
Clase "WholeG aph"
ou pu -li le-b anches
Mues a la salida de los esul ados más signi ica i os del algo i mo pa a el caso en el que se emplean
a ios
hilos.
ou pu -whole-g aph
La sección de código co espondien e apa ece e e enciada en el anexo [A.47].
Mues a la salida de los esul ados más signi ica i os del algo i mo pa a el caso en el que se emplea
un
solo hilo hilos.
main-algo i hm
La sección de código co espondien e apa ece e e enciada en el anexo [A.48].
Algo i mo p incipal de la clase WholeG aph.
En unción del alo de
sub-b anch-id
, esol e emos el g a o comple o o ami icaciones conc e as del
mismo. Se ha supues o que, en caso de que su alo sea 99,
se esuel e el g a o comple o
. En el caso de
que
sub-b anch-id
ome el alo de un id asociado a alguna de las ciudades del sp, se esol e an solo las
ami icaciones del á bol
con el nodo hijo del nodo pad e que enga sub-b anch-id como iden i icado de
su ciudad
. Dicho de o a mane a; en es e caso se esuel e el sp pa iendo de uno de los hijos del nodo con la
ciudad o igen. De es a mane a, no se conside an las soluciones inculadas a los o os hijos del nodo con la
ciudad de o igen, ya que pa en de o as amas.
44 Capí ulo 6. Mé odos implemen ados en el algo i mo
Con las siguien es imágenes, se ilus a la explicación an e io :
1
2
5
11
15
6
12
16
3
7
13
17
8
14
18
4
9
19
19
10
21
20
Figu a 6.5 G a o del iaje o comple o.
1
2
5
11
15
6
12
16
3
7
13
17
8
14
18
4
9
19
19
10
21
20
Figu a 6.6 Di isión en sub amas del g a o del iaje o.
A lo la go de odo el código p esen ado, se de inen lis as locales en las que se manejan las ciudades
in e medias del p oblema de mane a que se conside en aquellas adecuadas en cada uno de los casos que
de ine sub-b anch-id.
add-aux-ci ies
La sección de código co espondien e apa ece e e enciada en el anexo [A.49].
Añade ciudades auxilia es en aux-ci ies. Dichas ciudades se especi ican a la en ada.
add- es-ci ies
La sección de código co espondien e apa ece e e enciada en el anexo [A.50].
Añade ciudades es ingidas en es-ci ies. Dichas ciudades se especi ican a la en ada.
Clase "M sp"
sol e-only- es
La sección de código co espondien e apa ece e e enciada en el anexo [A.51].
Conclusiones
Compa ando con la solución desa olada an e io men e en el en o no de MATLAB, se puede a i ma lo
siguien e:
•No se emplea una base de da os eno me
, sino que se almacenan las amas den o de cada g a o. Tan
solo se emplea una base de da os al de ini cama ama, en la clase G aph.
•
Con espec o a MATLAB,
se emplean hilos
(que apo an concu encia) pa a pode esol e el mismo
p oblema en meno iempo pa a el mismo núme o de soluciones deseado. Ejecución
pa alela
del
código en luga de secuencial.
•
En el caso en el que nos encon emos an e un g a o con muchas ciudades in e medias, los desgloses de
los nodos i án asociados con una c eación de amas cada ez más co as. Es o se aduce en una meno
ca ga de in o mación almacenada po el p og ama (lo cual es una en aja).
•
En luga de conside a odos los da os almacenados en una base de da os de nodos (lo que se hacía en
MATLAB), en es a ocasión se de ine se ie de clases pa a c ea obje os que ope en y almacenen sob e
los da os más c uciales. Pe mi e di e encia a eas de al o y bajo ni el.
Una ez enume adas es as mejo as, a modo de conclusión, se puede es ablece que con es a nue a mene a
de abo da el algo i mo del P oblema del Viaje o se ob ienen:
•Más soluciones en meno iempo, si iéndonos del pa alelismo que nos p opo cionan a ios hilos.
•Meno in o mación asociada a las soluciones
, debido a que almacenamos la in o mación más ele-
an e en obje os del ipo ama, y que su longi ud disminuye con o me a anza el algo i mo que eco e
el g a o.
51

Posibles mejo as
De en e las p incipales mejo as que se pod ían lle a a cabo, pa iendo de la solución y los esul ados
p opues os a lo la go de es a memo ia, se pueden conside a las siguien es:
•
Emplea un núme o de hilos que se adecúe al p ocesado del o denado pa a esol e el p oblema
del iaje o de la mane a más e icien e posible (bajo cos e compu acional pa a el mayo núme o de
soluciones posible).
•
Vol iendo al ema de los hilos, conside a que se puede emplea es a écnica pa a ni eles in e io es del
g a o. Es deci , en luga de esol e las amas p incipales del g a o (asociadas a las ciudades/nodos del
p ime ni el), emplea hilos pa a sub amas del segundo o e ce ni el.
•
En el caso del MTSP con ciudades auxilia es, mejo a el epa o de ciudades en e iaje os. En ez
de hace se de mane a alea o ia, a e igua qué ciudad e asa más a cada iaje o pa a cede la a o o
iaje o. De la misma mane a, pod ía conside a se a qué iaje o pe judica menos ob ene una ciudad
cedida, sin ene po qué se el iaje o con menos cos e asociado a su solución.
•Usa a ios UAVS en el caso de la simulación en GAZEBO y esol e un MTSP.
53
Anexo I: Código empleado
Clases del TSP
Código A.1 Decla ación de clase Ci y.
1
2class Ci y
3{
4
5public:
6in id;
7s d:: ec o < loa > ci y;
8
9// Fo using " ind_i " wi h ci ies
10 bool ope a o == (cons Ci y& s) cons { e u n ci y == s.ci y && id == s.id; }
11 bool ope a o != (cons Ci y& s) cons { e u n !ope a o ==(s); }
12
13 oid ill(in id_, s d:: ec o < loa > ci y_);
14 s d:: ec o <s d::s ing> ge _s ing_da a();
15 };
Código A.2 Decla ación de clase Node.
1
2class Node: public Elemen {
3public:
4Node(){elemen _ ype=NODE;}
5
6// Fo using " ind_i " wi h nodes
7bool ope a o == (cons Node& s) cons { e u n label == s.label && ci y == s.
ci y && le el == s.le el; }
8bool ope a o != (cons Node& s) cons { e u n !ope a o ==(s); }
9
10 oid ill(Ci y ci y_,in label_,in le el_);
11 oid con e _elemen (Elemen e);
12 };
55
56 Capí ulo A. Anexo I: Código empleado
Código A.3 Decla ación de clase Pa h.
1
2class Pa h: public Elemen {
3public:
4Pa h(){elemen _ ype=PATH;}
5
6// Fo using " ind_i " wi h pa hs
7bool ope a o == (cons Pa h& s) cons { e u n o igin_id == s.o igin_id &&
des iny_id == s.des iny_id && a eled == s. a eled && dis ance == s.
dis ance; }
8bool ope a o != (cons Pa h& s) cons { e u n !ope a o ==(s); }
9bool ope a o < (cons Pa h& s) cons { e u n dis ance < s.dis ance; }
10
11 oid ill(in o igin_id_,in des iny_id_,bool a eled_, loa dis ance_);
12 };
Código A.4 Decla ación de clase B anch.
1
2class B anch
3{
4public:
5s d::deque<Elemen > elemen s;
6in las _elemen ; // "NODE" "PATH" o "-1"
7
8// CONSTRUCTOR
9B anch(){ inished= alse; las _elemen =-1; }
10
11 s d:: ec o <Ci y> ge _ci ies();
12 s d:: ec o <Node> ge _ ou e(Node n);
13 oid add_elemen (Elemen e);
14 loa ge _ o al_dis ance();
15 loa ge _ o al_dis ance_des iny_id(in d_id);
16 bool sea ch_node(Node n);
17 bool sea ch_pa h(Pa h n);
18
19 };
Código A.5 Decla ación de clase Elemen .
1
2class Elemen {
3public:
4in elemen _ ype=-1; // NODE o PATH
5// PATH CLASS DATA
6in o igin_id=-1;
7in des iny_id=-1;
8bool a eled=-1;
9 loa dis ance=-1;
10 // NODE CLASS DATA
11 in label=-1;
12 in le el=-1;
13 Ci y ci y;
14 };

A.1 Clases del TSP 57
Código A.6 Decla ación de clase G aph.
1
2class G aph
3{
4p i a e:
5
6Node * a ele ;
7Node * b eak_down;
8s d::deque<Node> nodes;
9s d::deque<Pa h> pa hs;
10 in global_node_id, global_pa h_id;
11 in b anches_id;
12
13 public:
14 s d:: ec o <B anch> b anches;
15 s d:: ec o <Ci y> ci ies;
16
17 // OUTPUT VARIABLES
18 in n_b anches_p i a e;
19 in ac o ial=1;
20 s d:: ec o < loa > dis ances;
21 in le el_;
22 s d:: ec o <Node> ou e_ou ;
23 os::Time begin;
24 loa min_dis ance;
25
26 G aph(s d:: ec o <Ci y> o igin,
27 s d:: ec o <Ci y> ci ies,
28 s d:: ec o <Ci y> des iny,
29 in n_b anches,s d::mu ex &m x, in le el__,in & h eshold); // CONSTRUCTOR
30 oid p in _nodes_da a_base();
31 oid p in _pa hs_da a_base();
32 oid p in _da a_base();
33 s d::pai <s d::lis <Node>,s d::lis <Pa h>> explo e_and_o de ();
34 loa dis ance_be ween_nodes(Node n1, Node n2);
35 oid a el_nea es _ci y(s d::pai <s d::lis <Node>,s d::lis <Pa h>> elemen s);
36 oid main_algo i hm(in n_b anches,s d::mu ex &m x, in le el__,in & h eshold);
37 s d::pai <s d::lis <Node>,s d::lis <Pa h>> emo e_b anches_elemen s(s d::pai <s d::
lis <Node>,s d::lis <Pa h>> elemen s);
38
39 in b eak_down_son();
40 oid c ea e_b anch(in &global_node_id_sa ed,in &mo e_bd,in & h eshold);
41 oid e esh_g aph_da a(Elemen e, Node n,Node sa e_ a ele ,in &mo e_bd ,s d::
ec o < loa > &dis ances,in & h eshold);
42 oid mo e_b eak_down();
43 s d:: ec o <Node> ge _ ou e(Node n);
44 s d:: ec o <in > p in _ esul s(s d::mu ex &m x,in & h eshold);
45 loa ge _ o al_dis ance(in b anch_id_,in & h eshold);
46
47 // OUTPUT METHODS
48 s d:: ec o <Node> ou e_ou pu ();
49 loa min_dis ance_ou pu ();
50 };
58 Capí ulo A. Anexo I: Código empleado
Código A.7 Decla ación de clase WholeG aph.
1
2class WholeG aph
3{
4p i a e:
5
6public:
7
8s d:: ec o <Ci y> es_ci ies;
9s d:: ec o <Ci y> aux_ci ies;
10 loa min_dis ance;
11
12 oid ou pu _li le_b anches(G aph up, G aph down,s d::mu ex &m x);
13 s d:: ec o <in > ou pu _whole_g aph(G aph down,s d::mu ex &m x,in & h eshold);
14 s d:: ec o <in > main_algo i hm(s d:: ec o <s d:: ec o < loa >> o igin,
15 s d:: ec o < loa > des iny,in ci ies_size,
16 in sub_b anch_id,in i e a ions,
17 s d::mu ex &m x,in numbe _ci ies_ a eled);
18
19 oid add_aux_ci ies(s d:: ec o <Ci y> ci ies,s d:: ec o <in > ids_ o_include);
20 oid add_ es_ci ies(s d:: ec o <Ci y> ci ies,s d:: ec o <in > ids_ o_include);
21
22 };
Clases del MTSP
Código A.8 Decla ación de clase M sp.
1
2class M sp
3{
4
5public:
6
7s d:: ec o < loa > min_dis ances;
8
9 oid sol e_only_ es(s d:: ec o <s d:: ec o < loa >> o igin,
10 s d:: ec o <s d:: ec o < loa >> ci ies,
11 s d:: ec o <s d:: ec o < loa >> des iny,
12 in sub_b anch_id,in i e a ions,
13 s d:: ec o <s d:: ec o <in >> es_ids,s d::mu ex &m x);
14
15 oid sol e_only_aux(s d:: ec o <s d:: ec o < loa >> o igin,
16 s d:: ec o <s d:: ec o < loa >> ci ies,
17 s d:: ec o <s d:: ec o < loa >> des iny,
18 in sub_b anch_id,in i e a ions,
19 s d:: ec o <in > aux_ids,s d::mu ex &m x);
20
21 oid sol e_aux_and_ es(s d:: ec o <s d:: ec o < loa >> o igin,
22 s d:: ec o <s d:: ec o < loa >> ci ies,
23 s d:: ec o <s d:: ec o < loa >> des iny,
24 in sub_b anch_id,in i e a ions,
25 s d:: ec o <s d:: ec o <in >> es_ids,
26 s d:: ec o <in > aux_ids,s d::mu ex &m x);
27
28 oid sol e_ sp(s d:: ec o <s d:: ec o < loa >> &o igin,
29 s d:: ec o <s d:: ec o < loa >> &ci ies,
A.3 Código del iche o p incipal 59
30 s d:: ec o <s d:: ec o < loa >> &des iny,
31 in sub_b anch_id,in i e a ions,
32 s d:: ec o <in > &aux_ids,
33 s d:: ec o <s d:: ec o <in >> & es_ids,
34 s d::mu ex &m x,
35 s d:: ec o <Ci y> &c,
36 s d:: ec o <s d:: ec o <in >> & e u n_ci ies_ids,
37 s d:: ec o <in > &ci ies_ o_ a el,
38 s d::lis <in > &aux_ids_lis );
39
40
41 };
Código del iche o p incipal
Código A.9 Fiche o p incipal: codigo-p incipal.cpp.
1
2#include "m sp_ iles/clases/M sp.h"
3
4using namespace s d;
5using s d:: ec o ;
6
7s d::mu ex m x;
8
9// Requi ed in o de o spli . x da a
10 oid spli (cons s ing &s, cha delim, ec o <s ing> &elems) {
11 s ings eam ss(s);
12 s ing i em;
13 while (ge line(ss, i em, delim)) { elems.push_back(i em);}}
14 ec o <s ing> spli (cons s ing &s, cha delim) {
15 ec o <s ing> elems;
16 spli (s, delim, elems);
17 e u n elems;}
18
19
20
21 in main( in a gc, cha ** a g ){
22
23 os::ini (a gc, a g , "codigo_p incipal");
24 os::NodeHandle n;
25 os::Publishe ma ke _pub = n.ad e ise< isualiza ion_msgs::Ma ke >("
isualiza ion_ma ke ", 10);
26 os::Ra e (30);
27 loa = 0.0;
28
29 // De ining ec o s wi h CITIES, ORIGIN and DESTINY
30 s d:: ec o <s d:: ec o < loa >> o igin, des iny;
31 s d:: ec o <s d:: ec o < loa >> ci ies;
32
33 o igin.push_back({0,1.1,0}); // O igin Ci y 1
34
35 // In e media e Ci ies
36 ci ies.push_back({1,2.3,-4}); // 2
37 ci ies.push_back({3,2,3}); // 3
38 ci ies.push_back({-2,8.4,4}); // 4
39 ci ies.push_back({7,9,9}); // 5
40 ci ies.push_back({2,-9,9}); // 6
60 Capí ulo A. Anexo I: Código empleado
41 ci ies.push_back({7,9,2}); // 7
42 ci ies.push_back({7,2,2}); // 8
43
44 des iny.push_back({10,10,10}); // Des iny Ci y 1
45 des iny.push_back({1.5,-2,1}); // Des iny Ci y 2
46
47
48 // Th ead wi h a single TSP
49 au o sp_ h ead = [](s d:: ec o <s d:: ec o < loa >> o igin,
50 s d:: ec o <s d:: ec o < loa >> ci ies,
51 s d:: ec o <s d:: ec o < loa >> des iny,
52 in sub_b anch_id,in i e a ions) {
53 M sp m sp;
54
55 s d:: ec o <s d:: ec o <in >> es_ids ={{7,5},{2}} ;
56 s d:: ec o <in > aux_ids={6,3,4};
57 m sp.sol e_aux_and_ es(o igin,ci ies,des iny,sub_b anch_id,i e a ions, es_ids,
aux_ids,m x);
58 // m sp.sol e_only_ es(o igin,ci ies,des iny,sub_b anch_id,i e a ions, es_ids,
m x);
59 // m sp.sol e_only_aux(o igin,ci ies,des iny,sub_b anch_id,i e a ions,aux_ids,
m x);
60
61
62 };
63
64
65
66 h ead h01( sp_ h ead,o igin,ci ies,des iny,WHOLE_GRAPH,ITERATIONS);
67
68
69 h01.join();
70
71 e u n 0;
72 }
Visualización RVIZ
Código A.10 Ciudades conside adas en la isualización de RVIZ.
1
2o igin.push_back({0,1.1,0}); // O igin Ci y 1
3
4// In e media e Ci ies
5ci ies.push_back({1,2.3,0}); // 2
6ci ies.push_back({3,2,0}); // 3
7ci ies.push_back({2,-8.4,0}); // 4
8ci ies.push_back({7,-9,0}); // 5
9ci ies.push_back({-2,9,0}); // 6
10 ci ies.push_back({-7,2,0}); // 7
11 ci ies.push_back({-7,9,0}); // 8
12
13 des iny.push_back({5,5,0}); // Des iny Ci y 1
14 des iny.push_back({5,-5,0}); // Des iny Ci y 2
15 des iny.push_back({-9,5,0}); // Des iny Ci y 3
A.7 Mé odos implemen ados en el algo i mo 67
Código A.23 Clase Node: ill.
1
2 oid Node:: ill(Ci y ci y_,in label_,in le el_){
3ci y=ci y_;
4label=label_;
5le el=le el_; };
Código A.24 Clase Node: con e -elemen .
1
2 oid Node::con e _elemen (Elemen e){
3ci y=e.ci y;
4label=e.label;
5le el=e.le el;};
Código A.25 Clase Pa h: ill.
1
2 oid Pa h:: ill(in o igin_id_,in des iny_id_,bool a eled_, loa dis ance_){
3o igin_id=o igin_id_;
4des iny_id=des iny_id_;
5 a eled= a eled_;
6dis ance=dis ance_;};
Código A.26 Clase B anch: add-elemen .
1
2 oid B anch::add_elemen (Elemen e){
3s d::s ing s;
4i (e.elemen _ ype==PATH){s="PATH";}
5i (e.elemen _ ype==NODE){s="NODE";}
6
7i (las _elemen ==e.elemen _ ype){}
8// s d::cou << "CANNOT ADD THE ELEMENT" << s d::endl;
9else i (las _elemen !=e.elemen _ ype){
10 i (las _elemen ==-1 && e.elemen _ ype==PATH){}
11 // s d::cou << "CANNOT ADD THE ELEMENT" << s d::endl;
12 else{
13 // s d::cou << "ADDED " << s << s d::endl;
14 las _elemen =e.elemen _ ype;
15 elemen s.push_back(e); }} };
Código A.27 Clase B anch: ge - o al-dis ance.
1
2 loa B anch::ge _ o al_dis ance(){
3 loa b anch_dis ance=0;
4 o (in i=0;i<elemen s.size();i++){
5i (elemen s[i].elemen _ ype==PATH){b anch_dis ance=b anch_dis ance+elemen s[i].
dis ance;};}
6 e u n b anch_dis ance ;};

68 Capí ulo A. Anexo I: Código empleado
Código A.28 Clase B anch: ge - o al-dis ance-des iny-id.
1
2 loa B anch::ge _ o al_dis ance_des iny_id(in d_id){
3 loa b anch_dis ance=0;
4 o (in i=0;i<elemen s.size();i++){
5i (elemen s[i].elemen _ ype==PATH){b anch_dis ance=b anch_dis ance+elemen s[i].
dis ance;
6i (elemen s[i].des iny_id==d_id){ e u n b anch_dis ance ;}};}
7}
Código A.29 Clase B anch: ge -ci ies.
1
2s d:: ec o <Ci y> B anch::ge _ci ies(){
3s d:: ec o <Ci y> c;
4 o (in i=0;i<elemen s.size();i++){
5i (elemen s[i].elemen _ ype==NODE){c.push_back(elemen s[i].ci y);};}
6 e u n c ;};
Código A.30 Clase B anch: sea ch-node.
1
2bool B anch::sea ch_node(Node n){
3 o (in i=0;i<elemen s.size();i=i+2){ // Only sea ch o nodes
4i ((n.le el==elemen s[i].le el)
5&&(n.ci y==elemen s[i].ci y) &&
6(n.label==elemen s[i].label)){ e u n ue;}}
7 e u n alse;}
Código A.31 Clase B anch: sea ch-pa h.
1
2bool B anch::sea ch_pa h(Pa h n){
3 o (in i=1;i<elemen s.size();i=i+2){ // Only sea ch o pa hs
4i ((n.des iny_id==elemen s[i].des iny_id)){ e u n ue;}}
5 e u n alse;}
Código A.32 Clase B anch: ge - ou e.
1
2s d:: ec o <Node> B anch::ge _ ou e(Node n){
3s d:: ec o <Node> nodes_ ou e; Node aux_node;
4 o (in i=0;i<elemen s.size();i=i+2){ // Only sea ch o nodes
5aux_node.con e _elemen (elemen s[i]);
6nodes_ ou e.push_back(aux_node);
7i (elemen s[i].label==n.label){b eak;/*Found he elemen and e u n*/ }}
8 e u n nodes_ ou e; // Elemen no ound
9}
A.7 Mé odos implemen ados en el algo i mo 69
Código A.33 Clase G aph: G aph (cons uc o ).
1
2G aph::G aph(s d:: ec o <Ci y> o igin_,
3s d:: ec o <Ci y> ci ies_,
4s d:: ec o <Ci y> des iny_,
5in n_b anches,s d::mu ex &m x, in le el__,in & h eshold){
6
7// Local a iables
8Node n; Elemen e;
9
10 // Id ha de ines "label" in each node/pa h om he g aph
11 global_node_id=1; b anches_id=1;
12
13 // Fill p i a e ec o o "ci ies"
14 ci ies.push_back(o igin_[0]);
15 o (in i=0;i<ci ies_.size();i++){ ci ies.push_back(ci ies_[i]); }
16 ci ies.push_back(des iny_[0]);
17
18 // C ea e i s node in DATABASE
19 n. ill(ci ies[0],global_node_id,1);
20 nodes.push_back(n);
21
22 //"T a ele " and "b eak_down" poin o he i s node
23 a ele = &nodes[0];
24 b eak_down = a ele ;
25
26 // C ea e i s BRANCH wi h i s ELEMENT (node; * a el)
27 e=* a ele ;
28 b anches. esize(b anches_id);
29 b anches[b anches_id-1].add_elemen (e);
30
31 // Algo i hm Code
32 main_algo i hm( n_b anches,m x, le el__, h eshold);
33
34 };
Código A.34 Clase G aph: explo e-and-o de .
1
2s d::pai <s d::lis <Node>,s d::lis <Pa h>> G aph::explo e_and_o de (){
3s d::pai <s d::lis <Node>,s d::lis <Pa h>> es; // Me hod ou pu (Nex (* a ele )
's pa hs and nodes)
4s d:: ec o <Node> ou e=ge _ ou e(* a ele ); // Elemen s ou e un il eaching
a he 's node
5s d::lis <Ci y> lis _o _ci ies; s d::lis <Node> nodes_c ea ed, nodes_ igh ; //
DISORDERED / ORDERED by dis ance
6Node n; Pa h p; s d::lis <Pa h> pa hs_c ea ed, pa hs_ igh ; // DISORDERED /
ORDERED by dis ance
7in id_ a iable, node_label = global_node_id; // The second label is he p i a e
one (global_node_id)
8B anch b; in ci ies_conside ed; // Local a iable o lambda exp ession & ci ies
conside ed in he b eak down
9
10 // De ining "ci ies" in a lis (excep DESTINY)
11 i (( a ele ->le el)==(ci ies.size()-1)){ci ies_conside ed=ci ies.size();} // Only
DESTINY is missed in g aph
12 else{ci ies_conside ed=(ci ies.size()-1);} // P e ious a els han he DESTINY
one.
70 Capí ulo A. Anexo I: Código empleado
13 o (in i=0;i<ci ies_conside ed;i++){ lis _o _ci ies.push_back(ci ies[i]); } //
Filling lis _o _ci ies
14
15 // Remo ing ci ies om he "lis _o _ci ies" (belonging o he " ou e")
16 o (in i=0;i<( ou e.size());i++){
17 lis _o _ci ies. emo e( ou e[i].ci y);}
18
19
20 // C ea ing bo h "nodes" and "pa hs" o he ci ies emaining in he "
lis _o _ci ies"
21 o (au o i =lis _o _ci ies.begin();i !=lis _o _ci ies.end();++i ){
22
23 // Label o sons
24 node_label++;
25
26 // Nodes & Pa hs C ea ed; DISORDERED
27 n. ill(*i ,node_label,( a ele ->le el)+1);
28 p. ill( a ele ->label,node_label, alse,dis ance_be ween_nodes((* a ele ),n));
29 nodes_c ea ed.push_back(n);pa hs_c ea ed.push_back(p);}
30
31
32 // Pu ing he DISORDERED da a in he ORDERED da a
33 while(!pa hs_c ea ed.emp y() && !nodes_c ea ed.emp y()){
34
35 // Sea ch he pa h wi h minimum dis ance (s o e in *pa h_i )
36 au o pa h_i = ind_i (pa hs_c ea ed.begin(), pa hs_c ea ed.end(), [&
pa hs_c ea ed] (cons Pa h& s)
37 {au o i =s d::min_elemen (pa hs_c ea ed.begin(), pa hs_c ea ed.end());
38 e u n ((s.dis ance)==(i ->dis ance));} );
39
40 // Sea ch he node wi h he ci y associa ed wi h (*pa h_i )
41 au o node_i = ind_i (nodes_c ea ed.begin(), nodes_c ea ed.end(), [pa h_i
] (cons Node& s)
42 { e u n ((s.label)==(pa h_i ->des iny_id));} );
43
44 // Filling "node_ igh /pa h_ igh " , wi h o de ed da a (ids-dis ance
ela ion).
45 global_node_id++;
46 n. ill(node_i ->ci y,global_node_id,( a ele ->le el)+1);
47 p. ill( a ele ->label,global_node_id, alse,pa h_i ->dis ance);
48 nodes_ igh .push_back(n);pa hs_ igh .push_back(p);
49 pa hs_c ea ed. emo e(*pa h_i ); nodes_c ea ed. emo e(*node_i ); // Remo e
om DISORDERED lis s and go on
50 }
51
52 // Re u n nodes and pa hs o de ed (_ igh )
53 es. i s =nodes_ igh ; es.second=pa hs_ igh ;
54 e u n es;
55
56 }
Código A.35 Clase G aph: dis ance-be ween-nodes.
1
2 loa G aph::dis ance_be ween_nodes(Node n1, Node n2){
3 loa accum=0;
4
5// Ci ies om bo h nodes wi h same leng h (supposed)
6 o (in i=0;i<n1.ci y.ci y.size();i++){
A.7 Mé odos implemen ados en el algo i mo 71
7accum=accum+pow((n1.ci y.ci y[i]-n2.ci y.ci y[i]),2);}
8 e u n s d::sq (accum);}
Código A.36 Clase G aph: a el-nea es -ci y.
1
2 oid G aph:: a el_nea es _ci y(s d::pai <s d::lis <Node>,s d::lis <Pa h>>
elemen s){
3// Inse s PATH and NODE in he b anch
4s d::lis <Node> n=elemen s. i s ; s d::lis <Pa h> p=elemen s.second;
5Pa h * pa h_poin e ; Elemen * e;
6
7//(Add (* a ele ) o he b anch
8e=&(* a ele ); b anches[b anches_id-1].add_elemen (*e);
9
10 // Sea ch o MIN PATH below he a ele
11 au o p_i = ind_i (p.begin(), p.end(), [&p] (cons Pa h& s)
12 {au o i =s d::min_elemen (p.begin(), p.end());
13 e u n ((s.dis ance)==(i ->dis ance));} );
14
15 // Add MIN PATH o he b anch
16 e=&(*p_i ); b anches[b anches_id-1].add_elemen (*e);
17
18 // Sea ch he node wi h he ci y associa ed wi h (*p_i )
19 au o n_i = ind_i (n.begin(), n.end(), [p_i ] (cons Node& s)
20 { e u n ((s.label)==(p_i ->des iny_id));} );
21
22 // (* a ele ) a els
23 // (USE "NODES" TO CONSERVE ADDRESS WHEN THIS METHOD ENDS)
24 o (in i=0;i<nodes.size();i++){
25 i (nodes[i]==(*n_i )){ a ele = &nodes[i];b eak;}}
26
27 // Add NODE o he b anch
28 e=&(* a ele ); b anches[b anches_id-1].add_elemen (*e);
29
30 }
Código A.37 Clase G aph: e esh-g aph-da a.
1
2 oid G aph:: e esh_g aph_da a(Elemen e, Node n,Node sa e_ a ele ,in &mo e_bd,s d
:: ec o < loa > &dis ances, in & h eshold ){
3
4// Re esh he (*b eak_down) poin e , and e ase he las b anch c ea ed
5// (because is emp y and useless)
6i (mo e_bd==1){ mo e_b eak_down();
7b anches.e ase(b anches.end() - 1);
8dis ances.e ase(dis ances.end() - 1);
9b anches_id--;
10 mo e_bd=0; }
11
12 // Re esh (* a ele ) and sa e i s add ess
13 a ele =b eak_down;
14 sa e_ a ele =* a ele ;
15 e=* a ele ;
16
72 Capí ulo A. Anexo I: Código empleado
17 // C ea e ano he b anch
18 b anches_id++;
19 b anches. esize(b anches_id);
20 b anches[b anches_id-1].add_elemen (e);
21
22 // Clea NODE and PATH da abases
23 nodes.clea (); pa hs.clea ();
24
25 // C ea e i s node in DATABASE
26 n. ill(sa e_ a ele .ci y,sa e_ a ele .label,sa e_ a ele .le el);
27 nodes.push_back(n);
28
29 // "T a ele " and "b eak_down" poin o he i s node in DATABASE
30 a ele = &nodes[0];
31 b eak_down = a ele ;
32
33 }
Código A.38 Clase G aph: emo e-b anches-elemen s.
1
2s d::pai <s d::lis <Node>,s d::lis <Pa h>> G aph:: emo e_b anches_elemen s(s d
::pai <s d::lis <Node>,s d::lis <Pa h>> elemen s){
3s d::lis <Node> nn=elemen s. i s ; s d::lis <Pa h> pp=elemen s.second;
4s d::pai <s d::lis <Node>,s d::lis <Pa h>> e _elemen s=elemen s;
5s d:: ec o <B anch> b=b anches;
6
7s d::lis <Node>::i e a o i _n;
8s d::lis <Pa h>::i e a o i _p;
9
10 // REMOVE NODES om OUTPUT VALUE ( e _elemen s)
11 o (in j=0;j<b.size();j++){
12 au o n_i = ind_i (nn.begin(), nn.end(), [&b,j] (cons Node& s)
13 { e u n b[j].sea ch_node(s);} );
14 i (n_i !=nn.end()){
15 e _elemen s. i s . emo e(*n_i );}}
16
17 // REMOVE PATHS om OUTPUT VALUE ( e _elemen s)
18 o (in j=0;j<b.size();j++){
19 au o p_i = ind_i (pp.begin(), pp.end(), [&b,j] (cons Pa h& s)
20 { e u n b[j].sea ch_pa h(s);} );
21 i (p_i !=pp.end()){
22 e _elemen s.second. emo e(*p_i );}}
23
24 // UPDATE NODES DATABASE
25 i _n = e _elemen s. i s .begin();
26 while (i _n!= e _elemen s. i s .end()){
27 nodes.push_back(*i _n);i _n++;}
28
29 // UPDATE PATHS DATABASE
30 i _p = e _elemen s.second.begin();
31 while (i _p!= e _elemen s.second.end()){
32 pa hs.push_back(*i _p); i _p++;}
33
34 e u n e _elemen s;
35 }

A.7 Mé odos implemen ados en el algo i mo 73
Código A.39 Clase G aph: b eak-down-son.
1
2in G aph::b eak_down_son(){
3s d::deque<Elemen > e; Node *bd=b eak_down;
4
5
6// Sea ch BREAK_DOWN in b anches
7 o (in j=0;j<b anches.size();j++){
8i (b anches[j].sea ch_node(*b eak_down)== ue && b anches[j].elemen s.size()
>1){
9e=b anches[j].elemen s;
10 au o e_i = ind_i (e.begin(), e.end(), [&bd,&e] (cons Elemen & s)
11 { e u n bd->label==s.label;} ); // Sea ch (*b eak_down) ID
12 i (e_i !=e.end() && e_i !=(e.end()-1)){ e u n ((e_i +2)->label)-1;} //
Re u n (*b eak_down) son's minimum ID
13 }
14 }
15 e u n -1; // I I don ind (*b eak_down)
16 }
Código A.40 Clase G aph: c ea e-b anch.
1
2 oid G aph::c ea e_b anch(in &global_node_id_sa ed,in &mo e_bd,in & h eshold){
3s d::pai <s d::lis <Node>,s d::lis <Pa h>> elemen s_c ea ed;
4in a; Pa h p; Node n; Elemen e; Node* a ele _aux;
5bool c;
6
7// (* a ele ) a els in each i e a ion, un il eaching des iny and de ining
b anch
8do{
9
10 // When I use THRESHOLD in MTSP. ADD DESTINY WHEN THIS CONDITION IS SATISFIED.
11 i ( h eshold!=(ci ies.size()) && h eshold== a ele ->le el){
12 global_node_id_sa ed++;
13 a ele _aux= a ele ;
14 n. ill(ci ies[ci ies.size()-1],global_node_id_sa ed,( a ele ->le el)+1);
15 nodes.push_back(n); a ele =&nodes[nodes.size()-1];
16 p. ill( a ele _aux->label, a ele ->label, alse,dis ance_be ween_nodes((*
a ele ),(* a ele _aux)));
17 pa hs.push_back(p);
18 e=p; b anches[b anches_id-1].add_elemen (e);
19 e=n; b anches[b anches_id-1].add_elemen (e);
20
21 c=((b anches[b anches_id-1].elemen s.size()))<
22 ((((2*(ci ies.size()-(b eak_down->le el)))-1)+2)-(2*(ci ies.size()-1-
h eshold)));
23
24 }
25 else{// NORMAL sp (o h eshold==ci es.size())
26
27 // Global id (conside global o (*b eak_down)'s son id)
28 i ( a ele ==b eak_down){global_node_id=b eak_down_son();
29 a=0;
30 i (global_node_id==-1){global_node_id=global_node_id_sa ed;a=1;}}
31 else{global_node_id=global_node_id_sa ed;a=1;}
32
74 Capí ulo A. Anexo I: Código empleado
33 // Conside possible (* a ele )'s sons.
34 elemen s_c ea ed=explo e_and_o de (); // (* a ele )'s sons, o de ed by
dis ance.
35 elemen s_c ea ed= emo e_b anches_elemen s(elemen s_c ea ed); // emo e
elemen s om p e ious b anches.
36 i (elemen s_c ea ed. i s .size()==0 && elemen s_c ea ed.second.size()==0){
// All elemen s emo ed
37 mo e_bd=1; b eak;} // (EXIT FROM DO-WHILE) (*b eak_down) goes o he nex
global id
38 a el_nea es _ci y(elemen s_c ea ed); // (* a ele ) a els o he nex
ci y
39 i (a==1){global_node_id_sa ed=global_node_id;} // Re esh global id
40
41 c=((b anches[b anches_id-1].elemen s.size()))<((((2*(ci ies.size()-(
b eak_down->le el)))-1)+2));
42 }
43
44 }while(c);
45 // Do un il a ele eaches des iny (WHILE CONDITION BASED ON NUMBER OF LEVELS
FROM GRAPH)
46 }
Código A.41 Clase G aph: mo e-b eak-down.
1
2 oid G aph::mo e_b eak_down(){
3s d::deque<Elemen > e; Node *bd=b eak_down, nn;
4
5
6// Sea ch BREAK_DOWN in b anches
7 o (in j=0;j<b anches.size();j++){
8e=b anches[j].elemen s;
9au o e_i = ind_i (e.begin(), e.end(), [&bd,&e] (cons Elemen & s)
10 { e u n (bd->label)+1==s.label;} );
11 i (e_i !=e.end()){
12 nn.con e _elemen (*e_i );
13 nodes.push_back(nn);
14 b eak_down=&(nodes[nodes.size()-1]);}
15 }
16
17 }
Código A.42 Clase G aph: ge - o al-dis ance.
1
2 loa G aph::ge _ o al_dis ance(in b anch_id_,in & h eshold){
3 loa e _dis ance=0; in b anch_id_aux=b anch_id_; // ID om b anch I am
conside ing
4s d:: ec o <B anch> b=b anches; bool mino ,equal;
5
6// Condi ion o se ing b anches esize
7i ( h eshold!=(ci ies.size())){ // THRESHOLD FOR MTSP
8mino =((b anches[b anches_id-1].elemen s.size()))<((((2*(ci ies.size()-1))-1)
+2)-(2*(ci ies.size()-1- h eshold)));
9equal=((b anches[b anches_id-1].elemen s.size()))==((((2*(ci ies.size()-1))
-1)+2)-(2*(ci ies.size()-1- h eshold)));
A.7 Mé odos implemen ados en el algo i mo 75
10 }
11 else{// USING NORMAL TSP
12 mino =((b anches[b anches_id-1].elemen s.size()))<(((2*(ci ies.size()-1))-1)
+2);
13 equal=((b anches[b anches_id-1].elemen s.size()))==(((2*(ci ies.size()-1))-1)
+2);}
14
15
16 // BRANCH WITH ELEMENTS
17 i (b anches[b anch_id_-1].elemen s.size()>0){
18 // B anch SMALLER han he one ha s a s in ORIGIN and eaches DESTINY
19 i (mino ){
20
21 // DISTANCE OF THE CURRENT LITTLE BRANCH
22 e _dis ance= e _dis ance+b anches[b anch_id_aux-1].ge _ o al_dis ance();
23
24 // Add li le b anches un il he ORIGIN (o igin_id=1)
25 o ( loa i=(b.size()-1);i>-1;i--){ // SMALL TO LARGER ONES (I mean
BRANCHES)
26 o ( loa j=(b[i].elemen s.size()-1);j>-1;j--){ // HIGHER TO LOWER LEVELS
27 // Sea ch o li le b anches ha end wi h he same node han "
b anch_id_aux" (and connec )
28 i (b[i].elemen s[j].des iny_id==b anches[b anch_id_aux-1].elemen s[1].
o igin_id){
29 e _dis ance= e _dis ance+b[i].ge _ o al_dis ance_des iny_id(b[i].
elemen s[j].des iny_id);
30 i (b[i].elemen s[1].o igin_id==1){ e u n e _dis ance;}
31 else{b anch_id_aux=i+1;} }
32 }} }
33
34 // B anch ha s a s in ORIGIN and eaches DESTINY
35 else i (equal){
36 e u n b anches[b anch_id_-1].ge _ o al_dis ance();}
37
38 }
39
40 // BRANCH WITHOUT ELEMENTS (I won' be he case)
41 else{s d::cou <<"B anch " << b anch_id_ <<" doesn' ha e elemen s. Re u n 0 as
dis ance." << s d::endl<< s d::endl;
42 e u n 0;}
43
44
45 }
Código A.43 Clase G aph: ge - ou e.
1
2s d:: ec o <Node> G aph::ge _ ou e(Node n){
3s d:: ec o <s d:: ec o <Node>> a ; s d:: ec o <Node> ou _; Node nn=n;
4
5do{
6 o (in i=0;i<b anches.size();i++){ // Sea ch all b anches
7i (b anches[i].sea ch_node(nn)==1){
8 a .push_back(b anches[i].ge _ ou e(nn)); // Sea ch b anch wi h (* a ele )
9nn= a [ a .size()-1][0];b eak;}} // Re esh node I wan o sea ch...
10 }while( a [ a .size()-1][0].le el>1); // ... un il I each op node (FATHER)
11
12 // OUTPUT NODE VECTOR
13 o ( loa i=( a .size()-1);i>-1;i--){
76 Capí ulo A. Anexo I: Código empleado
14 o (au o j=0;j<( a [i].size());j++){
15
16 // Pu in "ou _" all nodes om ou e s o ed in " a "
17 // (wi hou epea ing nodes alues wice)
18 i (j==0 && i==( a .size()-1)){ou _.push_back( a [i][j]);}
19 else i (i>0 && j==( a [i].size()-1)){}
20 else i (i==0 && j==( a [i].size()-1)){ ou _.push_back( a [i][j]);}
21 else{ ou _.push_back( a [i][j]);}
22 }}
23
24 e u n ou _; // Re u n ou e ("ou _")
25 }
Código A.44 Clase G aph: main-algo i hm.
1
2 oid G aph::main_algo i hm(in n_b anches,s d::mu ex &m x, in le el__,in &
h eshold){
3Node n, sa e_ a ele =* a ele ; in ac o ial_ alue;
4Elemen e; in global_node_id_sa ed=global_node_id, mo e_bd=0;
5
6le el_=le el__; // Filling p i a e a iable
7
8n_b anches++;
9n_b anches_p i a e=n_b anches;
10
11 // S a execu ion ime
12 begin = os::Time::now();
13
14 // Calcula e ac o ial (numbe o possible solu ions o he g aph)
15 ac o ial_ alue=(ci ies.size()-2);
16 o (in i = ac o ial_ alue; i>0; i--){ ac o ial *= i;
17 i ((( ac o ial_ alue-i)+2)== h eshold){b eak;}}
18
19 // C ea e a single b anch o he g aph
20 do{ c ea e_b anch(global_node_id_sa ed,mo e_bd, h eshold); // C ea es he b anch
21 dis ances.push_back(ge _ o al_dis ance(b anches_id, h eshold)); // Vec o o
g aph b anches'dis ances
22 // p in _da a_base(); // See da abase (PATHS AND NODES) i needed o debug
23 e esh_g aph_da a(e,n,sa e_ a ele ,mo e_bd,dis ances, h eshold); // Re eshes
(*b eak_down) and (* a ele )
24
25 }while(b anches.size()<(n_b anches) && (b anches.size()< ac o ial+1)); // Limi
o solu ions calcula ed
26
27 //s d::cou << "ENTRO EN G aph::main_algo i hm" << s d::endl;
28
29
30 // Minimum dis ance
31 au o _i =s d::min_elemen (dis ances.begin(), dis ances.end());
32
33 // Ge ing ou e o ci ies
34 n.con e _elemen (b anches[ _i -dis ances.begin()].elemen s[b anches[ _i -
dis ances.begin()].elemen s.size()-1]);
35 ou e_ou =ge _ ou e(n);
36
37 }
A.7 Mé odos implemen ados en el algo i mo 83
74 p e ious_ci ies_ids.push_back( e u n_ci ies_ids[i]);}
75
76 s d::cou << "(GO ON) MAX ID VALUE ("<< i _dis-p e ious_dis ances.begin()<<")
: "<< max_ alue << s d::endl;
77
78 // Imp o emen s TSP
79 i _min=s d::min_elemen (min_dis ances.begin(), min_dis ances.end());
80 i _max=s d::max_elemen (min_dis ances.begin(), min_dis ances.end());
81 z_min=(i _min-min_dis ances.begin());
82 z_max=(i _max-min_dis ances.begin());
83 ci ies_ o_ a el[z_min]++;
84 ci ies_ o_ a el[z_max]--;
85
86 }// IF his condi ion ge s sa is ied, go on wi h i e a ions in "sol e_only_aux"
87 else{end=1;
88 s d::cou << "(FINISHED) MAX ID VALUE: "<< max_ alue << s d::endl;}
89
90
91
92 }// END WHILE
93
94 s d::cou << s d::endl<< s d::endl<< s d::endl<< s d::endl;
95 s d::cou << "p e ious_ci ies_ids DATA: "<< s d::endl;
96
97 o (in i=0;i<p e ious_ci ies_ids.size();i++){
98 o (in j=0;j<p e ious_ci ies_ids[i].size();j++){
99 s d::cou << p e ious_ci ies_ids[i][j] << s d::endl;}s d::cou << s d::endl;}
100
101 };
Código A.54 Clase M sp: sol e- sp.
1
2 oid M sp::sol e_ sp(s d:: ec o <s d:: ec o < loa >> &o igin,
3s d:: ec o <s d:: ec o < loa >> &ci ies,
4s d:: ec o <s d:: ec o < loa >> &des iny,
5in sub_b anch_id,in i e a ions,
6s d:: ec o <in > &aux_ids,
7s d:: ec o <s d:: ec o <in >> & es_ids,
8s d::mu ex &m x,
9s d:: ec o <Ci y> &c,
10 s d:: ec o <s d:: ec o <in >> & e u n_ci ies_ids,
11 s d:: ec o <in > &ci ies_ o_ a el,
12 s d::lis <in > &aux_ids_lis ){
13
14 s d::lis <in >::i e a o i _lis ;
15 s d:: ec o <WholeG aph> sp;
16 sp. esize(des iny.size());
17
18
19
20 // Sol ing TSPs
21 o (in z=0;z<des iny.size();z++){
22
23 // z a ele conside s all AUX ci ies
24 sp[z].add_aux_ci ies(c,aux_ids);
25
26 // ADDING RES CITIES TO TRAVELERS (DIRECTLY)
27 i ( es_ids.size()>0){

84 Capí ulo A. Anexo I: Código empleado
28 sp[z].add_ es_ci ies(c, es_ids[z]); }
29
30 // z a ele
31 i ( es_ids.size()>0){ // AUX AND RES CITIES
32
33 e u n_ci ies_ids[z]= sp[z].main_algo i hm(o igin,des iny[z],
34 ci ies.size(),sub_b anch_id,i e a ions,m x,ci ies_ o_ a el[z
]+ es_ids[z].size());
35 }
36 else{// ONLY AUX CITIES
37 e u n_ci ies_ids[z]= sp[z].main_algo i hm(o igin,des iny[z],
38 ci ies.size(),sub_b anch_id,i e a ions,m x,ci ies_ o_ a el
[z]);}
39
40
41 // Remo e " e u n_ci ies_ids[z]" om "aux_ids"
42 // Copy IDs o a lis (in o de o emo e la e )
43 o (in i=0;i<aux_ids.size();i++){
44 aux_ids_lis .push_back(aux_ids[i]);}
45 // Remo e om lis ids s o ed in " e u n_ci ies_ids[0]"
46 o (in i=0;i< e u n_ci ies_ids[z].size();i++){
47 aux_ids_lis . emo e( e u n_ci ies_ids[z][i]);}
48 // Pu in ec o alues om lis again
49 i _lis =aux_ids_lis .begin();
50 aux_ids.clea ();
51 o (in i=0;i<aux_ids_lis .size();i++){
52 aux_ids.push_back(*i _lis );
53 s d::ad ance(i _lis , 1);}
54
55 // See "min_dis ance" ou pu
56 min_dis ances.push_back( sp[z].min_dis ance);
57 // s d::cou << " MIN DISTANCE "<< z <<": " << min_dis ances[z] << s d::endl;
58
59 }
60
61 }
Simulación en Gazebo
Código A.55 Ciudades especi icadas pa a Gazebo..
1
2// Fo MTSP
3// -----------------------------------------------------------
4// De ining ec o s wi h CITIES, ORIGIN and DESTINY
5s d:: ec o <s d:: ec o < loa >> o igin, des iny;
6s d:: ec o <s d:: ec o < loa >> ci ies;
7s d:: ec o <s d:: ec o <in >> de _ ack_poin s;
8
9o igin.push_back({0,1.1}); // O igin Ci y 1
10
11 // 4 In e media e Ci ies
12 ci ies.push_back({1,2.3}); // 2
13 ci ies.push_back({3,2}); // 3
14 ci ies.push_back({2,-8.4}); // 4
15 ci ies.push_back({7,-9}); // 5
16 ci ies.push_back({-2,9}); // 6
17 ci ies.push_back({-7,2}); // 7
A.8 Simulación en Gazebo 85
18 ci ies.push_back({-7,9}); // 8
19
20 des iny.push_back({5,5}); // Des iny Ci y 1
21
22 // -----------------------------------------------------
23
24 M sp m sp;
25 s d:: ec o <s d:: ec o <in >> es_ids ={{2,3,4,5}} ;
26 m sp.sol e_only_ es(o igin,ci ies,des iny,WHOLE_GRAPH,10, es_ids,m x);
27 de _ ack_poin s=m sp. e u n_ci _ids();
Código A.56 Asignación de coo denadas de las ciudades.
1
2// Loop o adding MTSP ack poin s
3 o (in i=0;i<de _ ack_poin s[0].size()+2;i++){
4g c::ual::Waypoin waypoin ;
5waypoin .heade . ame_id = "map";
6i (i==0){ // ORIGIN
7waypoin .pose.posi ion.x = o igin[0][0];
8waypoin .pose.posi ion.y = o igin[0][1];
9}
10 else i (i==(de _ ack_poin s[0].size()+1)){ // DESTINY
11 waypoin .pose.posi ion.x = des iny[0][0];
12 waypoin .pose.posi ion.y = des iny[0][1];
13 }
14 else i (i>0 && i<(de _ ack_poin s[0].size()+1)){ // CITIES
15 waypoin .pose.posi ion.x = ci ies[de _ ack_poin s[0][i-1]-2][0];
16 waypoin .pose.posi ion.y = ci ies[de _ ack_poin s[0][i-1]-2][1];
17 }
18 waypoin .pose.posi ion.z = ligh _le el; // ligh _le el;
19 waypoin .pose.o ien a ion.x = 0;
20 waypoin .pose.o ien a ion.y = 0;
21 waypoin .pose.o ien a ion.z = 0;
22 waypoin .pose.o ien a ion.w = 1;
23 pa h.push_back(waypoin );}
Código A.57 Sección de código enca gada del mo imien o del UAV.
1
2s d::cou << "Blocking e sion o goToWaypoin " << s d::endl;
3 o (au o p : pa h) {
4s d::cou << "Waypoin : " << p.pose.posi ion.x << ","<<
5p.pose.posi ion.y << ","<< p.pose.posi ion.z << ", ame_id: " << p.
heade . ame_id << s d::endl;
6ual.goToWaypoin (p);
7s d::cou << "A i ed!" << s d::endl;
8}
9
10 // Land
11 ual.land();
Índice de Figu as
2.1 G a o de es ado pa a es ciudades in e medias 6
2.2 G a o de es ado pa a es ciudades in e medias; ciudades 6
2.3 Posible solución del g a o pa a 3 ciudades in e medias 7
2.4 Algo i mo en clases: símil con un á bol 8
2.5 Diag ama UML: Clase Ci y 9
2.6 Diag ama UML: Clase Node 10
2.7 Diag ama UML: Clase Pa h 10
2.8 Diag ama UML: Clase B anch 11
2.9 Elemen os de la ama. En ojo los nodos. En azul los caminos 13
2.10 Diag ama UML: He encia de la clase Elemen 14
2.11 Algo i mo en clases: símil con un á bol 17
4.1 Resul ado en RVIZ con es iaje os 24
4.2 Coo denadas de las ciudades. Resul ados RVIZ 24
5.1 TSP: un solo hilo 28
5.2 Solución del TSP en el caso de un solo hilo 28
5.3 TSP: cua o hilos 29
5.4 Solución del TSP en el caso mul ihilo 30
5.5 MTSP: p ime a i e ación pa a ciudades auxilia es 31
5.6 MTSP: esul ados de p ime a i e ación (ciudades auxilia es) 31
5.7 MTSP: segunda i e ación pa a ciudades auxilia es 31
5.8 MTSP: esul ados de segunda i e ación (ciudades auxilia es) 31
5.9 MTSP: esul ados globales (ciudades auxilia es) 32
5.10 MTSP: esul ados con ciudades es ingidas 32
5.11 MTSP: p ime a i e ación pa a ciudades auxilia es y es ingidas 33
5.12 MTSP: esul ados de p ime a i e ación (ciudades auxilia es y es ingidas) 33
5.13 MTSP: segunda i e ación pa a ciudades auxilia es y es ingidas 33
5.14 MTSP: esul ados de segunda i e ación (ciudades auxilia es y es ingidas) 33
5.15 MTSP: esul ados globales (ciudades auxilia es y es ingidas) 34
6.1 Sub amas en el caso de es ciudades in e medias 39
6.2 Nume ación de amas: iaje o y desglose en dis in os nodos 40
6.3 Nume ación de amas: iaje o y desglose en el mismo nodo 41
6.4 Elemen os de una ama comple a del g a o 42
6.5 G a o del iaje o comple o 44
6.6 Di isión en sub amas del g a o del iaje o 44
7.1 Simulación en Gazebo: solución del TSP 48
7.2 Simulación en Gazebo: p ime a pa e del eco ido 49
7.3 Simulación en Gazebo: segunda pa e del eco ido y a e izaje 49
87

Índice de Códigos
7.1 Modi icación en el CMakeLis s 47
7.2 Compilación de los paque es eque idos 47
A.1 Decla ación de clase Ci y 55
A.2 Decla ación de clase Node 55
A.3 Decla ación de clase Pa h 56
A.4 Decla ación de clase B anch 56
A.5 Decla ación de clase Elemen 56
A.6 Decla ación de clase G aph 57
A.7 Decla ación de clase WholeG aph 58
A.8 Decla ación de clase M sp 58
A.9 Fiche o p incipal: codigo-p incipal.cpp 59
A.10 Ciudades conside adas en la isualización de RVIZ 60
A.11 Código pa a esol e el P oblema del Viaje o solo con ciudades es ingidas 61
A.12 Código RVIZ 61
A.13 Ciudades in e medias conside adas en TSP (main) 64
A.14 Código de un hilo pa a TSP 64
A.15 Ejecución en main pa a un hilo (TSP) 64
A.16 Ejecución en main pa a cua o hilos (TSP) 64
A.17 Ciudades in e medias conside adas en MTSP (main) 65
A.18 Código de un hilo pa a MTSP: ciudades auxilia es 65
A.19 Código de un hilo pa a MTSP: ciudades es ingidas 65
A.20 Código de un hilo pa a MTSP: ciudades es ingidas y auxilia es 66
A.21 Clase Ci y: ill 66
A.22 Clase Ci y: ge -s ing-da a 66
A.23 Clase Node: ill 67
A.24 Clase Node: con e -elemen 67
A.25 Clase Pa h: ill 67
A.26 Clase B anch: add-elemen 67
A.27 Clase B anch: ge - o al-dis ance 67
A.28 Clase B anch: ge - o al-dis ance-des iny-id 68
A.29 Clase B anch: ge -ci ies 68
A.30 Clase B anch: sea ch-node 68
A.31 Clase B anch: sea ch-pa h 68
A.32 Clase B anch: ge - ou e 68
A.33 Clase G aph: G aph (cons uc o ) 69
A.34 Clase G aph: explo e-and-o de 69
A.35 Clase G aph: dis ance-be ween-nodes 70
A.36 Clase G aph: a el-nea es -ci y 71
A.37 Clase G aph: e esh-g aph-da a 71
A.38 Clase G aph: emo e-b anches-elemen s 72
89
90 Índice de Códigos
A.39 Clase G aph: b eak-down-son 73
A.40 Clase G aph: c ea e-b anch 73
A.41 Clase G aph: mo e-b eak-down 74
A.42 Clase G aph: ge - o al-dis ance 74
A.43 Clase G aph: ge - ou e 75
A.44 Clase G aph: main-algo i hm 76
A.45 Clase G aph: ou e-ou pu 77
A.46 Clase G aph: min-dis ance-ou pu 77
A.47 Clase WholeG aph: ou pu -whole-g aph 77
A.48 Clase WholeG aph: main-algo i hm 77
A.49 Clase WholeG aph: add-aux-ci ies 78
A.50 Clase WholeG aph: add- es-ci ies 79
A.51 Clase M sp: sol e-only- es 79
A.52 Clase M sp: sol e-only-aux 80
A.53 Clase M sp: sol e-aux-and- es 81
A.54 Clase M sp: sol e- sp 83
A.55 Ciudades especi icadas pa a Gazebo. 84
A.56 Asignación de coo denadas de las ciudades 85
A.57 Sección de código enca gada del mo imien o del UAV 85
Bibliog a ía
[1]
M. Do igo and L. M. Gamba della, “An colony sys em: a coope a i e lea ning app oach o he a eling
salesman p oblem,” IEEE T ansac ions on e olu iona y compu a ion, ol. 1, no. 1, pp. 53–66, 1997.
[2]
S. Lin, “Compu e solu ions o he a eling salesman p oblem,” Bell Sys em Technical Jou nal, ol. 44,
no. 10, pp. 2245–2269, 1965.
[3]
C. C. Mu ay and A. G. Chu, “The lying sidekick a eling salesman p oblem: Op imiza ion o d one-
assis ed pa cel deli e y,” T anspo a ion Resea ch Pa C: Eme ging Technologies, ol. 54, pp. 86–109,
2015.
[4]
E. Semsch, M. Jakob, D. Pa licek, and M. Pechoucek, “Au onomous ua su eillance in complex u ban
en i onmen s,” in P oceedings o he 2009 IEEE/WIC/ACM In e na ional Join Con e ence on Web
In elligence and In elligen Agen Technology-Volume 02. IEEE Compu e Socie y, 2009, pp. 82–85.
[5]
C.-W. Lim, S. Pa k, C.-K. Ryoo, K. Choi, and J.-H. Cho, “A pa h planning algo i hm o su eillance
ua s wi h iming mission cons ains,” in ICCAS 2010. IEEE, 2010, pp. 2371–2375.
[6]
J. Isaacs and J. Hespanha, “Dubins a eling salesman p oblem wi h neighbo hoods: A g aph-based
app oach,” Algo i hms, ol. 6, no. 1, pp. 84–99, 2013.
[7]
J. Faigl and P. Váňa, “Unsupe ised lea ning o su eillance planning wi h eam o ae ial ehicles,” in
2017 In e na ional Join Con e ence on Neu al Ne wo ks (IJCNN). IEEE, 2017, pp. 4340–4347.
[8]
K. Obe meye , “Pa h planning o a ua pe o ming econnaissance o s a ic g ound a ge s in e ain,”
in AIAA Guidance, Na iga ion, and Con ol Con e ence, 2009, p. 5888.
[9]
X. Zhang, J. Chen, B. Xin, and Z. Peng, “A meme ic algo i hm o pa h planning o cu a u e-cons ained
ua s pe o ming su eillance o mul iple g ound a ge s,” Chinese Jou nal o Ae onau ics, ol. 27, no. 3,
pp. 622–633, 2014.
[10]
C. Lim, C. Ryoo, K. Choi, and J.-H. Cho, “Pa h gene a ion algo i hm o in elligence, su eillance
and econnaissance o an ua ,” in P oceedings o SICE Annual Con e ence 2010. IEEE, 2010, pp.
1274–1277.
[11]
S. Ra hinam and R. Sengup a, “Lowe and uppe bounds o a mul iple depo ua ou ing p oblem,” in
P oceedings o he 45 h IEEE Con e ence on Decision and Con ol. IEEE, 2006, pp. 5287–5292.
[12]
D. L. Applega e, R. E. Bixby, V. Ch a al, and W. J. Cook, The a eling salesman p oblem: a compu-
a ional s udy. P ince on uni e si y p ess, 2006.
[13]
S. Kuske, M. Gogolla, R. Kollmann, and H.-J. K eowski, “An in eg a ed seman ics o uml class, objec
and s a e diag ams based on g aph ans o ma ion,” in In e na ional Con e ence on In eg a ed Fo mal
Me hods. Sp inge , 2002, pp. 11–28.
[14]
A. Snyde , “Encapsula ion and inhe i ance in objec -o ien ed p og amming languages,” in ACM Sigplan
No ices, ol. 21, no. 11. ACM, 1986, pp. 38–45.
91
92 Bibliog a ía
[15]
P. R. S. J. C. F an Real, A u o To es-Gonzalez and A. Olle o, “Ual: an abs ac ion laye o unmanned
ehicles,” in 2nd In e na ional Symposium on Ae ial Robo ics (ISAR), 2018.