Interaction Homme Robot : Différence entre versions

Version actuelle datée du 12 juin 2015 à 07:47

Cahier des charges

Présentation générale du projet

Contexte

Emmanuelle Grangier, professeur à l’école supérieure d'art de Cambrai a pour projet la réalisation d’un spectacle de danse sur le thème de la relation homme-robot nommé Link Human Robot. Lors de ce spectacle, un danseur professionnel aura comme partenaire un robot NAO[1] avec lequel il interagira.

Objectif du projet

Le sujet qui nous est proposé consiste à réaliser un environnement permettant la communication entre un danseur professionnel et un robot NAO. Le but étant que le robot soit capable de reproduire les mouvements du danseur en temps réel.

Description du projet

Afin que notre robot soit en mesure de suivre les mouvements du danseur il est nécessaire de récupérer les informations venant des capteurs, de les traiter à l'aide d'un algorithme puis ensuite d'envoyer les commandes adéquates au robot NAO afin qu'il agisse en conséquence. Les contraintes à prendre en compte sont :

• Les contraintes mécaniques du NAO : limites des articulations, la vitesse du mouvement et l'équilibre du NAO.
• Le temps de réponse qui ne doit pas être trop long malgré la communication wifi avec le NAO.
• La précision des mouvements du NAO.
• Et la vitesse de calcul résultant de l’environnement NAO, celle-ci étant contournée en utilisant des langages de programmation de plus faible niveau (C, C++, Java).

Choix techniques : matériel et logiciel

Partie matérielle

Nous disposons d’un robot NAO V4 dont nous utiliserons:

• la centrale inertielle située dans le torse du robot et composée de 2 gyromètres et d’un accéléromètre.
• les capteurs de position, des roues codeuses, situés dans chaque articulations.
• les 26 moteurs du NAO.

Nous avons aussi à disposition un routeur pour nous permettre de nous connecter au NAO bien que la communication soit aussi possible en Ethernet, cette dernière étant cependant plus contraignante.

Les capteurs utilisés seront dans un premier temps constitués de plusieurs centrales inertielles. Puis il est envisagé par la suite d’utiliser un exosquelette si il est possible d’en obtenir un. Ce qui permettrait une plus grande précision lors de la reconnaissance des mouvements.

Partie software

Afin d'interagir avec le NAO, nous utiliserons les différents logiciels disponibles sur le site d’Aldebaran:

• Chorégraphe, le logiciel permettant de programmer le NAO à l’aide de différentes fonctions se présentant sous forme de blocs.

• C++ NAOqi SDK ou Python NAOqi SDK, les kits de développement qui permettent de compiler un code de niveau inférieur (python ou C++) qui communiquera directement avec l’OS du NAO.

Le langage choisi sera donc soit le Python, soit le C++. Ce choix sera déterminé en fonction de la facilité de mise en oeuvre de ces deux langages et de leur efficacité dans notre application. Nous envisageons le C++ dans un premier temps du fait de sa plus grande similitude avec le langage C et de son meilleur temps de réponse en utilisation temps réel.

Etapes du projet

• Étude de la documentation et familiarisation avec le NAO.
• Premiers tests en C++ afin de faire bouger le NAO et analyse du temps de réponse et de la précision.
• Modification du programme précédent afin de fonctionner à l’aide des centrales inertielles.
• Implémentation d’un client-serveur sur le NAO, éventuellement en UDP.
• Si obtention d’un exosquelette il sera nécessaire d’adapter le programme afin de gérer ce type de capteur.
• Si il reste du temps il sera possible de commencer la généralisation des étapes précedentes à toutes les articulations du NAO.

Avancement du Projet

Semaine 1

Lors de la semaine 1 nous avons voulu nous familiariser avec le NAO nous avons donc commencé par utiliser le logiciel Chorégraphe qui permet de programmer le NAO à l’aide de bloc préprogrammés. Ce logiciel permet aussi la programmation de blocs personnalisés en Python. A l’aide de ce logiciel nous avons testé quelques programmes simples afin de voir les possibilités du NAO. Cette première approche nous à permis d’entrevoir un peu les limites mécaniques du NAO.

Vue du logiciel Chorégraphe

