Wiki d'activités IMA - Contributions de l’utilisateur [fr]

Projets IMA5 2015/2016

2016-02-27T16:47:44Z

Tdanel : /* Répartition des binômes */

Merci de référencer vos pages de projets ici. Merci aussi d'uniformiser vos formats que ce soit en regardant la présentation des projets déjà créés ou en allant modifier le format des précédents si votre façon de faire vous semble la meilleure. Dans tous les cas un minimum de communication entre les binômes est conseillée.

Toutes les sources doivent être déposées sur notre archive GIT. Le service est disponible à l'URL [https://archives.plil.fr archives.plil.fr]. Connectez-vous avec vos identifiants Polytech'Lille. Sauf indication contraire de vos encadrants, rendez le projet public et mettez le lien sur votre Wiki. Vous pouvez trouver de la documentation sur ce système d'archives sur ce [https://git-scm.com/book/fr/v1 site].

== Répartition des binômes ==

<table border="1">
<tr>
<th>Projet</th>
<th>Elèves</th>
<th>Encadrant Ecole</th>
<th>Rapport décembre</th>
<th>Rapports finaux</th>
<th>Vidéo</th>
</tr>
<tr>
<td>[[P1 Convertisseur DC/DC pour réseau MTDC]]</td>
<td> Mehmet Ilter </td>
<td> Philippe DELARUE </td>
<td> </td>
<td> </td>
<td> </td>
</tr>
<tr>
<td>[[P2 Data Logger]]</td>
<td> Hidéo VINOT</td>
<td> Alexandre Boé / Thomas Vantroys </td>
<td> 15/12/2015 12;00</td>
<td> 23/02/2016 12:18, [[Fichier: Dossier_soutenance_P2_Data_Logger_Hideo_VINOT_IMA5SC_2015.pdf]]</td>
<td> mercredi 24 février à 10h avec L. Engels</td>
</tr>
<tr>
<td>[[P4 Jukebox multi-pièces]]</td>
<td> Alexandre Jouy / Julien hérin </td>
<td> Rodolphe Astori / Xavier Redon / Alexandre Boé / Thomas Vantroys </td>
<td> [[Fichier:Pre_soutenance_PFE_Jouy_herin.pdf]] </td>
<td> [[Fichier:Rapport PFE P4 Julien HERIN.pdf]] [[Fichier:Rapport_PFE_Jouy.zip]]</td>
<td> RDV pris le 23/02 avec L. Engels</td>
</tr>
<tr>
<td>[[P9 Système d'hébergement domestique]]</td>
<td> Romain Libaert / Timothée Teneur </td>
<td> Xavier Redon / Alexandre Boé / Thomas Vantroys </td>
<td> 15/12/2015, 10:21, [[Fichier:P9_LIBAERT_TENEUR_DEC15.pdf]]</td>
<td> [[Fichier:PFE_Final_TENEUR_LIBAERT.pdf]]</td>
<td> Tournée le 22/02 par M. Engels</td>
</tr>
<tr>
<td>[[P10 LILLIAD: Connected Learning Center | P10 LILLIAD: learning center connecté]]</td>
<td> Mageshwaran Sekar </td>
<td> Alexandre Boé / Thomas Vantroys </td>
<td> 15/12/2015,07:48, [[Fichier:P10_FYP_December_Report_M_SEKAR.pdf]]</td>
<td> [[Fichier:FYP-P10-Lilliad-MSEKAR.pdf]]</td>
<td> Fait maison</td>
</tr>
<tr>
<td>[[P11 Spectateur augmenté]]</td>
<td> Teresa Tumbragel </td>
<td> Alexandre Boé / Thomas Vantroys </td>
<td> </td>
<td> [[Fichier:Teresa Tumbragel-Rapport Spectateur Augmente.pdf]] </td>
<td> NA </td>
</tr>
<tr>
<td>[[P12 Capteurs enfouis pour vieillissement du béton]]</td>
<td> JULITA Alex </td>
<td> Alexandre Boé / Thomas Vantroys </td>
<td> </td>
<td> </td>
<td> </td>
</tr>
<tr>
<td>[[P14 Localisation dans le corps humain]]</td>
<td> Matthieu Marcadet </td>
<td> Alexandre Boé / Thomas Vantroys </td>
<td> 15/12/2015, 10:52, [[Fichier:Rapport_intermediaire_PFE_Marcadet.pdf]]</td>
<td> [[Fichier:Rapport_PFE_P14_Matthieu_Marcadet.pdf]]</td>
<td> RDV pris le 24/02 avec L. Engels</td>
</tr>
<tr>
<td>[[P18 Meuble intelligent]]</td>
<td> Kevin Colautti / Benjamin Lefort </td>
<td> Rémy Bernard / Alexandre Boé / Xavier Redon / Thomas Vantroys </td>
<td> 14/12/2015, 14:44, [[Fichier:P18_pre_soutenance.pdf]]</td>
<td> [[Fichier:Rapport_P18_COLAUTTI_LEFORT.pdf]]</td>
<td> Tournée le 22/02 par M. Engels</td>
</tr>
<tr>
<td>[[Automatic Soldering System Project|P20 Placeur de composants sur PCB]]</td>
<td> Jean Wasilewski & Pierre Letousey </td>
<td> Xavier Redon / Thomas Vantroys / Alexandre Boé </td>
<td> [[Fichier:P20_ASSP.pdf]]</td>
<td> [[Fichier:IMA5-ASSP-JWPL.pdf]]</td>
<td> Rendez-vous pris avec M. Engels le vendredi 25 Février à 10h </td>
</tr>
<tr>
<td>[[P24 Nuage pour sites Web]]</td>
<td> Jeremie Denechaud / Thibaut Scholaert </td>
<td> Xavier Redon / Thomas Vantroys </td>
<td> 15/12/2015, 12:02, [[Fichier:P24_Denechaud_Scholaert.pdf| Rapport intermédiaire de projet]]</td>
<td> [[Fichier:PFE_DENECHAUD_SCHOLAERT.pdf]]</td>
<td> Fait Maison</td>
</tr>
<tr>
<td>[[P25 Architecture ROS pour des véhicules autonomes intelligents]]</td>
<td> Jean-Michel Tournier / Cyril Smagghe </td>
<td> Vincent Coelen et Rochdi Merzouki </td>
<td> 15/12/2015,09:04, [[Fichier:P25-2015_Smagghe_Tournier_decembre.pdf]]</td>
<td> [[Fichier:Rapport_final_P25_Smagghe_Tournier.pdf]]</td>
<td>RDV mercredi 24 15h </td>
</tr>
<tr>
<td>[[P26 Robot de forgeage et d’usinage]]</td>
<td> Bertrand Yvernault / Louis Thebault </td>
<td> Rochdi Merzouki </td>
<td> 14/12/2015, 17:51</td>
<td> </td>
<td>Tournée le 22/02 par M. Engels </td>
</tr>
<tr>
<td>[[P27 Robot de fraiseuse]]</td>
<td> Flavien Royer / Maxime Morisse </td>
<td> Rochdi Merzouki </td>
<td> 14/12/2015, 19:06, [[Fichier:Rapport_Royer_Morisse.pdf]]</td>
<td>[[Fichier:RapportPFE_ROYER_MORISSE.pdf]] </td>
<td> mardi 23 février à 11h15 avec Laurent Engels</td>
</tr>

<tr>
<td>[[P30 Thermostat connecté et intelligent]]</td>
<td> TISSOT Elise / TIRABY Céline </td>
<td> Guillaume Renault / Alexandre Boé / Xavier Redon </td>
<td>15/12/2015, 00:35, [[Fichier:Rapport_PFE_TISSOT_TIRABY.pdf‎ ]] </td>
<td> [[Fichier:Rapport_final_PFE_TISSOT_TIRABY.pdf]] </td>
<td> Tournée le 19/02 par L. Engels </td>
</tr>
<tr>
<td>[[P32 Récupération d'énergie pour balise BLE]]</td>
<td> Quentin Sultana </td>
<td> Frédéric Giraud / Alexandre Boé / Thomas Vantroys </td>
<td> 13/12/2015, 19:31, [[Fichier:CR Miprojet_Sultana.pdf]]</td>
<td> [[Fichier:Rapport_P32_Sultana.pdf]]</td>
<td> RDV L. Engels le 24/02 </td>
</tr>
<tr>
<td>[[P33 Réalisations en faveur de l'accessibilité de jeux vidéos]]</td>
<td> Jérôme Bailet / Mehdi Zeggaï </td>
<td> GAPAS / Laurent Grisoni / Thomas Vantroys </td>
<td> 14/12/2015, 10:54, [[Fichier:PFE_IMA5_Rapport_intermediaire_Bailet_Zeggai.pdf]]</td>
<td> [[Fichier:PFE_IMA5_P33_Rapport_final_Bailet_Zeggai.pdf]]</td>
<td> RDV L. Engels le 25/02 </td>
</tr>
<tr>
<td>[[P35 Robot de test pour le sport de Golf]]</td>
<td> Deborah Saunders </td>
<td> Rochdi Merzouki </td>
<td> 15/12/2015, 12:07</td>
<td> </td>
<td> Vu avec M.Engels pour montage des videos avec voix off</td>
</tr>

<tr>
<td>[[P34 Optimisation de trajectoire pour un robot de curiethérapie]]</td>
<td> Sandra HAGE CHEHADE / Thomas DANEL </td>
<td> Vincent COELEN / Rochdi MERZOUKI </td>
<td> 15/12/2015, 12:02</td>
<td> 23/02/2016, 12:36</td>
<td> Tournée le 25/02 avec L. Engels</td>
</tr>

<tr>
<td>[[P40 Maquette mécatronique durcie d'ascenseur 5 étages]]</td>
<td> Louis CHAUCHARD / Romain IMBERT </td>
<td> Blaise CONRARD </td>
<td> 14/12/2015, 19:21</td>
<td> [[Fichier:IMA5_P40_Chauchard_Imbert_RapportFinal.pdf‎]] </td>
<td> RDV L. Engels le 23/02 </td>
</tr>
<tr>
<td>[[P13 Plateforme expérimentation IOT]]</td>
<td> ROCHE François </td>
<td> Alexandre Boé / Thomas Vantroys </td>
<td> 14/12/2015, 19:37, [[Fichier:PFE_P13_Plateforme_expérimentation_IOT.pdf]] </td>
<td> [[Fichier:PFE_P13-Plateforme_expérimentation.pdf]]</td>
<td> Tournée le 22/02 par M. Engels </td>
</tr>
<tr>
<td>[[Implantation d'un filtre FIR-FX-LMS sur FPGA pour l'annulation de Bruit Acoustique]]</td>
<td> Bown Alexander / Piat Valentin </td>
<td> Alexandre Boé / Thomas Vantroys </td>
<td> NA </td>
<td> </td>
<td> </td>
</tr>
<tr>
<td>[[Pilulier automatique]]</td>
<td> Manouk Simon / Corentin Duplouy </td>
<td> Alexandre Boé / Thomas Vantroys </td>
<td> NA </td>
<td> [[Fichier:Rapport Duplouy Simon.pdf]] </td>
<td> RDV L. Engels le 23/02 </td>
</tr>
</table>

P34 Optimisation de trajectoire pour un robot de curiethérapie

2015-09-24T11:56:40Z

Tdanel : Page créée avec « ==Cahier des charges== ===Présentation générale du projet=== ====Contexte==== ====Objectif du projet==== ====Description du projet==== ====Choix techniques : matérie... »

==Cahier des charges==
===Présentation générale du projet===

====Contexte====

====Objectif du projet====

====Description du projet====

====Choix techniques : matériel et logiciel====

==Étapes du projet==

Partie 1 : 

Partie 2 : 

Partie 3 : 

==Suivi de l'avancement du Projet==

===Semaine 1 (21/09/2015)===

===Semaine 2 (28/09/2015)===
===Semaine 3 (05/10/2015)===
===Semaine 4 (12/10/2015)===
===Semaine 5 (19/10/2015)===
===Semaine 6 (26/10/2015)===
===Semaine 7 (02/11/2015)===
===Semaine 8 (09/11/2015)===
===Semaine 9 (16/11/2015)===
===Semaine 10 (23/11/2015)===
===Semaine 11 (30/11/2015)===
===Semaine 12 (07/12/2015)===
===Semaine 13 (14/12/2015)===
===Semaine 14 (04/01/2016)===
===Semaine 15 (11/01/2016)===
===Semaine 16 (18/01/2016)===
===Semaine 17 (25/01/2016)===
===Semaine 18 (01/02/2016)===
===Semaine 19 (08/02/2016)===
===Semaine 20 (15/02/2016)===
===Semaine 21 (22/02/2016)===

== Fichiers Rendus ==

Projets IMA5 2015/2016

2015-09-24T11:54:56Z

Tdanel :

Projets IMA4 SC & SA 2014/2015

2015-05-11T23:18:24Z

Tdanel : /* Notes sur les projets */

Merci de référencer vos pages de projets ici. Merci aussi d'uniformiser vos formats que ce soit en regardant la présentation des projets déjà créés ou en demandant une modification du format des précédents si votre façon de faire vous semble la meilleure. Dans tous les cas un minimum de communication entre les binômes est conseillée.

== Répartition des binômes ==

{| class="wikitable"
! Projet !! Encadrants école !! Elèves
|-
| P1 [[Brique Lego augmentée]]
| Alexandre Boé / Thomas Vantroys / Xavier Redon / Emmanuelle Pichonat
| Alex JULITA / Déborah SAUNDERS
|-
| P4 [[Tracabilité hopital|Amélioration de la traçabilité en milieu hospitalier]]
| Alexandre Boé / Thomas Vantroys
| Julian BONVILLE / François ROCHE
|-
| P6 [[Vêtements intelligents]]
| Alexandre Boé / Thomas Vantroys
| Alexander BOWN / Valentin PIAT
|-
| P10 [[Malette Arduino éducative I]]
| Emmanuelle Pichonat / Thomas Vantroys / Alexandre Boé
| Yuqian HU
|-
| P10bis [[Malette Arduino éducative II]]
| Emmanuelle Pichonat / Thomas Vantroys / Alexandre Boé
| Mehmet ILTER
|-
| P11 [[Fabricarium - partage simplifié]]
| Alexandre Boé / Thomas Vantroys / Xavier Redon / Rodolphe Astori
| Jérémie DENECHAUD / Julien HERIN
|-
| P12 [[Art embarqué]]
| Alexandre Boé / Thomas Vantroys
| Thibaut SCHOLAERT / Jean-michel TOURNIER
|-
| P14 [[Aimant intelligent]]
| Alexandre Boé / Thomas Vantroys
| Louis THEBAULT / Bertrand YVERNAULT
|-
| P15 [[Thetweekproject|Machine à café]]
| Xavier Redon
| Jean WASILEWSKI / Pierre LETOUSEY
|-
| P17 [[Drone autonome]]
| Xavier Redon
| Kévin COLAUTTI / Benjamin LEFORT
|-
| P20 [[Balise de suivi de polluant]]
| Alexandre Boé / Thomas Vantroys
| Maxime MORISSE / Timothée TENEUR
|-
| P21 [[Réseau informatique et musique]]
| Xavier Redon
| Mageshwaran SEKAR
|-
| P23 [[Motorisation d'un véhicule électrique]]
| Walid Boughanmi
| Shuai HE / Quentin SULTANA
|-
| P24 [[Dé électronique communiquant]]
| Thomas Vantroys / Alexandre Boé
| Corentin DUPLOUY / Méhdi ZEGGAI
|-
| P26 [[Synthetiseur]]
| Thomas Vantroys / Alexandre Boé
| Alexis VINOT / Lu XIA
|-
| P28 [[e-theremin ]]
| Thomas Vantroys / Alexandre Boé
| Louis CHAUCHARD / Romain IMBERT
|-
| P30 [[Dessin 3D|Dessin 3D en environnement immersif]]
| Laurent Grisoni
| Alexandre JOUY / Romain LIBAERT
|-
| P31 [[Visualisation Open Data]]
| Alexandre Boé / Thomas Vantroys / Xavier Redon
| Elise TISSOT / Beatriz ODRIOZOLA
|-
| P33 [[Surveillance passive du sommeil]]
| Alexandre Boé / Thomas Vantroys
| Jérôme BAILET / Manouk SIMON
|-
| P34 [[Robot déformable|Console de contrôle pour robot déformable]]
| Jérémie Dequidt
| Ghada ELBEZ / Céline TIRABY
|-
| P35 [[RoboCup 2015]]
| Vincent Coelen / Rochdi Merzouki
| Sandra HAGE CHEHADE / Cyril SMAGGHE
|-
| P36 [[RoboCup 2015 - Pyro Team]]
| Vincent Coelen / Rochdi Merzouki
| Thomas DANEL / Romain KRIKORIAN
|-
| P37 [[Interaction Homme Robot]]
| Emmanuelle Grangier / Rochdi Merzouki
| Arnaud DESHAYS / Flavien ROYER
|}

== Matériel à acquérir ==

* P6 : Olimex platine ECG

== Notes sur les projets ==

{| class="wikitable"
| Projet || Mini-cahier des charges || Mi-parcours || Fin de parcours || Wiki terminé || Rapport || Vidéo
|-
| P1 [[Brique Lego augmentée]]
| Un cahier des charges très correct.
| Wiki à jour. Déjà une réalisation (3 briques). Veillez à équilibrer le travail et à rendre compte systématiquement à l'encadrant.
| Wiki à jour permettant de suivre l'état d'avancement du travail. Bien !
|
|
| RdV le 23/04/2015
|-
| P4 [[Tracabilité hopital|Amélioration de la traçabilité en milieu hospitalier]]
| Un cahier des charges très correct.
| Wiki assez complet. Capture des datamatrix avancé. Passez-vous de l'utilitaire <tt>motion</tt>. Il faut penser à concevoir l'application Web (base de données et interface). Quelles technologies Web comptez-vous utiliser ?
| Wiki presque à jour. Vous auriez pu mettre un mot de votre réalisation d'un serveur WebSocket. Diminuez la taille de vos captures d'écran pour rendre la lecture du Wiki plus agréable.
|Oui
|[[Fichier:Dossier_projet_S8_Bonville_Roche.pdf]]
| Vidéo tournée (L. Engels)
|-
| P6 [[Vêtements intelligents]]
| Un cahier des charges à peu près correct.
| Le wiki n'est pas à jour à mi-parcours (rien sur la semaine 5). Des réalisations assez limités pour l'instant (lecture de capteur avec un Arduino et une application Android basique). Des inquiétudes sur la finalisation du projet. Sauf erreur de ma part, aucune information supplémentaire en semaine 7, Wiki toujours au même point. Projet a priori compromis.
| Wiki totalement abandonné, juste quelques lignes sur les 4 premières semaines.
|
|
| Vidéo maison
|-
| P10 [[Malette Arduino éducative I]]
| Un cahier des charges très correct.
| Le Wiki est assez mal tenu, pour la partie programme les commentaires sont trop succints, il manque la réalisation de la semaine 5 pour la partie routage. Le travail pour la partie bibliothèque est bien avancé, il reste l'ordonnanceur à faire fonctionner avec les périphériques disponibles. Yuqian Hu envisage de commencer la seconde partie (malette éducative) durant les vacances. Manque de communication avec les encadrants en semaine 7 (pour les deux parties du projet).
| Le Wiki a été mis à jour en catastrophe. Il contient des indications sur les jeux programmés pour la seconde partie du projet. Pas de cahier des charges négociés avec les encadrants pour cette seconde partie. Rien sur la recherche de systèmes de programmation graphique des Arduino (demandé dans le cahier des charges oral de la seconde partie).
|
|
| RdV non pris
|-
| P10bis [[Malette Arduino éducative II]]
| Un cahier des charges très correct.
| Le Wiki très mal tenu. Aucune rédaction, uniquement des schémas. Rien pour la semaine 5. A mi-parcours le PCB, pourtant basique, n'est même pas routé. Agravation du problème en semaine 7. Projets 10 et 10bis dissociés pour éviter de pénaliser la binôme.
| Wiki totalement abandonné à la semaine 6, documents obsolètes sur le PCB ordonnanceur, rien d'autre. Peu de retours, une première version non satisfaisante du PCB ordonnanceur a été livrée. Des directives envoyées par courriel pour corriger le tir.
|
|
| RdV non pris
|-
| P11 [[Fabricarium - partage simplifié]]
| Un cahier des charges tout à fait correct.
| Wiki correct. Les réflexions sur la nature de la boite ont pris beaucoup de temps. Il y a assez peu de réalisations concrètes pour l'instant. Cela dit, avec des efforts, vous devez pouvoir terminer le projet en fournissant un prototype fonctionnel.
| Wiki presque à jour et un peu trop succint.
|
|
| Vidéo par le club vidéo
|-
| P12 [[Art embarqué]]
| Un cahier des charges très correct.
| Wiki mal tenu (manquent deux semaines dans la chronologie). Impossible de juger du travail réalisé. Non respect de la consigne notée, il sera pris en compte dans la notation finale. Inquiètude levée sur la qualité du travail (par les élèves et par l'encadrant) en semaine 6. Wiki mis à jour.
| Wiki tout à fait à jour même s'il n'est pas super détaillé. Alors comme ça ça marche mieux sous Linux ?
|
|
| Vidéo maison
|-
| P14 [[Aimant intelligent]]
| Un cahier des charges complet.
| Wiki complet mais avec des coquilles. Une certaine inquiétude sur la possibilité de mener le travail à bien. Ni la conception de l'aimant ni la réalisation de l'application Android ne sont très avancés. Rien n'incite à l'optimisme en semaine 7 (rien sur le Wiki, pas de nouvelle des élèves).
| Wiki presque à jour et plutôt détaillé et illustré. Merci de corriger les nombreuses coquilles. Beaucoup plus d'optimisme sur la réalisation du projet.
|
|[[Média:Rapport_projet_YT.pdf]]
|
| RdV non pris
|-
| P15 [[Thetweekproject|Machine à café]]
| Un cahier des charges très complet, parfait.
| Wiki très détaillé et bien rédigé. Une réalisation bien avancé. Pas d'inquiétude sur la quantité de travail (en particulier durant les vacances). Par contre le projet est très ambitieux, il est toujours difficile de prévoir s'il pourra être mené à bien.
| Le Wiki commence à être en décalage avec la réalisation, merci de le mettre à jour. Indiquez ce qui est fait et ce qu'il reste à faire.
|
|
| [https://www.youtube.com/watch?v=GXPjxfLEy3o Disponible sur Youtube]
|-
| P17 [[Drone autonome]]
| Cahier des charges correct.
| Wiki très correct, illustré mais avec quelques coquilles. Le matériel est déjà complètement pris en main à mi-parcours. Il reste à concevoir l'infrastructure de contrôle de la route du drône. Il est probable que le projet puisse être mené à bien.
| Wiki à jour mais les informations sont rares. Essayez de dresser un bilan des réalisation et une liste des tâches qui restent à réaliser. Au vu des retours IRL toutes les difficultés techniques semblent aplanies, confirmez cela dans le Wiki.
|
|
| Vidéo tournée (L. Engels)
|-
| P20 [[Balise de suivi de polluant]]
| Cahier des charges propre et complet avec les étapes à réaliser. Très bien.
| Le Wiki serait très correct s'il était à jour (rien sur la semaine 5 contrairement à la consigne). Le Wiki ne dit rien sur l'avancement de réalisation de la balise. C'est un peu inquiétant, un prototype fonctionnel pourra-t-il être présenté en fin de projet ? Une partie des inquiétudes est levée en semaine 6. Oubliez les deux Arduinos, faites fonctionner les deux shields sur le même. Demandez conseil à ce sujet à vos encadrants.
| Wiki à jour et très détaillé. L'avancé des travaux est parfaitement décrite.
|
|
| RdV le 23/04/2015
|-
| P21 [[Réseau informatique et musique]]
| Bon cahier des charges.
| Le Wiki n'est absolument pas tenu à jour. Pas de contact avec l'encadrement depuis 15 jours. Le projet semble abandonné, vous vous dirigez droit vers une catastrophe. Pas mieux en semaine 7 malgré une discussion avec l'élève.
| Le Wiki est un peu mieux tenu mais toujours pas à jour, peu d'informations dans ce Wiki. Pas de rapport oral.
|
|
| RdV non pris
|-
| P23 [[Motorisation d'un véhicule électrique]]
| Très bon cahier des charges avec un planning prévisionnel précis.
| Excellent Wiki, bien illustré et à jour. Vous semblez rencontrer un problème technique mais vous semblez aussi travailler à sa résolution. Il parait probable que vous soyez entrain de prendre du retard par rapport à votre planning.
| Wiki pratiquement à jour et détaillé. Les difficultés de mi-parcours semblent avoir été surmontées.
|
|
| RdV non pris
|-
| P24 [[Dé électronique communiquant]]
| Un cahier des charges correct.
| Wiki bien tenu, très bien illustré. Le travail réalisé est bien décrit. Vous semblez rencontrer un problème par rapport à la plateforme de développement. Conntinuez-vous avec un MBED, ou passez-vous sur un Arduino ? Il faut peut être en discuter avec votre encadrant.
| Wiki mis à jour en catastrophe (sans les illustrations) le 12 avril pour les semaines 6 à 10. Wiki plutôt complet expliquant bien le travail effectué. Il n'est pas encore clair que le projet puisse aboutir.
|
|
| RdV sollicité
|-
| P26 [[Synthetiseur]]
| Un cahier des charges assez original.
| Wiki bien tenu, très bien illustré, des informations intéressantes. Par contre laissez tomber l'idée farfelue de la Fedora sur vos PC. SMEWS s'installe à merveille sur une Debian. La remarque sur ActiveX me semble totalement hors sujet, que voulez-vous faire avec un ActiveX ? Attention au travail non coordonné entre la partie SMEWS et la partie contrôle de l'électronique analogique. Vous n'utilisez pas le même environnement (C++ pour l'IDE Web et C pur pour SMEWS). Vérifiez que vous pouvez intégrer les deux parties. Attention aussi concernant les PCB que vous souhaitez réaliser. Vu les délais et les problèmes de réalisation de PCB, le routage doit être fait au plus tôt (dans la semaine de la rentrée). Beaucoup de points délicats, la réalisation d'un prototype fonctionnel n'est pas assurée. Des efforts supplémentaires sont nécessaires.
| Wiki pratiquement à jour, bien illustré, correctement détaillé. Etat d'avancement très satisfaisant mais encore du travail à effectuer.
|
|
| RdV le 22/04/2015
|-
| P28 [[e-theremin ]]
| Cahier des charges en dernière limite.
| Wiki très correct, très bien illustré. Vous semblez rencontrer un problème de traitement de signal. Veuillez le caractériser précisément et consulter un de vos enseignants dans le domaine. Si vous devez concevoir un PCB pour la partie électronique analogique faite le de suite (dans la semaine de la rentrée).
| Le Wiki est plutôt à jour mais les informations données sont un peu maigres. Votre circuit fonctionne-t-il ? Pouvez-vous décrire plus précisément le développement de l'application X11 ?
|
|
| RdV le 22/04/2015
|-
| P30 [[Dessin 3D|Dessin 3D en environnement immersif]]
| Cahier des charges correct en dernière limite.
| Wiki très correct, illustré, travail effectué très bien décrit. Une première application autonome testée. Si j'ai bien compris il vous faut encore inclure cette application à Unity.
| Wiki un peu en décalage mais avec des informations détaillées. Une vidéo de démonstration, ce n'est pas particulièrement spectaculaire mais au moins cela montre qu'un prototype existe. Vous en êtes où actuellement ? Qu'on donné les tests ?
|
|
| Vidéo maison
|-
| P31 [[Visualisation Open Data]]
| Un cahier des charges.
| Wiki à jour, peu illustré, en particulier un schéma du système à réaliser serait le bienvenu. Le problème de la liste des gares avec le changement de syntaxe semble traité un peu rapidement. Vous semblez avoir tous les éléments pour un premier prototype, mettez le en place rapidement.
| Le Wiki n'est pas à jour. Effort important des encadrants pour remettre ce projet sur les rails. Aucune production des élèves à deux semaines de la fin du projet malgré une feuille de route précise. 
|
|
| RdV non pris
|-
| P33 [[Surveillance passive du sommeil]]
| Cahier des charges très complet. Parfait.
| Wiki complet et clair. Le bilan à mi-parcours est une bonne idée, il pourrait englober votre avancement sur le programme Android. Un schéma du fonctionnement global du système serait le bienvenu. Vous semblez être dans les temps pour présenter un prototype fonctionnel en fin de projet.
| Le Wiki n'est pas vraiment à jour. Description précise du travail.
|
|
| RdV non pris
|-
| P34 [[Robot déformable|Console de contrôle pour robot déformable]]
| Cahier des charges très correct.
| Un wiki très complet et très précis. Par contre certains des problèmes que vous rencontrez semblent pouvoir se résoudre facilement. Essayez de solliciter vos encadrants quand cela vous arrive. En particulier le problème de port série que vous décrivez semble être facile à corriger.
| Wiki à jour, détaillé et vous avez fait un réel effort de correction des coquilles. Sur le fond vous parlez surtout des absences de votre encadrant ou de la lenteur des mises à jour, pas passionnant.
|
|[[Média:Rapport_final_tiraby_elbez.pdf]]
| Vidéo avec Jérémie Dequidt
|-
| P35 [[RoboCup 2015]]
| Trés bon cahier des charges.
| Excellent Wiki très précis et bien illustré. Par contre le travail décrit semble être assez théorique (algorithmes). Un test sur le robotino est-il prévu pour valider la correction des algorithmes ?
| Wiki toujours très détaillé et illustré, pas tout à fait à jour.
|
| [[Fichier:RapportProjetSmaggheHage.pdf]]
|
|-
| P36 [[RoboCup 2015 - Pyro Team]]
| Parfait.
| Excellent Wiki, très précis, très bien illustré. Le travail semble être déjà très avancé.
| Toujours excellent, pratiquement à jour.
|
| [[Fichier:RapportProjetDanelKrikorian.pdf]]
| Video à la fin du wiki
|-
| P37 [[Interaction Homme Robot]]
| Un cahier des charges minimal en dernière limite.
| Wiki à jour, peu illustré. A vu de néophyte, 5 semaines pour arriver à envoyer les premiers ordres au Nao via le serveur TCP intégré semble être un délai un peu long. Il vous reste la moitié du projet pour obtenir des résultats. Inquiétudes levées après discussion avec Flavien ROYER. Une vidéo de démonstration montre une nette progression dans le projet. Probable dissociation des notes.
| Wiki à jour, travail correctement décrit.
|
|
| RdV non pris
|}

Fichier:RapportProjetDanelKrikorian.pdf

2015-05-11T23:03:49Z

Tdanel :

Projets IMA4 SC & SA 2014/2015

2015-05-11T23:03:13Z

Tdanel : /* Notes sur les projets */

Merci de référencer vos pages de projets ici. Merci aussi d'uniformiser vos formats que ce soit en regardant la présentation des projets déjà créés ou en demandant une modification du format des précédents si votre façon de faire vous semble la meilleure. Dans tous les cas un minimum de communication entre les binômes est conseillée.

== Répartition des binômes ==

{| class="wikitable"
! Projet !! Encadrants école !! Elèves
|-
| P1 [[Brique Lego augmentée]]
| Alexandre Boé / Thomas Vantroys / Xavier Redon / Emmanuelle Pichonat
| Alex JULITA / Déborah SAUNDERS
|-
| P4 [[Tracabilité hopital|Amélioration de la traçabilité en milieu hospitalier]]
| Alexandre Boé / Thomas Vantroys
| Julian BONVILLE / François ROCHE
|-
| P6 [[Vêtements intelligents]]
| Alexandre Boé / Thomas Vantroys
| Alexander BOWN / Valentin PIAT
|-
| P10 [[Malette Arduino éducative I]]
| Emmanuelle Pichonat / Thomas Vantroys / Alexandre Boé
| Yuqian HU
|-
| P10bis [[Malette Arduino éducative II]]
| Emmanuelle Pichonat / Thomas Vantroys / Alexandre Boé
| Mehmet ILTER
|-
| P11 [[Fabricarium - partage simplifié]]
| Alexandre Boé / Thomas Vantroys / Xavier Redon / Rodolphe Astori
| Jérémie DENECHAUD / Julien HERIN
|-
| P12 [[Art embarqué]]
| Alexandre Boé / Thomas Vantroys
| Thibaut SCHOLAERT / Jean-michel TOURNIER
|-
| P14 [[Aimant intelligent]]
| Alexandre Boé / Thomas Vantroys
| Louis THEBAULT / Bertrand YVERNAULT
|-
| P15 [[Thetweekproject|Machine à café]]
| Xavier Redon
| Jean WASILEWSKI / Pierre LETOUSEY
|-
| P17 [[Drone autonome]]
| Xavier Redon
| Kévin COLAUTTI / Benjamin LEFORT
|-
| P20 [[Balise de suivi de polluant]]
| Alexandre Boé / Thomas Vantroys
| Maxime MORISSE / Timothée TENEUR
|-
| P21 [[Réseau informatique et musique]]
| Xavier Redon
| Mageshwaran SEKAR
|-
| P23 [[Motorisation d'un véhicule électrique]]
| Walid Boughanmi
| Shuai HE / Quentin SULTANA
|-
| P24 [[Dé électronique communiquant]]
| Thomas Vantroys / Alexandre Boé
| Corentin DUPLOUY / Méhdi ZEGGAI
|-
| P26 [[Synthetiseur]]
| Thomas Vantroys / Alexandre Boé
| Alexis VINOT / Lu XIA
|-
| P28 [[e-theremin ]]
| Thomas Vantroys / Alexandre Boé
| Louis CHAUCHARD / Romain IMBERT
|-
| P30 [[Dessin 3D|Dessin 3D en environnement immersif]]
| Laurent Grisoni
| Alexandre JOUY / Romain LIBAERT
|-
| P31 [[Visualisation Open Data]]
| Alexandre Boé / Thomas Vantroys / Xavier Redon
| Elise TISSOT / Beatriz ODRIOZOLA
|-
| P33 [[Surveillance passive du sommeil]]
| Alexandre Boé / Thomas Vantroys
| Jérôme BAILET / Manouk SIMON
|-
| P34 [[Robot déformable|Console de contrôle pour robot déformable]]
| Jérémie Dequidt
| Ghada ELBEZ / Céline TIRABY
|-
| P35 [[RoboCup 2015]]
| Vincent Coelen / Rochdi Merzouki
| Sandra HAGE CHEHADE / Cyril SMAGGHE
|-
| P36 [[RoboCup 2015 - Pyro Team]]
| Vincent Coelen / Rochdi Merzouki
| Thomas DANEL / Romain KRIKORIAN
|-
| P37 [[Interaction Homme Robot]]
| Emmanuelle Grangier / Rochdi Merzouki
| Arnaud DESHAYS / Flavien ROYER
|}

== Matériel à acquérir ==

* P6 : Olimex platine ECG

== Notes sur les projets ==

{| class="wikitable"
| Projet || Mini-cahier des charges || Mi-parcours || Fin de parcours || Wiki terminé || Rapport || Vidéo
|-
| P1 [[Brique Lego augmentée]]
| Un cahier des charges très correct.
| Wiki à jour. Déjà une réalisation (3 briques). Veillez à équilibrer le travail et à rendre compte systématiquement à l'encadrant.
| Wiki à jour permettant de suivre l'état d'avancement du travail. Bien !
|
|
| RdV le 23/04/2015
|-
| P4 [[Tracabilité hopital|Amélioration de la traçabilité en milieu hospitalier]]
| Un cahier des charges très correct.
| Wiki assez complet. Capture des datamatrix avancé. Passez-vous de l'utilitaire <tt>motion</tt>. Il faut penser à concevoir l'application Web (base de données et interface). Quelles technologies Web comptez-vous utiliser ?
| Wiki presque à jour. Vous auriez pu mettre un mot de votre réalisation d'un serveur WebSocket. Diminuez la taille de vos captures d'écran pour rendre la lecture du Wiki plus agréable.
|Oui
|[[Fichier:Dossier_projet_S8_Bonville_Roche.pdf]]
| Vidéo tournée (L. Engels)
|-
| P6 [[Vêtements intelligents]]
| Un cahier des charges à peu près correct.
| Le wiki n'est pas à jour à mi-parcours (rien sur la semaine 5). Des réalisations assez limités pour l'instant (lecture de capteur avec un Arduino et une application Android basique). Des inquiétudes sur la finalisation du projet. Sauf erreur de ma part, aucune information supplémentaire en semaine 7, Wiki toujours au même point. Projet a priori compromis.
| Wiki totalement abandonné, juste quelques lignes sur les 4 premières semaines.
|
|
| Vidéo maison
|-
| P10 [[Malette Arduino éducative I]]
| Un cahier des charges très correct.
| Le Wiki est assez mal tenu, pour la partie programme les commentaires sont trop succints, il manque la réalisation de la semaine 5 pour la partie routage. Le travail pour la partie bibliothèque est bien avancé, il reste l'ordonnanceur à faire fonctionner avec les périphériques disponibles. Yuqian Hu envisage de commencer la seconde partie (malette éducative) durant les vacances. Manque de communication avec les encadrants en semaine 7 (pour les deux parties du projet).
| Le Wiki a été mis à jour en catastrophe. Il contient des indications sur les jeux programmés pour la seconde partie du projet. Pas de cahier des charges négociés avec les encadrants pour cette seconde partie. Rien sur la recherche de systèmes de programmation graphique des Arduino (demandé dans le cahier des charges oral de la seconde partie).
|
|
| RdV non pris
|-
| P10bis [[Malette Arduino éducative II]]
| Un cahier des charges très correct.
| Le Wiki très mal tenu. Aucune rédaction, uniquement des schémas. Rien pour la semaine 5. A mi-parcours le PCB, pourtant basique, n'est même pas routé. Agravation du problème en semaine 7. Projets 10 et 10bis dissociés pour éviter de pénaliser la binôme.
| Wiki totalement abandonné à la semaine 6, documents obsolètes sur le PCB ordonnanceur, rien d'autre. Peu de retours, une première version non satisfaisante du PCB ordonnanceur a été livrée. Des directives envoyées par courriel pour corriger le tir.
|
|
| RdV non pris
|-
| P11 [[Fabricarium - partage simplifié]]
| Un cahier des charges tout à fait correct.
| Wiki correct. Les réflexions sur la nature de la boite ont pris beaucoup de temps. Il y a assez peu de réalisations concrètes pour l'instant. Cela dit, avec des efforts, vous devez pouvoir terminer le projet en fournissant un prototype fonctionnel.
| Wiki presque à jour et un peu trop succint.
|
|
| Vidéo par le club vidéo
|-
| P12 [[Art embarqué]]
| Un cahier des charges très correct.
| Wiki mal tenu (manquent deux semaines dans la chronologie). Impossible de juger du travail réalisé. Non respect de la consigne notée, il sera pris en compte dans la notation finale. Inquiètude levée sur la qualité du travail (par les élèves et par l'encadrant) en semaine 6. Wiki mis à jour.
| Wiki tout à fait à jour même s'il n'est pas super détaillé. Alors comme ça ça marche mieux sous Linux ?
|
|
| Vidéo maison
|-
| P14 [[Aimant intelligent]]
| Un cahier des charges complet.
| Wiki complet mais avec des coquilles. Une certaine inquiétude sur la possibilité de mener le travail à bien. Ni la conception de l'aimant ni la réalisation de l'application Android ne sont très avancés. Rien n'incite à l'optimisme en semaine 7 (rien sur le Wiki, pas de nouvelle des élèves).
| Wiki presque à jour et plutôt détaillé et illustré. Merci de corriger les nombreuses coquilles. Beaucoup plus d'optimisme sur la réalisation du projet.
|
|[[Média:Rapport_projet_YT.pdf]]
|
| RdV non pris
|-
| P15 [[Thetweekproject|Machine à café]]
| Un cahier des charges très complet, parfait.
| Wiki très détaillé et bien rédigé. Une réalisation bien avancé. Pas d'inquiétude sur la quantité de travail (en particulier durant les vacances). Par contre le projet est très ambitieux, il est toujours difficile de prévoir s'il pourra être mené à bien.
| Le Wiki commence à être en décalage avec la réalisation, merci de le mettre à jour. Indiquez ce qui est fait et ce qu'il reste à faire.
|
|
| [https://www.youtube.com/watch?v=GXPjxfLEy3o Disponible sur Youtube]
|-
| P17 [[Drone autonome]]
| Cahier des charges correct.
| Wiki très correct, illustré mais avec quelques coquilles. Le matériel est déjà complètement pris en main à mi-parcours. Il reste à concevoir l'infrastructure de contrôle de la route du drône. Il est probable que le projet puisse être mené à bien.
| Wiki à jour mais les informations sont rares. Essayez de dresser un bilan des réalisation et une liste des tâches qui restent à réaliser. Au vu des retours IRL toutes les difficultés techniques semblent aplanies, confirmez cela dans le Wiki.
|
|
| Vidéo tournée (L. Engels)
|-
| P20 [[Balise de suivi de polluant]]
| Cahier des charges propre et complet avec les étapes à réaliser. Très bien.
| Le Wiki serait très correct s'il était à jour (rien sur la semaine 5 contrairement à la consigne). Le Wiki ne dit rien sur l'avancement de réalisation de la balise. C'est un peu inquiétant, un prototype fonctionnel pourra-t-il être présenté en fin de projet ? Une partie des inquiétudes est levée en semaine 6. Oubliez les deux Arduinos, faites fonctionner les deux shields sur le même. Demandez conseil à ce sujet à vos encadrants.
| Wiki à jour et très détaillé. L'avancé des travaux est parfaitement décrite.
|
|
| RdV le 23/04/2015
|-
| P21 [[Réseau informatique et musique]]
| Bon cahier des charges.
| Le Wiki n'est absolument pas tenu à jour. Pas de contact avec l'encadrement depuis 15 jours. Le projet semble abandonné, vous vous dirigez droit vers une catastrophe. Pas mieux en semaine 7 malgré une discussion avec l'élève.
| Le Wiki est un peu mieux tenu mais toujours pas à jour, peu d'informations dans ce Wiki. Pas de rapport oral.
|
|
| RdV non pris
|-
| P23 [[Motorisation d'un véhicule électrique]]
| Très bon cahier des charges avec un planning prévisionnel précis.
| Excellent Wiki, bien illustré et à jour. Vous semblez rencontrer un problème technique mais vous semblez aussi travailler à sa résolution. Il parait probable que vous soyez entrain de prendre du retard par rapport à votre planning.
| Wiki pratiquement à jour et détaillé. Les difficultés de mi-parcours semblent avoir été surmontées.
|
|
| RdV non pris
|-
| P24 [[Dé électronique communiquant]]
| Un cahier des charges correct.
| Wiki bien tenu, très bien illustré. Le travail réalisé est bien décrit. Vous semblez rencontrer un problème par rapport à la plateforme de développement. Conntinuez-vous avec un MBED, ou passez-vous sur un Arduino ? Il faut peut être en discuter avec votre encadrant.
| Wiki mis à jour en catastrophe (sans les illustrations) le 12 avril pour les semaines 6 à 10. Wiki plutôt complet expliquant bien le travail effectué. Il n'est pas encore clair que le projet puisse aboutir.
|
|
| RdV sollicité
|-
| P26 [[Synthetiseur]]
| Un cahier des charges assez original.
| Wiki bien tenu, très bien illustré, des informations intéressantes. Par contre laissez tomber l'idée farfelue de la Fedora sur vos PC. SMEWS s'installe à merveille sur une Debian. La remarque sur ActiveX me semble totalement hors sujet, que voulez-vous faire avec un ActiveX ? Attention au travail non coordonné entre la partie SMEWS et la partie contrôle de l'électronique analogique. Vous n'utilisez pas le même environnement (C++ pour l'IDE Web et C pur pour SMEWS). Vérifiez que vous pouvez intégrer les deux parties. Attention aussi concernant les PCB que vous souhaitez réaliser. Vu les délais et les problèmes de réalisation de PCB, le routage doit être fait au plus tôt (dans la semaine de la rentrée). Beaucoup de points délicats, la réalisation d'un prototype fonctionnel n'est pas assurée. Des efforts supplémentaires sont nécessaires.
| Wiki pratiquement à jour, bien illustré, correctement détaillé. Etat d'avancement très satisfaisant mais encore du travail à effectuer.
|
|
| RdV le 22/04/2015
|-
| P28 [[e-theremin ]]
| Cahier des charges en dernière limite.
| Wiki très correct, très bien illustré. Vous semblez rencontrer un problème de traitement de signal. Veuillez le caractériser précisément et consulter un de vos enseignants dans le domaine. Si vous devez concevoir un PCB pour la partie électronique analogique faite le de suite (dans la semaine de la rentrée).
| Le Wiki est plutôt à jour mais les informations données sont un peu maigres. Votre circuit fonctionne-t-il ? Pouvez-vous décrire plus précisément le développement de l'application X11 ?
|
|
| RdV le 22/04/2015
|-
| P30 [[Dessin 3D|Dessin 3D en environnement immersif]]
| Cahier des charges correct en dernière limite.
| Wiki très correct, illustré, travail effectué très bien décrit. Une première application autonome testée. Si j'ai bien compris il vous faut encore inclure cette application à Unity.
| Wiki un peu en décalage mais avec des informations détaillées. Une vidéo de démonstration, ce n'est pas particulièrement spectaculaire mais au moins cela montre qu'un prototype existe. Vous en êtes où actuellement ? Qu'on donné les tests ?
|
|
| Vidéo maison
|-
| P31 [[Visualisation Open Data]]
| Un cahier des charges.
| Wiki à jour, peu illustré, en particulier un schéma du système à réaliser serait le bienvenu. Le problème de la liste des gares avec le changement de syntaxe semble traité un peu rapidement. Vous semblez avoir tous les éléments pour un premier prototype, mettez le en place rapidement.
| Le Wiki n'est pas à jour. Effort important des encadrants pour remettre ce projet sur les rails. Aucune production des élèves à deux semaines de la fin du projet malgré une feuille de route précise. 
|
|
| RdV non pris
|-
| P33 [[Surveillance passive du sommeil]]
| Cahier des charges très complet. Parfait.
| Wiki complet et clair. Le bilan à mi-parcours est une bonne idée, il pourrait englober votre avancement sur le programme Android. Un schéma du fonctionnement global du système serait le bienvenu. Vous semblez être dans les temps pour présenter un prototype fonctionnel en fin de projet.
| Le Wiki n'est pas vraiment à jour. Description précise du travail.
|
|
| RdV non pris
|-
| P34 [[Robot déformable|Console de contrôle pour robot déformable]]
| Cahier des charges très correct.
| Un wiki très complet et très précis. Par contre certains des problèmes que vous rencontrez semblent pouvoir se résoudre facilement. Essayez de solliciter vos encadrants quand cela vous arrive. En particulier le problème de port série que vous décrivez semble être facile à corriger.
| Wiki à jour, détaillé et vous avez fait un réel effort de correction des coquilles. Sur le fond vous parlez surtout des absences de votre encadrant ou de la lenteur des mises à jour, pas passionnant.
|
|[[Média:Rapport_final_tiraby_elbez.pdf]]
| Vidéo avec Jérémie Dequidt
|-
| P35 [[RoboCup 2015]]
| Trés bon cahier des charges.
| Excellent Wiki très précis et bien illustré. Par contre le travail décrit semble être assez théorique (algorithmes). Un test sur le robotino est-il prévu pour valider la correction des algorithmes ?
| Wiki toujours très détaillé et illustré, pas tout à fait à jour.
|
| [[Fichier:RapportProjetSmaggheHage.pdf]]
| Prise de vues pendant la robocup
|-
| P36 [[RoboCup 2015 - Pyro Team]]
| Parfait.
| Excellent Wiki, très précis, très bien illustré. Le travail semble être déjà très avancé.
| Toujours excellent, pratiquement à jour.
|
| [[Fichier:RapportProjetDanelKrikorian.pdf]]
| Prise de vues pendant la robocup
|-
| P37 [[Interaction Homme Robot]]
| Un cahier des charges minimal en dernière limite.
| Wiki à jour, peu illustré. A vu de néophyte, 5 semaines pour arriver à envoyer les premiers ordres au Nao via le serveur TCP intégré semble être un délai un peu long. Il vous reste la moitié du projet pour obtenir des résultats. Inquiétudes levées après discussion avec Flavien ROYER. Une vidéo de démonstration montre une nette progression dans le projet. Probable dissociation des notes.
| Wiki à jour, travail correctement décrit.
|
|
| RdV non pris
|}

RoboCup 2015 - Pyro Team

2015-05-11T23:00:37Z

Tdanel : /* Vocabulaire */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
*Familiarisation avec l'environnement logiciel ROS
*Recherches de solutions pour la localisation (SLAM)
*Recherche de solution pour la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

[[Fichier:Mapcompet.PNG|thumb|center]]

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

===Semaine 10===

====Localisation - Extended Kalman Filter====

La partie '''prédiction''' du filtre marche parfaitement, quelques corrections ont dues être faites au niveau de la commande vitesse. En effet, Nous avions oublié de la transposer dans le repère global. Pour la partie '''correction''', nous rencontrons un problème de divergence du filtre, qui est probablement due à une mauvaise initialisation. En effet, nous utilisons des enregistrements faits sur notre piste (1/4 de la vraie) avec des initialisations correspondant à la piste originale. Le filtre ne peut donc pas converger car la position de départ est fausse.

===Semaine 11===

====Localisation - Extended Kalman Filter====

Des corrections sont en cours de validation. Nous essayons de définir une initialisation avec les paramètres de notre piste, ce qui n'est pas évident. Les programmes permettant d'extraire les positions machines du filtres ont été réalisé, ainsi qu'un launch pour pouvoir lancer tous les noeuds de la localisation.

===Semaine 12===

====Compétition Open German - mise en place====

Pendant les deux jours de mise en place, nous avons continué à travailler sur la localisation, qui est une partie critique et nécessaire à notre participation. Le filtre en lui même est fonctionnel et permet de corriger la position du robot, ainsi que celle des machines. Cependant, et ce n'est pas un petit "cependant"... Le fait que l'on ajoute des machines en cours de jeu perturbe fortement notre algorithme.

En effet, le filtre de Kalman est optimal si la position des repères est connu, or, un ajout de machine crée un recalage forcé, et si l'on voit plusieurs machines... C'est la '''catastrophe'''. L'ajout de machines est une partie sensible qui nécessite des tests sur la position mesurée, avec un changement de repère à partir de la position du robot, si celle ci dérive, tout dérive. Chaque machine initialisée à la mauvaise position, ou chaque élément détecté comme machine mais n'en étant pas une (faux positif) perturbe le recalage.

De nombreux tests ayant été fait sur la piste officiel avec différentes configurations nous ont permis de vérifier que l'odométrie mesurée ne dérivait que très peu. Il a donc été décidé de n'utiliser que l'odométrie et le scan laser pour la partie '''exploration'''. En effet, chaque machine mesurée est envoyée à un noeud ROS qui va créer une carte et la compléter au fur et à mesure de la partie. Le robot est donc repéré rien qu'avec son odométrie. Le fait que la phase d'exploration (découverte du terrain) ne dure que '''4 minutes''' nous a convaincu pour n'utiliser que cette mesure.

De plus, la carte créée sera utilisée pour la phase suivante, la phase de '''production'''. Cette phase consiste, au niveau de la localisation, à se repérer sur la piste et atteindre la machine demandée par l'exécuteur de tâches. Pour cette phase qui dure '''15 minutes''', l'utilisation du filtre est nécessaire. Donc, avec les coordonnées des machines mesurées pendant la première phase, on peut créer le vecteur d'état et la matrice faisant le lien entre les machines. De là, la taille de ces deux éléments ne variant plus, on peut corriger uniquement la position du robot et le filtre sera efficace.

====Compétition Open German - 1er jour====

Durant ce premier jour, nous avons commencé à ''merger'' nos branches respectives pour pouvoir faire tourner tous nos programmes. Cette phase est laborieuse et nous a pris du temps. De toute façon, toutes les équipes ne sont pas prêtes, les organisateurs non plus. La '''Referee Box''' n'est pas fonctionnel pour la phase de production, ce qui veut dire que nous nous concentrons actuellement sur l'exploration. Les matchs joués aujourd'hui ne nous rapporte donc pas de points. Les tests nous ont permis de valider individuellement chacun des algos, il ne reste "plus qu'à" rejoindre le tout.

====Compétition Open German - 2eme jour====

Niveau '''localisation''', la navigation à l'odométrie et la détection de machine sont fonctionnelles. Cependant, le moyennage fait sur les positions machines (on moyenne toutes les positions trouvées appartenant à une même machine pour n'avoir qu'un seul point) fait que le point correspondant peut être perturbé par des mesures aberrantes. C'est le cas lorsque le robot se déplace à vitesse élevée.

La détection de machine a été améliorée afin de ne rajouter QUE les machines, et pas les murs de la même taille. Nous allons donc retester le '''filtre de Kalman Etendu''' avec cette amélioration. Cependant, ce ne sera fait qu'après la compétition, puisque d'autres problèmes ont nécessité la présence de l'équipe sur certaines parties sensibles du déplacement.

Sur la piste nous avons fait des mesures, qui nous donnent la carte suivante :

[[Fichier:PisteSamediSoir.PNG | center]]

La visualisation du trajet pour découvrir ces machines est le suivant :

[[Fichier:DetectionSamediSoir.PNG | center | thumb]]

Et les positions machines, ainsi que leur orientation sont les suivantes :

[[Fichier:MachinesBagtestSamediSoir.PNG | center | thumb]]

====Compétition Open German - 3eme et dernier jour====

L'architecture choisie pour la localisation est la suivante :
*Navigation à l'odométrie
*Détection de machines et moyennage
*Quand on a vu toutes nos machines, on utilise la symétrie pour compléter les machines restantes
*Navigation et Localisation avec l'EKF (correction des positions machines et du robot)

====Localisation - Extended Kalman Filter====

Après de longs tests et des corrections importantes au niveau de l'ajout des machines dans le filtre, ce qui était la partie critique... Le meilleur résultat que l'on ait pu avoir avec l'EKF est celui ci dessous. Les machines étant en violet, la trajectoire calculée par l'EKF en vert et la trajectoire mesurée par les codeurs en rouge.

[[Fichier:BestResultWithEKF.PNG | center]]

Nous rappelons que tous nos codes sont disponibles sur notre [https://github.com/PyroTeam/robocup-pkg/tree/navigation-devel/navigation/localisation GIT].

====Video====

Enfin, afin d'illustrer nos résultats de manière ludique et attrayante, voici une vidéo qui vous donnera goût à la RoboCup !

https://www.youtube.com/watch?v=PB6g2krvCFI

RoboCup 2015 - Pyro Team

2015-05-07T14:50:32Z

Tdanel : /* Localisation - Extended Kalman Filter */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
*Familiarisation avec l'environnement logiciel ROS
*Recherches de solutions pour la localisation (SLAM)
*Recherche de solution pour la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

===Semaine 10===

====Localisation - Extended Kalman Filter====

La partie '''prédiction''' du filtre marche parfaitement, quelques corrections ont dues être faites au niveau de la commande vitesse. En effet, Nous avions oublié de la transposer dans le repère global. Pour la partie '''correction''', nous rencontrons un problème de divergence du filtre, qui est probablement due à une mauvaise initialisation. En effet, nous utilisons des enregistrements faits sur notre piste (1/4 de la vraie) avec des initialisations correspondant à la piste originale. Le filtre ne peut donc pas converger car la position de départ est fausse.

===Semaine 11===

====Localisation - Extended Kalman Filter====

Des corrections sont en cours de validation. Nous essayons de définir une initialisation avec les paramètres de notre piste, ce qui n'est pas évident. Les programmes permettant d'extraire les positions machines du filtres ont été réalisé, ainsi qu'un launch pour pouvoir lancer tous les noeuds de la localisation.

===Semaine 12===

====Compétition Open German - mise en place====

Pendant les deux jours de mise en place, nous avons continué à travailler sur la localisation, qui est une partie critique et nécessaire à notre participation. Le filtre en lui même est fonctionnel et permet de corriger la position du robot, ainsi que celle des machines. Cependant, et ce n'est pas un petit "cependant"... Le fait que l'on ajoute des machines en cours de jeu perturbe fortement notre algorithme.

En effet, le filtre de Kalman est optimal si la position des repères est connu, or, un ajout de machine crée un recalage forcé, et si l'on voit plusieurs machines... C'est la '''catastrophe'''. L'ajout de machines est une partie sensible qui nécessite des tests sur la position mesurée, avec un changement de repère à partir de la position du robot, si celle ci dérive, tout dérive. Chaque machine initialisée à la mauvaise position, ou chaque élément détecté comme machine mais n'en étant pas une (faux positif) perturbe le recalage.

De nombreux tests ayant été fait sur la piste officiel avec différentes configurations nous ont permis de vérifier que l'odométrie mesurée ne dérivait que très peu. Il a donc été décidé de n'utiliser que l'odométrie et le scan laser pour la partie '''exploration'''. En effet, chaque machine mesurée est envoyée à un noeud ROS qui va créer une carte et la compléter au fur et à mesure de la partie. Le robot est donc repéré rien qu'avec son odométrie. Le fait que la phase d'exploration (découverte du terrain) ne dure que '''4 minutes''' nous a convaincu pour n'utiliser que cette mesure.

De plus, la carte créée sera utilisée pour la phase suivante, la phase de '''production'''. Cette phase consiste, au niveau de la localisation, à se repérer sur la piste et atteindre la machine demandée par l'exécuteur de tâches. Pour cette phase qui dure '''15 minutes''', l'utilisation du filtre est nécessaire. Donc, avec les coordonnées des machines mesurées pendant la première phase, on peut créer le vecteur d'état et la matrice faisant le lien entre les machines. De là, la taille de ces deux éléments ne variant plus, on peut corriger uniquement la position du robot et le filtre sera efficace.

====Compétition Open German - 1er jour====

Durant ce premier jour, nous avons commencé à ''merger'' nos branches respectives pour pouvoir faire tourner tous nos programmes. Cette phase est laborieuse et nous a pris du temps. De toute façon, toutes les équipes ne sont pas prêtes, les organisateurs non plus. La '''Referee Box''' n'est pas fonctionnel pour la phase de production, ce qui veut dire que nous nous concentrons actuellement sur l'exploration. Les matchs joués aujourd'hui ne nous rapporte donc pas de points. Les tests nous ont permis de valider individuellement chacun des algos, il ne reste "plus qu'à" rejoindre le tout.

====Compétition Open German - 2eme jour====

Niveau '''localisation''', la navigation à l'odométrie et la détection de machine sont fonctionnelles. Cependant, le moyennage fait sur les positions machines (on moyenne toutes les positions trouvées appartenant à une même machine pour n'avoir qu'un seul point) fait que le point correspondant peut être perturbé par des mesures aberrantes. C'est le cas lorsque le robot se déplace à vitesse élevée.

La détection de machine a été améliorée afin de ne rajouter QUE les machines, et pas les murs de la même taille. Nous allons donc retester le '''filtre de Kalman Etendu''' avec cette amélioration. Cependant, ce ne sera fait qu'après la compétition, puisque d'autres problèmes ont nécessité la présence de l'équipe sur certaines parties sensibles du déplacement.

Sur la piste nous avons fait des mesures, qui nous donnent la carte suivante :

[[Fichier:PisteSamediSoir.PNG | center]]

La visualisation du trajet pour découvrir ces machines est le suivant :

[[Fichier:DetectionSamediSoir.PNG | center | thumb]]

Et les positions machines, ainsi que leur orientation sont les suivantes :

[[Fichier:MachinesBagtestSamediSoir.PNG | center | thumb]]

====Compétition Open German - 3eme et dernier jour====

L'architecture choisie pour la localisation est la suivante :
*Navigation à l'odométrie
*Détection de machines et moyennage
*Quand on a vu toutes nos machines, on utilise la symétrie pour compléter les machines restantes
*Navigation et Localisation avec l'EKF (correction des positions machines et du robot)

====Localisation - Extended Kalman Filter====

Après de longs tests et des corrections importantes au niveau de l'ajout des machines dans le filtre, ce qui était la partie critique... Le meilleur résultat que l'on ait pu avoir avec l'EKF est celui ci dessous. Les machines étant en violet, la trajectoire calculée par l'EKF en vert et la trajectoire mesurée par les codeurs en rouge.

[[Fichier:BestResultWithEKF.PNG | center]]

Nous rappelons que tous nos codes sont disponibles sur notre [https://github.com/PyroTeam/robocup-pkg/tree/navigation-devel/navigation/localisation GIT].

RoboCup 2015 - Pyro Team

2015-05-04T15:19:56Z

Tdanel : /* Etablissement du cahier des charges */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
*Familiarisation avec l'environnement logiciel ROS
*Recherches de solutions pour la localisation (SLAM)
*Recherche de solution pour la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

===Semaine 10===

====Localisation - Extended Kalman Filter====

La partie '''prédiction''' du filtre marche parfaitement, quelques corrections ont dues être faites au niveau de la commande vitesse. En effet, Nous avions oublié de la transposer dans le repère global. Pour la partie '''correction''', nous rencontrons un problème de divergence du filtre, qui est probablement due à une mauvaise initialisation. En effet, nous utilisons des enregistrements faits sur notre piste (1/4 de la vraie) avec des initialisations correspondant à la piste originale. Le filtre ne peut donc pas converger car la position de départ est fausse.

===Semaine 11===

====Localisation - Extended Kalman Filter====

Des corrections sont en cours de validation. Nous essayons de définir une initialisation avec les paramètres de notre piste, ce qui n'est pas évident. Les programmes permettant d'extraire les positions machines du filtres ont été réalisé, ainsi qu'un launch pour pouvoir lancer tous les noeuds de la localisation.

===Semaine 12===

====Compétition Open German - mise en place====

Pendant les deux jours de mise en place, nous avons continué à travailler sur la localisation, qui est une partie critique et nécessaire à notre participation. Le filtre en lui même est fonctionnel et permet de corriger la position du robot, ainsi que celle des machines. Cependant, et ce n'est pas un petit "cependant"... Le fait que l'on ajoute des machines en cours de jeu perturbe fortement notre algorithme.

En effet, le filtre de Kalman est optimal si la position des repères est connu, or, un ajout de machine crée un recalage forcé, et si l'on voit plusieurs machines... C'est la '''catastrophe'''. L'ajout de machines est une partie sensible qui nécessite des tests sur la position mesurée, avec un changement de repère à partir de la position du robot, si celle ci dérive, tout dérive. Chaque machine initialisée à la mauvaise position, ou chaque élément détecté comme machine mais n'en étant pas une (faux positif) perturbe le recalage.

De nombreux tests ayant été fait sur la piste officiel avec différentes configurations nous ont permis de vérifier que l'odométrie mesurée ne dérivait que très peu. Il a donc été décidé de n'utiliser que l'odométrie et le scan laser pour la partie '''exploration'''. En effet, chaque machine mesurée est envoyée à un noeud ROS qui va créer une carte et la compléter au fur et à mesure de la partie. Le robot est donc repéré rien qu'avec son odométrie. Le fait que la phase d'exploration (découverte du terrain) ne dure que '''4 minutes''' nous a convaincu pour n'utiliser que cette mesure.

De plus, la carte créée sera utilisée pour la phase suivante, la phase de '''production'''. Cette phase consiste, au niveau de la localisation, à se repérer sur la piste et atteindre la machine demandée par l'exécuteur de tâches. Pour cette phase qui dure '''15 minutes''', l'utilisation du filtre est nécessaire. Donc, avec les coordonnées des machines mesurées pendant la première phase, on peut créer le vecteur d'état et la matrice faisant le lien entre les machines. De là, la taille de ces deux éléments ne variant plus, on peut corriger uniquement la position du robot et le filtre sera efficace.

====Compétition Open German - 1er jour====

Durant ce premier jour, nous avons commencé à ''merger'' nos branches respectives pour pouvoir faire tourner tous nos programmes. Cette phase est laborieuse et nous a pris du temps. De toute façon, toutes les équipes ne sont pas prêtes, les organisateurs non plus. La '''Referee Box''' n'est pas fonctionnel pour la phase de production, ce qui veut dire que nous nous concentrons actuellement sur l'exploration. Les matchs joués aujourd'hui ne nous rapporte donc pas de points. Les tests nous ont permis de valider individuellement chacun des algos, il ne reste "plus qu'à" rejoindre le tout.

====Compétition Open German - 2eme jour====

Niveau '''localisation''', la navigation à l'odométrie et la détection de machine sont fonctionnelles. Cependant, le moyennage fait sur les positions machines (on moyenne toutes les positions trouvées appartenant à une même machine pour n'avoir qu'un seul point) fait que le point correspondant peut être perturbé par des mesures aberrantes. C'est le cas lorsque le robot se déplace à vitesse élevée.

La détection de machine a été améliorée afin de ne rajouter QUE les machines, et pas les murs de la même taille. Nous allons donc retester le '''filtre de Kalman Etendu''' avec cette amélioration. Cependant, ce ne sera fait qu'après la compétition, puisque d'autres problèmes ont nécessité la présence de l'équipe sur certaines parties sensibles du déplacement.

Sur la piste nous avons fait des mesures, qui nous donnent la carte suivante :

[[Fichier:PisteSamediSoir.PNG | center]]

La visualisation du trajet pour découvrir ces machines est le suivant :

[[Fichier:DetectionSamediSoir.PNG | center | thumb]]

Et les positions machines, ainsi que leur orientation sont les suivantes :

[[Fichier:MachinesBagtestSamediSoir.PNG | center | thumb]]

====Compétition Open German - 3eme et dernier jour====

L'architecture choisie pour la localisation est la suivante :
*Navigation à l'odométrie
*Détection de machines et moyennage
*Quand on a vu toutes nos machines, on utilise la symétrie pour compléter les machines restantes
*Navigation et Localisation avec l'EKF (correction des positions machines et du robot)

====Localisation - Extended Kalman Filter====

Après de longs tests et des corrections importantes au niveau de l'ajout des machines dans le filtre, ce qui était la partie critique... Le meilleur résultat que l'on ait pu avoir avec l'EKF est celui ci dessous. Les machines étant en violet, la trajectoire calculée par l'EKF en vert et la trajectoire mesurée par les codeurs en rouge.

[[Fichier:BestResultWithEKF.PNG | center]]

Nous rappelons que tous nos codes sont disponibles sur notre [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM GIT].

RoboCup 2015 - Pyro Team

2015-04-30T13:05:21Z

Tdanel : /* Localisation - Extended Kalman Filter */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

===Semaine 10===

====Localisation - Extended Kalman Filter====

La partie '''prédiction''' du filtre marche parfaitement, quelques corrections ont dues être faites au niveau de la commande vitesse. En effet, Nous avions oublié de la transposer dans le repère global. Pour la partie '''correction''', nous rencontrons un problème de divergence du filtre, qui est probablement due à une mauvaise initialisation. En effet, nous utilisons des enregistrements faits sur notre piste (1/4 de la vraie) avec des initialisations correspondant à la piste originale. Le filtre ne peut donc pas converger car la position de départ est fausse.

===Semaine 11===

====Localisation - Extended Kalman Filter====

Des corrections sont en cours de validation. Nous essayons de définir une initialisation avec les paramètres de notre piste, ce qui n'est pas évident. Les programmes permettant d'extraire les positions machines du filtres ont été réalisé, ainsi qu'un launch pour pouvoir lancer tous les noeuds de la localisation.

===Semaine 12===

====Compétition Open German - mise en place====

Pendant les deux jours de mise en place, nous avons continué à travailler sur la localisation, qui est une partie critique et nécessaire à notre participation. Le filtre en lui même est fonctionnel et permet de corriger la position du robot, ainsi que celle des machines. Cependant, et ce n'est pas un petit "cependant"... Le fait que l'on ajoute des machines en cours de jeu perturbe fortement notre algorithme.

En effet, le filtre de Kalman est optimal si la position des repères est connu, or, un ajout de machine crée un recalage forcé, et si l'on voit plusieurs machines... C'est la '''catastrophe'''. L'ajout de machines est une partie sensible qui nécessite des tests sur la position mesurée, avec un changement de repère à partir de la position du robot, si celle ci dérive, tout dérive. Chaque machine initialisée à la mauvaise position, ou chaque élément détecté comme machine mais n'en étant pas une (faux positif) perturbe le recalage.

De nombreux tests ayant été fait sur la piste officiel avec différentes configurations nous ont permis de vérifier que l'odométrie mesurée ne dérivait que très peu. Il a donc été décidé de n'utiliser que l'odométrie et le scan laser pour la partie '''exploration'''. En effet, chaque machine mesurée est envoyée à un noeud ROS qui va créer une carte et la compléter au fur et à mesure de la partie. Le robot est donc repéré rien qu'avec son odométrie. Le fait que la phase d'exploration (découverte du terrain) ne dure que '''4 minutes''' nous a convaincu pour n'utiliser que cette mesure.

De plus, la carte créée sera utilisée pour la phase suivante, la phase de '''production'''. Cette phase consiste, au niveau de la localisation, à se repérer sur la piste et atteindre la machine demandée par l'exécuteur de tâches. Pour cette phase qui dure '''15 minutes''', l'utilisation du filtre est nécessaire. Donc, avec les coordonnées des machines mesurées pendant la première phase, on peut créer le vecteur d'état et la matrice faisant le lien entre les machines. De là, la taille de ces deux éléments ne variant plus, on peut corriger uniquement la position du robot et le filtre sera efficace.

====Compétition Open German - 1er jour====

Durant ce premier jour, nous avons commencé à ''merger'' nos branches respectives pour pouvoir faire tourner tous nos programmes. Cette phase est laborieuse et nous a pris du temps. De toute façon, toutes les équipes ne sont pas prêtes, les organisateurs non plus. La '''Referee Box''' n'est pas fonctionnel pour la phase de production, ce qui veut dire que nous nous concentrons actuellement sur l'exploration. Les matchs joués aujourd'hui ne nous rapporte donc pas de points. Les tests nous ont permis de valider individuellement chacun des algos, il ne reste "plus qu'à" rejoindre le tout.

====Compétition Open German - 2eme jour====

Niveau '''localisation''', la navigation à l'odométrie et la détection de machine sont fonctionnelles. Cependant, le moyennage fait sur les positions machines (on moyenne toutes les positions trouvées appartenant à une même machine pour n'avoir qu'un seul point) fait que le point correspondant peut être perturbé par des mesures aberrantes. C'est le cas lorsque le robot se déplace à vitesse élevée.

La détection de machine a été améliorée afin de ne rajouter QUE les machines, et pas les murs de la même taille. Nous allons donc retester le '''filtre de Kalman Etendu''' avec cette amélioration. Cependant, ce ne sera fait qu'après la compétition, puisque d'autres problèmes ont nécessité la présence de l'équipe sur certaines parties sensibles du déplacement.

Sur la piste nous avons fait des mesures, qui nous donnent la carte suivante :

[[Fichier:PisteSamediSoir.PNG | center]]

La visualisation du trajet pour découvrir ces machines est le suivant :

[[Fichier:DetectionSamediSoir.PNG | center | thumb]]

Et les positions machines, ainsi que leur orientation sont les suivantes :

[[Fichier:MachinesBagtestSamediSoir.PNG | center | thumb]]

====Compétition Open German - 3eme et dernier jour====

L'architecture choisie pour la localisation est la suivante :
*Navigation à l'odométrie
*Détection de machines et moyennage
*Quand on a vu toutes nos machines, on utilise la symétrie pour compléter les machines restantes
*Navigation et Localisation avec l'EKF (correction des positions machines et du robot)

====Localisation - Extended Kalman Filter====

Après de longs tests et des corrections importantes au niveau de l'ajout des machines dans le filtre, ce qui était la partie critique... Le meilleur résultat que l'on ait pu avoir avec l'EKF est celui ci dessous. Les machines étant en violet, la trajectoire calculée par l'EKF en vert et la trajectoire mesurée par les codeurs en rouge.

[[Fichier:BestResultWithEKF.PNG | center]]

Nous rappelons que tous nos codes sont disponibles sur notre [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM GIT].

Fichier:BestResultWithEKF.PNG

2015-04-30T12:55:01Z

Tdanel :

RoboCup 2015 - Pyro Team

2015-04-30T12:54:17Z

Tdanel : /* Localisation - Extended Kalman Filter */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

===Semaine 10===

====Localisation - Extended Kalman Filter====

La partie '''prédiction''' du filtre marche parfaitement, quelques corrections ont dues être faites au niveau de la commande vitesse. En effet, Nous avions oublié de la transposer dans le repère global. Pour la partie '''correction''', nous rencontrons un problème de divergence du filtre, qui est probablement due à une mauvaise initialisation. En effet, nous utilisons des enregistrements faits sur notre piste (1/4 de la vraie) avec des initialisations correspondant à la piste originale. Le filtre ne peut donc pas converger car la position de départ est fausse.

===Semaine 11===

====Localisation - Extended Kalman Filter====

Des corrections sont en cours de validation. Nous essayons de définir une initialisation avec les paramètres de notre piste, ce qui n'est pas évident. Les programmes permettant d'extraire les positions machines du filtres ont été réalisé, ainsi qu'un launch pour pouvoir lancer tous les noeuds de la localisation.

===Semaine 12===

====Compétition Open German - mise en place====

Pendant les deux jours de mise en place, nous avons continué à travailler sur la localisation, qui est une partie critique et nécessaire à notre participation. Le filtre en lui même est fonctionnel et permet de corriger la position du robot, ainsi que celle des machines. Cependant, et ce n'est pas un petit "cependant"... Le fait que l'on ajoute des machines en cours de jeu perturbe fortement notre algorithme.

En effet, le filtre de Kalman est optimal si la position des repères est connu, or, un ajout de machine crée un recalage forcé, et si l'on voit plusieurs machines... C'est la '''catastrophe'''. L'ajout de machines est une partie sensible qui nécessite des tests sur la position mesurée, avec un changement de repère à partir de la position du robot, si celle ci dérive, tout dérive. Chaque machine initialisée à la mauvaise position, ou chaque élément détecté comme machine mais n'en étant pas une (faux positif) perturbe le recalage.

De nombreux tests ayant été fait sur la piste officiel avec différentes configurations nous ont permis de vérifier que l'odométrie mesurée ne dérivait que très peu. Il a donc été décidé de n'utiliser que l'odométrie et le scan laser pour la partie '''exploration'''. En effet, chaque machine mesurée est envoyée à un noeud ROS qui va créer une carte et la compléter au fur et à mesure de la partie. Le robot est donc repéré rien qu'avec son odométrie. Le fait que la phase d'exploration (découverte du terrain) ne dure que '''4 minutes''' nous a convaincu pour n'utiliser que cette mesure.

De plus, la carte créée sera utilisée pour la phase suivante, la phase de '''production'''. Cette phase consiste, au niveau de la localisation, à se repérer sur la piste et atteindre la machine demandée par l'exécuteur de tâches. Pour cette phase qui dure '''15 minutes''', l'utilisation du filtre est nécessaire. Donc, avec les coordonnées des machines mesurées pendant la première phase, on peut créer le vecteur d'état et la matrice faisant le lien entre les machines. De là, la taille de ces deux éléments ne variant plus, on peut corriger uniquement la position du robot et le filtre sera efficace.

====Compétition Open German - 1er jour====

Durant ce premier jour, nous avons commencé à ''merger'' nos branches respectives pour pouvoir faire tourner tous nos programmes. Cette phase est laborieuse et nous a pris du temps. De toute façon, toutes les équipes ne sont pas prêtes, les organisateurs non plus. La '''Referee Box''' n'est pas fonctionnel pour la phase de production, ce qui veut dire que nous nous concentrons actuellement sur l'exploration. Les matchs joués aujourd'hui ne nous rapporte donc pas de points. Les tests nous ont permis de valider individuellement chacun des algos, il ne reste "plus qu'à" rejoindre le tout.

====Compétition Open German - 2eme jour====

Niveau '''localisation''', la navigation à l'odométrie et la détection de machine sont fonctionnelles. Cependant, le moyennage fait sur les positions machines (on moyenne toutes les positions trouvées appartenant à une même machine pour n'avoir qu'un seul point) fait que le point correspondant peut être perturbé par des mesures aberrantes. C'est le cas lorsque le robot se déplace à vitesse élevée.

La détection de machine a été améliorée afin de ne rajouter QUE les machines, et pas les murs de la même taille. Nous allons donc retester le '''filtre de Kalman Etendu''' avec cette amélioration. Cependant, ce ne sera fait qu'après la compétition, puisque d'autres problèmes ont nécessité la présence de l'équipe sur certaines parties sensibles du déplacement.

Sur la piste nous avons fait des mesures, qui nous donnent la carte suivante :

[[Fichier:PisteSamediSoir.PNG | center]]

La visualisation du trajet pour découvrir ces machines est le suivant :

[[Fichier:DetectionSamediSoir.PNG | center | thumb]]

Et les positions machines, ainsi que leur orientation sont les suivantes :

[[Fichier:MachinesBagtestSamediSoir.PNG | center | thumb]]

====Compétition Open German - 3eme et dernier jour====

L'architecture choisie pour la localisation est la suivante :
*Navigation à l'odométrie
*Détection de machines et moyennage
*Quand on a vu toutes nos machines, on utilise la symétrie pour compléter les machines restantes
*Navigation et Localisation avec l'EKF (correction des positions machines et du robot)

====Localisation - Extended Kalman Filter====

Après de longs tests et des corrections importantes au niveau de l'ajout des machines dans le filtre, ce qui était la partie critique... Le meilleur résultat que l'on ait pu avoir avec l'EKF est celui ci dessous. Les machines étant en violet, la trajectoire calculée par l'EKF en vert et la trajectoire mesurée par les codeurs en rouge.

[[Fichier:BestResultWithEKF.PNG | center]]

RoboCup 2015 - Pyro Team

2015-04-30T12:54:04Z

Tdanel : /* Compétition Open German - 3eme et dernier jour */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

===Semaine 10===

====Localisation - Extended Kalman Filter====

La partie '''prédiction''' du filtre marche parfaitement, quelques corrections ont dues être faites au niveau de la commande vitesse. En effet, Nous avions oublié de la transposer dans le repère global. Pour la partie '''correction''', nous rencontrons un problème de divergence du filtre, qui est probablement due à une mauvaise initialisation. En effet, nous utilisons des enregistrements faits sur notre piste (1/4 de la vraie) avec des initialisations correspondant à la piste originale. Le filtre ne peut donc pas converger car la position de départ est fausse.

===Semaine 11===

====Localisation - Extended Kalman Filter====

Des corrections sont en cours de validation. Nous essayons de définir une initialisation avec les paramètres de notre piste, ce qui n'est pas évident. Les programmes permettant d'extraire les positions machines du filtres ont été réalisé, ainsi qu'un launch pour pouvoir lancer tous les noeuds de la localisation.

===Semaine 12===

====Compétition Open German - mise en place====

Pendant les deux jours de mise en place, nous avons continué à travailler sur la localisation, qui est une partie critique et nécessaire à notre participation. Le filtre en lui même est fonctionnel et permet de corriger la position du robot, ainsi que celle des machines. Cependant, et ce n'est pas un petit "cependant"... Le fait que l'on ajoute des machines en cours de jeu perturbe fortement notre algorithme.

En effet, le filtre de Kalman est optimal si la position des repères est connu, or, un ajout de machine crée un recalage forcé, et si l'on voit plusieurs machines... C'est la '''catastrophe'''. L'ajout de machines est une partie sensible qui nécessite des tests sur la position mesurée, avec un changement de repère à partir de la position du robot, si celle ci dérive, tout dérive. Chaque machine initialisée à la mauvaise position, ou chaque élément détecté comme machine mais n'en étant pas une (faux positif) perturbe le recalage.

De nombreux tests ayant été fait sur la piste officiel avec différentes configurations nous ont permis de vérifier que l'odométrie mesurée ne dérivait que très peu. Il a donc été décidé de n'utiliser que l'odométrie et le scan laser pour la partie '''exploration'''. En effet, chaque machine mesurée est envoyée à un noeud ROS qui va créer une carte et la compléter au fur et à mesure de la partie. Le robot est donc repéré rien qu'avec son odométrie. Le fait que la phase d'exploration (découverte du terrain) ne dure que '''4 minutes''' nous a convaincu pour n'utiliser que cette mesure.

De plus, la carte créée sera utilisée pour la phase suivante, la phase de '''production'''. Cette phase consiste, au niveau de la localisation, à se repérer sur la piste et atteindre la machine demandée par l'exécuteur de tâches. Pour cette phase qui dure '''15 minutes''', l'utilisation du filtre est nécessaire. Donc, avec les coordonnées des machines mesurées pendant la première phase, on peut créer le vecteur d'état et la matrice faisant le lien entre les machines. De là, la taille de ces deux éléments ne variant plus, on peut corriger uniquement la position du robot et le filtre sera efficace.

====Compétition Open German - 1er jour====

Durant ce premier jour, nous avons commencé à ''merger'' nos branches respectives pour pouvoir faire tourner tous nos programmes. Cette phase est laborieuse et nous a pris du temps. De toute façon, toutes les équipes ne sont pas prêtes, les organisateurs non plus. La '''Referee Box''' n'est pas fonctionnel pour la phase de production, ce qui veut dire que nous nous concentrons actuellement sur l'exploration. Les matchs joués aujourd'hui ne nous rapporte donc pas de points. Les tests nous ont permis de valider individuellement chacun des algos, il ne reste "plus qu'à" rejoindre le tout.

====Compétition Open German - 2eme jour====

Niveau '''localisation''', la navigation à l'odométrie et la détection de machine sont fonctionnelles. Cependant, le moyennage fait sur les positions machines (on moyenne toutes les positions trouvées appartenant à une même machine pour n'avoir qu'un seul point) fait que le point correspondant peut être perturbé par des mesures aberrantes. C'est le cas lorsque le robot se déplace à vitesse élevée.

La détection de machine a été améliorée afin de ne rajouter QUE les machines, et pas les murs de la même taille. Nous allons donc retester le '''filtre de Kalman Etendu''' avec cette amélioration. Cependant, ce ne sera fait qu'après la compétition, puisque d'autres problèmes ont nécessité la présence de l'équipe sur certaines parties sensibles du déplacement.

Sur la piste nous avons fait des mesures, qui nous donnent la carte suivante :

[[Fichier:PisteSamediSoir.PNG | center]]

La visualisation du trajet pour découvrir ces machines est le suivant :

[[Fichier:DetectionSamediSoir.PNG | center | thumb]]

Et les positions machines, ainsi que leur orientation sont les suivantes :

[[Fichier:MachinesBagtestSamediSoir.PNG | center | thumb]]

====Compétition Open German - 3eme et dernier jour====

L'architecture choisie pour la localisation est la suivante :
*Navigation à l'odométrie
*Détection de machines et moyennage
*Quand on a vu toutes nos machines, on utilise la symétrie pour compléter les machines restantes
*Navigation et Localisation avec l'EKF (correction des positions machines et du robot)

====Localisation - Extended Kalman Filter====

Après de longs tests et des corrections importantes au niveau de l'ajout des machines dans le filtre, ce qui était la partie critique... Le meilleur résultat que l'on ait pu avoir avec l'EKF est celui ci dessous. Les machines étant en violet, la trajectoire calculée par l'EKF en vert et la trajectoire mesurée par les codeurs en rouge.

[[Fichier:BestResultWithEKF.PNG]]

RoboCup 2015 - Pyro Team

2015-04-28T13:05:10Z

Tdanel : /* Compétition Open German - 2eme jour */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

===Semaine 10===

====Localisation - Extended Kalman Filter====

La partie '''prédiction''' du filtre marche parfaitement, quelques corrections ont dues être faites au niveau de la commande vitesse. En effet, Nous avions oublié de la transposer dans le repère global. Pour la partie '''correction''', nous rencontrons un problème de divergence du filtre, qui est probablement due à une mauvaise initialisation. En effet, nous utilisons des enregistrements faits sur notre piste (1/4 de la vraie) avec des initialisations correspondant à la piste originale. Le filtre ne peut donc pas converger car la position de départ est fausse.

===Semaine 11===

====Localisation - Extended Kalman Filter====

Des corrections sont en cours de validation. Nous essayons de définir une initialisation avec les paramètres de notre piste, ce qui n'est pas évident. Les programmes permettant d'extraire les positions machines du filtres ont été réalisé, ainsi qu'un launch pour pouvoir lancer tous les noeuds de la localisation.

===Semaine 12===

====Compétition Open German - mise en place====

Pendant les deux jours de mise en place, nous avons continué à travailler sur la localisation, qui est une partie critique et nécessaire à notre participation. Le filtre en lui même est fonctionnel et permet de corriger la position du robot, ainsi que celle des machines. Cependant, et ce n'est pas un petit "cependant"... Le fait que l'on ajoute des machines en cours de jeu perturbe fortement notre algorithme.

En effet, le filtre de Kalman est optimal si la position des repères est connu, or, un ajout de machine crée un recalage forcé, et si l'on voit plusieurs machines... C'est la '''catastrophe'''. L'ajout de machines est une partie sensible qui nécessite des tests sur la position mesurée, avec un changement de repère à partir de la position du robot, si celle ci dérive, tout dérive. Chaque machine initialisée à la mauvaise position, ou chaque élément détecté comme machine mais n'en étant pas une (faux positif) perturbe le recalage.

De nombreux tests ayant été fait sur la piste officiel avec différentes configurations nous ont permis de vérifier que l'odométrie mesurée ne dérivait que très peu. Il a donc été décidé de n'utiliser que l'odométrie et le scan laser pour la partie '''exploration'''. En effet, chaque machine mesurée est envoyée à un noeud ROS qui va créer une carte et la compléter au fur et à mesure de la partie. Le robot est donc repéré rien qu'avec son odométrie. Le fait que la phase d'exploration (découverte du terrain) ne dure que '''4 minutes''' nous a convaincu pour n'utiliser que cette mesure.

De plus, la carte créée sera utilisée pour la phase suivante, la phase de '''production'''. Cette phase consiste, au niveau de la localisation, à se repérer sur la piste et atteindre la machine demandée par l'exécuteur de tâches. Pour cette phase qui dure '''15 minutes''', l'utilisation du filtre est nécessaire. Donc, avec les coordonnées des machines mesurées pendant la première phase, on peut créer le vecteur d'état et la matrice faisant le lien entre les machines. De là, la taille de ces deux éléments ne variant plus, on peut corriger uniquement la position du robot et le filtre sera efficace.

====Compétition Open German - 1er jour====

Durant ce premier jour, nous avons commencé à ''merger'' nos branches respectives pour pouvoir faire tourner tous nos programmes. Cette phase est laborieuse et nous a pris du temps. De toute façon, toutes les équipes ne sont pas prêtes, les organisateurs non plus. La '''Referee Box''' n'est pas fonctionnel pour la phase de production, ce qui veut dire que nous nous concentrons actuellement sur l'exploration. Les matchs joués aujourd'hui ne nous rapporte donc pas de points. Les tests nous ont permis de valider individuellement chacun des algos, il ne reste "plus qu'à" rejoindre le tout.

====Compétition Open German - 2eme jour====

Niveau '''localisation''', la navigation à l'odométrie et la détection de machine sont fonctionnelles. Cependant, le moyennage fait sur les positions machines (on moyenne toutes les positions trouvées appartenant à une même machine pour n'avoir qu'un seul point) fait que le point correspondant peut être perturbé par des mesures aberrantes. C'est le cas lorsque le robot se déplace à vitesse élevée.

La détection de machine a été améliorée afin de ne rajouter QUE les machines, et pas les murs de la même taille. Nous allons donc retester le '''filtre de Kalman Etendu''' avec cette amélioration. Cependant, ce ne sera fait qu'après la compétition, puisque d'autres problèmes ont nécessité la présence de l'équipe sur certaines parties sensibles du déplacement.

Sur la piste nous avons fait des mesures, qui nous donnent la carte suivante :

[[Fichier:PisteSamediSoir.PNG | center]]

La visualisation du trajet pour découvrir ces machines est le suivant :

[[Fichier:DetectionSamediSoir.PNG | center | thumb]]

Et les positions machines, ainsi que leur orientation sont les suivantes :

[[Fichier:MachinesBagtestSamediSoir.PNG | center | thumb]]

====Compétition Open German - 3eme et dernier jour====

L'architecture choisie pour la localisation est la suivante :
*Navigation à l'odométrie
*Détection de machines et moyennage
*Quand on a vu toutes nos machines, on utilise la symétrie pour compléter les machines restantes
*Navigation et Localisation avec l'EKF (correction des positions machines et du robot)

Fichier:MachinesBagtestSamediSoir.PNG

2015-04-28T13:04:06Z

Tdanel :

Fichier:PisteSamediSoir.PNG

2015-04-28T13:02:17Z

Tdanel :

Fichier:DetectionSamediSoir.PNG

2015-04-28T13:01:31Z

Tdanel :

RoboCup 2015 - Pyro Team

2015-04-28T13:00:54Z

Tdanel : /* Compétition Open German - 2eme jour */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

===Semaine 10===

====Localisation - Extended Kalman Filter====

La partie '''prédiction''' du filtre marche parfaitement, quelques corrections ont dues être faites au niveau de la commande vitesse. En effet, Nous avions oublié de la transposer dans le repère global. Pour la partie '''correction''', nous rencontrons un problème de divergence du filtre, qui est probablement due à une mauvaise initialisation. En effet, nous utilisons des enregistrements faits sur notre piste (1/4 de la vraie) avec des initialisations correspondant à la piste originale. Le filtre ne peut donc pas converger car la position de départ est fausse.

===Semaine 11===

====Localisation - Extended Kalman Filter====

Des corrections sont en cours de validation. Nous essayons de définir une initialisation avec les paramètres de notre piste, ce qui n'est pas évident. Les programmes permettant d'extraire les positions machines du filtres ont été réalisé, ainsi qu'un launch pour pouvoir lancer tous les noeuds de la localisation.

===Semaine 12===

====Compétition Open German - mise en place====

Pendant les deux jours de mise en place, nous avons continué à travailler sur la localisation, qui est une partie critique et nécessaire à notre participation. Le filtre en lui même est fonctionnel et permet de corriger la position du robot, ainsi que celle des machines. Cependant, et ce n'est pas un petit "cependant"... Le fait que l'on ajoute des machines en cours de jeu perturbe fortement notre algorithme.

En effet, le filtre de Kalman est optimal si la position des repères est connu, or, un ajout de machine crée un recalage forcé, et si l'on voit plusieurs machines... C'est la '''catastrophe'''. L'ajout de machines est une partie sensible qui nécessite des tests sur la position mesurée, avec un changement de repère à partir de la position du robot, si celle ci dérive, tout dérive. Chaque machine initialisée à la mauvaise position, ou chaque élément détecté comme machine mais n'en étant pas une (faux positif) perturbe le recalage.

De nombreux tests ayant été fait sur la piste officiel avec différentes configurations nous ont permis de vérifier que l'odométrie mesurée ne dérivait que très peu. Il a donc été décidé de n'utiliser que l'odométrie et le scan laser pour la partie '''exploration'''. En effet, chaque machine mesurée est envoyée à un noeud ROS qui va créer une carte et la compléter au fur et à mesure de la partie. Le robot est donc repéré rien qu'avec son odométrie. Le fait que la phase d'exploration (découverte du terrain) ne dure que '''4 minutes''' nous a convaincu pour n'utiliser que cette mesure.

De plus, la carte créée sera utilisée pour la phase suivante, la phase de '''production'''. Cette phase consiste, au niveau de la localisation, à se repérer sur la piste et atteindre la machine demandée par l'exécuteur de tâches. Pour cette phase qui dure '''15 minutes''', l'utilisation du filtre est nécessaire. Donc, avec les coordonnées des machines mesurées pendant la première phase, on peut créer le vecteur d'état et la matrice faisant le lien entre les machines. De là, la taille de ces deux éléments ne variant plus, on peut corriger uniquement la position du robot et le filtre sera efficace.

====Compétition Open German - 1er jour====

Durant ce premier jour, nous avons commencé à ''merger'' nos branches respectives pour pouvoir faire tourner tous nos programmes. Cette phase est laborieuse et nous a pris du temps. De toute façon, toutes les équipes ne sont pas prêtes, les organisateurs non plus. La '''Referee Box''' n'est pas fonctionnel pour la phase de production, ce qui veut dire que nous nous concentrons actuellement sur l'exploration. Les matchs joués aujourd'hui ne nous rapporte donc pas de points. Les tests nous ont permis de valider individuellement chacun des algos, il ne reste "plus qu'à" rejoindre le tout.

====Compétition Open German - 2eme jour====

Niveau '''localisation''', la navigation à l'odométrie et la détection de machine sont fonctionnelles. Cependant, le moyennage fait sur les positions machines (on moyenne toutes les positions trouvées appartenant à une même machine pour n'avoir qu'un seul point) fait que le point correspondant peut être perturbé par des mesures aberrantes. C'est le cas lorsque le robot se déplace à vitesse élevée.

La détection de machine a été améliorée afin de ne rajouter QUE les machines, et pas les murs de la même taille. Nous allons donc retester le '''filtre de Kalman Etendu''' avec cette amélioration. Cependant, ce ne sera fait qu'après la compétition, puisque d'autres problèmes ont nécessité la présence de l'équipe sur certaines parties sensibles du déplacement.

Sur la piste nous avons fait des mesures, qui nous donnent la carte suivante :

[[Fichier:PisteSamediSoir.PNG]]

La visualisation du trajet pour découvrir ces machines est le suivant :

[[Fichier:DetectionSamediSoir.PNG]]

Et les positions machines, ainsi que leur orientation sont les suivantes :

[[Fichier:MachinesBagtestSamediSoir.PNG]]

====Compétition Open German - 3eme et dernier jour====

L'architecture choisie pour la localisation est la suivante :
*Navigation à l'odométrie
*Détection de machines et moyennage
*Quand on a vu toutes nos machines, on utilise la symétrie pour compléter les machines restantes
*Navigation et Localisation avec l'EKF (correction des positions machines et du robot)

RoboCup 2015 - Pyro Team

2015-04-28T10:53:38Z

Tdanel : /* Compétition Open German - 1er jour */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

===Semaine 10===

====Localisation - Extended Kalman Filter====

La partie '''prédiction''' du filtre marche parfaitement, quelques corrections ont dues être faites au niveau de la commande vitesse. En effet, Nous avions oublié de la transposer dans le repère global. Pour la partie '''correction''', nous rencontrons un problème de divergence du filtre, qui est probablement due à une mauvaise initialisation. En effet, nous utilisons des enregistrements faits sur notre piste (1/4 de la vraie) avec des initialisations correspondant à la piste originale. Le filtre ne peut donc pas converger car la position de départ est fausse.

===Semaine 11===

====Localisation - Extended Kalman Filter====

Des corrections sont en cours de validation. Nous essayons de définir une initialisation avec les paramètres de notre piste, ce qui n'est pas évident. Les programmes permettant d'extraire les positions machines du filtres ont été réalisé, ainsi qu'un launch pour pouvoir lancer tous les noeuds de la localisation.

===Semaine 12===

====Compétition Open German - mise en place====

Pendant les deux jours de mise en place, nous avons continué à travailler sur la localisation, qui est une partie critique et nécessaire à notre participation. Le filtre en lui même est fonctionnel et permet de corriger la position du robot, ainsi que celle des machines. Cependant, et ce n'est pas un petit "cependant"... Le fait que l'on ajoute des machines en cours de jeu perturbe fortement notre algorithme.

En effet, le filtre de Kalman est optimal si la position des repères est connu, or, un ajout de machine crée un recalage forcé, et si l'on voit plusieurs machines... C'est la '''catastrophe'''. L'ajout de machines est une partie sensible qui nécessite des tests sur la position mesurée, avec un changement de repère à partir de la position du robot, si celle ci dérive, tout dérive. Chaque machine initialisée à la mauvaise position, ou chaque élément détecté comme machine mais n'en étant pas une (faux positif) perturbe le recalage.

De nombreux tests ayant été fait sur la piste officiel avec différentes configurations nous ont permis de vérifier que l'odométrie mesurée ne dérivait que très peu. Il a donc été décidé de n'utiliser que l'odométrie et le scan laser pour la partie '''exploration'''. En effet, chaque machine mesurée est envoyée à un noeud ROS qui va créer une carte et la compléter au fur et à mesure de la partie. Le robot est donc repéré rien qu'avec son odométrie. Le fait que la phase d'exploration (découverte du terrain) ne dure que '''4 minutes''' nous a convaincu pour n'utiliser que cette mesure.

De plus, la carte créée sera utilisée pour la phase suivante, la phase de '''production'''. Cette phase consiste, au niveau de la localisation, à se repérer sur la piste et atteindre la machine demandée par l'exécuteur de tâches. Pour cette phase qui dure '''15 minutes''', l'utilisation du filtre est nécessaire. Donc, avec les coordonnées des machines mesurées pendant la première phase, on peut créer le vecteur d'état et la matrice faisant le lien entre les machines. De là, la taille de ces deux éléments ne variant plus, on peut corriger uniquement la position du robot et le filtre sera efficace.

====Compétition Open German - 1er jour====

Durant ce premier jour, nous avons commencé à ''merger'' nos branches respectives pour pouvoir faire tourner tous nos programmes. Cette phase est laborieuse et nous a pris du temps. De toute façon, toutes les équipes ne sont pas prêtes, les organisateurs non plus. La '''Referee Box''' n'est pas fonctionnel pour la phase de production, ce qui veut dire que nous nous concentrons actuellement sur l'exploration. Les matchs joués aujourd'hui ne nous rapporte donc pas de points. Les tests nous ont permis de valider individuellement chacun des algos, il ne reste "plus qu'à" rejoindre le tout.

====Compétition Open German - 2eme jour====

Niveau '''localisation''', la navigation à l'odométrie et la détection de machine sont fonctionnelles. Cependant, le moyennage fait sur les positions machines (on moyenne toutes les positions trouvées appartenant à une même machine pour n'avoir qu'un seul point) fait que le point correspondant peut être perturbé par des mesures aberrantes. C'est le cas lorsque le robot se déplace à vitesse élevée.

La détection de machine a été améliorée afin de ne rajouter QUE les machines, et pas les murs de la même taille. Nous allons donc retester le '''filtre de Kalman Etendu''' avec cette amélioration. Cependant, ce ne sera fait qu'après la compétition, puisque d'autres problèmes ont nécessité la présence de l'équipe sur certaines parties sensibles du déplacement.

====Compétition Open German - 3eme et dernier jour====

L'architecture choisie pour la localisation est la suivante :
*Navigation à l'odométrie
*Détection de machines et moyennage
*Quand on a vu toutes nos machines, on utilise la symétrie pour compléter les machines restantes
*Navigation et Localisation avec l'EKF (correction des positions machines et du robot)

RoboCup 2015 - Pyro Team

2015-04-24T20:21:39Z

Tdanel : /* Compétition Open German */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

===Semaine 10===

====Localisation - Extended Kalman Filter====

La partie '''prédiction''' du filtre marche parfaitement, quelques corrections ont dues être faites au niveau de la commande vitesse. En effet, Nous avions oublié de la transposer dans le repère global. Pour la partie '''correction''', nous rencontrons un problème de divergence du filtre, qui est probablement due à une mauvaise initialisation. En effet, nous utilisons des enregistrements faits sur notre piste (1/4 de la vraie) avec des initialisations correspondant à la piste originale. Le filtre ne peut donc pas converger car la position de départ est fausse.

===Semaine 11===

====Localisation - Extended Kalman Filter====

Des corrections sont en cours de validation. Nous essayons de définir une initialisation avec les paramètres de notre piste, ce qui n'est pas évident. Les programmes permettant d'extraire les positions machines du filtres ont été réalisé, ainsi qu'un launch pour pouvoir lancer tous les noeuds de la localisation.

===Semaine 12===

====Compétition Open German - mise en place====

Pendant les deux jours de mise en place, nous avons continué à travailler sur la localisation, qui est une partie critique et nécessaire à notre participation. Le filtre en lui même est fonctionnel et permet de corriger la position du robot, ainsi que celle des machines. Cependant, et ce n'est pas un petit "cependant"... Le fait que l'on ajoute des machines en cours de jeu perturbe fortement notre algorithme.

En effet, le filtre de Kalman est optimal si la position des repères est connu, or, un ajout de machine crée un recalage forcé, et si l'on voit plusieurs machines... C'est la '''catastrophe'''. L'ajout de machines est une partie sensible qui nécessite des tests sur la position mesurée, avec un changement de repère à partir de la position du robot, si celle ci dérive, tout dérive. Chaque machine initialisée à la mauvaise position, ou chaque élément détecté comme machine mais n'en étant pas une (faux positif) perturbe le recalage.

De nombreux tests ayant été fait sur la piste officiel avec différentes configurations nous ont permis de vérifier que l'odométrie mesurée ne dérivait que très peu. Il a donc été décidé de n'utiliser que l'odométrie et le scan laser pour la partie '''exploration'''. En effet, chaque machine mesurée est envoyée à un noeud ROS qui va créer une carte et la compléter au fur et à mesure de la partie. Le robot est donc repéré rien qu'avec son odométrie. Le fait que la phase d'exploration (découverte du terrain) ne dure que '''4 minutes''' nous a convaincu pour n'utiliser que cette mesure.

De plus, la carte créée sera utilisée pour la phase suivante, la phase de '''production'''. Cette phase consiste, au niveau de la localisation, à se repérer sur la piste et atteindre la machine demandée par l'exécuteur de tâches. Pour cette phase qui dure '''15 minutes''', l'utilisation du filtre est nécessaire. Donc, avec les coordonnées des machines mesurées pendant la première phase, on peut créer le vecteur d'état et la matrice faisant le lien entre les machines. De là, la taille de ces deux éléments ne variant plus, on peut corriger uniquement la position du robot et le filtre sera efficace.

====Compétition Open German - 1er jour====

Durant ce premier jour, nous avons commencé à ''merger'' nos branches respectives pour pouvoir faire tourner tous nos programmes. Cette phase est laborieuse et nous a pris du temps. De toute façon, toutes les équipes ne sont pas prêtes, les organisateurs non plus. La '''Referee Box''' n'est pas fonctionnel pour la phase de production, ce qui veut dire que nous nous concentrons actuellement sur l'exploration. Les matchs joués aujourd'hui ne nous rapporte donc pas de points. Les tests nous ont permis de valider individuellement chacun des algos, il ne reste "plus qu'à" rejoindre le tout.

RoboCup 2015 - Pyro Team

2015-04-24T19:26:09Z

Tdanel : /* Semaine 11 */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

===Semaine 10===

====Localisation - Extended Kalman Filter====

La partie '''prédiction''' du filtre marche parfaitement, quelques corrections ont dues être faites au niveau de la commande vitesse. En effet, Nous avions oublié de la transposer dans le repère global. Pour la partie '''correction''', nous rencontrons un problème de divergence du filtre, qui est probablement due à une mauvaise initialisation. En effet, nous utilisons des enregistrements faits sur notre piste (1/4 de la vraie) avec des initialisations correspondant à la piste originale. Le filtre ne peut donc pas converger car la position de départ est fausse.

===Semaine 11===

====Localisation - Extended Kalman Filter====

Des corrections sont en cours de validation. Nous essayons de définir une initialisation avec les paramètres de notre piste, ce qui n'est pas évident. Les programmes permettant d'extraire les positions machines du filtres ont été réalisé, ainsi qu'un launch pour pouvoir lancer tous les noeuds de la localisation.

===Semaine 12===

====Compétition Open German====

Pendant les deux jours de mise en place, nous avons continué à travailler sur la localisation, qui est une partie critique et nécessaire à notre participation. Le filtre en lui même est fonctionnel et permet de corriger la position du robot, ainsi que celle des machines. Cependant, et ce n'est pas un petit "cependant"... Le fait que l'on ajoute des machines en cours de jeu perturbe fortement notre algorithme.

En effet, le filtre de Kalman est optimal si la position des repères est connu, or, un ajout de machine crée un recalage forcé, et si l'on voit plusieurs machines... C'est la '''catastrophe'''. L'ajout de machines est une partie sensible qui nécessite des tests sur la position mesurée, avec un changement de repère à partir de la position du robot, si celle ci dérive, tout dérive. Chaque machine initialisée à la mauvaise position, ou chaque élément détecté comme machine mais n'en étant pas une (faux positif) perturbe le recalage.

De nombreux tests ayant été fait sur la piste officiel avec différentes configurations nous ont permis de vérifier que l'odométrie mesurée ne dérivait que très peu. Il a donc été décidé de n'utiliser que l'odométrie et le scan laser pour la partie '''exploration'''. En effet, chaque machine mesurée est envoyée à un noeud ROS qui va créer une carte et la compléter au fur et à mesure de la partie. Le robot est donc repéré rien qu'avec son odométrie. Le fait que la phase d'exploration (découverte du terrain) ne dure que '''4 minutes''' nous a convaincu pour n'utiliser que cette mesure.

De plus, la carte créée sera utilisée pour la phase suivante, la phase de '''production'''. Cette phase consiste, au niveau de la localisation, à se repérer sur la piste et atteindre la machine demandée par l'exécuteur de tâches. Pour cette phase qui dure '''15 minutes''', l'utilisation du filtre est nécessaire. Donc, avec les coordonnées des machines mesurées pendant la première phase, on peut créer le vecteur d'état et la matrice faisant le lien entre les machines. De là, la taille de ces deux éléments ne variant plus, on peut corriger uniquement la position du robot et le filtre sera efficace.

RoboCup 2015 - Pyro Team

2015-04-16T13:07:49Z

Tdanel : /* Semaine 9 */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

===Semaine 10===

====Localisation - Extended Kalman Filter====

La partie '''prédiction''' du filtre marche parfaitement, quelques corrections ont dues être faites au niveau de la commande vitesse. En effet, Nous avions oublié de la transposer dans le repère global. Pour la partie '''correction''', nous rencontrons un problème de divergence du filtre, qui est probablement due à une mauvaise initialisation. En effet, nous utilisons des enregistrements faits sur notre piste (1/4 de la vraie) avec des initialisations correspondant à la piste originale. Le filtre ne peut donc pas converger car la position de départ est fausse.

===Semaine 11===

====Localisation - Extended Kalman Filter====

Des corrections sont en cours de validation. Nous essayons de définir une initialisation avec les paramètres de notre piste, ce qui n'est pas évident. Les programmes permettant d'extraire les positions machines du filtres ont été réalisé, ainsi qu'un launch pour pouvoir lancer tous les noeuds de la localisation.

RoboCup 2015 - Pyro Team

2015-04-12T10:19:17Z

Tdanel : /* Localisation - Extended Kalman Filter */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*(Fx.transpose()); 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

le code pour la partie ''correction'' :

void correction(VectorXd &xMean, MatrixXd &P, Vector3d xPredicted, geometry_msgs::Pose2D m){
int taille = P.rows();
//on vérifie si la machine est présente ou pas dans le vecteur d'état
int i = checkStateVector(xMean, m); 
if (i != 0){
//calcul de H
MatrixXd H(3, taille);
H = buildH(taille, i); 
//calcul de Pm
MatrixXd Pm(taille, taille);
Pm = buildPm(P, i); 
//calcul de Z
MatrixXd Z(taille, taille);
Z = H*Pm*(H.transpose()); 
//calcul du gain de Kalman
MatrixXd K(taille, taille);
K = P*(H.transpose())*(Z.inverse()); 
//mise à jour du vecteur xMean
xMean = xMean + K*(xMean.block(xMean.rows()-3,0,3,1) - xPredicted); 
//mise à jour de la matrice P
P = P - K*Z*(K.transpose());
}
else {
addMachine(m, xMean, P);
}
}

Pour notre cas, on doit ajouter des nouvelles machines, alors que pour un EKF classique on observe juste des machines déjà répertoriées. Il faut donc une fonction qui permet d'ajouter une machine dans le vecteur d'état. La voilà :

void addMachine(geometry_msgs::Pose2D machine, VectorXd &xMean, MatrixXd &P){
//on redimensionne xMean et P pour accueillir la nouvelle machines
xMean.conservativeResize(xMean.rows() + 3);
P.conservativeResize(P.rows()+3,P.cols()+3); 
//on remplit avec les coordonnées de la nouvelle machine
xMean(xMean.rows()-3) = machine.x;
xMean(xMean.rows()-2) = machine.y;
xMean(xMean.rows()-1) = machine.theta; 
//calcul de tous les PLi
//initialisation des PLi à 0
P.block(P.rows() - 3, 0, 3, P.cols()).setZero();
P.block(0, P.cols() - 3, P.rows(), 3).setZero(); 
for (int j = 0; j < xMean.rows(); j = j + 3){
//position de la nouvelle machines par rapport au robot et à toutes les autres
double x = xMean(xMean.rows()-3) - xMean(j );
double y = xMean(xMean.rows()-2) - xMean(j+1);
double theta = xMean(xMean.rows()-1) - xMean(j+2); 
P(P.rows()-3, j ) = x;
P(P.rows()-2, j+1) = y;
P(P.rows()-1, j+2) = theta; 
P(j , P.rows()-3) = x;
P(j+1, P.rows()-2) = y;
P(j+2, P.rows()-1) = theta; 
P(j , j ) = xMean(j ) - xMean(0);
P(j+1, j+1) = xMean(j+1) - xMean(1);
P(j+2, j+2) = xMean(j+2) - xMean(2); 
}
}

RoboCup 2015 - Pyro Team

2015-04-11T16:11:56Z

Tdanel : /* Localisation - Extended Kalman Filter */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

le code pour la partie ''prédiction'' :

//on ressort la position du robot pour l'état n+1
//les paramètres sont le vecteurs où on a la position du robot à l'état n, la matrice P, les commandes en vitesses et le temps à l'état n
Vector3d prediction(VectorXd xMean, MatrixXd &P, geometry_msgs::Pose2D cmdVel, ros::Time &temps){
ros::Duration duree = ros::Time::now() - temps;
double periode = duree.toSec(); 
Vector3d cmdVelVect = Pose2DToVector(cmdVel);
//le xMean.topLeftCorner(3,1) correspond à la position du robot
Vector3d xPredicted = xMean.topLeftCorner(3,1) + periode*cmdVelVect; 
MatrixXd Fx;
Fx = MatrixXd::Identity(P.rows(), P.cols());
Fx(0,0) = xPredicted(0);
Fx(1,1) = xPredicted(1);
Fx(2,2) = xPredicted(2); 
//mise à jour de P
P = Fx*P*Fx; 
//mise à jour du temps
temps = ros::Time::now(); 
return xPredicted;
}

RoboCup 2015 - Pyro Team

2015-04-02T08:23:40Z

Tdanel : /* Génération du noeud Map */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Grid Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

RoboCup 2015 - Pyro Team

2015-04-01T15:28:11Z

Tdanel : /* Génération du noeud Map */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Génération du noeud Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

*Calcul des positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit. Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot en plus par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

*Remplissage des positions sur la map :

On met alors tous les points de ce rectangle à une probabilité 100 dans le vecteur Map. Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi. L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités correspondantes aux cases de ce rectangle, nous utilisons la fonctions floor() pour "convertir" nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions appelées et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

RoboCup 2015 - Pyro Team

2015-04-01T15:23:23Z

Tdanel : /* Génération du noeud Map */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Génération du noeud Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine)) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;

Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67))) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

Maintenant, nous passons aux positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit.
Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

Il reste ensuite à mettre tout les points de se rectangles à une probabilité 100 dans le vecteur Map.
Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi.
L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités de ce rectangle, nous utilisons la fonctions floor pour convertir nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

RoboCup 2015 - Pyro Team

2015-04-01T15:20:38Z

Tdanel : /* Génération du noeud Map */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Génération du noeud Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine et la taille.

Pour la taille on choisit de diviser la map en case de 10 cm et l'origine choisie est le x du centre de la map et en y l'abscisse du premier mur (voir la map ci-dessous). Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions ainsi que le main :

Ce vecteur de Pose2D (x, y et theta (angle de la machine) va permettre d'enregistrer la position des machines, c'est une variable global qui sera mise à jour avec le Callback :

std::vector<geometry_msgs::Pose2D> tab;

*Récupération des machines envoyées par le noeud du SLAM (sur le topic /landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

*Création d'une map vide de taille 14m sur 8m. Chaque case est initialisée à une valeur nulle, on remplit également les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 c'est un point interdit et 0 quand c'est un point sans danger pour le passage du robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;

Map.data.assign(Map.info.width*Map.info.height, 0); //map vide
}

*Construction des points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67)) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
...
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif puisque c'est le remplissage des données connues, donc en "dur".

Maintenant, nous passons aux positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit.
Etant donné que les machines doivent être des rectangles de 110cm sur 75cm (réellement 70 x 35 mais on compte la moitié du robot par sécurité), on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

Il reste ensuite à mettre tout les points de se rectangles à une probabilité 100 dans le vecteur Map.
Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi.
L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités de ce rectangle, nous utilisons la fonctions floor pour convertir nos flottants en entier et accéder à la case correspondante. On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

Ci dessous on a les mains, avec les différentes fonctions et la syntaxe particulière à ROS.

*Noeud général pour le traitement et le remplissage de la map :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

*Noeud test qui publie 2 positions de machines :

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map :

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

RoboCup 2015 - Pyro Team

2015-04-01T14:37:20Z

Tdanel : /* Génération du noeud Map */

__TOC__
 
==Cahier des charges==

[[Fichier:logoPyro_icone.png|center]]

===Présentation générale du projet===

Avec plusieurs IMAs, nous avons créer une équipe pour participer à la [http://www.robocup-logistics.org/ Logistic League] de la RoboCup. Lors de la compétition de l'Open German, version européenne de la RoboCup, il faudra mettre en place un système autonome de production à l'aide de Robotinos (robots mobiles de Festo ayant un système d'exploitation Linux) afin de réaliser des produits en fonction des demandes de l'arbitre du jeu : la Referee Box. Nous traiterons dans ce projet de l'aspect navigation des robots, composé d'une partie localisation et d'une partie déplacement.

====Contexte====

La compétition de l'Open German se déroule en quatre phases spécifiques : phase de début-de-jeu, phase d'exploration, phase de production, phase de fin-de-jeu. La navigation sera utilisée dans la phase d'exploration afin de réaliser une carte de la zone de jeu permettant de définir des zones de passage entre les machines. Dans la phase de production, la navigation permettra de se déplacer à partir de la carte créée au préalable (avec les obstacles fixes) et des robots (obstacles mobiles) se déplaçant en même temps.

====Objectif du projet====

Fournir aux Robotinos un système capable de se localiser et de parcourir des trajectoires calculées à partir de coordonnées envoyées par le [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager].

====Description du projet====

[[Fichier:ProjetS8.png|thumb]]

*Localiser correctement le robot (à 5 cm près)
*Localiser les éléments fixes :
**Murs
**Machines
*Générer une trajectoire selon :
**les demandes du "[http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]"
**la détection d'obstacles dynamiques (robots)
*Assurer le suivi de la trajectoire

====Choix techniques : matériel et logiciel====

*Utilisation de 3 Robotinos équipés chacun de :
**1 détecteur laser pouvant réaliser des mesures à 240°
**1 gyroscope
**9 capteurs SHARP (télémètres infrarouges)
**3 codeurs incrémentaux présents en sortie de chaque moteur du Robotino
*Utilisation de [http://wiki.ros.org/hydro ROS Hydro]
*Utilisation de différents langages : C++ ou Python
*Utilisation de Linux Ubuntu 12.04

===Etapes du projet===

*Conception du schéma global des différentes parties :
**Localisation avec le SLAM
**Création de la carte avec le SLAM
**Génération de trajectoire avec A-Star
**Execution de trajectoire avec le noeud ROS pour Robotino robotino_local_move

*Codage des différents noeuds ROS

*Tests de précision et de robustesse sur robot

*Validation du modèle

*Améliorations possibles :
**Fusionner les maps des 3 robots
**D-Star Lite au lieu de A-Star
**Filtrage des données laser

====Vocabulaire====

Nos comptes rendus peuvent contenir des expressions que nous employons régulièrement mais qui ne sont parfois pas évidentes. Nous proposons alors cette partie vocabulaire afin d'aider le lecteur à comprendre ces expressions.

* '''Noeud :''' C'est un programme qui tourne de manière indépendante et qui réalise une tâche spécifique.

* '''Type :''' C'est un format spécifique de message. Il définit les valeurs que peut prendre une donnée, ainsi que les opérateurs qui peuvent lui être appliqués.

* '''Odométrie :''' L’odométrie est une technique permettant d'estimer la position d'un véhicule en mouvement. Cette mesure de bas niveau est présente sur quasiment tous les robots mobiles, grâce à des capteurs embarqués permettant de mesurer le déplacement du robot (de ses roues).

==Avancement du Projet==
===Semaine 1===

*Participation aux Finales Nationales des Olympiades des Métiers en robotique mobile (4ème)

===Semaine 2===

====Etablissement du cahier des charges====

*Prise de connaissance des contraintes d'environnement
*Dialogue avec l'équipe responsable de la partie [http://projets-imasc.plil.net/mediawiki/index.php?title=RoboCup_2015 Manager]
Familiarisation avec l'environnement logiciel ROS
Recherches de solutions pour la localisation (SLAM) et la génération de trajectoire (algorithme A-star)

====Recherche d'un algorithme de recherche de chemin====

Nous avons fait nos recherches en nous orientant sur le déplacement du Robotino afin de trouver un algorithme de recherche de chemin, capable de trouver le chemin le plus court, puisqu'un des buts de la compétition étant la rapidité. 
Plusieurs algorithmes sont donc ressortis : 
*L'algorithme Dijkstra (On part du point de départ et on cherche le chemin le plus court vers le point d'arrivée en cherchant dans TOUTES les directions) 
*L'algorithme A-Star (Basé sur Dijkstra, cet algo s'oriente directement vers le point d'arrivée en minimisant la distance avec celui-ci) 
*L'algorithme D-Star Lite (Amélioration du A-Star avec bufferisation des précédents calculs pour éviter les calculs inutiles en cas d'arrivée d'un obstacle mobile sur la trajectoire) 

Nous avons visualisé les différents algorithmes sur [http://qiao.github.io/PathFinding.js/visual/ PathFinding]

Fonctionnement de l'algorithme A-Star : 

*Tout d'abord, on calcule la distance qui sépare le robot du point d'arrivée, et on définit les points interdits par les obstacles. (la zone bleue correspond
à l'obstacle et la zone verte est une zone ou le robot touche l'obstacle. Ces deux zones constituent la zone interdite). 

[[Image:Projet_S8_A_STAR_1.PNG | 400px | center]][[Image:Projet_S8_A_STAR_2.PNG | 400px | center]]

*Ensuite le Robot trouve un premier noeud sur la grille qu'on lui a attribué. Si le noeud n'est pas dans une zone interdite, il sauvegarde ce noeud.

[[Image:Projet_S8_A_STAR_3.PNG | 400px | center]][[Image:Projet_S8_A_STAR_4.PNG | 400px | center]]

*Il répète l'opération jusqu'à trouver le chemin qui le mènera à l'arrivée 

[[Image:Projet_S8_A_STAR_5.PNG | 400px | center]]

*Attention : c'est seulement lorsqu'il aura trouvé le chemin complet que le robot va se déplacer !

====Recherche d'une solution pour la localisation====

Pour la localisation nous allons utilisé un '''''Algorithme de Localisation et Cartographie Simultanée''''' (SLAM en anglais):

[[Fichier:SLAM.png | center | thumb]]

Explicitation des différentes parties :

*Odometry change : changement de position
*Odometry update : mise à jour de l'odométrie
*Re-observation : vérification des changements à partir de toutes les données disponibles
*New observations : mise à jours de la matrice d'état

*Laser scan : scan laser brut
*Landmark Extraction : extraction des objets caractéristiques
*Data Association : association entre objets scannés et objets en mémoire

Ce schéma sera explicité à la séance prochaine.

===Semaine 3===

====Découpage du travail à effectuer====

Nous avons découper le travail de génération de map et de la correction de position de la manière suivante :

[[Fichier:SchémaProjetS8-1.png | 800px | center]]

( Nous recommandons au lecteur de se référer à la partie vocabulaire pour comprendre les termes techniques )

Chaque case du diagramme précédent correspond à un noeud ROS.

Décrivons précisément le rôle de chaque noeud :
*'''Laser''' récupère les données brutes du laser.
*'''Filtrage''' permet, comme son nom l'indique, d'enlever les éventuels bruits qui affectent les données du laser.
*'''Reconnaissance des droites''' traite les données laser filtrées pour détecter les droites présentes (Transformée de Hough ou Ransac)
*'''Reconnaissance des formes''' extrapole les objets (machines) à partir des droites.
*'''Corrélation emplacements machines''' lie la position des machines détectées au laser et les positions envoyées par RefBoxCom pour éviter les incohérences.
*'''Odométrie''' récupère la position du robot par rapport au point de départ. Il renvoie une position corrigée grâce au gyroscope(Pose) et une position non corrigée par le gyroscope (Twist)
*'''Traitement de position''' corrige l'erreur sur le Twist avec les données du gyroscope (par dérivation).
*'''Fusion de Kalman''' est un algorithme qui va coupler les données de position et les données du laser afin de fournir un modèle et une position précise. C'est le noeud central du SLAM.
*'''Stockage machines + robot''' transforme les positions des machines et du robot en grille. Il sera couplé aux détections d'obstacles.
*'''Grid Map''' renvoit une map utilisée ensuite pour les déplacements et les calculs de trajectoires (A_Star)
*'''RefBoxCom''' stocke et renvoie la position des machines.
*'''Position robot''' renvoie la position exacte du robot sur la grille.
*'''Manager''' Noeud qui envoie la position à atteindre.
*'''Pathfinder''' recherche le chemin demandé par le Manager sur une grille
*'''Parcoureur''' exécute le chemin trouvé en fournissant les vitesses au noeud Déplacement.
*'''Déplacement''' (noeud robotino_local_move_server), qui donne les commandes aux moteurs à partir de la vitesse linéaire et angulaire envoyée par le Parcoureur.

====Recherche de documentation sur ROS et l'API du Robotino====

Nous avons cherchés toutes les documentations techniques nécessaires pour la programmation de chaque noeud : [http://wiki.ros.org/fr Wiki_ROS] et [http://wiki.ros.org/robotino API_Robotino], nous permettant de mettre à jour le schéma global, notamment au niveau des types.

====Etude de l'algorithme A-star précédent====

Nous nous sommes ensuite concentrés sur le noeud GridMap qui contient ''Map.cpp'' :

Dans le code de l'an dernier, ce noeud générait une grille en dur. Cependant, cette solution n'est pas possible cette année car nous ne connaissons pas toutes les informations sur la piste (on connaît à peu près la position mais pas l'orientation). Il va donc falloir générer une grille en fonction du noeud précédent qui nous renverra un type ''OccupancyGrid''.

Etant donné que ce noeud tournera en permanence, si des informations changent pendant que le robot est sur la piste (détection des nouvelles machines par exemple), le noeud ''Map.cpp'' modifiera la grille en temps réel.

La grille est décomposée en case pour le A-Star :

Si c'est une case interdite (obstacle) ont associe a la case la valeur 1, si elle est autorisée, on associe la valeur 0. On illustre le principe précédent par un exemple :

[[Fichier:Projet_S8_Creation_Map.PNG | 800px | center]]

Cependant, le noeud "Grid Map" va nous délivrer un vecteur et non une matrice par le biais de ''OccupancyGrid.Data''.Dans notre exemple précédent, on récupère le vecteur suivant :

[[Fichier:Projet_S8_Creation_Map_2.PNG | 800px | center]]

On choisit donc de transformer ce vecteur en matrice, car l'algorithme A-Star fonctionne sur un graphe, pas un vecteur. On connait la largeur (en case) de la grille grâce au type ''OccupancyGrid.info.width''.

Le pseudo-code pour générer la matrice à partir de ce vecteur sera le suivant :

fonction create_grid (OccupancyGrid.Data vecteur[size]) : matrice[height].[width]
{
matrice[height].[width] ;
int t <- size;
Pour (i de 0 à height)
faire:
{
Pour (j de 0 à width)
faire :
{
matrice[i].[j] <- vecteur[t] ;
t++;
}
}
return matrice;
}

===Semaine 4===

====Localisation - Landmarks Extraction====

Nous nous concentrons essentiellement sur la première partie, qui est la partie : "Landmarks Extraction" (extraction des points de repères). On travaille ici dans le repère du laser, qui est différent de celui du robot, et du global. La transposition dans le repère global se fera dans la partie "Data Association". On doit ainsi, dans l'ordre :

☑ récupérer des données lasers via le topic /scan 
☑ convertir les coordonnées polaires en coordonnées cartésiennes en filtrant les données aberrantes (> range_max et < range_min) 
☑ implémenter l'algorithme [http://fr.wikipedia.org/wiki/RANSAC RANSAC] (Random SAmple Consensus) de détection de droites : 
☑ détecter la "meilleure" droite à partir des points en sélectionnant aléatoirement des combinaisons de points 
☑ stocker le meilleur modèle (angle + tableau de points) 
☑ recommencer tant qu'il reste assez de points à traiter (5) 
☑ extraire les segments à partir des modèles trouvés et des points 
☑ extraire les points extrèmes 
☑ calculer la taille du segment 
☑ chercher des angles droits, des segments de 35 ou 70 cm (largeur et longueur d'une machine) 
☑ extrapoler les positions des machines dans le repère du laser 
☑ définir le barycentre de la machine ainsi que son angle 
☐ stocker ces positions 

====Pathfinder====

Nous avons aussi pris du temps avec '''Valentin VERGEZ''', qui a développé, l'an dernier, les codes pour le déplacement du Robotino et le calcul de trajectoire

Nous avons éclairci quelques points sur cette partie du projet :
*Les commentaires sur l'utilité de chaque fonction ont été rajoutés.
*Le "code mort" a été nettoyé ainsi que les parties inutiles.
*Nous devons trouver un moyen de tester cette partie du code seule. (A priori graphiquement).

Etant donné l'arrivée d'un nouvel ordinateur au sein de l'équipe, nous en avons profités pour installer les logiciels qui nous permettrons de faire tourner les noeuds codés sur les Robotino :

*Installation d'Ubuntu 12.04 ( c'est cette version qui permet de faire tourner ROS Hydro)

*Installation de ROS Hydro (ensemble d'outils informatiques open source permettant de développer des logiciels pour la robotique.)

*Installation des paquets nécessaires (catkin, Desktop Install, ROS-Base, Individual Package)

*Installation de GIT (outil de bas niveau permettant de gérer l'évolution du contenu d'une arborescence)

*Vérification que toutes les fonctionnalités de ROS Hydro étaient présentes ( [http://wiki.ros.org/ROS/Tutorials Tutoriel])

La récupération du code complet de l'année dernière et une solution a été trouvée afin de tester le code de génération de trajectoire A-star:

*Pour visualiser une Map ainsi qu'un Path (chemin), on peut utiliser l'outils [http://wiki.ros.org/rviz RVIZ].
*Dans RVIZ, on peut ajouter un Path (topic /path), ainsi qu'ajouter une Map (/grid): Ceci correspond exactement à ce que nous souhaitons visualiser pour la partie calcul de trajectoire.
*Pour publier une Map fictive, on executera le script fakeGrid.sh
*Pour générer un chemin, on lancera le pathfinder (rosruns rbqt_pathfinder server : notre code) et on effectuera une requête avec le script pathReq.sh.

Si tout se passe correctement, le chemin devrait s'afficher en vert sur la map.

Nous avons aussi du communiquer notre avancement aux autres parties de l'équipe. Le schéma général ainsi qu'un outil permettant d'expliquer les différents algorithmes de recherche de trajectoire optimale nous ont aidé à leur communiquer nos objectifs et les processus choisis.

L'algorithme A-Star que nous allons utiliser donne le résultat final suivant (utilisation du site [http://qiao.github.io/PathFinding.js/visual/ PathFinding] ):

[[Fichier:Projet S8 A STAR E.PNG | center | thumb ]]

* le chemin optimal calculé par A-Star 
* le point de départ
* le point d'arrivée
* les points interdits (obstacle)
* les points que l'algorithme a déjà calculé
* les points que l'algorithme aurait calculé à la prochaine itération

====Localisation - Landmarks extraction - filtrage====

Dans cette partie, nous avons commencé à définir une classe laserScan qui contient la fonction polarToCart, elle convertit les données laser (tableau de ranges) en liste de points (x,y) :

void laserScan::PolarToCart (){
Point p;
for (int i=0; i<m_ranges.size(); i++){
if((m_ranges[i]>getRangeMin()) && (m_ranges[i]<getRangeMax())){
p.setX(m_ranges[i]*cos(getAngleMin() + i*getAngleInc()));
p.setY(m_ranges[i]*sin(getAngleMin() + i*getAngleInc()));
m_points.push_back(p);
}
}
}

====Localisation - Landmarks extraction - reconnaissance de droite====

Dans cette partie, nous avons déterminé quel algorithme nous allons utiliser : [http://fr.wikipedia.org/wiki/RANSAC#L.27algorithme RANSAC]. L'algorithme de RANSAC trouve la meilleure droite dans un nuage de points. On itère plusieurs fois cet algorithme afin de trouver l'ensemble de toutes les droites du scan laser. On a donc en sortie une liste des droites composants le nuage de points initial.

Entrée : listOfPoints -> liste de points initiale
Sortie : meilleur_modèle -> les paramètres de la droite qui correspondent le mieux aux points
-> tableau de points à partir desquels la droite a été estimée
-> erreur du modèle de droite par rapport aux points

tant que la liste contient assez de points
itérateur := 0
meilleur_modèle := aucun
tant que itérateur < k (fixé par une loi de probabilité)
on choisit deux points au hasard dans la liste
on fabrique un modèle_possible à partir de ces deux points
Pour chaque point de la liste qui ne sont pas dans le modele_possible
si le point s'ajuste au modele_possible avec une erreur inférieure à t
Ajouter ce point au modele
si le nombre de points dans le modele est suffisant pour valider l'existence du modele
erreur := coefficient de corrélation de la régression linéaire de ensemble_points
si erreur < erreur du meilleur_modele
meilleur_modèle := modèle_possible

stocker meilleur_modèle dans liste de modèles
retirer meilleur_ensemble_points de la liste des points initiale
recommencer avec la liste modifiée

===Semaine 5===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous avons adapté l'algorithme précédent afin de mieux correspondre aux structures que nous avons créées :
*Modele
**Liste de points
**Liste d'indices
**Corrélation
**Droite
***Point
***Angle
**Segment
***Point "min"
***Point "max"
***Angle
***Taille

Nous avons aussi codé toutes les fonctions nécessaires dans l'algorithme de Ransac, dont celle permettant la correction de l'approximation de droite par régression linéaire des points correspondants à un modèle trouvé (''linReg()'') qui nous a donné matière à réflexion. Aussi, notre première idée qui était de travailler avec des ''std::vector'' a du changer puisque l'aspect récursif de l'algorithme que nous allons utiliser nécessite de multiples suppressions sur les données à entrer dans l'algorithme de Ransac. On travaille donc maintenant avec des ''std::list''.

Tous nos codes sont disponibles sur la branche [https://github.com/PyroTeam/robocup-pkg/tree/feature/navigation/SLAM/navigation/localisation/src feature/navigation/SLAM du GIT de l'équipe PyroTeam].

===Semaine de vacances de février===

====Localisation - Landmarks Extraction - reconnaissance de droite====

Nous devons réaliser un noeud permettant de valider notre algorithme, c'est-à-dire implémenter sous ROS notre code C++ afin de tester, soit directement sur le robot, soit avec des [http://wiki.ros.org/Bags bagfiles]. Cette étape est très importante pour pouvoir passer à la suite : extrapoler les droites trouvées en segments puis en objets.

Notre programme consistant à reconnaître les droites à partir du scan laser est fonctionnel et a été testé à partir du scan laser suivant :

[[Fichier:ScanLaser.png | center | thumb]]

Processus de validation :

*Lancer le programme ''rosrun localisation test_polar_to_cart''

résultat : on trouve 5 droites

m = -0.0229637 p = -0.24079
nb de points dans la droite : 246
coeff de correlation : 0.812376

m = -0.0315132 p = 3.7506
nb de points dans la droite : 102
coeff de correlation : 0.928089

m = 32.9243 p = -139.943
nb de points dans la droite : 77
coeff de correlation : 0.914475

m = 7.04634 p = -15.42
nb de points dans la droite : 32
coeff de correlation : 0.970724

m = 0.253029 p = 1.32235
nb de points dans la droite : 28
coeff de correlation : 0.979578

ce qui correspond à l'image suivante :

[[Fichier:DroitesTrouvées.png | center | thumb]]

On voit que la recherche de droite marche bien, mais peut être améliorée en ajoutant des conditions de proximité des points correspondants à une seule droite, pour éviter le phénomène que l'on voit au milieu. Le résultat après avoir ajouté cette condition et en ayant affiné au maximum les paramètres, au dépend d'un temps de calcul beaucoup plus élevé (9,416 secondes au lieu de 0,536 s) :

[[Fichier:DroitesTrouvées2.png | center | thumb]]

Les impératifs de la compétition nous imposant une certaine rapidité pour se localiser et détecter les machines, nous retiendrons la solution la plus rapide et nous séparerons les droites en segment après leur détection.

Pour valider l'algorithme de régression linéaire nous avons :
*Récupéré tous les points correspondants à chaque droite
*Rentré ces points sur Excel
*Fait une courbe de tendance
*Affiché l'équation de droite et le coefficient de corrélation
*Comparé avec ce qu'a trouvé notre programme
*Conclu que notre programme était bon

===Semaine 6===

====Localisation - Landmarks Extraction - reconnaissance de machines====

Les objectifs de cette semaine :

*à partir de la liste de droites, extraire les segments
*extrapoler les positions machines à partir d'un segment complet (35 ou 70 cm)
*réaliser une interface graphique pour visualiser les segments trouvés en temps réel
*regarder ce qu'on doit rentrer dans l' '''Extended Kalman Filter''' au niveau du format des positions machines et de l'odométrie pour pouvoir réaliser le '''Data Association''' de façon cohérente avec la suite.

L'extraction de segment à partir des 5 droite précédemment trouvées nous donne 7 segments:

Segment 1 
Min(-1.04033e-08, -0.238)
Max(4.24699, -0.26092)
taille : 4.24705
angle : -0.309205

Segment 2 
Min(-1.64267e-07, 3.758)
Max(3.4172, 3.63358)
taille : 3.41946
angle : -2.08521

Segment 3 
Min(4.24452, -0.234635)
Max(4.33124, 2.18017)
taille : 2.41636
angle : 87.9432

Segment 4 
Min(2.30055, 0.871242)
Max(2.32511, 0.736592)
taille : 0.136872
angle : -79.6622

Segment 5 
Min(2.50359, 2.18655)
Max(2.53413, 2.56542)
taille : 0.380104
angle : 85.3914

Segment 6 
Min(2.73122, 2.05171)
Max(3.049, 2.03728)
taille : 0.318107
angle : -2.5999

Segment 7 
Min(1.14156, 1.64221)
Max(1.39648, 1.61891)
taille : 0.255984
angle : -5.22068

[[Fichier:SegmentsTrouvés.png | center | thumb]]

Tout cela en 0,540s !

A partir de la liste de segments précédente, on extrait une liste de machine à partir des segments de 35 ou 70 cm, qui sont les tailles des machines. Comme le scan que nous utilisons pour le moment n'est pas un scan avec une vraie machine, mais disposant quand même de 2 segments d'à peu près 35 cm, nous utilisons ces deux segments comme ''petit côté'' de la machine. On trouve ainsi deux machines :

Machine 1
centre en (2.53292, 2.20155)
orientation : 175.391°

Machine 2
centre en (3.06493, 2.05243)
orientation : 87.4001°

Une amélioration de l'algorithme a été faite pour la détection de machine. En effet, dans le cas où on détectait un petit et un grand côté de machine, on trouvait deux machines avec des centres à peu près au même endroit. Maintenant, on fusionne ces données en créant un point milieu entre les deux données, et on prend l'orientation du grand côté.

On a commencé à faire des tests sur ROS directement avec un noeud factice qui publie un scan laser à une fréquence de 10Hz, ce qui est la fréquence réelle du laser Hokuyo. La détection se fait assez rapidement et de façon précise. Des tests sur le robot sont prévus en début de semaine 7 avec les vraies machines.

===Semaine 7===

====Localisation - Data Association====

L'objectif principal de cette semaine est de coder le noeud '''Data Association''' qui liera la position donnée par l'odométrie (mesure relative de la position par rapport à un point de départ à peu près connu) avec les machines trouvées dans le champ du laser.

Il faudra aussi corriger l'odométrie donné par le nœud ''robotino_odometry_node'', qui donne l'odométrie (mesure de la position et de la vitesse du robot). Le problème est que nous utilisons un gyroscope externe, car le gyroscope de base est de très basse qualité. Or, si le nœud corrige parfaitement la position, il ne corrige pas du tout la vitesse angulaire ! Nous devons donc réaliser un nœud qui le fera pour nous.

D'après la dernière version du sujet 2015, l'aire de jeu de 6 x 12 m est séparée en zones de 1,5 x 2 m de large, dans lesquelles se situent une seule machine. Chaque équipe a 6 machines dont la zone est connue, donnée par la Referee Box. Le but du nœud ROS de ''Data Association'' sera de transposer les positions machines dans le repère global de la piste, ainsi le Manager pour savoir devant quelle machine le robot est, et connaître sa position et son orientation.

La piste sera donc comme cela :

[[Fichier:Piste.PNG | center | thumb]]

La correction d'odométrie a fait apparaître une erreur de mesure d'environ 1,7°/s sur la vitesse angulaire, à faible vitesse. Pour de la Localisation et Cartographie Simultanée, c'est assez important. Ça justifie donc la nécessité de faire cet ajustement.

Pour la suite, il a été décidé de fusionner les nœuds '''Data Association''' et '''Extended Kalman Filter''' puisque l'association de données n'étant au final qu'un changement de repère, la structure de nœud ROS n'est pas adaptée.

Nous avons trouvé un [https://github.com/yangyimeng/machine-learning/blob/master/slam/EKF/SLAM%20course.pdf PDF] expliquant l'algorithme de SLAM. Grâce à celui ci nous avons déterminer les éléments à rentrer dans le noeud, ainsi que les différentes opérations incluses dans ce filtre. Pour les calculs matriciels, nous allons probablement utiliser la librairie EIGEN pour travailler sur des [http://eigen.tuxfamily.org/dox/classEigen_1_1Matrix.html matrices] de façon optimisée. La prochaine étape sera de "typer" nos informations (position robot + machine + murs) pour pouvoir les intégrer facilement au filtre. Nous avons aussi décidé d'inclure les murs comme ''landmark'' dans la matrice de covariance du vecteur d'état.

Il a été décidé de faire une association de données avant la détection de machines, pour distinguer les pans de murs directement afin de stocker ces informations directement à la source. On pourra éventuellement détecter les coins intérieurs de la piste pour plus de précision dans le recalage. Pour le reste, rien ne change.

===Semaine 8===

====Localisation - Landmarks Extraction - vidéo====

Pour visualiser les résultats de la détection des machines nous avons utiliser l'interface graphique disponible avec ROS, RVIZ. Nous visualisons alors le scan laser (points blancs), le résultat de la détection de segments (en rouge) et les positions des machines trouvées (en vert). Un premier test a été fait sur le scan originel, celui sur lequel nous avons travaillé depuis le début.

[https://www.youtube.com/watch?v=XSn4s1y-XT0 Visualisation des machines]

On rencontre un problème quand le robot est exactement parallèle ou perpendiculaire à un mur. Après vérification dans le programme, il s'avère que la détection ne fonctionne pas pour ces cas. Ce bug est maintenant corrigé :

[https://www.youtube.com/watch?v=rKeqewpuLoQ Correction du bug]
====Localisation - Data Association====

La première étape consiste juste à faire un changement de repère, d'abord du repère laser vers le repère robot, puisque le laser sera placé en avant du robot, puis du repère robot au repère global, comme ci dessous :

[[Fichier:Chgt_Repère.png |center | thumb]]

la matrice correspondant au changement (laser -> robot) est la suivante :

0 -1 0
1 0 0.2
0 0 1

la matrice 2x2 en haut à gauche correspond à la rotation de 90°, et le vecteur (0, 0.2) correspond à la translation.

Celle du changement (robot -> global) avec alpha angle mesuré par le gyroscope depuis la position initiale (angle 0) :

cos(alpha) -sin(alpha) posInit.x
sin(alpha) cos(alpha) posInit.y
0 0 1

l'étape suivante est de vérifier si les machines vues par le laser sont bien des machines, c'est-à-dire si les positions mesurées à partir des données lasers correspond aux zones occupées par des machines envoyées par la Referee Box.

====Génération du noeud Map====

L'autre objectif de cette semaine a été de coder le noeud qui génère la map. La map est générée sous forme d'OccupancyGrid. Ce type est constitué d'un seul vecteur dont la première case correspond à la case en bas à gauche de la map. D'autres paramètres sont réglable comme l'origine, la taille etc.

Pour la taille on choisi de diviser la map en case de 10 cm (100 mm) et l'origine choisi est le centre de la mep en x et en y le premier mur (voir la map ci-dessous)

Afin d'expliquer comment est constituée cette map, prenons le code et analysons les différentes fonctions:

#include "Map.hpp"

ce vecteur de Pose2D (x et y) va permettre d'enregistrer le vecteur qui contient la position des machines :

std::vector<geometry_msgs::Pose2D> tab;

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "server");
ros::NodeHandle n;
ros::Subscriber sub_poses_machine = n.subscribe("/machines", 1000, Poses_Machine_Callback); // Récupération des machines
ros::Publisher Map_Pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 1000); // Préparation de la publication de la map
ROS_INFO("Ready to Generate the Map");
float xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
nav_msgs::OccupancyGrid Map;
Create_Empty_Map(Map);
Set_Wall(Map);
for (int z=0;z<tab.size();z++)
{
Get_One_Point_Of_The_Rectangle(tab[z].x, xA, tab[z].y, yA, tab[z].theta, 275, 450);
Set_Machines_In_Map(100, tab[z].theta, xA, yA, Map);
}
Map_Pub.publish(Map); // publication de la map
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

Cette fonction va récupérer les machines envoyées par le noeud du slam (sur le message landmarks):

void Poses_Machine_Callback(const deplacement_msg::LandmarksConstPtr &machines)
{
tab=machines->landmarks;
for (int i=0; i<tab.size(); i++)
{
tab[i].x = tab[i].x*1000;
tab[i].y = tab[i].y*1000;
}

}

Cette fonction va créer une map vide de taille 14m sur 8m. Chaque case à une valeur nulle, il remplit aussi les autres paramètres de OccupancyGrid:
(On rappelle que la technique utilisée est un vecteur qui représente chaque case auxquelles il associe une probabilité 100 étant un point interdit et 0 un point sans danger de passage pour le robot)

void Create_Empty_Map(nav_msgs::OccupancyGrid &Map)
{
Map.info.origin.position.x=-7000;
Map.info.origin.position.y=-1000;
Map.info.origin.position.z=0;
Map.info.origin.orientation.x=0;
Map.info.origin.orientation.y=0;
Map.info.origin.orientation.z=0;
Map.info.origin.orientation.w=1;
Map.info.map_load_time=ros::Time::now();
Map.info.resolution=100;
Map.info.width=140;
Map.info.height=80;
Map.data.assign(Map.info.width*Map.info.height, 0);
}

Ce noeud va construire les points fixes de la map donc les murs (probabilité 100) et les zones à risques (probabilité 50):

void Set_Wall(nav_msgs::OccupancyGrid &Map)
{
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<13)||(i>67)) Map.data[j+i*140]=100;
if ((j<13)||(j>127)) Map.data[j+i*140]=100;
}
}
for (int i=0;i<80;i++)
{
for (int j=0;j<140;j++)
{
if ((i<10)||(i>70)) Map.data[j+i*140]=50;
if ((j<10)||(j>130)) Map.data[j+i*140]=50;
}

Nous ne mettons pas tout le code de cette fonction étant donné qu'il est long et répétitif afin de tout remplir

Maintenant, nous passons aux positions machines:

Lorsqu'on reçoit une position machine, c'est en fait son centre et son orientation par rapport à x que l'on reçoit.
Etant donné que les machines doivent être des rectangles de 90cm sur 55cm, on décide d'abord de déterminer un des coins du rectangle et plus précisément le coin en bas à droite de ce dernier:

void Get_One_Point_Of_The_Rectangle(float x, float &xA, float y, float &yA, float theta, float largeur, float longueur)
{
float x1,y1;
x1 = x - cos(theta)*longueur;
y1 = y - sin(theta)*longueur;
xA = x1 + sin(M_PI_2-theta)*largeur;
yA = y1 - cos(M_PI_2-theta)*largeur;

}

Il reste ensuite à mettre tout les points de se rectangles à une probabilité 100 dans le vecteur Map.
Pour les calculs on différencie le cas ou l'angle de la machine est entre 0 et pi/2 et le cas où il est entre pi/2 et pi.
L'algorithme est le suivant :

A partir du coin, on incrémente x et y de sorte à longer le grand coté et de remplir chaque case (incrémentation de 5 cm).
Une fois arrivé au bout, on incrémente x et y de 5cm sur la largeur à partir du coin et on recommence l'opération précédente.
Ceci jusqu'à remplir le complètement le rectangle.

Afin d'établir les probabilités de ce rectangle, nous utilisons la fonctions floor pour convertir nos flottants en entier et accéder à la case correspondante.
On notera aussi que la case 1470 correspond à l'origine de notre map.

void Set_Machines_In_Map(float rank, float theta, float xA, float yA, nav_msgs::OccupancyGrid &Map)
{
float x=xA;
float y=yA;
if (theta<=M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x + cos(theta)*50;
y = y + sin(theta)*50;
}
x = xA - j*cos(M_PI_2-theta)*50;
y = yA + j*sin(M_PI_2-theta)*50;
}
}
if (theta>M_PI_2)
{
for (int j=0;j<10;j++)
{
for (int i=0;i<18;i++)
{
Map.data[1470+floor(x/rank)+floor(y/rank)*140]=100;
x = x - sin(theta-M_PI_2)*50;
y = y + cos(theta-M_PI_2)*50;
}
x = xA - j*cos(theta-M_PI_2)*50;
y = yA - j*sin(theta-M_PI_2)*50;
}
}
}

On crée un noeud test qui publie 2 positions de machines :
#include "Test.hpp"

/*========== Main ==========*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "test");
ros::NodeHandle n;
ros::Publisher Pose_Machines_Pub = n.advertise<deplacement_msg::Landmarks>("/machines", 1000);
ROS_INFO("Ready to send poses machines");
int xA,yA;
ros::Rate loop_rate(1);
while(ros::ok())
{
deplacement_msg::Landmarks machines_msgs;
geometry_msgs::Pose2D tab;
tab.x=-2;
tab.y=2;
tab.theta=0;
machines_msgs.landmarks.push_back(tab);
tab.x=5;
tab.y=4;
tab.theta=2.5;
machines_msgs.landmarks.push_back(tab);
Pose_Machines_Pub.publish(machines_msgs);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

On visualise le résultat grâce au logiciel RVIZ qui peut lire les topics publiés et donc le topic /map !:

===Semaine 9===

====Localisation - Extended Kalman Filter====

l'objectif de cette semaine sera de coder toutes les fonctions nécessaires à l'implémentation finale du [http://fr.wikipedia.org/wiki/Filtre_de_Kalman#Filtre_de_Kalman_.C3.A9tendu filtre de Kalman étendu] :

*initialisations des différents éléments
*étape de prédiction
*étape de mise à jour
*publication des positions machines et du robot dans le repère global via un topic ROS

Le schéma ci-dessous nous montre les deux étapes du filtre de Kalman :

[[Fichier:Kalman.png | center]]

RoboCup 2015 - Pyro Team

2015-03-29T16:14:32Z

Tdanel : /* Localisation - Extended Kalman Filter */

RoboCup 2015 - Pyro Team

2015-03-29T16:13:49Z

Tdanel : /* Localisation - Extended Kalman Filter */

Fichier:Kalman.png

2015-03-29T16:13:10Z

Tdanel : a téléversé une nouvelle version de « Fichier:Kalman.png »

Fichier:Kalman.png

2015-03-29T16:10:56Z

Tdanel :

RoboCup 2015 - Pyro Team

2015-03-29T16:09:52Z

Tdanel : /* Localisation - Extended Kalman Filter */

RoboCup 2015 - Pyro Team

2015-03-29T15:41:27Z

Tdanel : /* Localisation - Extended Kalman Filter */

RoboCup 2015 - Pyro Team

2015-03-29T15:03:02Z

Tdanel : /* Semaine 8 */

RoboCup 2015 - Pyro Team

2015-03-29T14:27:39Z

Tdanel : /* Localisation - Data Association */

RoboCup 2015 - Pyro Team

2015-03-29T11:22:15Z

Tdanel : /* Localisation - Landmarks Extraction - vidéo */

RoboCup 2015 - Pyro Team

2015-03-27T21:33:56Z

Tdanel : /* Localisation - Landmarks Extraction - vidéo */

RoboCup 2015 - Pyro Team

2015-03-27T21:33:22Z

Tdanel : /* Localisation - Landmarks Extraction - vidéo */

RoboCup 2015 - Pyro Team

2015-03-26T10:22:38Z

Tdanel : /* Localisation - Data Association */

RoboCup 2015 - Pyro Team

2015-03-26T10:09:35Z

Tdanel : /* Semaine 7 */

Fichier:Chgt Repère.png

2015-03-26T10:08:28Z

Tdanel :

RoboCup 2015 - Pyro Team

2015-03-20T14:21:29Z

Tdanel : /* Localisation - Data Association */

RoboCup 2015 - Pyro Team

2015-03-19T16:58:38Z

Tdanel : /* Localisation - Data Association */

RoboCup 2015 - Pyro Team

2015-03-18T16:55:10Z

Tdanel : /* Localisation - Data Association */

RoboCup 2015 - Pyro Team

2015-03-18T16:54:16Z

Tdanel : /* Localisation - Data Association */

RoboCup 2015 - Pyro Team

2015-03-16T15:48:44Z

Tdanel : /* Localisation - Data Association */

RoboCup 2015 - Pyro Team

2015-03-15T22:17:03Z

Tdanel : /* Localisation - Data Association */

RoboCup 2015 - Pyro Team

2015-03-15T22:15:48Z

Tdanel : /* Localisation - Landmarks Extraction - reconnaissance de machines */