Module de cartographie autonome pour Turtlebot

Table des matières

Introduction

Notre projet consiste à réaliser la navigation autonome d'un robot dans un environnement clos. De nos jours, ce type de navigation peut être utile par exemple pour l'exploration et la cartographie d'un environnement inconnu.

Le robot que nous allons utiliser est un Turtlebot, un robot simple et intuitif composé d'une base Kobuki, une Asus Xtion Pro, des structures Turtlebot et un Netbook Asus. Afin de faire fonctionner le robot et de pouvoir le manipuler, nous utiliserons principalement ROS (Robot Operating System) et ses différents outils.

Images/turtlebot2_front.jpg

A la fin de cette page, vous pourrez retrouver les répertoires des principaux codes.

Présentation du problème

Dans le cas de la navigation autonome, le robot doit impérativement pouvoir se repérer dans son environnement afin de pouvoir se diriger et savoir vers quel emplacement inexploré il doit se diriger. Une fois que le robot s'est correctement repéré dans l'environnement, il peut ensuite effectuer la cartographie à l'aide des laser scans qu'il génère. Lorsque le robot se déplacera ou au bout d'un certain temps, la cartographie sera alors mise à jour. En déplaçant le robot dans la pièce, il finira donc par obtenir une cartographie complète de la pièce.

Cependant, ce que nous souhaitons obtenir est une cartographie autonome d'un environnement clos, il faut donc mettre en place un algorithme décisionnel permettant de déterminer où le robot doit se diriger et quel point il doit atteindre. Cet algorithme permettra au robot de se diriger vers les points de la carte qui sont inexplorés. Il faudra prendre en compte aussi dans cet algorithme décisionnel, les points non accessibles et le moyen le plus rapide et le plus sûr de parcourir l'ensemble de l'environnement.

Avant de pouvoir mettre en oeuvre l'algorithme décisionnel, nous devrons dans un premier temps apprendre à manipuler les robots que nous allons utiliser puis dans un second temps, nous pourrons mettre en place l'algorithme décisionnel et résoudre les différents problèmes que nous rencontrerons.

Le résultat approximatif que nous désirons obtenir à la fin de la navigation autonome :

Images/map.png

Simulation du robot dans un environnement virtuel sur V-REP

Présentation de V-REP

Pour simuler un robot dans un environnement clos idéal, nous avons utilisé le simulateur robot V-REP qui fournit une architecture de contrôle permettant de contrôler indépendamment chaque objets ou modèles, particulièrement grâce à des noeuds ROS dans notre cas. V-REP permet la simulation d'environnements 3D avec une grande librairies d'objets mais également de robots. Dans le cadre de notre simulation, nous avons donc choisis le Pioneer p3dx qui est un robot à contrôle différentiel pourvu d'un laser scan de type Hokuyo.

Images/pioneer.png

Dans un premier temps, nous avons récupéré la scène disponible sur sirien.metz.supelec.fr : scene.ttt qui nous a permis de placer notre robot dans un environnement clos adapté à nos objectifs.

Images/VREP.png

En lançant le roscore et la simulation de notre scène V-REP, il est alors possible de communiquer avec notre robot par le biais de messages publiés sur les topics créés par notre robot.

Mise en place d'un contrôleur sous ROS

La première question qu'il est nécessaire de se poser avant de commencer est comment faire pour communiquer avec le robot? On souhaite pouvoir envoyer des ordres de commande à notre robot qui traduiront les vitesses de rotation à appliquer pour chacun de ses deux roues. On utilise pour ce faire le principe des topic ROS qui serviront à véhiculer des messages ROS qui correspondront à nos commandes de vitesse. Pour pouvoir contrôler notre robot, il nous faut donc lui publier des messages de type Float64 (std_msgs/Float64) sur les topics /vrep/leftWheelCommand et /vrep/rightWheelCommand. Ces messages correspondent à la valeur de la vitesse angulaire désirée pour chacune des roues, en m/s. L'idée est de pouvoir envoyer des messages de type geometry_msgs/Twist qui sont le format standard pour les messages de vélocité. Les twists sont en réalité des torseurs cinématiques : ils comportent donc un vecteur de vitesse ainsi qu'un vecteur de rotation. Le robot se déplaçant dans un univers en deux dimensions, la vitesse linéaire est donc nécessairement selon l'axe x et la vitesse angulaire est donc nécessairement selon l'axe z.

Connaissant la distance entre les deux roues (L = 33cm ) et le rayon de chacune des roues (R = 20cm ), on peut déterminer les relations liant la vitesse linéaire et angulaire aux vitesses de rotation de chacune des deux roues:

ωL  =  (v − ω(L)/(2))/(R) ωR  =  (v + ω(L)/(2))/(R)

Pour communiquer entre différents topics, on utilise des noeuds qui sont des petits programmes ROS qui vont réaliser nos opérations, écouter les noeuds et éventuellement publier de nouvelles informations sur de nouveaux topics. On peut donc écrire un simple noeud de type subscriber qui écoute les twists entrant sur le topic cmd_vel, les converti en vitesse angulaire pour chacune des roues et publie le résultat sur les topics /vrep/leftWheelCommand et /vrep/rightWheelCommand:

controler.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-


import rospy
import geometry_msgs.msg
import std_msgs.msg


