carmenconfiguration.cpp
Go to the documentation of this file.
1 #include <cstdlib>
2 #include "gmapping/log/carmenconfiguration.h"
3 #include <iostream>
4 #include <sstream>
5 #include <assert.h>
6 #include <sys/types.h>
7 #include <gmapping/sensor/sensor_odometry/odometrysensor.h>
8 #include <gmapping/sensor/sensor_range/rangesensor.h>
9 
10 
11 #define LINEBUFFER_SIZE 10000
12 
13 namespace GMapping {
14 
15 using namespace std;
16 
17 istream& CarmenConfiguration::load(istream& is){
18  clear();
19  char buf[LINEBUFFER_SIZE];
20  bool laseron=false;
21  bool rlaseron=false;
22  bool rlaser1=false;
23  bool rlaser2=false;
24 
25  string beams;
26  string rbeams;
27 
28  while (is){
29  is.getline(buf, LINEBUFFER_SIZE);
30  istringstream lis(buf);
31 
32  string qualifier;
33  string name;
34 
35  if (lis)
36  lis >> qualifier;
37  else
38  continue;
39  //this is a workaround for carmen log files
40  //the number lf laser beams should be specofoed in the config
41  //part of the log
42  if (qualifier=="FLASER"){
43  laseron=true;
44  lis >> beams;
45  }
46  if (qualifier=="RLASER"){
47  rlaseron=true;
48  lis >> rbeams;
49  }
50  if (qualifier=="ROBOTLASER1"){
51  string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
52  lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> beams;
53  rlaser1=true;
54  }
55  if (qualifier=="ROBOTLASER2"){
56  string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
57  lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> rbeams;
58  rlaser2=true;
59  }
60  if (qualifier!="PARAM")
61  continue;
62  if (lis)
63  lis >> name;
64  else continue;
65 
66 
67  vector<string> v;
68  while (lis){
69  string cparm;
70  lis >> cparm;
71  if (lis)
72  v.push_back(cparm);
73  }
74  insert(make_pair(name, v));
75  }
76  if (laseron || rlaser1){
77  vector<string> v;
78  v.push_back(beams);
79  insert(make_pair("laser_beams", v));
80  cerr << "FRONT LASER BEAMS FROM LOG: " << beams << endl;
81  v.clear();
82  v.push_back("on");
83  insert(make_pair("robot_use_laser", v));
84  }
85  if (rlaseron || rlaser2){
86  vector<string> v;
87  v.push_back(rbeams);
88  insert(make_pair("rear_laser_beams", v));
89  cerr << "REAR LASER BEAMS FROM LOG: " << beams << endl;
90  v.clear();
91  v.push_back("on");
92  insert(make_pair("robot_use_rear_laser", v));
93  }
94  return is;
95 }
96 
97 SensorMap CarmenConfiguration::computeSensorMap() const{
98  //this boring stuff is for retrieving the parameters from the loaded tokens
99 
100  SensorMap smap;
101  //odometry
102  OdometrySensor* odometry=new OdometrySensor("ODOM");
103  OdometrySensor* truepos=new OdometrySensor("TRUEPOS", true);
104 
105  smap.insert(make_pair(odometry->getName(), odometry));
106  smap.insert(make_pair(truepos->getName(), truepos));
107  //sonars
108  const_iterator key=find("robot_use_sonar");
109  if (key!=end() && key->second.front()=="on"){
110  RangeSensor* sonar=new RangeSensor("SONAR");
111 
112  //the center of the sonar is the center of the base
113  sonar->m_pose.x=sonar->m_pose.y=sonar->m_pose.theta=0;
114 
115  double maxrange=10.;
116  key=find("robot_max_sonar");
117  if (key!=end()){
118  maxrange=atof(key->second.front().c_str());
119  cerr << "max sonar:" << maxrange << endl;
120  }
121 
122  unsigned int sonar_num=0;
123  key=find("robot_num_sonars");
124  if (key!=end()){
125  sonar_num=atoi(key->second.front().c_str());
126  cerr << "robot_num_sonars" << sonar_num << endl;
127  }
128 
129  key=find("robot_sonar_offsets");
130  if (key!=end()){
131  const vector<string> & soff=key->second;
132 
133  if( (soff.size()/3<sonar_num)){
134  cerr << __func__ << ": Error " << soff.size()
135  << " parameters for defining the sonar offsets"
136  << " while the specified number of sonars requires "
137  << sonar_num*3 << " parameters at least" << endl;
138  } else {
139  cerr << __func__ << ": Ok " << soff.size() << " parameters for defining the sonar offsets of " << sonar_num << " devices" << endl;
140  }
141 
142 
143  RangeSensor::Beam beam;
144 
145  for (unsigned int i=0; i<sonar_num*3; i+=3){
146  beam.span=M_PI/180.*7.5;
147  beam.pose.x=atof(soff[i].c_str());
148  beam.pose.y=atof(soff[i+1].c_str());
149  beam.pose.theta=atof(soff[i+2].c_str());
150  beam.maxRange=maxrange;
151  sonar->m_beams.push_back(beam);
152  cerr << "beam_x" << beam.pose.x;
153  cerr << " beam_y" << beam.pose.y;
154  cerr << " beam_theta" << beam.pose.theta << endl;;
155  }
156  }
157  sonar->updateBeamsLookup();
158  smap.insert(make_pair(sonar->getName(), sonar));
159  }
160 
161  //laser
162  key=find("robot_use_laser");
163 
164  if (key!=end() && key->second.front()=="on"){
165  RangeSensor* laser=new RangeSensor("FLASER");
166  laser->newFormat=false;
167  //by default the center of the robot is the center of the laser
168  laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
169  key=find("robot_frontlaser_offset");
170  if (key!=end()){
171  laser->m_pose.x=atof(key->second.front().c_str());
172  cerr << "FRONT OFFSET= " << laser->m_pose.x << endl;
173  }
174 
175 
176 
177  RangeSensor::Beam beam;
178 
179  //double angle=-.5*M_PI;
180  unsigned int beam_no=180;
181 
182  key=find("laser_beams");
183  if (key!=end()){
184  beam_no=atoi(key->second.front().c_str());
185  cerr << "FRONT BEAMS="<< beam_no << endl;
186  }
187 
188  double maxrange=50;
189  double resolution=1.;
190 
191 
192  if (beam_no==180 || beam_no==181)
193  resolution =1.;
194  else if (beam_no==360 || beam_no==361)
195  resolution =.5;
196  else if (beam_no==540 || beam_no==541)
197  resolution =.5;
198  else if (beam_no==769) {
199  resolution =360./1024.;
200  maxrange = 4.1;
201  }
202  else if (beam_no==682) {
203  resolution =360./1024.;
204  maxrange = 4.1;
205  }
206  else if (beam_no==683) {
207  resolution =360./1024.;
208  maxrange = 5.5;
209  }
210  else {
211  key=find("laser_front_laser_resolution");
212  if (key!=end()){
213  resolution=atof(key->second.front().c_str());
214  cerr << "FRONT RES " << resolution << endl;
215  }
216  }
217 
218  laser->m_beams.resize(beam_no);
219  double center_beam=(double)beam_no/2.;
220  uint low_index=(uint)floor(center_beam);
221  uint up_index=(uint)ceil(center_beam);
222  double step=resolution*M_PI/180.;
223  double angle=beam_no%2?0:step;
224  unsigned int i=beam_no%2?0:1;
225  for (; i<low_index+1; i++, angle+=step){
226  beam.span=0;
227  beam.pose.x=0;
228  beam.pose.y=0;
229  beam.s = 1;
230  beam.c = 1;
231  beam.pose.theta=-angle;
232  beam.maxRange=maxrange;
233  laser->m_beams[low_index-i]=beam;
234  beam.pose.theta=angle;
235  laser->m_beams[up_index+i-1]=beam;
236  }
237  laser->updateBeamsLookup();
238  smap.insert(make_pair(laser->getName(), laser));
239  cerr << "front beams " << beam_no << endl;
240  cerr << "maxrange " << maxrange << endl;
241  }
242 
243 
244  key=find("robot_use_laser");
245  if (key!=end() && key->second.front()=="on"){
246  RangeSensor* laser=new RangeSensor("ROBOTLASER1");
247  laser->newFormat=true;
248  cerr << "ROBOTLASER1 inserted" << endl;
249  //by default the center of the robot is the center of the laser
250  laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
251  key=find("robot_frontlaser_offset");
252  if (key!=end()){
253  laser->m_pose.x=atof(key->second.front().c_str());
254  cerr << "FRONT OFFSET=" << laser->m_pose.x << endl;
255  }
256 
257  RangeSensor::Beam beam;
258 
259  //double angle=-.5*M_PI;
260  unsigned int beam_no=180;
261 
262  key=find("laser_beams");
263  if (key!=end()){
264  beam_no=atoi(key->second.front().c_str());
265  cerr << "FRONT BEAMS="<< beam_no << endl;
266  }
267 
268  double maxrange=50;
269  double resolution=1.;
270 
271 
272  if (beam_no==180 || beam_no==181)
273  resolution =1.;
274  else if (beam_no==360 || beam_no==361)
275  resolution =.5;
276  else if (beam_no==540 || beam_no==541)
277  resolution =.5;
278  else if (beam_no==769)
279  resolution =360./1024.;
280  else if (beam_no==683) {
281  resolution =360./1024.;
282  maxrange=5.50;
283  }
284  else {
285  key=find("laser_front_laser_resolution");
286  if (key!=end()){
287  resolution=atof(key->second.front().c_str());
288  cerr << "FRONT RES" << resolution << endl;
289  }
290  }
291 
292  laser->m_beams.resize(beam_no);
293  double center_beam=(double)beam_no/2.;
294  uint low_index=(uint)floor(center_beam);
295  uint up_index=(uint)ceil(center_beam);
296  double step=resolution*M_PI/180.;
297  double angle=beam_no%2?0:step;
298  unsigned int i=beam_no%2?0:1;
299  for (; i<low_index+1; i++, angle+=step){
300  beam.span=0;
301  beam.pose.x=0;
302  beam.pose.y=0;
303  beam.s=0;
304  beam.c=1;
305  beam.pose.theta=-angle;
306  beam.maxRange=maxrange;
307  laser->m_beams[low_index-i]=beam;
308  beam.pose.theta=angle;
309  laser->m_beams[up_index+i-1]=beam;
310  }
311  laser->updateBeamsLookup();
312  smap.insert(make_pair(laser->getName(), laser));
313  cerr << "front beams" << beam_no << endl;
314  }
315 
316 
317  //vertical laser
318  key=find("robot_use_rear_laser");
319 
320  if (key!=end() && key->second.front()=="on"){
321  RangeSensor* laser=new RangeSensor("RLASER");
322 
323  //by default the center of the robot is the center of the laser
324  laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
325  laser->m_pose.theta=M_PI;
326  key=find("robot_rearlaser_offset");
327  if (key!=end()){
328  laser->m_pose.x=atof(key->second.front().c_str());
329  cerr << "REAR OFFSET = " << laser->m_pose.x << endl;
330  }
331 
332 
333 
334  RangeSensor::Beam beam;
335 
336  //double angle=-.5*M_PI;
337  unsigned int beam_no=180;
338 
339  key=find("rear_laser_beams");
340  if (key!=end()){
341  beam_no=atoi(key->second.front().c_str());
342  cerr << "REAR BEAMS="<< beam_no << endl;
343  }
344 
345  double maxrange=89;
346  double resolution=1.;
347 
348 
349  if (beam_no==180 || beam_no==181)
350  resolution =1.;
351  else if (beam_no==360 || beam_no==361)
352  resolution =.5;
353  else if (beam_no==540 || beam_no==541)
354  resolution =.5;
355  else if (beam_no==769)
356  resolution =360./1024.;
357  else {
358  key=find("laser_rear_laser_resolution");
359  if (key!=end()){
360  resolution=atof(key->second.front().c_str());
361  cerr << "REAR RES" << resolution << endl;
362  }
363  }
364 
365  laser->m_beams.resize(beam_no);
366  double center_beam=(double)beam_no/2.;
367  uint low_index=(uint)floor(center_beam);
368  uint up_index=(uint)ceil(center_beam);
369  double step=resolution*M_PI/180.;
370  double angle=beam_no%2?0:step;
371  unsigned int i=beam_no%2?0:1;
372  for (; i<low_index+1; i++, angle+=step){
373  beam.span=0;
374  beam.pose.x=0;
375  beam.pose.y=0;
376  beam.s=0;
377  beam.c=1;
378  beam.pose.theta=-angle;
379  beam.maxRange=maxrange;
380  laser->m_beams[low_index-i]=beam;
381  beam.pose.theta=angle;
382  laser->m_beams[up_index+i-1]=beam;
383  }
384  laser->updateBeamsLookup();
385  smap.insert(make_pair(laser->getName(), laser));
386  cerr<< "rear beams" << beam_no << endl;
387  }
388 
389  key=find("robot_use_rear_laser");
390  if (key!=end() && key->second.front()=="on"){
391  RangeSensor* laser=new RangeSensor("ROBOTLASER2");
392  laser->newFormat=true;
393  cerr << "ROBOTLASER2 inserted" << endl;
394  //by default the center of the robot is the center of the laser
395  laser->m_pose.x=laser->m_pose.y=0;
396  laser->m_pose.theta=M_PI;
397  key=find("robot_rearlaser_offset");
398  if (key!=end()){
399  // laser->m_pose.x==atof(key->second.front().c_str());
400  cerr << "REAR OFFSET not used" << laser->m_pose.x << endl;
401  }
402 
403  RangeSensor::Beam beam;
404 
405  //double angle=-.5*M_PI;
406  unsigned int beam_no=180;
407 
408  key=find("rear_laser_beams");
409  if (key!=end()){
410  beam_no=atoi(key->second.front().c_str());
411  cerr << "REAR BEAMS="<< beam_no << endl;
412  }
413 
414  double maxrange=50;
415  double resolution=1.;
416 
417 
418  if (beam_no==180 || beam_no==181)
419  resolution =1.;
420  else if (beam_no==360 || beam_no==361)
421  resolution =.5;
422  else if (beam_no==540 || beam_no==541)
423  resolution =.5;
424  else if (beam_no==769)
425  resolution =360./1024.;
426  else {
427  key=find("laser_rear_laser_resolution");
428  if (key!=end()){
429  resolution=atof(key->second.front().c_str());
430  cerr << "REAR RES" << resolution << endl;
431  }
432  }
433 
434  laser->m_beams.resize(beam_no);
435  double center_beam=(double)beam_no/2.;
436  uint low_index=(uint)floor(center_beam);
437  uint up_index=(uint)ceil(center_beam);
438  double step=resolution*M_PI/180.;
439  double angle=beam_no%2?0:step;
440  unsigned int i=beam_no%2?0:1;
441  for (; i<low_index+1; i++, angle+=step){
442  beam.span=0;
443  beam.s=0;
444  beam.c=1;
445  beam.pose.x=0;
446  beam.pose.y=0;
447  beam.pose.theta=-angle;
448  beam.maxRange=maxrange;
449  laser->m_beams[low_index-i]=beam;
450  beam.pose.theta=angle;
451  laser->m_beams[up_index+i-1]=beam;
452  }
453  laser->updateBeamsLookup();
454  smap.insert(make_pair(laser->getName(), laser));
455  cerr << "rear beams" << beam_no << endl;
456  }
457 
458 
459  return smap;
460 }
461 
462 };
463 
GMapping::SensorMap
std::map< std::string, Sensor * > SensorMap
Definition: sensor.h:20
rangesensor.h
is
ifstream is(argv[c])
carmenconfiguration.h
GMapping::CarmenConfiguration::computeSensorMap
virtual SensorMap computeSensorMap() const
Definition: carmenconfiguration.cpp:97
GMapping
Definition: configfile.cpp:34
GMapping::Sensor::getName
std::string getName() const
Definition: sensor.h:14
GMapping::RangeSensor::m_beams
std::vector< Beam > m_beams
Definition: rangesensor.h:31
GMapping::RangeSensor::updateBeamsLookup
void updateBeamsLookup()
Definition: rangesensor.cpp:22
GMapping::RangeSensor::Beam::s
double s
Definition: rangesensor.h:20
GMapping::RangeSensor::m_pose
OrientedPoint m_pose
Definition: rangesensor.h:30
GMapping::RangeSensor::Beam::span
double span
Definition: rangesensor.h:18
GMapping::RangeSensor
Definition: rangesensor.h:11
GMapping::RangeSensor::Beam::maxRange
double maxRange
Definition: rangesensor.h:19
GMapping::point::y
T y
Definition: point.h:16
GMapping::orientedpoint::theta
A theta
Definition: point.h:60
GMapping::RangeSensor::Beam::c
double c
Definition: rangesensor.h:20
odometrysensor.h
GMapping::CarmenConfiguration::load
virtual std::istream & load(std::istream &is)
Definition: carmenconfiguration.cpp:17
GMapping::RangeSensor::Beam
Definition: rangesensor.h:16
maxrange
double maxrange
Definition: gfs2stream.cpp:22
GMapping::RangeSensor::newFormat
bool newFormat
Definition: rangesensor.h:28
GMapping::RangeSensor::Beam::pose
OrientedPoint pose
Definition: rangesensor.h:17
GMapping::OdometrySensor
Definition: odometrysensor.h:10
GMapping::point::x
T x
Definition: point.h:16
LINEBUFFER_SIZE
#define LINEBUFFER_SIZE
Definition: carmenconfiguration.cpp:11

openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Thu Oct 19 2023 02:25:51