Après nous être familiarisés avec le NAO nous avons commencé à étudier la documentation fournie par Aldebaran dans le but de définir le langage de programmation que nous allions utiliser ainsi que se procurer les logiciels nécessaires à la programmation du NAO. Nous avons aussi commencé à étudier les différentes fonctions permettant de contrôler les moteurs du NAO. Pour bouger le NAO il est possible de contrôle les moteurs de deux manières différentes:

• un déplacement relatif effectué par rapport à la position actuelle du moteur.
• ou bien un déplacement absolu permettant de placer le moteur dans une position angulaire bien définie.

Semaine 2

Au cours de la deuxième semaine, nous nous sommes intéressés au kit de développement C++ NAOqi SDK qui permet de programmer le NAO via un langage bas niveau, à savoir le C++. Cette semaine s’est ainsi principalement orientée sur l’étude de la documentation d’Aldebaran Community. Nous avons donc suivi les procédures d’installation du kit et des différents outils nécessaires à son bon fonctionnement. Le système d’exploitation le plus efficace pour programmer le NAO en C++ était selon nous Linux car c’est le seul permettant de créer des modules distants qui s'exécutent directement sur le NAO. Nous nous sommes donc servis de Linux pour la suite de ce projet car nous pensions au départ avoir besoin de réaliser un client serveur sur le NAO pour permettre la communication avec notre PC. Il s’avère qu'un client TCP est déjà présent sur le NAO donc nous n'avons pour le moment pas besoin de créer de modules distants.

Nous avons ainsi installé qiBuild afin de créer des projets multi-plate-formes (linux et NAOqi OS) et CMake afin de générer les makefiles et les répertoires de travail pour chacune des plate-formes afin de compiler ces projets sur celles-ci. Suite à l’installation des logiciels nécessaires à la programmation du NAO nous avons commencé à étudier des exemples de codes que nous avons trouvé dans la documentation. Dans un second temps, nous avons commencé l’apprentissage du C++ afin de pouvoir être en mesure de créer nos propres programmes à compiler sur le NAO.

Semaine 3

Lors de la semaine 3 nous avons commencé à tester différents codes obtenus via la documentation d’Aldebaran mais nous avons rencontré différents problèmes. Le premier fut un problème d’inclusion des librairies fournies par naoqi-sdk, nous avons donc dû chercher sur le web car nous n’étions pas familiers avec qibuild ou Cmake. Nous avons appris que lors de la création d’un nouveau projet, qibuild nous créait un Makefile au sein de projet afin de simplifier la compilation, à partir de là nous avons modifié ce Makefile afin d’inclure les librairies donc nous avions besoin lors de la compilation.

Ensuite nous nous sommes heurtés à un problème de connexion avec le NAO. Lors de la recherche afin de résoudre ce problème nous nous sommes rendus compte que la version 1.14 de ces différents logiciels était bien plus documentée et plus facile d’utilisation (beaucoup de modifications concernant la syntaxe pour l'utilisation de l'API) que la version 2.1 que nous utilisions. C’est pourquoi après de nombreux essais afin de résoudre notre problème sans succès nous avons voulu tenter de contourner le problème en utilisant cette version 1.14 quitte à revenir à la version 2.1 si nécessaire. Cette version fut difficile à se procurer car les archives d’Aldebaran n'étaient pas facile à trouver.

Suite à cela nous avons installé ces logiciels afin de pouvoir les tester lors de la semaine 4.

Semaine 4

La version 1.14 s’est révélée être réellement plus facile d’utilisation et nous avons réussi à faire nos premiers test de commande à distance du NAO via le serveur TCP préexistant. Nous avons donc commencé à modifier ces exemples et à créer de nouveau programmes pour tester les différentes fonctions dont nous allions nous servir présentes dans l’API.

Au cours de cette semaine nous avons réussi à faire dire n’importe quelle phrase au NAO et écris des programmes simples afin de faire bouger les différentes parties du NAO (mains, jambes, tète, etc...). En annexe seront présentés ces deux programmes assez simples permettant de faire parler le NAO et de faire bouger sa tète. La partie importante est la partie permettant la connexion au NAO.

Par la suite nous allons améliorer légèrement ce programme pour y ajouter un menu permettant de bouger le NAO en temps réel via des évènements clavier ce qui nous permettra d’avoir une idée concrète du temps de réponse du NAO car pour le moment le temps de réponse est assez long probablement à cause du temps de connexion au NAO.

