sensorlog.cpp
Go to the documentation of this file.
2 
3 #include <iostream>
4 #include <sstream>
5 #include <assert.h>
8 
9 #define LINEBUFFER_SIZE 100000
10 
11 namespace GMapping {
12 
13 using namespace std;
14 
15 SensorLog::SensorLog(const SensorMap& sm): m_sensorMap(sm){
16 }
17 
19  for (iterator it=begin(); it!=end(); it++)
20  if (*it) delete (*it);
21 }
22 
23 istream& SensorLog::load(istream& is){
24  for (iterator it=begin(); it!=end(); it++)
25  if (*it) delete (*it);
26  clear();
27 
28  char buf[LINEBUFFER_SIZE];
29  while (is){
30  is.getline(buf, LINEBUFFER_SIZE);
31  istringstream lis(buf);
32 
33  string sensorname;
34 
35  if (lis)
36  lis >>sensorname;
37  else
38  continue;
39 
40 
41 
42  SensorMap::const_iterator it=m_sensorMap.find(sensorname);
43  if (it==m_sensorMap.end()){
44  continue;
45  }
46 
47  Sensor* sensor=it->second;
48 
49  SensorReading* reading=0;
50  OdometrySensor* odometry=dynamic_cast<OdometrySensor*>(sensor);
51  if (odometry)
52  reading=parseOdometry(lis, odometry);
53 
54  RangeSensor* range=dynamic_cast<RangeSensor*>(sensor);
55  if (range)
56  reading=parseRange(lis, range);
57  if (reading)
58  push_back(reading);
59  }
60  return is;
61 
62 }
63 
65  OdometryReading* reading=new OdometryReading(osen);
66  OrientedPoint pose;
67  OrientedPoint speed;
68  OrientedPoint accel;
69  is >> pose.x >> pose.y >> pose.theta;
70  is >> speed.x >>speed.theta;
71  speed.y=0;
72  is >> accel.x;
73  accel.y=accel.theta=0;
74  reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel);
75  return reading;
76 }
77 
78 RangeReading* SensorLog::parseRange(istream& is, const RangeSensor* rs) const{
79  if(rs->newFormat){
80  string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
81  is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy >> remission_mode;
82  }
83 
84  unsigned int size;
85  is >> size;
86  assert(size==rs->beams().size());
87 
88  RangeReading* reading=new RangeReading(rs);
89  //cerr << "#R=" << size << endl;
90  reading->resize(size);
91  for (unsigned int i=0; i<size; i++){
92  is >> (*reading)[i];
93  }
94  if (rs->newFormat){
95  int reflectionBeams;
96  is >> reflectionBeams;
97  double reflection;
98  for (int i=0; i<reflectionBeams; i++)
99  is >> reflection;
100  }
101  //FIXME XXX
102  OrientedPoint laserPose;
103  is >> laserPose.x >> laserPose.y >> laserPose.theta;
104  OrientedPoint pose;
105  is >> pose.x >> pose.y >> pose.theta;
106  reading->setPose(pose);
107  double a,b,c;
108  if (rs->newFormat){
109  string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis;
110  is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis;
111  } else {
112  is >> a >> b >> c;
113  }
114  string s;
115  is >> a >> s;
116  is >> a;
117  reading->setTime(a);
118  return reading;
119 }
120 
121 OrientedPoint SensorLog::boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const {
122  xmin=ymin=1e6;
123  xmax=ymax=-1e6;
124  bool first=true;
125  OrientedPoint start;
126  for (const_iterator it=begin(); it!=end(); it++){
127  double lxmin=0., lxmax=0., lymin=0., lymax=0.;
128  const SensorReading* reading=*it;
129  const OdometryReading* odometry=dynamic_cast<const OdometryReading*> (reading);
130  if (odometry){
131  lxmin=lxmax=odometry->getPose().x;
132  lymin=lymax=odometry->getPose().y;
133  }
134 
135  const RangeReading* rangeReading=dynamic_cast<const RangeReading*> (reading);
136  if (rangeReading){
137  lxmin=lxmax=rangeReading->getPose().x;
138  lymin=lymax=rangeReading->getPose().y;
139  if (first){
140  first=false;
141  start=rangeReading->getPose();
142  }
143  }
144  xmin=xmin<lxmin?xmin:lxmin;
145  xmax=xmax>lxmax?xmax:lxmax;
146  ymin=ymin<lymin?lymin:lymin;
147  ymax=ymax>lymax?ymax:lymax;
148  }
149  return start;
150 }
151 
152 };
153 
const std::vector< Beam > & beams() const
Definition: rangesensor.h:24
#define LINEBUFFER_SIZE
Definition: sensorlog.cpp:9
void setAcceleration(const OrientedPoint &acceleration)
const SensorMap & m_sensorMap
Definition: sensorlog.h:23
void setSpeed(const OrientedPoint &speed)
std::istream & load(std::istream &is)
Definition: sensorlog.cpp:23
OdometryReading * parseOdometry(std::istream &is, const OdometrySensor *) const
Definition: sensorlog.cpp:64
std::map< std::string, Sensor * > SensorMap
Definition: sensor.h:20
unsigned int c
Definition: gfs2stream.cpp:41
void setPose(const OrientedPoint &pose)
ifstream is(argv[c])
const OrientedPoint & getPose() const
SensorLog(const SensorMap &)
Definition: sensorlog.cpp:15
OrientedPoint boundingBox(double &xmin, double &ymin, double &xmax, double &ymax) const
Definition: sensorlog.cpp:121
RangeReading * parseRange(std::istream &is, const RangeSensor *) const
Definition: sensorlog.cpp:78
const OrientedPoint & getPose() const
Definition: rangereading.h:22


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20