L=33*10**(-2)
R=20*10**(-2)
leftWheelCommand=rospy.Publisher('vrep/leftWheelCommand',std_msgs.msg.Float64, queue_size=1)
rightWheelCommand=rospy.Publisher('vrep/rightWheelCommand',std_msgs.msg.Float64, queue_size=1)

def convert(msg):
    v=msg.linear.x
    w=msg.angular.z
    wl=(v-w*L/2)/R
    wr=(v+w*L/2)/R
    leftWheelCommand.publish(std_msgs.msg.Float64(wl))
    rightWheelCommand.publish(std_msgs.msg.Float64(wr))

def controler():
    rospy.init_node('controler', anonymous=True)
    rospy.loginfo("Lancement controleur")
    rospy.Subscriber('cmd_vel', geometry_msgs.msg.Twist, convert)
    rospy.spin()



if __name__=='__main__':
    controler()

Il est désormais possible de contrôler notre robot par des twists standards publié sur le topic cmd_vel. Pour le tester, on peut notamment utiliser un simple teleop.

SLAM (Simultaneous Localization and Mapping) avec gmapping

On veut désormais être capable de réaliser une cartographie de l'environnement lorsque l'on déplace ce dernier. Pour ce faire, on va utiliser le package gmapping basé sur un laserscan qui fournit une SLAM (Simultaneous Localization and Mapping). La localisation se fait alors par odométrie et à partir des tf qui correspondent à des changements de référentiels pour les différentes parties du robot. Elle est donc purement théorique. Les tf étant déjà réalisés par VREP, il suffit de régler le scan du gmapping sur celui envoyé par le robot de vrep (topic /vrep/front_scan).

En souscrivant aux deux topics tf et scan, le noeud gmapping publie alors la map sur le topic /map. Il est possible d'afficher la cartographie actuelle à l'aide de l'outil de visualisation 3D Rviz. On crée alors le launch file regroupant tous ces paramètres:

<launch>

<!-- Il faut d'abord avoir lancé la scène vrep et démarré la simulation -->
  <node pkg="cartographie_tl" name="controler" type="controler.py"/>
  <node pkg="rviz" name="rviz" type="rviz"/>
  <node pkg="gmapping" name="slam_gmapping" type="slam_gmapping">
   <remap from="scan" to="/vrep/front_scan"/>
   <param name="base_frame" value="Pioneer_p3dx"/>
   <param name="odom_frame" value="odom"/>
   <param name="map_frame" value="map"/>
   <param name="maxRange" value="6"/>
   <param name="maxURange" value="5.9"/>
  </node>

</launch>

En déplaçant le robot dans son environnement, on obtient le résultat suivant:

Images/CartographieVREP.png

Mise en place de la navigation stack avec amcl et move_base

La dernière étape pour établir notre cartographie autonome est de mettre en place la planification de trajectoires pour un goal donné (navigation stack). Pour ce faire, on va utiliser les packages AMCL (Adaptative Monte Carlo Localization) et move_base.

Le package AMCL fournit une localisation probabiliste du robot à partir d'un filtre particulaire basé sur la map obtenue par le gmapping. Celui-ci estime les modèles des murs à partir de la map pour obtenir une localisation du robot en fonction des informations reçue par le laser scan. Ce package s'additionne alors à la localisation SLAM basée sur l'odométrie pour garantir un certain asservissement de l'information de position.

Le package move_base quant à lui permet à partir d'une map donnée (/map), de la position du robot (/tf et odom), des informations des capteurs (vrep/front_scan) et du goal (move_base_simple/goal) de planifier une trajectoire et de l'exécuter en envoyant des twists adaptés (cmd_vel). On utilise ici le principe de costmap qui correspond à une dilation des obstacle pour créer des zones d'accès interdites au robot afin d'éviter tout blocage. Il existe deux costmaps qui contiennent les informations des obstacles du monde :

  • Une utilisée pour la planification globale, c'est à dire une planification à long terme
  • Une utilisée pour la planification locale qui comprend une approche dynamique et permet de s'adapter en cours de trajectoire.

De la même manière, ce package contient deux planners qui sont le local planner et le global planner. Pour configurer le move_base, il faut alors configurer quatre fichiers:

  • costmap_common_params.yaml : Contient les informations que les deux costmap doivent suivre (surface du robot dans la map, portée des obstacles et topic du laserscan)
  • global_costmap_params.yaml : Contient les informations sur la map, la fréquence d'actualisation de la costmap globale.
  • local_costmap_params.yaml : Contient la taille des costmap locales, les informations sur la map et la fréquence d'actualisation de la costmap locale
  • base_local_planner_params.yaml : Qui contient les informations relatives aux limites de vélocité et d'accélération.

On remarquera ici que dans le cadre de notre simulation, il n'est pas nécessaire de mettre en place un global_planner_params.yaml qui utilise une planification globale à l'aide de calcul de coûts de trajectoire (notamment par des algorithmes de Dijkstra ou de calcul de grille). On se contente donc ici de planifier notre trajectoire.

Toutes ces fonctionnalités peuvent être représentées sur le schéma ci-dessous:

Images/NavigationStack.png

On peut alors mettre en place la navigation stack en lançant le noeud suivant, qui exploite les fichiers paramètres .yaml précédemment décrits:

cartographie.launch