Semaine 5

Le programme permettant le mouvement du NAO en réponse à des événements clavier à été réalisé, de plus nous avons ajouté un fichier texte qui reçoit directement le temps de réponse de chaque mouvement. Nous avons remarqué que la connexion au NAO prend un certain temps à s'établir en revanche une fois la connexion établie le temps de réponse du NAO aux commandes est de l'ordre de la milliseconde. Pour le moment le temps maximum que nous avons observé est de 3.5ms ce qui parait correct pour une application temps réel.

Nous continuons notre apprentissage du C++ et envisageons de faire une interface graphique pour mettre en pratique nos connaissances. En plus de ceci nous regardons plus en détails toutes les fonctions présentent au sein de l'API du NAO afin d'améliorer nos programmes et de voir si nous ne trouvons pas certaines fonctions qui pourraient nous être fort utiles pour la suite. Il ne reste que quelques test à faire durant les vacances avant de passer à l’acquisition des données via les capteurs de type centrale inertielle. Nous espérons pouvoir travailler dès lundi sur la partie vraiment concrète de notre projet si nous avons les capteurs à notre disposition.

Semaine 5.5

Lors des vacances nous avons fait les tests complémentaires permettant de valider la réponse du NAO à des événements clavier. De plus à l'aide des recherches faite sur l'API du NAO nous avons fait quelques amélioration à notre programme. Maintenant au début de chaque application le NAO se met en position debout pour pouvoir facilement effectuer de nombreuses actions, puis à la fin du programme après avoir exécute tout ce qui lui est demandé ou dans le cas ou une exception est survenue le NAO se remet en position assise.

Semaine 6

Cette semaine nous avons réalisé la vidéo permettant de voir la réponse du NAO à des événements clavier ci-dessous. D'autre part nous avons commencé à travailler à l'aide de capteurs, pour l'instant il s'agit d'un simple gyroscope dont nous avons réussi à acquérir les données. Bien qu'elles se présentent sous forme chaotique pour le moment nous cherchons à résoudre ce problème. Il s'agit d'un problème de configuration du port série car nous arrivons à observer les bonnes valeurs via minicom. Mais nous n'avons pas encore trouvé ou se situe le problème étant donné que le baud rate ainsi que le format des données semble bien configuré. Une fois ce problème résolu, nous espérons y arriver lundi ou durant le week-end, nous devrions être capable de réaliser un programme permettant de bouger la tête du NAO suivant l'angle du gyromètre.

Ainsi que l'ajout du lien vers le git dans les annexes comportant différents programmes pour le NAO ainsi que le programme d'acquisition des donnée via port série qui ne fonctionne pas pour le moment.

Fichier:NaoClavier.avi

Semaine 7

Le programme permettant de récupérer les valeurs du gyromètre a été débogué il s'agissait en fait d'un problème d'affichage car nous utilisions un buffer défini en char et non en unsigned char. Nous avons donc réussi à récupérer les valeurs provenant du gyromètre et nous avons implémenté ce code afin de bouger la tête du NAO en temps réel en réponse aux mouvement du gyromètre. (Annexe 3)

Fichier:NAOGyro.avi

Maintenant que cette partie a été réalisé notre but va être d'analyser la robustesse de notre programme, généraliser avec plusieurs gyromètres ou bien utiliser d'autres capteurs. Pour l'instant je pense que nous allons commencer par la partie robustesse car c'est ce qui nous est demandé. Nous allons donc faire des tests ou nous allons voir si le NAO est capable de gérer de nombreuses requêtes simultanées, comme par exemple des demandes de mouvement multiples sur différentes articulations.

Après discussion avec notre encadrant les objectifs pour le moment en plus du test de robustesse sont:

• Analyse de la précision concernant le programme permettant de bouger la tête avec le gyromètre ainsi qu'une amélioration de ce programme.
• Réalisation d'une interface graphique qui pourrait incorporer les fonctions utilisés jusqu’à maintenant c'est à dire bouger différentes articulations et permettre le déplacement du NAO.
• Et pour finir faire bouger le NAO par l'intermédiaire d'un Robotino au lieu d'une interaction humaine pour le moment car sans exosquelette il semble plus intéressant de faire une communication ou le NAO se déplacerait suivant les mouvements du Robotino. De plus pour que la communication soit réciproque il sera intégré des actions réalisées par le Robotino lors d'une action sur certains capteurs du NAO mais ceci reste encore à définir.

