sensorstream.cpp
Go to the documentation of this file.
00001 #include <assert.h>
00002 #include <sstream>
00003 #include "sensorstream.h"
00004 //#define LINEBUFFER_SIZE 1000000 //for not Cyrill to unbless me, it is better to exagerate :-))
00005 // Can't declare a buffer that big on the stack.  So we'll risk Cyrill's
00006 // unblessing, and make it smaller.
00007 #define LINEBUFFER_SIZE 8192
00008 
00009 namespace GMapping {
00010 
00011 using namespace std;
00012 
00013 //SensorStream
00014 SensorStream::SensorStream(const SensorMap& sensorMap) :m_sensorMap(sensorMap){}
00015 
00016 SensorStream::~SensorStream(){}
00017 
00018 SensorReading* SensorStream::parseReading(std::istream& is, const SensorMap& smap){
00019         SensorReading* reading=0;
00020         if (is){
00021                 char buf[LINEBUFFER_SIZE];
00022                 is.getline(buf, LINEBUFFER_SIZE);
00023                 istringstream lis(buf);
00024                 
00025                 string sensorname;
00026                 
00027                 if (lis){
00028                         lis >>sensorname; 
00029                 } else 
00030                         return 0;
00031                 
00032                 SensorMap::const_iterator it=smap.find(sensorname);
00033                 if (it==smap.end()){
00034                         return 0;
00035                 }
00036                 
00037                 Sensor* sensor=it->second;
00038                 
00039                 OdometrySensor* odometry=dynamic_cast<OdometrySensor*>(sensor);
00040                 if (odometry)
00041                         reading=parseOdometry(lis, odometry);
00042                 
00043                 RangeSensor* range=dynamic_cast<RangeSensor*>(sensor);
00044                 if (range)
00045                         reading=parseRange(lis, range);
00046         }
00047         return reading;
00048 }
00049 
00050 OdometryReading* SensorStream::parseOdometry(std::istream& is, const OdometrySensor* osen ){
00051         OdometryReading* reading=new OdometryReading(osen);
00052         OrientedPoint pose;
00053         OrientedPoint speed;
00054         OrientedPoint accel;
00055         is >> pose.x >> pose.y >> pose.theta;
00056         is >> speed.x >>speed.theta;
00057         speed.y=0;
00058         is >> accel.x;
00059         accel.y=accel.theta=0;
00060         reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel);
00061         double timestamp, reltimestamp;
00062         string s;
00063         is >> timestamp >>s >> reltimestamp;
00064         reading->setTime(timestamp);
00065         return reading;
00066 }
00067 
00068 RangeReading* SensorStream::parseRange(std::istream& is, const RangeSensor* rs){
00069         //cerr << __PRETTY_FUNCTION__ << endl;
00070         if(rs->newFormat){
00071                 string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
00072                 is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode;
00073                 //cerr << " New format laser msg" << endl;
00074         }
00075         unsigned int size;
00076         is >> size;
00077         assert(size==rs->beams().size());
00078         RangeReading* reading=new RangeReading(rs);
00079         reading->resize(size);
00080         for (unsigned int i=0; i<size; i++){
00081                 is >> (*reading)[i];
00082         }
00083         if (rs->newFormat){
00084                 int reflectionBeams;
00085                 is >> reflectionBeams;
00086                 double reflection;
00087                 for (int i=0; i<reflectionBeams; i++)
00088                         is >> reflection;
00089         }
00090         OrientedPoint laserPose;
00091         is >> laserPose.x >> laserPose.y >> laserPose.theta;
00092         OrientedPoint pose;
00093         is >> pose.x >> pose.y >> pose.theta;
00094         reading->setPose(pose);
00095 
00096         if (rs->newFormat){
00097                 string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis;
00098                 is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis;
00099         } 
00100 //      else {
00101 //        double a,b,c;
00102 //              is >> a >> b >> c;
00103 //      }
00104         double timestamp, reltimestamp;
00105         string s;
00106         is >> timestamp >>s >> reltimestamp;
00107         reading->setTime(timestamp);
00108         return reading;
00109 
00110 }
00111 
00112 //LogSensorStream
00113 LogSensorStream::LogSensorStream(const SensorMap& sensorMap, const SensorLog* log):
00114         SensorStream(sensorMap){
00115         m_log=log;
00116         assert(m_log);
00117         m_cursor=log->begin();
00118 }
00119 
00120 LogSensorStream::operator bool() const{
00121         return m_cursor==m_log->end();
00122 }
00123 
00124 bool LogSensorStream::rewind(){
00125         m_cursor=m_log->begin();
00126         return true;
00127 }
00128 
00129 SensorStream& LogSensorStream::operator >>(const SensorReading*& rd){
00130         rd=*m_cursor;
00131         m_cursor++;
00132         return *this;
00133 }
00134 
00135 //InputSensorStream
00136 InputSensorStream::InputSensorStream(const SensorMap& sensorMap, std::istream& is):
00137         SensorStream(sensorMap), m_inputStream(is){
00138 }
00139 
00140 InputSensorStream::operator bool() const{
00141         return (bool) m_inputStream;
00142 }
00143 
00144 bool InputSensorStream::rewind(){
00145         //m_inputStream.rewind();
00146         return false;
00147 }
00148 
00149 SensorStream& InputSensorStream::operator >>(const SensorReading*& reading){
00150         reading=parseReading(m_inputStream, m_sensorMap);
00151         return *this;
00152 }
00153 
00154 };
00155 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13