<launch>
  <master auto="start"/>

  <!--- Run AMCL -->
  <include file="$(find amcl)/examples/amcl_omni.launch" />

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
	<rosparam file="$(find cartographie_tl)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
	<rosparam file="$(find cartographie_tl)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
	<rosparam file="$(find cartographie_tl)/param/local_costmap_params.yaml" command="load" />
	<rosparam file="$(find cartographie_tl)/param/global_costmap_params.yaml" command="load" />
	<rosparam file="$(find cartographie_tl)/param/base_local_planner_params.yaml" command="load" />
  </node>
</launch>

On peut alors envoyer de goals à notre robot par l'intermédiaire de move_base_simple/goal dans une map créée dynamiquement.

Noeud de navigation autonome

Récupération de la map sur python et conversion des coordonnées en pixel

Maintenant que nous avons tous les outils nécessaire pour mapper un environnement dynamiquement et envoyer des goals à notre robot qui planifiera de manière autonome la trajectoire pour atteindre ce dernier, on va pouvoir s'intéresser à l'aspect décisionnel pour envoyer de manière automatique des goals à notre robot afin qu'il explorer son environnement de manière autonome.

Pour ce faire, il va nous falloir récupérer la cartographie de notre robot sous python afin de pouvoir traiter cette image à l'aide d'un algorithme décisionnel pour en récupérer un nouvel goal en fonction de la position du robot. C'est le rôle de la classe turtlebot_map.py. On utilise alors la méthode get_map(self) qui permet de récupérer les metadatas de la map (largeur, hauteur, résolution) mais également les données de la map. Ces données sont contenues dans le vecteur mapData qui est un vecteur à une dimension représentant des des données 2D à l'aide de la méthode row major order.

Images/RowMajorOrder.gif

Ces données représentent donc la carte en pixels prenant des valeurs entre 0 et 100 caractérisant la probabilité d'occupation. On trouve alors trois valeurs dans ce vecteur : 0 pour le sol, 100 pour les murs et -1 pour les espaces inconnus. Connaissant la largeur de la map en pixel (with), on obtient finalement les informations sur le pixel (i,j) par : mapData(i*width + y). Cependant, la map obtenue correspond au symétrique de la map réelle, il faut donc inverser les valeurs de celles-ci.

def get_map(self):
 		# The map is accessed by calling the service dynamic_map()
		get_map = rospy.ServiceProxy('dynamic_map', GetMap)
		# Request the map as well as the metadata
		m = get_map().map
		width=m.info.width
		height = m.info.height
		res = m.info.resolution
		origin = m.info.origin
		data = m.data
		self.metadata = (width, height, res) #metadata pour la map
		x_origin = origin.position.x
		y_origin = origin.position.y
		theta_origin =  (tf.transformations.euler_from_quaternion((origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w)))[2]
		self.pose_origin = (x_origin, y_origin, theta_origin) #origine de la map

		# we also need to know where we are
		t0 = rospy.Time(0)
		self.listener.waitForTransform('map', self.base_link, t0, rospy.Duration(1))
		((x_robot,y_robot,z), rot) = self.listener.lookupTransform('map', self.base_link, t0)
		euler = tf.transformations.euler_from_quaternion(rot)
		theta_robot = euler[2]
		self.pose_robot = (x_robot, y_robot, theta_robot) #position du robot

		# mapData is the symmetric copy of the data in order to be able to modify the values. mapData is represented by  a row major order method.
		self.mapData=list(data)
		for i in range(height):
		  for j in range(width):
		     self.mapData[i*width+j]=data[(height-1-i)*width+j]
		
		return (self.metadata, self.mapData)

Cette méthode permet également l'actualisation de la position du robot et l'origine pour le système de coordonnées de la map.

Pouvant désormais récupérer et modifier les valeurs de la map, il faut maintenant faire le lien entre la carte réelle en mètres et la carte en pixel obtenue. Pour ce faire, il faut convertir les coordonnées du robot dans le référentiel d'odométrie à partir des coordonnées du point en pixel. On utilise les méthodes pix_to_pose(self, pose_in_im) et pose_to_pix(self, pose_robot).

Le robot est repéré dans le référentiel lié à l'origine. Pour obtenir les coordonnées de l'image en pixel, il nous faut donc nous ramener au référentiel absolu de la map pour ensuite se ramener au référentiel de l'image en prenant compte la résolution de la map.

Images/ReferentielsMap.png

On obtient donc les relations suivantes entre les coordonnées en pixel de l'image et les coordonnées du robot.

ximage  =  ((yorigin − yrobot))/(res) + h yimage  =   − ((xorigin − xrobot))/(res)

On peut alors écrire la méthode pix_to_pose(self, pose_in_im) et sa réciproque pose_to_pix(self, pose_robot) :

def pose_to_pix(self, pose_robot):
    w = self.metadata[0]
    h = self.metadata[1]
    res = self.metadata[2]
    # We first convert pose_robot from the "map" frame to the image frame
    x_robot, y_robot,theta_robot = pose_robot
    x_origin, y_origin, theta_origin= self.pose_origin
    xr_in_im = (y_origin - y_robot) / res + h
    yr_in_im = (x_robot -  x_origin) / res  
    # And apply a rotation
    theta_in_im = theta_robot - theta_origin
    return (int(xr_in_im), int(yr_in_im), theta_in_im)

Algorithme de décision pour la nouvelle étape de navigation