Semaine 8

Lors de la semaine 8 nous avons amélioré le code permettant de faire bouger la tête avec le gyromètre et analysé la précision de celui-ci, nous avons un écart de position inférieur à 3°. Des tests ont été réalisés concernant la robustesse, nous avons fait un programme qui commandait 6 moteurs du NAO de manière simultanée en temps réel, on remarque que si l'on essaie de commander trop de moteurs les mouvements ne sont plus assez fluides il pourra être intéressant de refaire un test avec seulement 4 moteurs de commandés.

Concernant la partie avec le Robotino nous avons récupéré du code concernant la communication distante avec celui-ci nous allons commencer à mettre en place la communication lors de la semaine 9.

Semaine 9

La semaine 9 a été consacrée à la communication avec le Robotino. Pour cela nous avons récupéré un Robotino3 avec lequel nous allons communiquer en utilisant Robotino API2 une API utilisable en C++. Nous avons donc commencé à étudier les documents qui nous ont été fournis lors de la semaine 8.

De plus le programme NAO permettant la gestion du gyromètre a été modifié pour s'interrompre correctement lors de la réception d'un SIGINT.

Semaine 10

Peu de travail réalisé en semaine 10 suite à des problèmes avec le Robotino. Réalisation d'un code permettant de récupérer les valeurs de position du Robotino alors qu'il se déplace afin de décrire un cercle. En revanche ce code n'a pas encore été testé.

Semaine 11

Suite aux problèmes rencontrés en semaine 10 nous utilisons maintenant un autre Robotino en attendant que celui que nous utilisions soit à nouveau fonctionnel. Le code réalisé en semaine 10 a été testé et fonctionne correctement. Il sera bientôt en Annexe le temps de commenter correctement. Nous arrivons à récupérer les valeurs de position du Robotino et elles sont envoyées au programme NAO via un serveur UDP il nous reste à tester le programme NAO et à ajuster les déplacements des deux robots.

Semaine 12

Pour la semaine 12 le programme NAO a été testé. Vu que nous n'avons pas accès au Robotino dû à la RobotCup nous avons travaillé sur le rapport. Les tests finaux seront réalisés lors des vacances.

Semaine 13 et 14

Pendant les vacances nous avons réalisé les derniers tests et la communication entre les deux robots semble fonctionnelle bien que l'utilisation d'une structure contenant les données de position du Robotino a été remplacée par une succession de trois floats séparés par un quatrième float qui indique une fin de paquet. Ceci car nous n'arrivions pas a la faire fonctionner avec la structure sur le Robotino et donc choisi d'utiliser une autre méthode faute de temps pour débugger.

De plus le code Robotino a été modifié afin de pouvoir le déplacer comme nous le souhaitons suivant 4 directions.(Annexe 4)

Annexes

Lien github contenant les fichiers pour ce projet : Github

Annexe 1

Voici à quoi ressemble le programme permettant de faire parler le NAO.

#include <iostream>
#include <stdlib.h>

//Librairie permettant la creation du proxy
#include <alcommon/alproxy.h>

