Sphero-Carto

Le but de cet article est de proposer une utilisation du robot Sphero d'Orbotix pour établir la cartographie d'un environnement.
le code est disponible là : https://drive.google.com/file/d/0B0zEK4yLB5C6TUhLc0FRbVVKMVU/view?usp=sharing

En effet dans le projet Smag0, et d'après les trucs que j'ai pu lire un peu partout (SMA Ferber...), un robot/agent autonome se doit d'avoir une représentation de son environnement. J'avais donc envisagé il y a longtemps, d'utiliser le robot Sphero afin d'établir une cartographie de l'environnement en le faisant se balader dans l'environnement, en récupérant ses coordonnées, et en notant les points de collision. Cela pourrait permettre, par exemple à déterminer si une pièce est rangée et s'il faut envoyer d'autres robots autonomes pour la ranger.

Exemple d'application au projet Smag0 : Sphero établit une cartographie un jour où la chambre est rangée et la stocke comme étant la cartographie de référence avec points de collision pour les pieds de meubles, et autres objets. Lorsque les jours suivants, Sphero repasse, on compare les deux cartographies, si les points de collision se sont déplacés : les objets ont bougé, et il faut envisager de les ranger, ou déterminer si leur nouvelle position est respectable.

Après cette brève introduction, passons au fourneau :


LA RECETTE DU SPHERO-CARTO.
Ingrédients :
- 1 script sphero.js permettant d'accéder au fonctionnalités du robot Sphéro : https://github.com/orbotix/sphero.js  (les fonctionnalités sont là : https://github.com/orbotix/sphero.js/blob/master/lib/devices/sphero.js )
- 1 serveur node.js
- quelques workers JS...


