Mapping et localisation d'un robot dans une interface Web

Cette documentation explique comment créer une carte à partir d'un robot pour l'intégrer ensuite dans une interface Web et afficher la localisation du robot. Ce travail se base sur cet article : Voir l'article

Vidéo de démonstration de la solution : Voir la vidéo YouTube

• http://wiki.ros.org/rosbag : Un ensemble d'outils pour l'enregistrement et la lecture de topics ROS.
• http://wiki.ros.org/gmapping : Création d'une carte 2D à partir des lasers d'un robot.
• http://wiki.ros.org/map_server : Permet de publier et d'enregistrer des cartes.
• http://wiki.ros.org/rosbridge_server : Publie les topics ROS dans un WebSocket au format JSON.
• http://wiki.ros.org/roslibjs : Librairie JS, permet la lecture et l'écriture de topics en interaction avec le WebSocket.
• http://wiki.ros.org/ros2djs : Librairie JS, permet d'afficher les cartes dans une interface web.
• http://wiki.ros.org/amcl : Un système de localisation probabiliste pour un robot se déplaçant en 2D.
• http://wiki.ros.org/nodelet : Fournit un moyen d'exécuter plusieurs algorithmes dans le même processus.
• http://wiki.ros.org/image_proc : Rectification d'image et traitement de couleurs.
• http://wiki.ros.org/depth_image_proc : Traitement des “depth images” (transformation en “point cloud” notamment).
• http://wiki.ros.org/pointcloud_to_laserscan : Transforme un “point cloud” en “laser scan”.
• http://wiki.ros.org/rb1_base_common : Contrôle de la base de RB-1.
• http://wiki.ros.org/pepper_bringup : Connexion à Pepper.

Tous ces packages peuvent être installés en utilisant la commande :

sudo apt-get install ros-<ros_distribution>-<ros_package>

La liste des packages disponibles sur une distribution ROS peut être affichée comme ceci :

apt-cache search ros-<ros_distribution>

Pour Pepper, il faut utiliser ROS Kinetic (installable sur Ubuntu 16.04).
Pour RB-1, il faut utiliser ROS Indigo (installable sur Ubuntu 14.04).

Un “bag” dans ROS permet d'enregistrer et de rejouer à volonté les topics publiés par un robot pendant le temps d'enregistrement (voir les commandes disponibles).
On lance ROS si ce n'est pas déjà fait :

roscore

On se connecte au robot. Pour Pepper :

roslaunch pepper_bringup pepper_full_py.launch nao_ip:=<ip_de_pepper> roscore_ip:=127.0.0.1

Pour RB1 (il faut au préalable être connecté au WiFi du robot) :

export ROS_MASTER_URI=http://rb1:11311

On lance l'enregistrement des topics fournis par le robot :
NB : l'option “-a” permet d'enregistrer tous les topics mais si on souhaite uniquement quelques topics, c'est également possible et cela permet d'alléger le bag.

rosbag record -O </chemin/et/nomDuBag>.bag -a
rosbag record -O </chemin/et/nomDuBag>.bag /<topic_1> /<topic_2>

On fait maintenant se déplacer le robot comme on le souhaite. On arrête ensuite l'enregistrement avec CTRL + C.
On peut inspecter le contenu du rosbag avec la commande :

rosbag info <nom_du_rosbag>.bag

On lance ROS si ce n'est pas déjà fait :

roscore

Si on utilise un rosbag, on indique à ROS d'utiliser le temps de la simulation :
NB : il faut impérativement lancer cette commande avant de lancer tous les nodes.

rosparam set use_sim_time true

On génère ensuite une carte à l'aide de slam_gmapping. Il faut passer en argument le topic du robot correspondant au type sensor_msgs/LaserScan (i.e. les lasers du robot). Par défaut, les cartes générées sont trop “dé-zoomées”, il faut donc jouer sur la résolution (paramètre delta) et éventuellement sur les dimensions.
Par exemple, pour RB1, la commande suivante génère une carte exploitable :

rosrun gmapping slam_gmapping scan:=/front_laser/scan _xmin:=-1 _xmax:=1 _ymin:=-1 _ymax:=1 _delta:=0.035 _maxUrange:=10

On rejoue le bag :

rosbag play --clock <nom_du_bag>.bag

Avant d'annuler le gmapping, on enregistre la carte :

rosrun map_server map_saver -f <map_name>

Exemple de carte obtenue avec les lasers de RB-1 :

Les traits que l'on voit sont des aberrations dues aux lasers, on peut réduire leur impact en jouant sur les paramètres lors de la génération de la carte (paramètre Urange).

Les lasers de Pepper n'étant pas assez puissants, on ne peut pas utiliser son topic sensor_msgs/LaserScan. Il faut donc générer la carte à partir de la caméra 3D, ce qui demande plusieurs transformations des données récupérées. On lance un nodelet manager :