int main(int argc, char* argv[])
{
  // Nous allons connecter notre Broker a Naoqi
  int pport = 9559;
  std::string pip = "127.0.0.1";

  // On teste le nombre d'arguments
  if (argc != 1 && argc != 3 && argc != 5)
  {
    std::cerr << "Wrong number of arguments!" << std::endl;
    std::cerr << "Usage: mymodule [--pip robot_ip] [--pport port]" << std::endl;
    exit(2);
  }

  // Si il y a un seul argument il s'agit de l'IP ou du PORT
  if (argc == 3)
  {
    if (std::string(argv[1]) == "--pip")
      pip = argv[2];
    else if (std::string(argv[1]) == "--pport")
      pport = atoi(argv[2]);
    else
    {
      std::cerr << "Wrong number of arguments!" << std::endl;
      std::cerr << "Usage: mymodule [--pip robot_ip] [--pport port]" << std::endl;
      exit(2);
    }
  }

  // Si il y a deux arguments on specifie le PORT et L'IP
  if (argc == 5)
  {
    if (std::string(argv[1]) == "--pport"
        && std::string(argv[3]) == "--pip")
    {
      pport = atoi(argv[2]);
      pip = argv[4];
    }
    else if (std::string(argv[3]) == "--pport"
             && std::string(argv[1]) == "--pip")
    {
      pport = atoi(argv[4]);
      pip = argv[2];
    }
    else
    {
      std::cerr << "Wrong number of arguments!" << std::endl;
      std::cerr << "Usage: mymodule [--pip robot_ip] [--pport port]" << std::endl;
      exit(2);
    }
  } 


  /**
   * On cree un proxy qui permet d'appeler un module defini dans l'API
   * La methode pour appeler ce module ce presente comme ceci :
   * AL::ALProxy proxy(<module_name>, <robot_IP>, <robot_port>);
   */
  // Creation d'un proxy vers ALTextToSpeechProxy
  AL::ALProxy proxy("ALTextToSpeech", pip, pport);

  /**
   * Si la methode ne renvoie rien
   * On l'appelle de cette maniere :
   * proxy.callVoid(<bind_method>, <parameter>, ...)
   */
  // On appelle la methode Say
  proxy.callVoid("say", std::string("Bonjour je m'appelle NAO!"));

  return 0;
}

Annexe 2

Ce programme fait tourner la tête du Nao dans un sens puis dans l'autre avant de revenir en position initiale.

#include <iostream>
#include <stdlib.h>
#include <stdio.h>

#include <alcommon/alproxy.h>           //Librairie permettant la creation du proxy
#include <alproxies/almotionproxy.h>    //Librairie contenant les fonctions pour faire bouger le NAO


int main(int argc, char* argv[])
{
  // Nous allons connecter notre Broker a Naoqi
  ...
  Toute la partie connexion est identique au programme précédent.
 
  /** Le nom de la partie que l'on souhaite bouger */
  const AL::ALValue jointName = "HeadYaw";      //Ici il s'agit de la tete


  try {
    /** On cree notre proxy qui servira a appeler les methodes pour bouger la tete
    * Les arguments du constructeur sont :
    * - L'addresse IP du NAO
    * - Le port utilisé. Par default il s'agit du port 9559
    */
    AL::ALMotionProxy motion(pip, 9559);


    /** Afin de bouger la tete on doit vérifier qu'elle n'est plus asservie
    * Pour cela on va mettre la "durete" de la tete a la valeur maxi, c'est dans cette position que l'on peut bouger la tete
    */
    /** Durete voulue */
    AL::ALValue stiffness = 1.0f;
    /** Temps en seconde pour atteindre cette valeur */
    AL::ALValue time = 1.0f;
    /** Appel de la methode pour realiser l'action */
    motion.stiffnessInterpolation(jointName, stiffness, time); 


    /** Ensuite on va bouger la tete
    * Pour cela on va lui donner des angles et le temps utilisé pour atteindre ces angles
    */
    /** On défini une liste d'angles, en radians. */
    AL::ALValue targetAngles = AL::ALValue::array(-1.5f, 1.5f, 0.0f);
    /** On defini les temps correspondants, en secondes. */
    AL::ALValue targetTimes = AL::ALValue::array(3.0f, 6.0f, 9.0f);
    /** Ensuite on peut choisir si les angles sont des positions absolues ou relatives par rapport a la position actuelle de la tete. */
    /** Ici se sont des angles absolus. */
    bool isAbsolute = true;

    /** Puis on appelle notre fonction. */
    motion.angleInterpolation(jointName, targetAngles, targetTimes, isAbsolute);


    /** Pour finir on remet la durete a sa valeur initiale. */
    stiffness = 0.0f;
    time = 1.0f;
    motion.stiffnessInterpolation(jointName, stiffness, time);


    }
    catch (const AL::ALError& e) {
      std::cerr << "Caught exception: " << e.what() << std::endl;
      exit(1);
    }
exit(0);
}

Annexe 3

Voici le programme final permettant de réaliser un suivi de l'angle du gyromètre avec la tête du Nao. Il y a peu de commentaires car toute la configuration est identique au programme précédent. La partie gestion du port série n'est pas détaillée ici mais elle est disponible sur Github.

struct sigaction action;
bool inter = true;