Maintenant que l'on peut récupérer la map, les coordonnées du robot dans l'image et convertir celles-ci en coordonnées dans le référentiel d'odométrie, nous allons traiter l'image obtenue afin de choisir le nouveau goal à atteindre pour la navigation.

Le principe de base du choix de la nouvelle trajectoire repose sur le fait que nous allons chercher des pixels connus adjacents à des pixels inconnus pour envoyer le robot sur un de ces pixels connus, lui permettant ainsi de découvrir une nouvelle zone. Tout d'abord, il nous faut sélectionner les zones accessibles et éliminer les points impossibles parmi les zones libres. En effet, il est nécessaire de prendre en compte les costmaps afin d'éviter que le robot ne tente d'atteindre un point sans pouvoir trouver de planification. Ce serait en effet le cas si le robot voyait à travers l'ouverture d'une porte sans pouvoir passer à travers. Pour ce faire, une utilise un algorithme de diffusion qui diffuse en partant de la position du robot. On définit ainsi une critère d'accessibilité en vérifiant que pour chaque point, aucun mur n'est présent dans un rayon de rayon_inflate:

def is_accessible(x,y,rayon_inflate=4): #Renvoie True si l'on n'est pas près d'un mur
    width = metadata[0]
    if(mapData[x*width+y]==0):
        for m in range(-rayon_inflate,rayon_inflate+1): #On regarde si l'on n'est pas près d'un mur (costmap)
            for n in range(-rayon_inflate,rayon_inflate+1):
                if(mapData[(x+m)*width+(y+n)]==100):
                    return False
        return True
    return False
Images/diffusion.gif

En diffusant alors dans les zones libre selon ce critère, on obtient finalement l'algorithme ci-dessous qui utilise une pile explicite qui se rempli avec tous les points potentiels. En effet, pour chaque pixel, on regarde tous les pixels adjacents en les ajoutant dans la pile et en testant le critère de manière récursive. Chaque pixel accessible est alors caractérisé par une nouvelle valeur arbitraire. (ici -2). Pour que l'algorithme démarre même si le robot est proche d'un mur, il faut automatiquement considérer tous les pixels compris dans une zone définie comme accessibles de base.

def remplissage_diff(): #Diffuse la zone d'accessibilité en prenant en compte l'espacement des murs
    pile=[]
    x_robot=pose_in_im[0] #Position du robot
    y_robot=pose_in_im[1]
    for i in range(-3,4):
        for j in range(-3,4):
            pile.append([x_robot+i,y_robot+j]) #On définit tous pixels autours du robot comme accessible pour que l'algorithme puisse toujours se lancer
    print("Analyse des zones accessibles")
    while pile != []:
        [x,y]=pile.pop()
        width = metadata[0]
        height = metadata[1]
        mapData[x*width+y]=-2 #La valeur arbitraire "-2" correspond à une zone accessible pour le robot
        for k in range(-1,2):
            for l in range(-1,2): #Pour chaque pixels adjacents au pixel actuel, on regarde si la zone est accessible
                if(k !=0 or l !=0):
                    if is_accessible(x+k,y+l):
                        pile.append([x+k,y+l])
        print("Analyse terminée")
return

On obtient finalement le résultat suivant qui garanti l'accessibilité de chacun des points.

Images/mapDiffusion.png

La dernière partie de notre algorithme consiste alors à balayer la carte et à tester la disponibilité de chaque pixel. Pour tester cette disponibilité, on regarder pour chaque pixel accessible si un de ses pixels adjacents est inconnu. Dans ce cas, on renvoie les coordonnées de ce pixel.

def is_free(x,y):
    width = metadata[0]
    if(mapData[x*width+y]==-2):
        for k in range(-1,2):
            for l in range(-1,2): # On regarde les cellules adjacentes
                if(k != 0 or l != 0):
                    if(mapData[(x+k)*width+(y+l)] == -1):  # Si une de ces cellules adjacentes est inconnue
                        print("Bordure trouvée")
                        return True
                return False
    else:
        return False

Le balayage de la map se fait par des carrés où le rayon des carrés augmente à chaque itération. On initialise la taille des carrés à 10 pixels pour éviter que le robot ne s'obstine à atteindre un pixel qui serait sous lui et qu'il ne pourrait pas actualiser.

Finalement, au terme de cet algorithme, on parvient finalement à obtenir le nouveau point à atteindre qui est accessible par le robot pour l'exploration de la carte.

Cartographie autonome

Maintenant que l'on sait déterminer à partir d'une carte en pixel les points nécessaires à atteindre, on peut désormais s'intéresser à la mise en boucle de la navigation autonome. Ainsi, le robot effectue les actions dans l'ordre suivant:

  • Le robot effectue une rotation en boucle ouverte (c'est à dire en publiant directement les twists) de 360 degrés sur lui-même pour faire un premier mapping de son environnement.
  • On récupère la map et les metadata associées à l'aide de l'instance de classe TurtleBotMap.py.
  • On récupère la position du robot dans l'image à l'aide de la conversion par la méthode pose_to_pix de TurtleBotMap.py
  • On effectue le remplissage par diffusion des zones accessibles.
  • On recherche le plus proche voisin remplissant le critère d'un pixel adjacent inconnu parmi les zones accessibles actualisées précédemment.
  • On envoie le robot atteindre ce nouveau goal en convertissant ces coordonnées pixel en coordonnées pour l'odométrie.

Pour pouvoir envoyer ces goals au robot, on utilise donc l'api de move_base. Il nous faut alors créer des messages de type MoveBaseGoal. On remplit donc le header (numéro du goal, moment de l'émission, le serveur map) ainsi que la position (geometry_msgs/Pose) qui contient la position et l'orientation. De plus, on doit attendre que le goal soit atteint avant d'en lancer un nouveau, c'est pourquoi qu'on lance la méthode wait_for_client() du client move_base.