rosrun nodelet nodelet manager __name:=nodelet_manager

On lance notre nodelet depth_image_proc/convert_metric pour convertir au bon format :
Erreur bloquante ici, à revoir.

rosrun nodelet nodelet load depth_image_proc/convert_metric nodelet_manager image_raw:=/pepper_robot/camera/depth/camera/image_raw

On lance image_proc qui retire la distorsion des images :

rosrun image_proc image_proc image_raw:=image camera_info:=/pepper_robot/camera/depth/camera/camera_info

On lance notre nodelet depth_image_proc/point_cloud_xyz pour faire un nuage de points à partir de l'image raw de la caméra :

rosrun nodelet nodelet load depth_image_proc/point_cloud_xyz nodelet_manager image_rect:=/image_rect camera_info:=/camera/depth/camera/camera_info

On lance le package pointcloud_to_laserscan qui génère un sensor_msgs/LaserScan à partir du nuage de points :

rosrun pointcloud_to_laserscan pointcloud_to_laserscan_node cloud_in:=/points

Puis on génère la carte comme vu dans la partie 3.

Le principe est le suivant : rosbridge_server publie un WebSocket contenant les informations des topics au format JSON. roslibjs vient ensuite souscrire aux topics et permet la récupération des données dans une page Web (voir le tutoriel roslibjs). Enfin, ros2djs permet l’affichage d’un élément de type nav_msgs/OccupancyGrid (le format ROS des cartes) dans l’interface (voir le tutoriel ros2djs).

Pour cela, on publie un topic de map à partir du fichier de map enregistré.
NB : il faut bien utiliser le fichier .yaml et non le fichier .pgm de la carte.

rosrun map_server map_server <yaml_file>.yaml

On lance le WebSocket :

roslaunch rosbridge_server rosbridge_websocket.launch

Voici le code HTML de la page que nous avons utilisé. Attention à bien avoir les fichiers des librairies css et javascript positionnées au bon endroit.