//Gestion du SIGINT afin de permettre l’arrêt du programme
void hand(int sig){
	if (sig == SIGINT){
		printf("SIGINT\n");
		inter = false;
	}
	else{
		printf("bizarre !\n");
	}
}

int main(int argc, char* argv[])
{
  //Connexion 
  ...

  /** Nom du joint à bouger. */
  const AL::ALValue names = "HeadYaw";
  int fd;
  double angle;
  unsigned char buf[8] = {0};
  action.sa_handler = hand;
  sigaction(SIGINT, &action, NULL);

  init_serial(&fd); /Initialisation du port serie

  AL::ALMotionProxy motion(pip, 9559);
  AL::ALRobotPostureProxy robotPosture(pip,9559);


  try {

    //Make the NAO stand
    robotPosture.goToPosture("Stand", 1.0f);

    AL::ALValue stiffness = 1.0f;
    AL::ALValue time = 1.0f;
    motion.stiffnessInterpolation("Head", stiffness, time);

    double newAngle=0.0,a0=0,rangle;
    while(a0 == 0){
	getData(buf,fd);
	a0 = getAngle(buf);
    }

    while(inter){
		
	getData(buf,fd);
	angle = getAngle(buf);
	if (angle != 0){
		rangle = angle -a0;
	}

	if (rangle > 180){
		rangle = rangle - 360;
	}
	else if (rangle < -180){
		rangle = rangle + 360;
	}
	rangle = (rangle*PI)/180;
	std::vector<float> sensorAngle = motion.getAngles(names, true); 

	double changes = rangle - sensorAngle[0];
	std::cout << "changes: " << changes << std::endl;
	float fractionMaxSpeed = 0.10f;
	// Appelle la fonction changeAngles qui permet de bouger un joint à la vitesse désirée
	if(changes > 0.07 || changes < -0.07){
		motion.changeAngles(names, changes, fractionMaxSpeed);
	}
	else{
	} 			
    }

    stiffness = 0.0f;
    time = 1.0f;
    motion.stiffnessInterpolation("Head", stiffness, time);

    robotPosture.goToPosture("Sit", 1.0f);

 
    } 
    catch (const AL::ALError& e) {
      std::cerr << "Caught exception: " << e.what() << std::endl;

      robotPosture.goToPosture("Sit", 1.0f);

      exit(1);
    }
exit(0);
}

Annexe 4

Voici le programme Robotino qui permet de déplacer le Robotino tout en envoyant les données de position via un serveur UDP.

#define _USE_MATH_DEFINES
#include <cmath>
#include <iostream>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <stdio.h>
#include <unistd.h>
#include <netdb.h>
#include <string.h>
#include <signal.h>

#include "rec/robotino/api2/all.h"


using namespace rec::robotino::api2;

bool _run = true;

void messageUDP(char *hote,char *service,float message,int taille){

	 struct addrinfo precisions,*resultat;
	 int statut;
	 int s;
 
 	/* Creation de l'adresse de socket */
 	memset(&precisions,0,sizeof precisions);
	precisions.ai_family=AF_UNSPEC;
	precisions.ai_socktype=SOCK_DGRAM;
	statut=getaddrinfo(hote,service,&precisions,&resultat);
	if(statut<0){ perror("messageUDPgenerique.getaddrinfo"); exit(EXIT_FAILURE); }

	/* Creation d'une socket */
	s=socket(resultat->ai_family,resultat->ai_socktype,resultat->ai_protocol);
	if(s<0){ perror("messageUDPgenerique.socket"); exit(EXIT_FAILURE); }

	/* Option sur la socket */
	int vrai=1;
	if(setsockopt(s,SOL_SOCKET,SO_BROADCAST,&vrai,sizeof(vrai))<0){
		perror("initialisationServeurUDPgenerique.setsockopt (BROADCAST)");
		exit(-1);
	}

	/* Envoi du message */
	int nboctets=sendto(s,&message,taille,0,resultat->ai_addr,resultat->ai_addrlen);
	if(nboctets<0){ perror("messageUDPgenerique.sento"); exit(EXIT_FAILURE); }

	/* Liberation de la structure d'informations */
	freeaddrinfo(resultat);

	/* Fermeture de la socket d'envoi */
	close(s);
}

void sigint_handler( int signum )
{
	_run = false;
}

