Laboratoire SUPINFO de recherche en robotique

Utilisation ROS

Utilisation ROS

10 mar, 2013

Création, développement, compilation et test d’un nœud ROS pour contrôler un robot

Lego Mindstorm NXT

 

Introduction :

Au cours du dernier tutoriel (Installation ROS fuerte) nous avons vu comment installer et configurer ROS. Nous avons également installé le stack NXT fournissant Driver et API pour contrôler les robots Mindstorm NXT.

Ce tutoriel explique comment utiliser cette API pour développer une première application permettant de contrôler un robot mobile. Lors de la réalisation de ce tutoriel, le robot utilisé fut LunarNXT.

N’oubliez pas de faire les tutoriels fournis par ROS (http://www.ros.org/wiki/ROS/Tutorials/) afin d’avoir une bonne compréhension des principes et fonctions utilisés ci-après.

La lecture de la documentation du package « nxt_ros » (http://www.ros.org/wiki/nxt_ros) permet d’aller plus loin dans le contrôle d’un NXT.

 

Création d’un package :

Un package est un ensemble de ressources (nœuds, librairies, messages, etc.) légères et réutilisables. À la fin de ce tutoriel, notre package contiendra deux applications (nœuds).

Premièrement, nous allons nous assurer que notre environnement est opérationnel et que les bonnes variables d’environnement ont été assignées :

source ~/fuerte_workspace/setup.sh
roscd

 

A ce stade, nous devrions être dans le dossier workspace créé dans le tutoriel précédent.

Nous allons donc créer le package « nxt_first » en précisant que nous aurons pour dépendance les packages : « roscpp », « nxt_msgs », « nxt_ros »

roscreate-pkg nxt_first roscpp nxt_msgs nxt_ros

 

Cette commande devrait (en fonction de votre configuration) afficher un warning spécifiant que votre répertoire courant ne fait pas partie du ROS_PACKAGE_PATH. Pas d’inquiétude, nous allons remédier à ça :

rosws set nxt_first
source setup.sh

 

Théoriquement, votre package est prêt à être compilé. Afin de vérifier lancez :

rosmake nxt_first

 

Conception :

Le but de ce tutoriel est de bien comprendre le fonctionnement des nœuds et des topics.

Le package nxt_ros contient un nœud (nxt_ros) qui une fois lancé , créer les topics permettant de communiquer avec notre robot. La communication entre l’ordinateur et le NXT est gérée par le nœud nxt_ros.
Il nous faut donc une application capable d’envoyer des messages sur les topics du NXT.
Nous allons développer un système comportant deux applications.

  • commander qui va nous permettre d’intercepter les entrées clavier de l’utilisateur et de les envoyer sur un topic de ROS.
  • nxt_first nous permettant de faire le lien entre notre topic et ceux de nxt_ros.

 

tuto_utilisation_ros

 

Nous allons donc nous concentrer sur le développement des applications « commander » et « nxt_first ».

 

Création de notre message

Comme expliqué sur la documentation de ROS, la communication par le biais d’un topic consiste à envoyer un message d’un « publisher » à un « subscriber ».
Le message que nous allons envoyer de notre application « commander » à notre application « nxt_first » contiendra un type de déplacement, un sens ainsi qu’une valeur indiquant la puissance demandée.
Afin de générer ce message, nous allons créer un dossier « msg » dans le répertoire racine de notre package ainsi qu’un fichier permettant de donner sa description.

roscd nxt_first
mkdir msg
touch msg/Order.msg

 

Maintenant nous allons éditer le fichier Order.msg (avec votre éditeur préféré) :

~/fuerte_workspace/nxt_first/msg/Order.msg
int8 order 
float64 effort
bool direction

 

« order » correspond à un ordre : ici nous utiliserons 0 pour un déplacement linéaire et 1 pour tourner.
« effort » nous indique la puissance demandée aux moteurs : nous essayerons dans la mesure du possible d’envoyer un effort positif
« direction » indique le sens du déplacement : ici true signifie vers l’avant ou à droite, false signifie ver l’arrière ou a gauche (en fonction de la valeur de order)
Nous devons à présent indiquer à ROS que lors de la compilation du package, il devra générer notre message.
Nous allons donc éditer le fichier CMakeLists.txt (à la racine du package) et décommenter la ligne 20 : rosbuild_genmsg()

~/fuerte_workspace/nxt_first/CMakeLists.txt

 

Compilons le :

rosmake nxt_first

Normalement, cette dernière commande nous a généré différents fichiers contenant les prototypes et implémentations d’une classe : nxt_first::Order. Notamment le fichier :

~/fuerte_workspace/nxt_first/msg_gen/cpp/include/nxt_first/Order.h

 

Création de l’application nxt_first

Nous allons créer le fichier source qui contiendra notre application.Pour notre application, nous aurons besoin d’une classe Adventurer représentant notre robot. Au sein de cette classe nous développerons les méthodes permettant de diriger le robot : move et turn. Nous développerons également les méthodes capables d’écouter et de publier sur un topic ROS.

Pour ce faire nous allons créer le fichier nxt_first.cpp

roscd nxt_first
touch src/nxt_first.cpp

Nous pouvons maintenant éditer ce fichier !

~/fuerte_workspace/nxt_first/src/nxt_first.cpp

 

Les includes :

#include <ros/ros.h> // nous donne l'accès aux fonctionnalités et objets natifs de ROS
#include <cmath> // nous permettra de récupérer la valeur absolue de l'effort
#include <nxt_msgs/JointCommand.h> // récupère le message JointCommand du package nxt_msgs
#include <nxt_first/Order.h> // récupère le message que nous avons créé précédement

 

Notre classe principale :

Afin de pouvoir communiquer avec ROS, nous aurons besoin d’un NodeHandle.

Pour envoyer et recevoir des messages au travers des topics ROS, nous aurons besoin d’un Subscriber et d’un Publisher.

Et pour finir, nous aurons besoin de spécifier au stack nxt_ros à quel moteur nous souhaitons envoyer des ordres. Les moteurs sont identifié par des chaînes de caractères, ici : m_sRightWheel et m_sLeftWheel.

class Adventurer
{
public:
 Adventurer() { init(); }
 ~Adventurer() { ; }
private:
 // permet d'intéragir avec ros (récupération des parametre, déclaration des subscriber, des publisher, etc.
 ros::NodeHandle m_NodeHandle;

 // représente le publisher permettant de publier des ordres aux moteurs
 ros::Publisher m_WheelsPublisher;
 // représente le subscriber recevant des ordres de notre autre application
 ros::Subscriber m_OrderSubscriber;

 // nom du moteur droit
 std::string m_sRightWheel;
 //nom du moteur gauche
 std::string m_sLeftWheel;
 // initialise les variables du noeud
 void init();
 // callback appellé à la reception d'un ordre
 void order_cb(const nxt_first::Order::ConstPtr& msg);

 // fait avancer ou reculer le robot
 void move(float fEffort = 0.0, bool bAhead = true);
 // fait tourner le robots à droite ou à gauche
 void turn(float fEffort = 0.0, bool bRight = true);
};

Les variables « m_sRightWheel » et « m_sLeftWheel » spécifient les noms utilisés par nxt_ros pour différencier nos moteurs. Nous verrons plus loin comment les assigner.

 

Le corps de notre classe :

La méthode init :

Cette méthode va nous permettre d’assigner nos variables membres.

void Adventurer::init()
{
 // strings temporaires
 std::string sSubscribeName,
 sRightWheel,
 sLeftWheel;
 // essaye de récupérer le nom du topic sur lequel on recoie les ordres
 // si aucun parametre n'est pas trouvé, on utilise une valeur par defaut
 if (!m_NodeHandle.getParam("sub_name", sSubscribeName))
 sSubscribeName = "adventurer_order"; // valeur par defaut

 // création du subscriber pour recevoir les ordres
 // on passe en parametre la callback (order_cb) et l'objet qui l'appellera (this)
 m_OrderSubscriber = m_NodeHandle.subscribe(sSubscribeName, 1, &Adventurer::order_cb, this);
 // création du publisher pour envoyer des ordres au robot
 m_WheelsPublisher = m_NodeHandle.advertise<nxt_msgs::JointCommand>("joint_command", 1);
 // cf. ci-avant pour la variable sSubscribeName
 if (!m_NodeHandle.getParam("right_wheel", sRightWheel))
 sRightWheel = "motor_r";
 if (!m_NodeHandle.getParam("left_wheel", sLeftWheel))
 sLeftWheel = "motor_l";
 m_sRightWheel = sRightWheel;
 m_sLeftWheel = sLeftWheel;
}

 

La méthode order_cb(const nxt_first::Order::ConstPtr& msg)

L’objet nxt_first::Order::ConstPtr , déclaré dans le header généré à partir de notre fichier msg/Order.msg est en réalité un boost::shared_ptr. Vous trouverez plus d’informations ici ou encore ici.

~/fuerte_workspace/nxt_first/msg_gen/cpp/include/nxt_first/Order.h
typedef boost::shared_ptr< ::nxt_first::Order_<ContainerAllocator> const> ConstPtr;

 

Dans tous les cas, notre méthode est en fait une callback qui sera appelée automatiquement par ROS   lorsque ce dernier recevra un message sur le topic du nom sSubscribeName de la méthode init. Le ConstPtr nous permettra de récupérer les valeurs du message.

void Adventurer::order_cb(const nxt_first::Order::ConstPtr& msg)
{
 if (msg->order == 0)
 move(msg->effort, msg->direction);
 else
 turn(msg->effort, msg->direction);
}

 

La méthode move(float fEffort, bool bAhead)

void Adventurer::move(float fEffort, bool bAhead)
{
 // création de deux messages <JointCommand> (1 pour le moteur droit et 1 pour le moteur gauche)
 nxt_msgs::JointCommand rightCommand;
 nxt_msgs::JointCommand leftCommand;
 // assignation des noms 
 rightCommand.name = m_sRightWheel;
 leftCommand.name = m_sLeftWheel;
 // pour un déplacement linéaire on assigne le même effort aux deux moteurs
 if (bAhead)
 rightCommand.effort = leftCommand.effort = std::abs(fEffort);
 else
 rightCommand.effort = leftCommand.effort = -std::abs(fEffort);
 // on publie
 m_WheelsPublisher.publish(rightCommand);
 m_WheelsPublisher.publish(leftCommand);
}

 

La méthode turn(float fEffort, bool bRight)

void Adventurer::turn(float fEffort, bool bRight)
{
 // cf. ligne 73
 nxt_msgs::JointCommand rightCommand;
 nxt_msgs::JointCommand leftCommand;

 rightCommand.name = m_sRightWheel;
 leftCommand.name = m_sLeftWheel;
 // pour tourner on applique un effort inverse sur chaque moteur
 if (bRight)
 {
 rightCommand.effort = -fEffort;
 leftCommand.effort = fEffort;
 }
 else
 {
 rightCommand.effort = fEffort;
 leftCommand.effort = -fEffort;
 }
 m_WheelsPublisher.publish(rightCommand);
 m_WheelsPublisher.publish(leftCommand);
}

 

La fonction main :

Notre classe est prête, il ne nous reste plus qu’à l’instancier et laisser ROS gérer les événements pour nous grace à la méthode ros::spin().

int main(int argc, char** argv)
{
 ros::init(argc, argv, "nxt_first");
 Adventurer a;
 ros::spin();
 return 0;
}

 

Compilation :

Nous allons désormais indiquer dans le fichier CMakeLists.txt de compiler nos sources afin d’en faire un éxécutable en ajoutant la ligne :

~/fuerte_workspace/nxt_first/CMakeLists.txt
rosbuild_add_executable(nxt_first src/nxt_first.cpp)

Pour lancer la compilation,

rosmake nxt_first

 

Test :

Nous allons maintenant tester notre application (sans utiliser le robot). Pour se faire nous allons ouvrir quatre terminaux. (Nous allons utiliser quelques commandes de ROS, voici une bonne aide).
Dans le premier terminal, nous allons lancer le noyau ROS:

roscore

Dans le deuxième nous, lançons notre application :

rosrun nxt_first nxt_first

Dans le troisième nous allons vérifier les topics créés par notre application en lançant :

rostopic list

Cette dernière commande devrait afficher les topics suivant :

/adventurer_order
/joint_command
/rosout
/rosout_agg

Les topics « rosout » et « rosout_agg » sont les topics du online casino noyau de ROS, nous n’allons pas nous en préoccuper.
Les deux autres topics correspondent aux Publisher et Subscriber de notre application (cf. Notre classe principale).

Dans ce même terminal nous allons maintenant écouter ce qui se passe sur le topic « /joint_command » ce topic sera par la suite partagé avec le noeud nxt_ros :

rostopic echo joint_command

Et dans le dernier terminal nous allons envoyer un message en ligne de commande afin de voir si notre application le reçoit bien et si elle publie correctement :

rostopic pub -1 /adventurer_order nxt_first/Order '{ order: 0, effort: 0.7, direction: 0 }'

En revenant sur le troisième terminal, nous pouvons constater que deux messages ont été publiés (un par moteur) :

name: motor_r
effort: -0.699999988079
---
name: motor_l
effort: -0.699999988079
---

Nous avons notre première application permettant de communiquer entre différents nœuds ROS.
Dans les deux prochains chapitres nous verrons comment réaliser une télécommande et connecter notre application au NXT.

 

Création de l’application commander

Le principe de cette application est d’intercepter les entrées clavier dans la console, de les interpréter afin de générer un message Order et de l’envoyer par le biais d’un Publisher.
Nous aurons donc besoin de méthodes pour modifier le fonctionnement de la console, d’un NodeHandle, d’un Publisher et d’un Order (notre message).

Nous allons commencer par créer le fichier teleop.cpp

roscd nxt_first
touch src/teleop.cpp

Et ainsi l’éditer :

~/fuerte_workspace/nxt_first/src/teleop.cpp

 

Les includes :

#include <signal.h>
#include <termios.h>
#include <stdio.h>
#include "ros/ros.h"
#include <cmath>
#include <nxt_first/Order.h>

 

Quelques define nous permettant d’interpréter les entrées utilisateurs :

#define ACCELERATE 'a' // incrémente l'effort courant
#define DECELERATE 'e' // décrémente l'effort courant
#define GO_AHEAD 'z' // ordre d'avancer
#define TURN_LEFT 'q' // ordre de tourner à gauche
#define GO_BACK 's' // ordre de reculer
#define TURN_RIGHT 'd' // ordre de tourner à droite
#define STOP 'x' // ordre de s’arrêter

 

Quelques constantes pour la génération des Order :

const int MOVE_ORDER(0), BACK_DIR(0), LEFT_DIR(0);
const int TURN_ORDER(1), AHEAD_DIR(1), RIGHT_DIR(1);

 

Quelques fonctions permettant d’obtenir une console en « raw » :

Ici nous allons créer deux fonctions. La première « quit » va nous permettre de revenir à une console normale en quittant l’application. La deuxième quand à elle va nous permettre d’intercepter directement les touches saisies dans la console.

Pour plus d’informations : c’est ici.

int kfd = 0;
static struct termios old_term, new_term;

void quit(int sig)
{
 tcsetattr(kfd, TCSANOW, &old_term);
 ros::shutdown();
 exit(0);
}

void initTermios(int echo)
{
 tcgetattr(kfd, &old_term);
 memcpy(&new_term, &old_term, sizeof(struct termios));
 new_term.c_lflag &= ~(ICANON | ECHO);
 new_term.c_cc[VEOL] = 1;
 new_term.c_cc[VEOF] = 2;
 tcsetattr(kfd, TCSANOW, &new_term);
}

 

Notre classe principale :

Nous allons déclarer ici la seule classe de notre nœud.

class Orderer
{
public:
 Orderer() { init(); }
 ~Orderer() { ; }
 // methode gerant l'entrée clavier
 void process();
private:
 ros::NodeHandle m_NodeHandle;
 // représente le publisher qui sera utilisé pour envoyer des ordres au noeud "adventurer_node"
 ros::Publisher m_Publisher;

 // représente l'ordre qui sera transmit au noeud "adventurer_node"
 // il permet de garder un l'historique du dernier ordre passé
 nxt_first::Order m_Order;
 // stoque la valeur de l'effort transmit aux moteurs
 int m_nCurrentEffort;
 // initialise les parametres du noeud (instancie le publisher)
 void init();
 // utilise le publisher pour envoyer m_Order
 void sendOrder(char receivedOrder);
};

 

Le corps de notre class :

La méthode init :

Cette méthode va nous permettre d’instancier notre Publisher et mettre la console en « raw »

void Orderer::init()
{
 std::string sName;

 // essaye de récuperer le parametre "pub_name" représentant le topic sur lequel on envoie les ordres.
 // si aucun parametre de ce nom n'est trouvé, on utilise la valeur par defaut : "adventurer_order"
 if (!m_NodeHandle.getParam("pub_name", sName))
 sName = "adventurer_order";
 // indique que le publisher publiera des messages du type nxt_first::Order au topic du nom sName précedement assigné
 // le '1' indique au topic de ne pas stocker l'ordre si il n'est pas consommé
 m_Publisher = m_NodeHandle.advertise<nxt_first::Order>(sName, 1);
 // valeur initiale pour l'effort
 m_nCurrentEffort = 1;
 // permet de lire la console 'sans appuyer sur entré'
 initTermios(0);
}

 

La méthode process :

Cette méthode est en fait une boucle infinie. A chaque entrée utilisateur dans la console, cette méthode va l’intercepter et appeler la méthode « sendOrder »

void Orderer::process()
{
 char c;
 puts("Keyboard input");
 puts("--------------");
 puts("[z q s d | x ]");
 while (true)
 {
 // lie la console
 if (read(kfd, &c, 1) < 0)
 exit(-1);
 sendOrder(c);
 }
}

 

La méthode sendOrder :

Cette méthode va interpréter l’entrée utilisateur et envoyer l’ordre correspondant sur le topic ROS.

void Orderer::sendOrder(char c)
{
 // associe une touche à un ordre
 switch(c)
 {
 case GO_AHEAD:
 m_Order.order = MOVE_ORDER;
 m_Order.direction = AHEAD_DIR;
 break;
 case TURN_LEFT:
 m_Order.order = TURN_ORDER;
 m_Order.direction = LEFT_DIR;
 break;
 case GO_BACK:
 m_Order.order = MOVE_ORDER;
 m_Order.direction = BACK_DIR;
 break;
 case TURN_RIGHT:
 m_Order.order = TURN_ORDER;
 m_Order.direction = RIGHT_DIR;
 break;
 case ACCELERATE:
 m_nCurrentEffort ;
 break;
 case DECELERATE:
 if (m_nCurrentEffort > 0)
 m_nCurrentEffort--;
 break;
 case STOP:
 m_nCurrentEffort = 0;
 break;
 }

 // 0.72 effort réel = 1 effort théorique 
 // valeur trouvée empiriquement specifique à notre construction
 m_Order.effort = (72.0f * (float)m_nCurrentEffort) / 100.0f;
 // envoie l'ordre sur le topic
 m_Publisher.publish(m_Order);
}

 

La fonction main :

Notre main va initialiser notre application comme étant une application ROS, ensuite nous instancierons un objet Orderer.
Nous en profiterons pour connecter le signal émit par le ctrl-c afin de remettre la console dans un mode plus conventionnel (pour plus d’informations : c’est ici).
Pour finir nous appellerons notre boucle infinie (process).

int main (int argc, char** argv)
{
 ros::init(argc, argv, "nxt_teleop");
 Orderer o;
 // permet d'intercepter le ctrl-c
 signal(SIGINT, quit);
 o.process();
 ros::spin();
 return 0;
}

 

Compilation :

Nous allons désormais indiquer dans le fichier CMakeLists.txt de compiler nos sources afin d’en faire un éxécutable en ajoutant la ligne :

~/fuerte_workspace/nxt_first/CMakeLists.txt
rosbuild_add_executable(commander src/teleop.cpp)

Pour lancer la compilation,

rosmake nxt_first

 

Test :

Nous allons à présent tester la combinaison de nos deux applications.
Pour ce faire nous allons reprendre nos quatre terminaux.
Dans le premier nous relancerons la commande :

roscore

Dans le deuxième la commande :

rostopic echo joint_command

Dans le troisième :

rosrun nxt_first nxt_first

Dans le dernier terminal nous allons lancer notre nouvelle application :

rosrun nxt_first commander

Dans ce dernier terminal, nous pouvons essayer notre application avec les touches que nous avons définies (z | q | s | d | a | e | x) et observer le résultat.

 

Test sur le robot :

Préparation :

Comme expliqué sur la documentation du noeud nxt_ros nous allons commencer par déclarer notre robot et ces capteurs dans un fichier yaml. Le fichier ci-après correspond à la description de notre robot LunarNXT :
Création du fichier :

roscd nxt_first
touch robot.yaml

Puis nous l’éditons :

~/fuerte_workspace/nxt_first/robot.yaml
nxt_robot:
 - type: motor
 name: motor_l
 port: PORT_A
 desired_frequency: 20.0

 - type: motor
 name: motor_r
 port: PORT_B
 desired_frequency: 20.0

 - type: motor
 name: motor_aux
 port: PORT_C
 desired_frequency: 1.0

 - type: touch
 frame_id: touch_frame
 name: touch_l
 port: PORT_1
 desired_frequency: 1.0

 - type: touch
 frame_id: touch_frame
 name: touch_r
 port: PORT_2
 desired_frequency: 1.0

 - type: ultrasonic
 frame_id: ultrasonic_link
 name: ultrasonic_sensor
 port: PORT_3
 spread_angle: 0.2
 min_range: 0.01
 max_range: 1.0
 desired_frequency: 1.0

 - type: color
 port: PORT_4
 desired_frequency: 20.0
 name: color_frame
 frame_id: color_frame

Nous allons également créer un fichier robot.launch. Un fichier « launch » permet de lancer plusieurs noeuds / application ainsi que le noyau ROS en une ligne de commande. La documentation : c’est ici.

Création du fichier :

touch robot.launch

Et nous l’éditons :

~/fuerte_workspace/nxt_first/robot.launch
<launch>
<!-- DEBUT NXT -->
 <!-- nxt_ros launch -->
 <node pkg="nxt_ros" type="nxt_ros.py" name="nxt_ros" output="screen" respawn="true">
 <rosparam command="load" file="$(find nxt_first)/robot.yaml" />
 </node>
<!-- FIN NXT -->

</launch>

 

Exécution :

Assurez-vous que votre robot est allumé et connecté à l’ordinateur.
Lancez trois terminaux.
Dans le premier nous allons lancer le noyau ROS ainsi que le nœud grâce au fichier launch que nous venons de créer :

roslaunch nxt_first robot.launch

Dans le deuxième :

rosrun nxt_first nxt_first

Dans le dernier :

rosrun nxt_first commander

Vous pouvez désormais tester votre robot ainsi que vos applications ! :)

Il ne vous reste plus qu’à développer vos propres applications ainsi que vos propres robots !

FacebookTwitterGoogle+
468 ad

Laisser un commentaire