En ce qui concerne la méthode de rotation, il nous suffit d'envoyer des twists sur le topic cmd_vel_mux/input/navi de manière persistante (avec une fréquence de 10 Hz) jusqu'à ce que le robot ait fait un tour sur lui-même.

Les deux méthodes décrites précédemment sont explicités dans le code suivant:

client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
client.wait_for_server()

def reach_goal(x, y, theta):
    target_quat = tf.transformations.quaternion_from_euler(0, 0, theta)
    t0=rospy.Time.now()
    goal=MoveBaseGoal()

    goal.target_pose.header.seq=goal_number
    goal.target_pose.header.stamp=t0
    goal.target_pose.header.frame_id=target_frame
    goal.target_pose.pose.position = Point(x, y, 0)
    goal.target_pose.pose.orientation.x = target_quat[0]
    goal.target_pose.pose.orientation.y = target_quat[1]
    goal.target_pose.pose.orientation.z = target_quat[2]
    goal.target_pose.pose.orientation.w = target_quat[3]

    # Sends the goal to the action server.
    client.send_goal(goal)

    # Waits for the server to finish performing the action.
    client.wait_for_result()
    print(client.get_result())

    # Prints out the result of executing the action
    return client.get_result()

def rotate(vitesse = 1):
    twist = Twist()
    twist.linear.x = 0
    twist.angular.z = vitesse  #vitesse en radians par secondes
    rate=rospy.Rate(10)
    timer = 0
    #Demarrage de la rotation
    while timer <  2*pi/vitesse: #On doit envoyer des messages de manière persistance pour que le robot continue de tourner
        cmd_vel.publish(twist)
        rate.sleep()
        timer = timer+0.1 #Fréquence de 10 Hz donc 0.1 secondes

Ainsi, on va continuer la boucle d'action tant qu'il est possible de trouver un plus proche voisin. C'est à dire tant que le résultat de find_ppv() n'est pas un vecteur "NaN".

De plus, on définit une méthode qui est lancée lors de l'interruption du programme (Ctrl + C) qui permet l'enregistrement de l'image sous format png.

def whenshutdown():
    rospy.loginfo("Stop node")
    width = metadata[0]
    height = metadata[1]
    image_array = np.zeros((height, width,3), dtype=int)

    # Plotting the map
    for i in range(height):
        for j in range(width):
            if(mapData[i*width+j] == -1): # Unknown
                image_array[i,j,0] = 255
                image_array[i,j,1] = 255
                image_array[i,j,2] = 255
            elif(mapData[i*width+j] == 0 or mapData[i*width+j] == -2): # Free
                image_array[i,j,0] = 125
                image_array[i,j,1] = 125
                image_array[i,j,2] = 125
            elif(mapData[i*width+j] == 100): # Walls
                image_array[i,j,0] = 0
                image_array[i,j,1] = 0
                image_array[i,j,2] = 0

    # Plotting the location of the robot
    for i in range(-3,4):
        for j in range(-3,4):
            image_array[pose_in_im[0]+i, pose_in_im[1]+j] = (255, 0, 0)

    # Plotting its orientation
    for i in range(10):
        image_array[int(pose_in_im[0]+i*sin(-pose_in_im[2])), int(pose_in_im[1]+i*cos(-pose_in_im[2]))] = (0, 0, 255)
        scipy.misc.imsave('map.png', image_array)

Application de la navigation autonome au Turtlebot

Prise en main du Turtlebot

Afin de travailler avec le Turtlebot, nous allons éviter de devoir écrire les différentes commandes et les différents scripts sur l'ordinateur Asus lié au Turtlebot car il est préférable de demander une force capacité de calcul à l'ordinateur fixe plutôt qu'à l'ordinateur lié au Turtlebot. Nous allons donc travailler sur le roscore de l'ordinateur fixe en établissant une connexion SSH entre celui-ci et l'ordinateur du Turtlebot. La configuration et les tutoriels du Turtlebot sont présents sur le site de ROS à cette adresse : turtlebot_tutorials

Une fois que tous les packages nécessaires pour l'utilisation du robot sont installés, on peut désormais s'intéresser à la mise en place de la connexion entre l'ordinateur fixe et le Turtlebot. Pour cela, il faut définir les variables ROS_HOSTNAME et ROS_MASTER_URI. La variable ROS_HOSTNAME correspond alors à l'adresse IP de la connexion wlan du Turtlebot. Par ailleurs, il faut également définir la variable ROS_MASTER_URI qui utilise l'adresse DNS et le port 11311 de l'ordinateur fixe sur lequel nous travaillons.

On peut désormais établir la liaison entre le Turtlebot et l'ordinateur par une connexion SSH en ne lançant qu'un seul roscore sur le PC.