class MyCom : public Com
{
public:
	MyCom()
	{
	}
	void errorEvent( const char* errorString )
	{
		std::cerr << "Error: " << errorString << std::endl;
	}
	void connectedEvent()
	{
		std::cout << "Connected." << std::endl;
	}
	void connectionClosedEvent()
	{
		std::cout << "Connection closed." << std::endl;
	}
	void logEvent( const char* message, int level )
	{
		std::cout << message << std::endl;
	}
	void pingEvent( float timeMs )
	{
		std::cout << "Ping: " << timeMs << "ms" << std::endl;
	}
};

class MyBumper : public Bumper
{
public:
	MyBumper()
		: bumped( false )
	{
	}
	void bumperEvent( bool hasContact )
	{
		bumped |= hasContact;
		std::cout << "Bumper has " << ( hasContact ? "contact" : "no contact") << std::endl;
	}
	bool bumped;
}; 

MyBumper bumper;
MyCom com;
OmniDrive omniDrive;
Odometry odometry;

void init( const std::string& hostname )
{
	// Connect
	std::cout << "Connecting...";
	com.setAddress( hostname.c_str() );

	com.connectToServer( true );

	if( false == com.isConnected() )
	{
		std::cout << std::endl << "Could not connect to " << com.address() << std::endl;
		rec::robotino::api2::shutdown();
		exit( 1 );
	}
	else
	{
		std::cout << "success" << std::endl;
		odometry.set(0,0,0,false);
	}
}

void move(char * hote)
{
	float mouvement[2] = {0.0f, 0.0f};
	double x=0,y=0,phi=0,sep=0;
	unsigned int *seq;
	char *service="5000";
	sep = 4000;
	int t0=com.msecsElapsed(),t=0,dir=0;
	
       //Menu to choose direction
	while (dir != 1 && dir != 2 && dir != 3 && dir != 4 )
	{	
		std::cout << "which direction :" << std::endl;
		std::cout << "1 for front" << std::endl;
		std::cout << "2 for back" << std::endl;
		std::cout << "3 for left" << std::endl;
		std::cout << "and 4 for right" << std::endl;	
		std::cin >> dir;
	}
	
	if (dir == 1)
	{
		mouvement[0]=0.2f;
		mouvement[1]=0.0f;
	}
	else if (dir == 2)
	{
		mouvement[0]=-0.2f;
		mouvement[1]=0.0f;
	}
	else if (dir == 3)
	{
		mouvement[0]=0.0f;
		mouvement[1]=0.2f;
	}
	else if (dir == 4)
	{
		mouvement[0]=0.0f;
		mouvement[1]=-0.2f;
	}

	while( com.isConnected() && false == bumper.value() && _run && t<3000)
	{ 
		omniDrive.setVelocity( mouvement[0], mouvement[1], 0 ); //set direction
		com.processEvents();
		
		odometry.readings (&x,&y,&phi,seq);
               //send data with sep between each packet
		messageUDP(hote,service,(float)x,sizeof(x));
		messageUDP(hote,service,(float)y,sizeof(y));
		messageUDP(hote,service,(float)phi,sizeof(phi));
		messageUDP(hote,service,(float)sep,sizeof(sep));
		odometry.set(0,0,0,false);
		t=com.msecsElapsed()-t0; //compute time to move while t < 3 seconds
	}
}

void destroy()
{
	com.disconnectFromServer();
}

int main( int argc, char **argv )
{
	std::string hostname = "172.26.1.1";
	if( argc > 1 )
	{
		hostname = argv[1];
	}

	struct sigaction act;
	memset( &act, 0, sizeof( act ) );
	act.sa_handler = sigint_handler;
	sigaction( SIGINT, &act, NULL );
	char *hote = argv[2]; 

	try
	{
		init(hostname);
		while( com.isConnected() && false == bumper.value() && _run)
			move(hote);
		destroy();
	}
	catch( const rec::robotino::api2::RobotinoException& e )
	{
		std::cerr << "Com Error: " << e.what() << std::endl;
	}
	catch( const std::exception& e )
	{
		std::cerr << "Error: " << e.what() << std::endl;
	}
	catch( ... )
	{
		std::cerr << "Unknow Error" << std::endl;
	}

	rec::robotino::api2::shutdown();

}

Fichiers Rendus

media:Rapport_ProjetHumanRobot.pdf‎