sensorlog.cpp
Go to the documentation of this file.
00001 #include "sensorlog.h"
00002 
00003 #include <iostream>
00004 #include <sstream>
00005 #include <assert.h>
00006 #include <sensor_odometry/odometrysensor.h>
00007 #include <sensor_range/rangesensor.h>
00008 
00009 #define LINEBUFFER_SIZE 100000
00010 
00011 namespace GMapping {
00012 
00013 using namespace std;
00014 
00015 SensorLog::SensorLog(const SensorMap& sm): m_sensorMap(sm){
00016 }
00017 
00018 SensorLog::~SensorLog(){
00019         for (iterator it=begin(); it!=end(); it++)
00020                 if (*it) delete (*it);
00021 }
00022 
00023 istream& SensorLog::load(istream& is){
00024         for (iterator it=begin(); it!=end(); it++)
00025                 if (*it) delete (*it);
00026         clear();
00027         
00028         char buf[LINEBUFFER_SIZE];
00029         while (is){
00030                 is.getline(buf, LINEBUFFER_SIZE);
00031                 istringstream lis(buf);
00032                 
00033                 string sensorname;
00034                 
00035                 if (lis) 
00036                         lis >>sensorname; 
00037                 else 
00038                         continue;
00039                 
00040                 
00041                         
00042                 SensorMap::const_iterator it=m_sensorMap.find(sensorname);
00043                 if (it==m_sensorMap.end()){
00044                         continue;
00045                 }
00046                 
00047                 Sensor* sensor=it->second;
00048                 
00049                 SensorReading* reading=0;
00050                 OdometrySensor* odometry=dynamic_cast<OdometrySensor*>(sensor);
00051                 if (odometry)
00052                         reading=parseOdometry(lis, odometry);
00053                 
00054                 RangeSensor* range=dynamic_cast<RangeSensor*>(sensor);
00055                 if (range)
00056                         reading=parseRange(lis, range);
00057                 if (reading)
00058                         push_back(reading);
00059         }
00060         return is;
00061         
00062 }
00063 
00064 OdometryReading* SensorLog::parseOdometry(istream& is, const OdometrySensor* osen) const{
00065         OdometryReading* reading=new OdometryReading(osen);
00066         OrientedPoint pose;
00067         OrientedPoint speed;
00068         OrientedPoint accel;
00069         is >> pose.x >> pose.y >> pose.theta;
00070         is >> speed.x >>speed.theta;
00071         speed.y=0;
00072         is >> accel.x;
00073         accel.y=accel.theta=0;
00074         reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel);
00075         return reading;
00076 }
00077 
00078 RangeReading* SensorLog::parseRange(istream& is, const RangeSensor* rs) const{
00079         if(rs->newFormat){
00080                 string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
00081                 is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy >> remission_mode;
00082         }
00083         
00084         unsigned int size;
00085         is >> size;
00086         assert(size==rs->beams().size());
00087         
00088         RangeReading* reading=new RangeReading(rs);
00089         //cerr << "#R=" << size << endl;
00090         reading->resize(size);
00091         for (unsigned int i=0; i<size; i++){
00092                 is >> (*reading)[i];
00093         }
00094         if (rs->newFormat){
00095                 int reflectionBeams;
00096                 is >> reflectionBeams;
00097                 double reflection;
00098                 for (int i=0; i<reflectionBeams; i++)
00099                         is >> reflection;
00100         }
00101         //FIXME XXX
00102         OrientedPoint laserPose;
00103         is >> laserPose.x >> laserPose.y >> laserPose.theta;
00104         OrientedPoint pose;
00105         is >> pose.x >> pose.y >> pose.theta; 
00106         reading->setPose(pose);
00107         double a,b,c;
00108         if (rs->newFormat){
00109                 string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis;
00110                 is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis;
00111         } else {
00112                 is >> a >> b >> c;
00113         }
00114         string s;
00115         is >> a >> s;
00116         is >> a;
00117         reading->setTime(a);
00118         return reading;
00119 }
00120 
00121 OrientedPoint SensorLog::boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const {
00122         xmin=ymin=1e6;
00123         xmax=ymax=-1e6;
00124         bool first=true;
00125         OrientedPoint start;
00126         for (const_iterator it=begin(); it!=end(); it++){
00127                 double lxmin=0., lxmax=0., lymin=0., lymax=0.;
00128                 const SensorReading* reading=*it;
00129                 const OdometryReading* odometry=dynamic_cast<const OdometryReading*> (reading);
00130                 if (odometry){
00131                         lxmin=lxmax=odometry->getPose().x;
00132                         lymin=lymax=odometry->getPose().y;
00133                 }
00134                 
00135                 const RangeReading* rangeReading=dynamic_cast<const RangeReading*> (reading);
00136                 if (rangeReading){
00137                         lxmin=lxmax=rangeReading->getPose().x;
00138                         lymin=lymax=rangeReading->getPose().y;
00139                         if (first){
00140                                 first=false;
00141                                 start=rangeReading->getPose();  
00142                         }
00143                 }
00144                 xmin=xmin<lxmin?xmin:lxmin;
00145                 xmax=xmax>lxmax?xmax:lxmax;
00146                 ymin=ymin<lymin?lymin:lymin;
00147                 ymax=ymax>lymax?ymax:lymax;
00148         }
00149         return start;
00150 }
00151 
00152 };
00153 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21