sensorstream.cpp
Go to the documentation of this file.
1 #include <assert.h>
2 #include <sstream>
4 //#define LINEBUFFER_SIZE 1000000 //for not Cyrill to unbless me, it is better to exagerate :-))
5 // Can't declare a buffer that big on the stack. So we'll risk Cyrill's
6 // unblessing, and make it smaller.
7 #define LINEBUFFER_SIZE 8192
8 
9 namespace GMapping {
10 
11 using namespace std;
12 
13 //SensorStream
14 SensorStream::SensorStream(const SensorMap& sensorMap) :m_sensorMap(sensorMap){}
15 
17 
19  SensorReading* reading=0;
20  if (is){
21  char buf[LINEBUFFER_SIZE];
22  is.getline(buf, LINEBUFFER_SIZE);
23  istringstream lis(buf);
24 
25  string sensorname;
26 
27  if (lis){
28  lis >>sensorname;
29  } else
30  return 0;
31 
32  SensorMap::const_iterator it=smap.find(sensorname);
33  if (it==smap.end()){
34  return 0;
35  }
36 
37  Sensor* sensor=it->second;
38 
39  OdometrySensor* odometry=dynamic_cast<OdometrySensor*>(sensor);
40  if (odometry)
41  reading=parseOdometry(lis, odometry);
42 
43  RangeSensor* range=dynamic_cast<RangeSensor*>(sensor);
44  if (range)
45  reading=parseRange(lis, range);
46  }
47  return reading;
48 }
49 
51  OdometryReading* reading=new OdometryReading(osen);
52  OrientedPoint pose;
53  OrientedPoint speed;
54  OrientedPoint accel;
55  is >> pose.x >> pose.y >> pose.theta;
56  is >> speed.x >>speed.theta;
57  speed.y=0;
58  is >> accel.x;
59  accel.y=accel.theta=0;
60  reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel);
61  double timestamp, reltimestamp;
62  string s;
63  is >> timestamp >>s >> reltimestamp;
64  reading->setTime(timestamp);
65  return reading;
66 }
67 
69  //cerr << __func__ << endl;
70  if(rs->newFormat){
71  string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
72  is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode;
73  //cerr << " New format laser msg" << endl;
74  }
75  unsigned int size;
76  is >> size;
77  assert(size==rs->beams().size());
78  RangeReading* reading=new RangeReading(rs);
79  reading->resize(size);
80  for (unsigned int i=0; i<size; i++){
81  is >> (*reading)[i];
82  }
83  if (rs->newFormat){
84  int reflectionBeams;
85  is >> reflectionBeams;
86  double reflection;
87  for (int i=0; i<reflectionBeams; i++)
88  is >> reflection;
89  }
90  OrientedPoint laserPose;
91  is >> laserPose.x >> laserPose.y >> laserPose.theta;
92  OrientedPoint pose;
93  is >> pose.x >> pose.y >> pose.theta;
94  reading->setPose(pose);
95 
96  if (rs->newFormat){
97  string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis;
98  is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis;
99  }
100 // else {
101 // double a,b,c;
102 // is >> a >> b >> c;
103 // }
104  double timestamp, reltimestamp;
105  string s;
106  is >> timestamp >>s >> reltimestamp;
107  reading->setTime(timestamp);
108  return reading;
109 
110 }
111 
112 //LogSensorStream
114  SensorStream(sensorMap){
115  m_log=log;
116  assert(m_log);
117  m_cursor=log->begin();
118 }
119 
120 LogSensorStream::operator bool() const{
121  return m_cursor==m_log->end();
122 }
123 
125  m_cursor=m_log->begin();
126  return true;
127 }
128 
130  rd=*m_cursor;
131  m_cursor++;
132  return *this;
133 }
134 
135 //InputSensorStream
136 InputSensorStream::InputSensorStream(const SensorMap& sensorMap, std::istream& is):
137  SensorStream(sensorMap), m_inputStream(is){
138 }
139 
140 InputSensorStream::operator bool() const{
141  return (bool) m_inputStream;
142 }
143 
145  //m_inputStream.rewind();
146  return false;
147 }
148 
151  return *this;
152 }
153 
154 };
155 
const SensorMap & m_sensorMap
Definition: sensorstream.h:18
SensorStream(const SensorMap &sensorMap)
const std::vector< Beam > & beams() const
Definition: rangesensor.h:24
virtual SensorStream & operator>>(const SensorReading *&)
const SensorLog * m_log
Definition: sensorstream.h:43
static SensorReading * parseReading(std::istream &is, const SensorMap &smap)
LogSensorStream(const SensorMap &sensorMap, const SensorLog *log)
static OdometryReading * parseOdometry(std::istream &is, const OdometrySensor *)
void setAcceleration(const OrientedPoint &acceleration)
virtual SensorStream & operator>>(const SensorReading *&)
void setSpeed(const OrientedPoint &speed)
std::istream & m_inputStream
Definition: sensorstream.h:33
void setTime(double t)
Definition: sensorreading.h:14
std::map< std::string, Sensor * > SensorMap
Definition: sensor.h:20
void setPose(const OrientedPoint &pose)
#define LINEBUFFER_SIZE
Definition: sensorstream.cpp:7
ifstream is(argv[c])
SensorLog::const_iterator m_cursor
Definition: sensorstream.h:44
InputSensorStream(const SensorMap &sensorMap, std::istream &is)
static RangeReading * parseRange(std::istream &is, const RangeSensor *)


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