"C'est dans les vieux pots qu'on fait la meilleure soupe"
Pensons maintenant notre appli comme un système multi-agents, comme des programmes autonomes qui communiquent entre eux, nous aurons par exemple :
- un module (ou worker en javascript) qui s'occupera du serveur de fichier,
- un autre de la gestion des commandes du robot sphero,
- un autre de l'interface visuelle,
- un autre s'occupera de la conception de la carte,
- on pourra en trouver un qui se chargera du stockage de l'information,
- un déterminera l'algorithme à utiliser pour une meilleure exploration de l'environnement...
- ... (on pourra en ajouter d'autre selon le besoin et les fonctionnalités envisagées)


Le serveur de fichier : 
Commençons par installer nodejs si ce n'est pas encore fait, puis installez le module sphero : "$ npm install sphero" comme préconisé ici https://github.com/orbotix/sphero.js

le module socket.io et le module keypress "npm install socket.io",  `npm install keypress`

Ensuite, je me suis basé sur le tutoriel du nouveau site P5.js pour l'interaction entre le serveur node.js et le module d'affichage p5.js : http://p5js.org/tutorials/ ou https://github.com/processing/p5.js/wiki/p5.js,-node.js,-socket.io

Choix a faire : Socket ? worker ?

code  de base du serveur serveur.js , à lancer avec la commande : node serveur.js

// http://smag0.blogspot.fr/2015/11/sphero-carto.html

// HTTP Portion

var http = require('http');

// URL module

var url = require('url');

var path = require('path');







// Using the filesystem module

var fs = require('fs');

// make sure you install this first - `npm install keypress`

var keypress = require("keypress");



var server = http.createServer(handleRequest);

server.listen(8080);

console.log('Server started on port 8080');



//SPHERO

var sphero = require("sphero");

var orb = sphero("COM3");

var stop = orb.roll.bind(orb, 0, 0),

  roll = orb.roll.bind(orb, 150);

var detectionCollisionInterval; //interval pour la detection de collision

var collisionBool=0;

var xSphero,ySphero=0;

var direction=0;





function handleRequest(req, res) {

  // What did we request?

  var pathname = req.url;



  // If blank let's ask for index.html

  if (pathname == '/') {

    pathname = '/index.html';

  }



  // Ok what's our file extension

  var ext = path.extname(pathname);



  // Map extension to file type

  var typeExt = {

    '.html': 'text/html',

    '.js': 'text/javascript',

    '.css': 'text/css'

  };



  // What is it?  Default to plain text

  var contentType = typeExt[ext] || 'text/plain';



  // User file system module

  fs.readFile(__dirname + pathname,

    // Callback function for reading

    function(err, data) {

      // if there is an error

      if (err) {

        res.writeHead(500);

        return res.end('Error loading ' + pathname);

      }

      // Otherwise, send the data, the contents of the file

      res.writeHead(200, {

        'Content-Type': contentType

      });

      res.end(data);

    }

  );

}





// WebSocket Portion

// WebSockets work with the HTTP server

var io = require('socket.io').listen(server);

var socketA;



// Register a callback function to run when we have an individual connection

// This is run for each individual user that connects

io.sockets.on('connection',

  // We are given a websocket object in our function

  function(socket) {



    console.log("We have a new client: " + socket.id); 

 socketA=socket;

    // When this user emits, client side: socket.emit('otherevent',some data);

    socket.on('mouse',

      function(data) {

        // Data comes in as whatever was sent, including objects

        console.log("Received: 'mouse' " + data.x + " " + data.y);



        // Send it to all other clients

        socket.broadcast.emit('mouse', data);



        // This is a way to send to everyone including sender

        // io.sockets.emit('message', "this goes to everyone");



      }

    );

    socket.on('color',

      function(data) {

        // Data comes in as whatever was sent, including objects

        console.log("Received: 'color' " + data.r + " " + data.g + " " + data.b);



        orb.color({

          red: data.r,

          green: data.g,

          blue: data.b

        });

        // Send it to all other clients

        // socket.broadcast.emit('mouse', data);



        // This is a way to send to everyone including sender

        // io.sockets.emit('message', "this goes to everyone");



      }

    );

    socket.on('start',

      function() {

        // Data comes in as whatever was sent, including objects

        console.log("Received: 'start' ");

        start();

      }

    );



    socket.on('stop',

      function() {

        // Data comes in as whatever was sent, including objects

        console.log("Received: 'stop' ");

        clearInterval(detectionCollisionInterval);

        orb.roll(0, 0);

      }

    );

    socket.on('startCalib',

      function() {

        // Data comes in as whatever was sent, including objects

        console.log("Received: 'startCalib' ");

        console.log("::START CALIBRATION::");

        orb.startCalibration();

      }

    );

    socket.on('stopCalib',

      function() {

        // Data comes in as whatever was sent, including objects

        console.log("Received: 'stopCalib' ");

        initialiseLocator();

        console.log("::FINISH CALIBRATION::");

        orb.finishCalibration();

      }

    );



    socket.on('disconnect', function() {

      console.log("Client has disconnected");

    });

  }

);





////////////////////////////////

// SPHERO

////////////////////////////////



orb.connect(function() {

  listen();

  orb.color("green");

  //option configurelocator

  initialiseLocator();







  orb.getBluetoothInfo(function(err, data) {

    console.log("bluetooth info fetched");

    if (err) {

      console.error("err:", err);

    }

    console.log("data:", data);

  });

   orb.detectCollisions();



  // orb.streamVelocity();

  // orb.streamGyroscope();







  var opts = {

    n: 200,

    m: 1,

    mask1: 0x00000000,

    pcnt: 0,

    mask2: 0x0D800000

  };



  orb.setDataStreaming(opts);



  orb.on("velocity", function(data) {

    console.log("::STREAMING VELOCITY::");

    console.log("  data:", data);

  });



  orb.on("dataStreaming", function(data) {

    //console.log("streaming data packet recieved");

 //  console.log("  data:", data);

 xSphero=data.xOdometer.value;

 ySphero=data.yOdometer.value*-1;

 collisionBool=0;

    console.log(data.xOdometer.value+" "+data.yOdometer.value);

  var dataToSend = {

    x: xSphero,

    y: ySphero,

 z: collisionBool // 0 car pas de collision

  };

 socketEmit("position", dataToSend);

  });



  orb.on("gyroscope", function(data) {

    console.log("::STREAMING GYROSCOPE::");

    console.log("  data:", data);

  });



  orb.on("collision", function(data) {

    console.log("collision detected");

   // console.log("  data:", data);



    orb.color("red");

    collisionBool=1;

   var dataToSend = {

    x: xSphero,

    y: ySphero,

 z: collisionBool // 0 car pas de collision

  };





 socketEmit("position", dataToSend);

    setTimeout(function() {

      orb.color("green");

    collisionBool=0;

    }, 1000);

  });

});





function socketEmit(entete , data){

 if(socketA!=null){

 socketA.emit(entete, data);

 }

};



function start() {/*

orb.roll(200, direction);

//Determinons les premieres limites

//limite haute

while(collisionBool==0){

 setInterval(function() {

 orb.roll(200, direction);

 }, 2000);

}

direction=direction*-1; //demi-tour

//limite basse

while(collisionBool==0){

 setInterval(function() {

 orb.roll(200, direction);

 }, 2000); 

}

*/

if(collisionBool==0){

 detectionCollisionInterval =setInterval(function() {

   //direction = Math.floor(Math.random() * 360);

    orb.roll(150, direction);



  }, 2000);

}else{

console.log("collision ne peut avancer");

direction = Math.floor(Math.random() * 360);

}

 

 

  

  // roll orb in a random direction, changing direction every second

 /* detectionCollisionInterval = setInterval(function() {

    var direction = Math.floor(Math.random() * 360);

    orb.roll(100, direction);

    //  readLocator()

  }, 2000);*/

};



function initialiseLocator() {

  var opts = {

    flags: 0x01,

    x: 0x0000,

    y: 0x0000,

    yawTare: 0x0

  };

  orb.configureLocator(opts, function(err, data) {

    console.log(err || "CONFIGURE LOCATOR data: " + data);

  });

  orb.setHeading(0, function(err, data) {

      console.log(err || "Heading réglé sur 0: " + data);

    });

 direction=0;

};



function readLocator() {

  orb.readLocator(function(err, data) {

    if (err) {

      console.log("error: ", err);

    } else {

      console.log("dataLocator:");

      console.log("  xpos:", data.xpos);

      console.log("  ypos:", data.ypos);

      console.log("  xvel:", data.xvel);

      console.log("  yvel:", data.yvel);

      console.log("  sog:", data.sog);

      console.log(data);

      console.log(data[0]);

      console.log("");

    }

  });

};



function handle(ch, key) {

  var stop = orb.roll.bind(orb, 0, 0),

    roll = orb.roll.bind(orb, 60);



  if (key.ctrl && key.name === "c") {

    process.stdin.pause();

    process.exit();

  }



  if (key.name === "e") {

    orb.startCalibration();

  }



  if (key.name === "q") {

    orb.finishCalibration();

  }



  if (key.name === "up") {

    roll(0);



  }



  if (key.name === "down") {

    roll(180);

  }



  if (key.name === "left") {

    roll(270);

  }



  if (key.name === "right") {

    roll(90);

  }



  if (key.name === "space") {

    stop();

    clearInterval(detectionCollisionInterval);

  }

}



function listen() {

  keypress(process.stdin);

  process.stdin.on("keypress", handle);



  console.log("starting to listen for arrow key presses");



  process.stdin.setRawMode(true);

  process.stdin.resume();

}

fichier sketch.js

//les exemples pour SPHERO : https://github.com/orbotix/sphero.js/blob/master/examples/
//https://github.com/orbotix/sphero.js

/*
 * @name Slider
 * @description You will need to include the 
 * <a href="http://p5js.org/reference/#/libraries/p5.dom">p5.dom library</a>
 * for this example to work in your own project.<br><br>
 * Move the sliders to control the R, G, B values of the background.
 */
var rSlider, gSlider, bSlider,speedSlider;
var myDiv0;
var rougeDiv;
var vertDiv;
var bleuDiv;
// Keep track of our socket connection
var socket;
var xSphero, ySphero;
var positionsSphero = [];
var positionSphero;

function setup() {
  // create canvas
  createCanvas(windowWidth, windowHeight);
  background(0);
  positionSphero = createVector(0, 0, 0);
  // Start a socket connection to the server
  // Some day we would run this server somewhere else
  socket = io.connect('http://localhost:8080');
  // We make a named event called 'mouse' and write an
  // anonymous callback function
  socket.on('mouse',
    // When we receive data
    function(data) {
      console.log("Got: " + data.x + " " + data.y);
      // Draw a blue circle
      fill(0, 0, 255);
      noStroke();
      ellipse(data.x, data.y, 80, 80);
    }
  );

  socket.on('position',
    // When we receive data
    function(data) {
      console.log("Position: " + data.x + " " + data.y + " " + data.z);
      // Draw a blue circle
      // fill(100, 100, 255);
      // noStroke();
      // ellipse(data.x, data.y, 10, 10);
      xSphero = data.x;
      ySphero = data.y;
      collision = data.z;
      positionSphero = createVector(xSphero, ySphero, collision);
      append(positionsSphero, positionSphero);
    }
  );

  textSize(15)
  noStroke();

  // create sliders
  rSlider = createSlider(0, 255, 255);
  rSlider.position(720, 20);
  gSlider = createSlider(0, 255, 230);
  gSlider.position(720, 50);
  bSlider = createSlider(0, 255, 107);
  bSlider.position(720, 80);
  speedSlider= createSlider(0, 255, 66);
  speedSlider.position(720, 110);

  //boutons
  button = createButton('start Calibration');
  button.position(20, 120);
  button.mousePressed(startCalib);
  button = createButton('stop Calibration');
  button.position(150, 120);
  button.mousePressed(stopCalib);
  button = createButton('start');
  button.position(20, 150);
  button.mousePressed(start);
  button = createButton('stop');
  button.position(100, 150);
  button.mousePressed(stop);
    button = createButton('clear');
  button.position(180, 150);
  button.mousePressed(clear);

  //affiche
  rougeDiv = document.getElementById("rouge");
  vertDiv = document.getElementById("vert");
  bleuDiv = document.getElementById("bleu");
  xDiv = document.getElementById("positionX");
  yDiv = document.getElementById("positionY");
}

function draw() {
  var r = rSlider.value();
  var g = gSlider.value();
  var b = bSlider.value();
  var speed=speedSlider.value();
  background(r, g, b);
  text("red", 800, 20);
  text("green", 800, 50);
  text("blue", 800, 80);
   text("speed", 800, 110);
   
  // console.log(r, g, b);
  if ((r != rougeDiv.innerHTML) || (g != vertDiv.innerHTML) || (b != bleuDiv.innerHTML)) {
    rougeDiv.innerHTML = r;
    vertDiv.innerHTML = g;
    bleuDiv.innerHTML = b;
    sendRGB(r, g, b);
  }
  xDiv.innerHTML = xSphero;
  yDiv.innerHTML = ySphero;
  translate(width / 2, height / 2);
  fill(0);
  ellipse(xSphero, ySphero, 10, 10);
  for (i in positionsSphero) {
    var position = positionsSphero[i];
    if (position.z == 0) {
      fill(0, 255, 0);
    } else {
      fill(255, 0, 0);
    }
    ellipse(position.x, position.y, 3, 3);
  }
}


function start() {
  console.log("start p5");
  socket.emit('start', null);
}

function stop() {
  console.log("stop p5");
  socket.emit('stop', null);
}

function clear() {
  console.log("clear");
  positionsSphero = [];
  positionSphero = createVector(0, 0, 0);
 // socket.emit('clear', null);
}

function startCalib() {
  console.log("start calib");
  socket.emit('startCalib', null);
}

function stopCalib() {
  console.log("stop calib");
  socket.emit('stopCalib', null);
}


/*
function mouseDragged() {
  // Draw some white circles
  fill(255);
  noStroke();
  ellipse(mouseX, mouseY, 80, 80);
  // Send the mouse coordinates
  sendmouse(mouseX, mouseY);
}*/
//ENVOI de la couleur
function sendRGB(r, g, b) {
  // Make a little object with rgb 
  var data = {
    r: r,
    g: g,
    b: b
  };
  // Send that object to the socket
  socket.emit('color', data);
}
// Function for sending to the socket
function sendmouse(xpos, ypos) {
  // We are sending!
  console.log("sendmouse: " + xpos + " " + ypos);

  // Make a little object with  and y
  var data = {
    x: xpos,
    y: ypos
  };

  // Send that object to the socket
  socket.emit('mouse', data);
}

fichier index.html

 <!DOCTYPE html>
<html>
  <head>
    <meta charset="UTF-8">
    <title>Sphero</title>
    <script src="libraries/p5.js" type="text/javascript"></script>

    <script src="libraries/p5.dom.js" type="text/javascript"></script>
    <script src="libraries/p5.sound.js" type="text/javascript"></script>

    <style> body {padding: 0; margin: 0;} canvas {vertical-align: top;} </style>

   <!-- <script language="javascript" type="text/javascript" src="//cdnjs.cloudflare.com/ajax/libs/p5.js/0.2.9/p5.min.js"></script>-->
    <script language="javascript" type="text/javascript" src="sketch.js"></script>
    <script language="javascript" type="text/javascript" src="/socket.io/socket.io.js"></script>
  </head>
  <body>
  <!-- <div id="connected" style="width:100px;height:100px">not connected</div> -->
  <div id="rouge">r</div>
  <div id="vert">b</div>
  <div id="bleu">v</div>
  <div id="positionX">X</div>
   <div id="positionY">Y</div>
  </body>
</html>

le code est disponible là : https://drive.google.com/file/d/0B0zEK4yLB5C6TUhLc0FRbVVKMVU/view?usp=sharing


Pour la gestion des webworkers avec nodejs : npm install webworker