====== 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 : [[https://arxiv.org/pdf/1704.04797.pdf|Voir l'article]]

Vidéo de démonstration de la solution : [[https://youtu.be/xF7-tTI0zrE|Voir la vidéo YouTube]]

===== Liste des packages utilisés =====

  * 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).

===== Création d'un rosbag =====

Un "bag" dans ROS permet d'enregistrer et de rejouer à volonté les topics publiés par un robot pendant le temps d'enregistrement (voir [[http://wiki.ros.org/rosbag/Commandline|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

===== Création d'une carte =====

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 [[http://wiki.ros.org/gmapping#slam_gmapping|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 :\\
{{:screenshot_19.png?200|}}\\
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//).

==== Spécificités de Pepper ====

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.

===== Intégration de la carte dans une interface Web =====

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 [[http://wiki.ros.org/roslibjs/Tutorials/BasicRosFunctionality|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 [[http://wiki.ros.org/ros2djs/Tutorials/VisualizingAMap|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.
<hidden>
<code html>

<!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>
</code>
</hidden>

===== Affichage de la localisation du robot =====

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.

===== Travaux futurs =====
==== Pour Pepper ====

  * 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

==== Pour le déplacement de RB-1 à la localisation spécifiée ====

  * 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//)
{{tag> pepper ros }}