<!doctype html>
<html lang="fr">
	<head>
		<!-- Required meta tags -->
		<meta charset="utf-8">
		<meta name="viewport" content="width=device-width, initial-scale=1, shrink-to-fit=no">
		<link rel="apple-touch-icon" sizes="180x180" href="apple-touch-icon.png">
		<link rel="icon" type="image/png" sizes="32x32" href="favicon-32x32.png">
		<link rel="icon" type="image/png" sizes="16x16" href="favicon-16x16.png">
		<link rel="manifest" href="manifest.json">
		<link rel="mask-icon" href="safari-pinned-tab.svg" color="#5bbad5">
		<meta name="theme-color" content="#ffffff">
		<link rel="icon" href="favicon.ico">
 
		<!-- jQuery first, then Popper.js, then Bootstrap JS -->
		<script src="js/jquery-3.3.1.min.js"></script>
		<script src="js/popper.min.js"></script>
		<script src="js/bootstrap.min.js"></script>
		<script src="https://static.robotwebtools.org/EaselJS/current/easeljs.js"></script>
		<script src="https://static.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>
		<script src="https://static.robotwebtools.org/roslibjs/current/roslib.js"></script>
		<script src="js/ros2d.min.js"></script>
 
		<!-- Bootstrap CSS -->
		<link rel="stylesheet" href="css/bootstrap.min.css">
		<link rel="stylesheet" href="css/style.css">
 
		<title>Interface de navigation</title>
	</head>
 
	<body>
		<nav class="navbar navbar-expand-md bg-dark navbar-dark">
			<div class="container">
 
				<!-- Brand -->
				<a class="navbar-brand" href="#">
					<img src="android-chrome-192x192.png" alt="Logo" style="width:25px;">&nbsp;&nbsp;Interface de navigation
				</a>
			</div> 
		</nav>
 
		<div class="container">
			<div class="row mt-3">
				<div class="col">
					<div id="map"></div>
				</div>
				<div class="col">
					<h2>Contrôle du robot</h2>
					<p>Pour contrôler le robot, cliquez simplement sur la map.</p>
					<p>Le point bleu représente la localisation actuelle du robot. Le point rouge représente la destination choisie.</p>
				</div>
			</div>
		</div>
 
		<footer class="footer mt-4">
			<div class="container">
				<span class="text-muted">© IMT Atlantique</span>
			</div>
		</footer>
 
		<script>
 
			$(document).ready(function() {
 
				// Variables sur la map
				var posImageX = 0;
				var posImageY = 0;
				var widthMap = 0;
				var heightMap = 0;
				var resolutionMap = 0;
				var svg;
 
				// Position d'origine du robot
				var posOriginX = 0;
				var posOriginY = 0;
 
				// Position du clic
				var posClicX, posClicY;
 
				// Connexion à ROS
				var ros = new ROSLIB.Ros({
					url : 'ws://localhost:9090'
				});
 
				ros.on('connection', function() {
					console.log("Connecté au Web Socket");
				});
 
				// On souscrit au topic /map_metadata
				// ----------------------------------
				var listenerMetaData = new ROSLIB.Topic({
					ros : ros,
					name : '/map_metadata',
					messageType : 'nav_msgs/MapMetaData'
				});
 
				listenerMetaData.subscribe(function(message) {
 
					console.log("Abonnement au topic /map_metadata");
 
					// On récupère les métadonnées de la map
					posOriginX = message.origin.position.x;
					posOriginY = message.origin.position.y;
					widthMap = message.width;
					heightMap = message.height;
					resolutionMap = message.resolution;
					listenerMetaData.unsubscribe();
 
					console.log('OriginX = ' + posOriginX + ', OriginY = ' + posOriginY);
 
					// On créé le viewer
					var viewer = new ROS2D.Viewer({
						divID : 'map',
						width : widthMap,
						height : heightMap
					});
 
					// Préparation de la map
					var gridClient = new ROS2D.OccupancyGridClient({
						ros : ros,
						rootObject : viewer.scene
					});
 
					// Scale the canvas to fit to the map
					gridClient.on('change', function() {
						viewer.scaleToDimensions(gridClient.currentGrid.width, gridClient.currentGrid.height);
						viewer.shift(gridClient.currentGrid.pose.position.x, gridClient.currentGrid.pose.position.y);
					});
 
					var image = $('#map canvas');
					posImageX = image.offset().left;
					posImageY = image.offset().top;
 
					// On créé le svg
					svg = document.createElementNS("http://www.w3.org/2000/svg", "svg");
					svg.setAttribute('id', 'svg-canvas');
					svg.setAttribute('style', 'position: absolute; top: ' + posImageY + 'px; left: ' + posImageX + 'px;');
					svg.setAttribute('width', widthMap);
					svg.setAttribute('height', heightMap);
					document.body.appendChild(svg);
 
					$('#svg-canvas').on("click", function(e){
						posClicX = e.pageX - posImageX;
						posClicY = e.pageY - posImageY;
 
						console.log('Clic X = ' + posClicX + ' / ' + widthMap + ', clic Y = ' + posClicY + ' / ' + heightMap);
 
						$('circle.destination').remove();
						var shape = document.createElementNS("http://www.w3.org/2000/svg", "circle");
						shape.setAttributeNS(null, "cx", posClicX);
						shape.setAttributeNS(null, "cy", posClicY);
						shape.setAttributeNS(null, "r",	3);
						shape.setAttributeNS(null, "fill", 'red');
						shape.setAttributeNS(null, "class", 'destination');
						svg.appendChild(shape);
					});
 
					// On souscrit au topic /amcl_pose
					// ----------------------------------
					var listener = new ROSLIB.Topic({
						ros : ros,
						name : '/amcl_pose',
						messageType : 'geometry_msgs/PoseWithCovarianceStamped'
					});
 
					console.log("Abonnement au topic /amcl_pose");
 
					listener.subscribe(function(message) {
 
						var posX = (0 + (message.pose.pose.position.x - posOriginX)/resolutionMap);
						var posY = (heightMap + -1*(message.pose.pose.position.y - posOriginY)/resolutionMap);
 
						console.log('X = ' + posX + ', Y = ' + posY);
						$('circle.source').remove();
						var shape = document.createElementNS("http://www.w3.org/2000/svg", "circle");
						shape.setAttributeNS(null, "cx", posX);
						shape.setAttributeNS(null, "cy", posY);
						shape.setAttributeNS(null, "r",	3);
						shape.setAttributeNS(null, "fill", 'blue');
						shape.setAttributeNS(null, "class", 'source');
						svg.appendChild(shape);
						//listener.unsubscribe();
 
					});
				});
 
			});
 
		</script>
	</body>
</html>

Pour la localisation du robot, on utilise le package amcl.
Si ce n'est pas déjà fait, on publie la carte :

rosrun map_server map_server <yaml_file>.yaml

On lance la lecture du bag :

rosbag play --clock <nom_du_bag>.bag

On lance la détection AMCL à partir des lasers (pour RB1, /front_laser/scan)

rosrun amcl amcl scan:=<nom_du_topic_laser_scan>

La page Web s'abonne au topic de positionnement et affiche dynamiquement la position du robot sur la carte.

• Troubleshooting de l'erreur avec le package depth_image_proc/convert_metric pour pouvoir générer une carte à partir de la camera de Pepper
• Normalement, la suite devrait fonctionner comme avec RB-1

• Publier les coordonnées de destination depuis l'interface web vers ROS en utilisant roslibjs
• Utiliser ces coordonnées pour déplacer RB-1 (probablement grâce au package rb1_base_common / navigation ou control)