bash:user$ ssh turtlebot@turtle2.smart.metz.supelec.fr

Pou initialiser le Turtlebot, il est nécessaire de lancer le bringup sur ce dernier en lançant le fichier launch du package turtlebot_bringup qui s'intitule minimal.launch.

De nombreux outils sont déjà pris en charge par le bringup du turtlebot notamment la génération des tf mais également le controler que nous avons développé précédemment. Ainsi, à l'aide d'un simple teleop publiant sur le topic cmd_vel_mux, il est possible de publier directement des twists de commande au Turtlebot.

De plus, de nombreux packages sont déjà disponibles sur le Turtlebot, notamment pour la navigation avec des demo de configuration pour le gmapping et le move_base. Ainsi, en récupérant les fichiers .yaml décrits précédemment, on va pouvoir obtenir les configurations adaptées pour la mise en place du move_base pour notre Turtlebot.

On crée donc un fichier launch intitulé cartographie_turtlebot.launch où on fait appel aux autres fichiers launch nécessaires pour réaliser la cartographie de l'environnement, c'est à dire le package move_base, amlc et gmapping. Ces différents fichiers sont disponibles dans le package turtlebot_navigation mais il est néanmoins nécessaire de les importer dans notre projet afin de pouvoir en modifier les paramètres. Une fois la cartographie mise en place, il suffit de lancer notre noeud de navigation turtlebot_explorer.py (en modifiant la valeur de base_frame adaptée au Turtlebot) qui va alors décider des emplacements où doit se diriger le robot et qui nous permettra donc d'effectuer la navigation autonome du robot.

Conversion des messages depthimage en Laserscan

Pour pouvoir assurer une cartographie 2D efficace d'un environnement 3D, il est nécessaire de tenir compte de certains paramètres. En effet, le package gmapping réalise une cartographie à partir de données de type LaserScan. Or, le Turtlebot est doté d'une caméra Asus Xtion Pro qui fournit un nuage de point et une image en profondeur. Il est donc nécessaire de convertir cette image en profondeur en laserscan pour pouvoir appliquer les modules de cartographie développés précédemment.

Le problème majeur rencontré dans cette conversion relève de la nature très restrictive de la partie de l'image convertie. En effet, tout se passe comme le le laserscan était réalisé au niveau de la camera, ce qui empêche alors le robot de voir les petits éléments au sol (fils, balles, etc.) mais également en hauteur (tables) et conduit à des erreurs de cartographie.

Pour réaliser cette conversion, il existe un package déjà existant nommé depthimage_to_laserscan qui prend en argument une hauteur de scan (scan_height) et qui va balayer l'image en profondeur centrée au milieu vertical de l'image d'une hauteur correspondante à la hauteur de scan. Le problème de ce package est qu'il n'élimine pas le sol du scan ce qui conduit inévitablement à considérer le sol comme un obstacle lorsque l'on augmente la hauteur du scan.

Pour modifier le package depthimage_to_laserscan, il faut donc l'importer comme projet personnel sur le Turtlebot afin de pouvoir le modifier et le recompiler par la suite. On renomme le package depth_to_laser, puis on modifie tous les autres fichiers nécessaires afin que la cartographie fonctionne avec ce package depth_to_laser à la place du depthimage_to_laserscan.

