SsUtils.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------




/* @{ */

#ifndef __SS_UTILS_H_
#define __SS_UTILS_H_

#ifdef MIP_HOST_APPLE
#include <applePatch.h>
#endif

#include <stdio.h>
#include <math.h>
#include <vector>
#include <iostream>
#include <string>
#include <sstream>
#include <sys/time.h>
#include <sys/select.h>
#include <assert.h>
#include <pthread.h>

#include <MIPMatrix.h>
// #include "stdRobot.h"
#include <SE2.h>
#include <Scan.h>
#include <Time.h>


using namespace std;

namespace MipBaselib {
class SsUtils{
 private:
 public:
  static bool encrypt(int &num, stringstream &ss){
   ss << num;
   return true;
  }
  
  static bool decrypt(int &num, stringstream &ss){
   if (ss.fail()) {
    cout << "WARNING: bad double stringstream." << endl;
    return false;
   }
   ss >> num;
   return true;
  }
  
  static bool encrypt(UCoordmm &num, stringstream &ss){
   ss << num;
   return true;
  }
  
  static bool decrypt(UCoordmm &num, stringstream &ss){
   if (ss.fail()) {
    cout << "WARNING: bad double stringstream." << endl;
    return false;
   }
   ss >> num;
   return true;
  }
  
  static bool encrypt(Decimal &num, stringstream &ss){
   ss << num;
   return true;
  }
  
  static bool decrypt(Decimal &num, stringstream &ss){
   if (ss.fail()) {
    cout << "WARNING: bad double stringstream." << endl;
    return false;
   }
   ss >> num;
   return true;
  }
  
  static bool encrypt(Decimal &num1, Decimal &num2, stringstream &ss){
   ss << num1 << " " << num2;
   return true;
  }
  
  static bool decrypt(Decimal &num1, Decimal &num2, stringstream &ss){
   if (ss.fail()) {
    cout << "WARNING: bad double stringstream, no first double." << endl;
    return false;
   }
   ss >> num1;
   if (ss.fail()) {
    cout << "WARNING: bad double stringstream, no second double." << endl;
    return false;
   }
   ss >> num2;
   return true;
  }
  
  static bool encrypt(Position &pos, stringstream &ss){
   ss << pos.x() << " " << pos.y();
   return true;
  }
  
  static bool decrypt(Position &pos, stringstream &ss){
   if (ss.fail()) {
    cout << "WARNING: bad double stringstream, no first double." << endl;
    return false;
   }
   Decimal x, y;
   ss >> x;
   if (ss.fail()) {
    cout << "WARNING: bad double stringstream, no second double." << endl;
    return false;
   }
   ss >> y;
   pos = Position(x, y);
   return true;
  }
  
  static bool encrypt(Pose &pose, stringstream &ss){
   ss << pose.pos().x() << " " << pose.pos().y() << " " << pose.ori().dCast2Pi();
   return true;
  }
  
  static bool decrypt(Pose &pose, stringstream &ss){
   if (ss.fail()) {
    cout << "WARNING: bad Pose stringstream, no x." << endl;
    return false;
   }
   double x, y, th;
   ss >> x;
   if (ss.fail()) {
    cout << "WARNING: bad Pose stringstream, no y." << endl;
    return false;
   }
   ss >> y;
   if (ss.fail()) {
    cout << "WARNING: bad Pose stringstream, no theta." << endl;
    return false;
   }
   ss >> th;
   pose = Pose(Position(x, y), th);
   return true;
  }
  
  static bool encrypt(PosiFeatures &feat, stringstream &ss){
   int size = feat.size();
   ss << size;
   for (int i = 0; i < size; i++){
    ss << " " << feat[i].getPosition().x() << " " << feat[i].getPosition().y() << " ";
    if (feat[i].associated()){
     ss << feat[i].getId();
    }
    else{
     ss << "na";
    }
   }
   return true;
  }
  
  static bool decrypt(PosiFeatures &feat, stringstream &ss){
   if (ss.fail()) {
    cout << "WARNING: bad PosiFeatures stringstream, no size." << endl;
    return false;
   }
   int size;
   ss >> size;
   for (int i = 0; i < size; i++){
    Decimal x, y;
    // Variable ID not used
//     int id;
    if (ss.fail()) {
     cout << "WARNING: received broken setPosiFeatures message" << endl;
     return false;
    }
    ss >> x;
    if (ss.fail()) {
     cout << "WARNING: received broken setPosiFeatures message" << endl;
     return false;
    }
    ss >> y;
    if (ss.fail()) {
     cout << "WARNING: received broken setPosiFeatures message" << endl;
     return false;
    }
    string pid;
    ss >> pid;
    if (pid == "na"){
     feat.push_back(PosiFeature(Position(x, y)));
    }
    else{
     int id = atoi(pid.c_str());
     feat.push_back(PosiFeature(Position(x, y), id));
    }
   }
   return true;
  }
  
  static bool encrypt(Time &time, stringstream &ss){
   ss << time.sec() << " " << time.usec();
   return true;
  }
  
  static bool decrypt(Time &time, stringstream &ss){
   if (ss.fail()) {
    cout << "WARNING: bad time stringstream, no secs." << endl;
    return false;
   }
   long int sec, usec;
   ss >> sec;
   if (ss.fail()) {
    cout << "WARNING: bad time stringstream, no usecs." << endl;
    return false;
   }
   ss >> usec;
   time = Time(sec, usec);
   return true;
  }
  
  static bool encrypt(Ray &ray, stringstream &ss){
   ss << ray.reading() << " " << ray.bearing().dCast2Pi() << " " << ray.valid();
   return true;
  }
  
  static bool decrypt(Ray &ray, stringstream &ss){
   bool valid;
//    Decimal bearing, reading;
   int bearing, reading;
   if (ss.fail()) {
    cout << "WARNING: bad ray stringstream, no reading." << endl;
    return false;
   }
   ss >> reading;
   if (ss.fail()) {
    cout << "WARNING: bad ray stringstream, no bearing." << endl;
    return false;
   }
   ss >> bearing;
   if (ss.fail()) {
    cout << "WARNING: bad ray stringstream, no valid." << endl;
    return false;
   }
   ss >> valid;
   ray = Ray(reading/1000.0, Angle(bearing/1000.0), valid);
   return true;
  }
  
  static bool decrypt(Ray &ray, stringstream &ss, int rayNum){
   bool valid;
   
   Decimal bearing = rayNum * ( 0.006135923*2 )  + ( 384/2 ) * (  0.006135923*2  ) + 1.832595715;
   int reading;
   if (ss.fail()) {
    cout << "WARNING: bad ray stringstream, no reading." << endl;
    return false;
   }
   ss >> reading;
   if (ss.fail()) {
    cout << "WARNING: bad ray stringstream, no bearing." << endl;
    return false;
   }
   ss >> valid;
   ray = Ray(reading/1000.0, Angle(bearing), valid);
   return true;
  }
  
  static bool decrypt(Ray &ray, stringstream &ss, int rayNum, Decimal scale, bool getBearing);
  
  static bool encrypt(Scan &scan, stringstream &ss){
   int size;
   scan.getSize(size);
   ss << size << " ";
   Ray temp;
   for (int i = 0; i < size; i++){
    scan.getRay(temp, i);
    encrypt(temp, ss);
    ss << " ";
   }
   return true;
  }
  
  static bool decrypt(Scan &scan, stringstream &ss);
};

}; // end of namespace

#endif



/* @} */