sensorstream.cpp

Go to the documentation of this file.

#include <assert.h>
#include <sstream>
#include "sensorstream.h"
//#define LINEBUFFER_SIZE 1000000 //for not Cyrill to unbless me, it is better to exagerate :-))
// Can't declare a buffer that big on the stack.  So we'll risk Cyrill's
// unblessing, and make it smaller.
#define LINEBUFFER_SIZE 8192

namespace GMapping {

using namespace std;

//SensorStream
SensorStream::SensorStream(const SensorMap& sensorMap) :m_sensorMap(sensorMap){}

SensorStream::~SensorStream(){}

SensorReading* SensorStream::parseReading(std::istream& is, const SensorMap& smap){
        SensorReading* reading=0;
        if (is){
                char buf[LINEBUFFER_SIZE];
                is.getline(buf, LINEBUFFER_SIZE);
                istringstream lis(buf);
                
                string sensorname;
                
                if (lis){
                        lis >>sensorname; 
                } else 
                        return 0;
                
                SensorMap::const_iterator it=smap.find(sensorname);
                if (it==smap.end()){
                        return 0;
                }
                
                Sensor* sensor=it->second;
                
                OdometrySensor* odometry=dynamic_cast<OdometrySensor*>(sensor);
                if (odometry)
                        reading=parseOdometry(lis, odometry);
                
                RangeSensor* range=dynamic_cast<RangeSensor*>(sensor);
                if (range)
                        reading=parseRange(lis, range);
        }
        return reading;
}

OdometryReading* SensorStream::parseOdometry(std::istream& is, const OdometrySensor* osen ){
        OdometryReading* reading=new OdometryReading(osen);
        OrientedPoint pose;
        OrientedPoint speed;
        OrientedPoint accel;
        is >> pose.x >> pose.y >> pose.theta;
        is >> speed.x >>speed.theta;
        speed.y=0;
        is >> accel.x;
        accel.y=accel.theta=0;
        reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel);
        double timestamp, reltimestamp;
        string s;
        is >> timestamp >>s >> reltimestamp;
        reading->setTime(timestamp);
        return reading;
}

RangeReading* SensorStream::parseRange(std::istream& is, const RangeSensor* rs){
        //cerr << __PRETTY_FUNCTION__ << endl;
        if(rs->newFormat){
                string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
                is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode;
                //cerr << " New format laser msg" << endl;
        }
        unsigned int size;
        is >> size;
        assert(size==rs->beams().size());
        RangeReading* reading=new RangeReading(rs);
        reading->resize(size);
        for (unsigned int i=0; i<size; i++){
                is >> (*reading)[i];
        }
        if (rs->newFormat){
                int reflectionBeams;
                is >> reflectionBeams;
                double reflection;
                for (int i=0; i<reflectionBeams; i++)
                        is >> reflection;
        }
        OrientedPoint laserPose;
        is >> laserPose.x >> laserPose.y >> laserPose.theta;
        OrientedPoint pose;
        is >> pose.x >> pose.y >> pose.theta;
        reading->setPose(pose);

        if (rs->newFormat){
                string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis;
                is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis;
        } 
//      else {
//        double a,b,c;
//              is >> a >> b >> c;
//      }
        double timestamp, reltimestamp;
        string s;
        is >> timestamp >>s >> reltimestamp;
        reading->setTime(timestamp);
        return reading;

}

//LogSensorStream
LogSensorStream::LogSensorStream(const SensorMap& sensorMap, const SensorLog* log):
        SensorStream(sensorMap){
        m_log=log;
        assert(m_log);
        m_cursor=log->begin();
}

LogSensorStream::operator bool() const{
        return m_cursor==m_log->end();
}

bool LogSensorStream::rewind(){
        m_cursor=m_log->begin();
        return true;
}

SensorStream& LogSensorStream::operator >>(const SensorReading*& rd){
        rd=*m_cursor;
        m_cursor++;
        return *this;
}

//InputSensorStream
InputSensorStream::InputSensorStream(const SensorMap& sensorMap, std::istream& is):
        SensorStream(sensorMap), m_inputStream(is){
}

InputSensorStream::operator bool() const{
        return (bool) m_inputStream;
}

bool InputSensorStream::rewind(){
        //m_inputStream.rewind();
        return false;
}

SensorStream& InputSensorStream::operator >>(const SensorReading*& reading){
        reading=parseReading(m_inputStream, m_sensorMap);
        return *this;
}

};