Pour pouvoir modifier la méthode de conversion de l'image en profondeur en laserscan, nous allons donc intervenir sur les fichiers DepthImageToLaserScan.cpp et DepthImageToLaserScan.h. Dans le fichier .h, c'est dans la fonction convert que nous réalisons le traitement de l'image du Turtlebot point par point. Le code de cette fonction est le suivant :

 template<typename T>
    void convert(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model, 
		 const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{
      // Use correct principal point from calibration
      float center_x = cam_model.cx();
      float center_y = cam_model.cy();

      // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
      double unit_scaling = depth_to_laser::DepthTraits<T>::toMeters( T(1) );
      float constant_x = unit_scaling / cam_model.fx();
      float constant_y = unit_scaling / cam_model.fy();
      
      const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
      int row_step = depth_msg->step / sizeof(T);

      int offset = (int)(cam_model.cy()-scan_height/2);
      depth_row += offset*row_step; // Offset to center of image


      for(int v = offset; v < offset+scan_height_; v++, depth_row += row_step){
	if(v <= cam_model.cy()) {
		// Rays to the roof 
		for (int u = 0; u < (int)depth_msg->width; u++) {// Loop over each pixel in row 
			T depth = depth_row[u];
			double r = depth;
			double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out
			int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;
			if (depth_to_laser::DepthTraits<T>::valid(depth)){ // Not NaN or Inf
				// Calculate in XYZ
				double x = (u - center_x) * depth * constant_x;
				double y = (v - center_y) * depth * constant_y;
				double z = depth_to_laser::DepthTraits<T>::toMeters(depth);
				// Calculate actual distance
				r = sqrt(pow(x, 2.0) + pow(z, 2.0));
			}
			if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){
				scan_msg->ranges[index] = r;
			}

		}
	}
	else {
		double alpha = atan2((v - center_y) * constant_y, unit_scaling);
		// Rays to the floor
		for (int u = 0; u < (int)depth_msg->width; u++) {// Loop over each pixel in row 
			T depth = depth_row[u];
			double r = depth;
			double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out
			int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;
			double R;
			if (depth_to_laser::DepthTraits<T>::valid(depth)){ // Not NaN or Inf
				// Calculate in XYZ
				double x = (u - center_x) * depth * constant_x;
				double y = (v - center_y) * depth * constant_y;
				double z = depth_to_laser::DepthTraits<T>::toMeters(depth);
				// Calculate actual distance
				r = sqrt(pow(x, 2.0) + pow(z, 2.0));
				R = sqrt(pow(y, 2.0) + pow(z, 2.0));
			if(!is_floor(y) && use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){
				scan_msg->ranges[index] = r;
			}
			}

		}
	}
      }

Dans un premier temps, nous devons tout d'abord récupérer de nombreuses variables notamment le centre horizontal et vertical de l'image et différentes constantes permettant d'obtenir les longueurs en mètres. On récupère aussi une matrice contenant les profondeurs de chaque point de l'image obtenue par la caméra du Turtlebot. On va donc alors traiter les différentes profondeurs en parcourant la matrice, pour cela on va donc utiliser deux boucles for.

La première boucle va parcourir verticalement l'image en parcourant un intervalle de longueur égale à scan_height et centré au centre de la caméra en utilisant une variable v. On parcourt du haut vers le bas. Le traitement appliqué sera différent lorsque la variable v sera inférieur à offset + scan_height/2 (les éléments plus hauts que la caméra du Turtlebot) et lorsque la variable v sera supérieur à offset + scan_height/2 (les éléments au sol, donc notamment le sol). Lorsqu'on va traiter le sol, on va aussi récupérer l'angle alpha (qui est à 0 lorsque v est égal à offset + scan_height/2).

Concernant la deuxième boucle for, elle va nous permettre de parcourir horizontalement l'image avec la variable u en parcourant toute la largeur de l'image (intervalle width centré en width/2). On récupère l'angle th qui est nul lorsque l'on a u=width/2. On va alors calculer les différentes variables x,y et z en mètres correspondant à un repère où x est l'axe horizontal, y l'axe vertical et z correspond à la distance en mètres entre la caméra et le plan de l'image considéré (à un depth donné). Ce repère et ces variables correspondent en fait au modèle Pinhole camera. La figure suivante permet de comprendre comment fonctionne le modèle Pinhole camera.

Images/pinholeCamera.png

Les variables x et y sont donc calculés ainsi :

x  =  (u − centerx)*depth*constantx y  =  (v − centery)*depth*constanty

On va alors pouvoir calculer r, la distance réelle du point par rapport à la caméra en utilisant tout simplement Pythagore (r étant l'hypoténuse).

Une fois qu'on a calculé les différentes variables nécessaires, on va alors pouvoir traiter les différents points afin de savoir s’il faut les prendre en compte et les rajouter aux points du laser scan ou s’il faut les ignorer. Si la condition du if est vrai, alors le point est rajouté avec sa longueur r.

bool DepthImageToLaserScan::use_point(const float new_value, const float old_value, const float range_min, const float range_max) const{  
	  // Check for NaNs and Infs, a real number within our limits is more desirable than these.
	     bool new_finite = std::isfinite(new_value);
	     bool old_finite = std::isfinite(old_value);
	         
	     // Infs are preferable over NaNs (more information)
	     if(!new_finite && !old_finite){ // Both are not NaN or Inf.
	     	if(!isnan(new_value)){ // new is not NaN, so use it's +-Inf value.
	     		return true; //Si la valeur est infinie, on la prend
	     	}
	     	return false; // Do not replace old_value, SI la valeur est NaN, on la prend pas
	     }
	     // If not in range, don't bother
bool DepthImageToLaserScan::is_floor(float h_scan) const {

      double hauteur = 0.265;
      double delta = 0.04;
      bool is_floor = fabs(h_scan-hauteur) < delta;
      return is_floor; //Retourne True si c'est le sol

}

Les fonctions de la condition sont présentes dans le fichier DepthImageToLaserScan.cpp, il y a la fonction is_floor et la fonction use_point. La fonction use_point est utilisé dans tous les cas de valeurs de v, elle effectue plusieurs tests entre la nouvelle valeur à tester (r) et l'ancienne (scan_msg->ranges[index]), on va notamment tester si les valeurs sont définies, si elles sont infinies, si elles sont dans l'intervalle où peut observer la caméra et si la nouvelle valeur est plus petite que l'ancienne. On renverra true si :

  • les valeurs sont toutes les deux Infini ou Nan et que la nouvelle valeur n'est pas Nan
  • l'ancienne valeur est infinie ou Nan et la nouvelle est finie
  • la nouvelle valeur est inférieure à l'ancienne

La fonction is_floor permet tout simplement de vérifier si les éléments qu'on observe correspondent au sol ou si ce n'est pas le cas. On y définit une variable hauteur correspondant à la hauteur de la caméra par rapport au sol (obtenue avec un tf_echo entre les deux frames concernés) et une variable delta correspondant à la marge d'erreur acceptée afin de pouvoir considérer si c'est le sol ou non. La condition pour exprimer si l'élément qu'on voit est le sol ou non est juste obtenue en comparant y avec la hauteur du robot (en prenant en compte de l'écart delta).

Une fois qu'on a effectué toutes ces modifications, on peut alors remarquer que notre robot peut observer les éléments présents au sol d'une hauteur minimale d'environ 10 cm et qu'il ne prend pas en compte le sol comme étant un obstacle.

Images/LaserScan.png

Réglage des paramètres du Turtlebot

Il y a plusieurs paramètres qui sont intéressants à régler afin d'avoir un fonctionnement du Turtlebot optimal notamment pour le gmapping, le déplacement autonome du robot ou aussi la génération du laser scan.

Tout d'abord, concernant les valeurs que nous avons choisies pour le depth_to_laser, il faut trouver un compromis entre précision de détection et potentielles erreurs. En effet, du fait de l'instabilité bancale du Turtlebot, il est impossible de définir avec précision une hauteur du Turtlebot et un delta efficace. Un des problèmes persistants est que le robot a tendance à voir certains points au sol durant une très courte durée (lorsqu'il est penché en avant par exemple) et cela a tendance à perturber la navigation intégrer ces obstacles dans la local_costmap. Il faut donc ajuster les paramètres hauteur et delta du fichier DepthImageToLaserScan.cpp afin de pouvoir régler cette précision en fonction du Turtlebot utilisé.

De plus, il y a de nombreux paramètres dans le fichier gmapping.launch.xml qui peuvent être intéressés à modifier afin d'accélérer la cartographie de l'environnement ou au contraire de la ralentir avec pour conséquence de la rendre plus stable. En effet, on se retrouve une fois de plus face à un dilemme : soit on accélère la cartographie afin de permettre à la navigation d'être plus rapide mais on doit faire face à des erreurs plus ou moins graves dans la cartographie, soit on garantit une cartographie précise en rendant la navigation lente. On peut notamment évoquer les paramètres intervenant dans ces choix : map_update_interval; linearUpdate; angularUpdate qui modifient l'actualisation de la carte. Un autre argument pour le gmapping est le paramètres minimumScore qui considère si le résultat du scan correspond à la cartographie déjà établie. Ainsi, en augmentant suffisamment ce paramètre on peut garantir une stabilité du robot dans la carte et ainsi éviter qu'il saute entre deux positions.

Pour garantir une cartographie plus précise, on peut également modifier les paramètres qui se situent dans le dossier param. On peut notamment régler différents paramètres sur les costmap locales ou globales, la planification de trajectoires ou aussi la vitesse du robot. Dans le but de stabiliser la cartographie, nous avons donc décidé de diminuer la vitesse du robot et notamment de diminuer sa vitesse maximale. On a donc diminuer certaines valeurs du fichier dwa_local_planner_params.yaml notamment max_vel_x (de 0.5 à 0.3), max_trans_vel (de 0.5 à 0.3) et acc_lim_x (de 1.0 à 0.7).

Conclusion

L'objectif principal de ce projet était de cartographier un espace clos à partir d'un robot se déplaçant dans un plan 2D, et ce de manière autonome et totalement automatisée. Pour répondre à cette problématique principale, il a fallu dans un premier temps prendre en main le robot ainsi que les différents packages de navigation et de planification de trajectoire que sont les packages gmapping, amcl et move_base.

En prenant soin d'adapter les différents signaux pour ces différents outils, et plus particulièrement en générant un laserscan à partir d'un nuage de points 3D, il a alors été possible de cartographier l'environnement de notre robot, le Turtlebot, ainsi que de le localiser dans cette cartographie.

La récupération et le traitement des données obtenues a permis finalement de réaliser un noeud fonctionnant sur un algorithme de décision pour déterminer les différentes zones à explorer, en prenant en compte de l'accessibilité de chacune d'entre elles.

En choisissant alors les différents paramètres des packages cités précédemment pour un compromis entre efficacité et rapidité de cartographie ainsi que de précision au niveau des obstacles et tolérance au niveau du sol, nous avons finalement pu obtenir une cartographie efficace s'appliquant à un espace clos pour finalement produire en sortie une map pixel de notre environnement.

Annexe

Utilisation des différents noeuds et fichiers launch du projet:

Il est nécessaire d'installer le package cartographie_turtlebot à la fois sur le robot et l'ordinateur. En ce qui concerne le package depth_to_laser, il suffit de l'installer sur le Turtlebot.

Sur le PC, il faut lancer le roscore:

bash:user$ roscore

Sur le Turtlebot, il faut lancer la configuration minimale du bringup, ainsi que le noeud de cartographie. Remarque: Si on souhaite modifier les paramètres précédemment cités correspondant à la vitesse de mapping, il faut modifier les paramètres situés dans le fichier launch : includes/gmapping.launch.xml. En ce qui concerne la hauteur du laserscan, on peut modifier le paramètre scan_height dans le fichier cartographie_turtlebot.launch mais il faut également étalonner les paramètres hauteur et delta situés dans le fichier DepthImageToLaserScan.cpp du package depth_to_laser

bash:user$ roslaunch turtlebot_bringup minimal.launch
bash:user$ roslaunch cartographie_turtlebot cartographie_turtlebot.launch

Sur le PC, maintenant que la cartographie est lancée, il ne reste plus qu'à exécuter le noeud de navigation. On peut également observer le résultat avec Rviz.

bash:user$ rosrun cartographie_turtlebot turtlebot_explorer.py
bash:user$ roslaunch turtlebot_rviz_launchers view_navigation.launch

Les codes utilisés sont sur GitHub aux adresses suivantes :