Smag0: smag-sphero

Affichage des articles dont le libellé est smag-sphero. Afficher tous les articles

11/11/2015

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

04/12/2014

SpheroCarto - Utiliser Sphero pour cartographier un environnement, et le visualiser avec Processing (APDE)

Une fois n'est pas coutume, prenons le temps de faire le point sur une partie essentielle du projet.

Dans le système SMAG comme je le prévoit, les différents modules robotiques doivent être amenés à échanger les informations qu'ils possèdent, et l'une des informations primordiales pour un objet mobile, est d'abord de savoir dans quel environnement il évolue. L'hypothèse de départ étant un "robot qui range ma chambre" on devra avoir un ou plusieurs robot qui permette aux autres de percevoir l'environnement. N'importe quel appareil mobile, connecté, capable de déterminer sa position par rapport à son point de départ pourra faire l'affaire.

J'avais dans l'idée depuis un bout de temps, d'utiliser SPHERO , une balle programmable, pour tenter de créer une "cartographie" de l'environnement maison. J'en ai trouvé une d'occasion (merci au parquet trop glissant de Matthias !).

Pour rappel, dans l'idée, le système SMAG est un système multi-agent composé de multiples agents indépendants, qui remplissent un rôle. Dans ce qui nous intéresse ici, nous pouvons considérer que la cartographie de l'environnement se décompose en trois 3 composants majeurs.

On aura :
- le robot mobile (ici SPHERO)
- une interface pour commander le robot
- une interface pour visualiser les informations

Dans notre exemple, l'interface pour commander le SPHERO et celle pour visualiser les infos seront toutes les deux dans notre téléphone mais indépendantes (pour l'instant).

Pour l'interface de commande, j'ai choisi Eclipse qui a fait ses preuves en développement Android, et pour l'interface de visualisation, il m'est apparu plus easy d'utiliser Processing, qui fait de jolies interfaces très visuelles, réactives, et souples... ou plutôt APDE sa version mobile.
J'ai utilisé les versions suivantes :
- Eclipse Luna avec le plugin ADT pour (Android Developpement Tool)
- APDE (ou Processing)
- le Sphero Android SDK (avec la version actuelle : Notice: The Sphero Android SDK works with Android 2.2+ and Java Compiler Level 6.0(1.6)+ )
- et ne pas oublier, qu'il te faut évidemment un SPHERO... (Y'en a peut-être un pas trop loin de chez toi qui t'attend sur le bon coin ???)

Hop, une fois que tu as ton SPHERO en main, les choses intéressantes commencent :

MISE EN PLACE DE L'ENVIRONNEMENT

1. Télécharge le Sphero Android SDK et décompresse -le.
2. Lance Eclipse Luna, si tu ne l'a pas encore, télécharge le, et pendant ce temps, installe l'appli APDE depuis GooglePlay.
3. Une fois Eclipse lancé, on va commencer par se la jouer feignasse... on n'est pas là pour se prendre la tête. Le composant essentiel pour établir la cartographie de l'environnement est sans aucun doute la position de notre robot SPHERO. On va donc s'appuyer sur un des exemples du Sphero Android SDK ... le LOCATOR.
Donc la feignasse, elle ouvre son Eclipse Luna, et elle crée un nouveau projet avec New / Project. Ensuite dans l'assistant (wizzard) choir un projet du type Android (si Android n'apparait pas en dessous de Général, c'est certainement que le plugin ADT n'est pas installé (voir dans Help / Eclipse MarketPlace, et prendre ADT by Google Inc))
Sous Android choisir "Android Project from existing Code" , next et là parcourir jusqu'au Sphero Android SDK téléchargé et décompressé précédemment . et allez jusqu'au dossier Locator. moi dans Browse, j'ai : C:\Users\David\Downloads\Sphero-Android-SDK-master\Sphero-Android-SDK-master\samples\Locator

Laissez faire la Magik Mystique, et Hop !
Nous volià avec une première appli Locator que l'on peut lancer avec un coup de Run dans Eclipse... Là , si vous n'avez jamais utilisé le plugin ADT, il faut configurer l'émulateur (on trouve de la doc là dessus assez facilement).

Moi je veux tester en direct sur mon téléphone, alors je lance le Run mais l'émulateur plante alors je l'arrête , mais je m'en f... parce que c'est pas ça qui m'intéresse... ce qui m'interesse, c'est le fichier APK qui a été créé par le lancement de la commande RUN. il se trouve dans le répertoire bin du répertoire Locator de votre workspace Eclipse (celui que l'on vous demande de confirmer à chaque démarrage)

Ce fichier APK, je le copie sur la carte sd de mon téléphone(via le câble USB). Ensuite, avec un explorateur (comme ES Explorer ) je peux parcourir ma carte et installer l'application que je viens de créer.

D'autres exemples nous seront utiles. On va donc refaire la manip avec le chemin qui s'arrête à la liste des exemple (samples) : C:\Users\David\Downloads\Sphero-Android-SDK-master\Sphero-Android-SDK-master\samples

Là on va cocher : Collision + UISample + Streming Example ( + Locator mais il doit déjà être grisé car déjà installé) + cochez l'option "Copy projects into workspace"

En fait je suis plus feignasse que feignasse, on va utiliser UISample qui est un exemple déjà tout prêt d'interface. Tu peux la tester en faisant comme tout à l'heure un RUN (as android Appli. !) En copiant le fichier présent dans <<ton workspace>>/UISample/bin/ c'est le fichier UiSample.apk

Hop, comme ça, on pourra piquer du code dans ces exemples.

Maintenant au boulot !

... non en fait, trop long de recopier tout le code, si tu veux le récupérer, il est là : https://github.com/scenaristeur/sphero-carto . Il n'est pas encore nickel, il faut que je revoie la correction de direction lors des collision, la mise en veille du tél...
A ta dispo pour corriger, ajouter de nouvelles fonctionnalités... Si tu as des idées, on en discute |-)

+ découvert récemment : un mec qui utilise App Inventor pour contrôler son Sphero. J'ai testé, il reste quelques fignolages, encore, je crois. Je vous ferais signe. Abonnez-vous pour être informé des nouveautés .

Smag0

Pages