Go to the documentation of this file.00001 #include <cstdlib>
00002 #include "carmenconfiguration.h"
00003 #include <iostream>
00004 #include <sstream>
00005 #include <assert.h>
00006 #include <sys/types.h>
00007 #include <sensor_odometry/odometrysensor.h>
00008 #include <sensor_range/rangesensor.h>
00009
00010
00011 #define LINEBUFFER_SIZE 10000
00012
00013 namespace GMapping {
00014
00015 using namespace std;
00016
00017 istream& CarmenConfiguration::load(istream& is){
00018 clear();
00019 char buf[LINEBUFFER_SIZE];
00020 bool laseron=false;
00021 bool rlaseron=false;
00022 bool rlaser1=false;
00023 bool rlaser2=false;
00024
00025 string beams;
00026 string rbeams;
00027
00028 while (is){
00029 is.getline(buf, LINEBUFFER_SIZE);
00030 istringstream lis(buf);
00031
00032 string qualifier;
00033 string name;
00034
00035 if (lis)
00036 lis >> qualifier;
00037 else
00038 continue;
00039
00040
00041
00042 if (qualifier=="FLASER"){
00043 laseron=true;
00044 lis >> beams;
00045 }
00046 if (qualifier=="RLASER"){
00047 rlaseron=true;
00048 lis >> rbeams;
00049 }
00050 if (qualifier=="ROBOTLASER1"){
00051 string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
00052 lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> beams;
00053 rlaser1=true;
00054 }
00055 if (qualifier=="ROBOTLASER2"){
00056 string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode;
00057 lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> rbeams;
00058 rlaser2=true;
00059 }
00060 if (qualifier!="PARAM")
00061 continue;
00062 if (lis)
00063 lis >> name;
00064 else continue;
00065
00066
00067 vector<string> v;
00068 while (lis){
00069 string cparm;
00070 lis >> cparm;
00071 if (lis)
00072 v.push_back(cparm);
00073 }
00074 insert(make_pair(name, v));
00075 }
00076 if (laseron || rlaser1){
00077 vector<string> v;
00078 v.push_back(beams);
00079 insert(make_pair("laser_beams", v));
00080 cerr << "FRONT LASER BEAMS FROM LOG: " << beams << endl;
00081 v.clear();
00082 v.push_back("on");
00083 insert(make_pair("robot_use_laser", v));
00084 }
00085 if (rlaseron || rlaser2){
00086 vector<string> v;
00087 v.push_back(rbeams);
00088 insert(make_pair("rear_laser_beams", v));
00089 cerr << "REAR LASER BEAMS FROM LOG: " << beams << endl;
00090 v.clear();
00091 v.push_back("on");
00092 insert(make_pair("robot_use_rear_laser", v));
00093 }
00094 return is;
00095 }
00096
00097 SensorMap CarmenConfiguration::computeSensorMap() const{
00098
00099
00100 SensorMap smap;
00101
00102 OdometrySensor* odometry=new OdometrySensor("ODOM");
00103 OdometrySensor* truepos=new OdometrySensor("TRUEPOS", true);
00104
00105 smap.insert(make_pair(odometry->getName(), odometry));
00106 smap.insert(make_pair(truepos->getName(), truepos));
00107
00108 const_iterator key=find("robot_use_sonar");
00109 if (key!=end() && key->second.front()=="on"){
00110 RangeSensor* sonar=new RangeSensor("SONAR");
00111
00112
00113 sonar->m_pose.x=sonar->m_pose.y=sonar->m_pose.theta=0;
00114
00115 double maxrange=10.;
00116 key=find("robot_max_sonar");
00117 if (key!=end()){
00118 maxrange=atof(key->second.front().c_str());
00119 cerr << "max sonar:" << maxrange << endl;
00120 }
00121
00122 unsigned int sonar_num=0;
00123 key=find("robot_num_sonars");
00124 if (key!=end()){
00125 sonar_num=atoi(key->second.front().c_str());
00126 cerr << "robot_num_sonars" << sonar_num << endl;
00127 }
00128
00129 key=find("robot_sonar_offsets");
00130 if (key!=end()){
00131 const vector<string> & soff=key->second;
00132
00133 if( (soff.size()/3<sonar_num)){
00134 cerr << __PRETTY_FUNCTION__ << ": Error " << soff.size()
00135 << " parameters for defining the sonar offsets"
00136 << " while the specified number of sonars requires "
00137 << sonar_num*3 << " parameters at least" << endl;
00138 } else {
00139 cerr << __PRETTY_FUNCTION__ << ": Ok " << soff.size() << " parameters for defining the sonar offsets of " << sonar_num << " devices" << endl;
00140 }
00141
00142
00143 RangeSensor::Beam beam;
00144
00145 for (unsigned int i=0; i<sonar_num*3; i+=3){
00146 beam.span=M_PI/180.*7.5;
00147 beam.pose.x=atof(soff[i].c_str());
00148 beam.pose.y=atof(soff[i+1].c_str());
00149 beam.pose.theta=atof(soff[i+2].c_str());
00150 beam.maxRange=maxrange;
00151 sonar->m_beams.push_back(beam);
00152 cerr << "beam_x" << beam.pose.x;
00153 cerr << " beam_y" << beam.pose.y;
00154 cerr << " beam_theta" << beam.pose.theta << endl;;
00155 }
00156 }
00157 sonar->updateBeamsLookup();
00158 smap.insert(make_pair(sonar->getName(), sonar));
00159 }
00160
00161
00162 key=find("robot_use_laser");
00163
00164 if (key!=end() && key->second.front()=="on"){
00165 RangeSensor* laser=new RangeSensor("FLASER");
00166 laser->newFormat=false;
00167
00168 laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
00169 key=find("robot_frontlaser_offset");
00170 if (key!=end()){
00171 laser->m_pose.x=atof(key->second.front().c_str());
00172 cerr << "FRONT OFFSET= " << laser->m_pose.x << endl;
00173 }
00174
00175
00176
00177 RangeSensor::Beam beam;
00178
00179
00180 unsigned int beam_no=180;
00181
00182 key=find("laser_beams");
00183 if (key!=end()){
00184 beam_no=atoi(key->second.front().c_str());
00185 cerr << "FRONT BEAMS="<< beam_no << endl;
00186 }
00187
00188 double maxrange=50;
00189 double resolution=1.;
00190
00191
00192 if (beam_no==180 || beam_no==181)
00193 resolution =1.;
00194 else if (beam_no==360 || beam_no==361)
00195 resolution =.5;
00196 else if (beam_no==540 || beam_no==541)
00197 resolution =.5;
00198 else if (beam_no==769) {
00199 resolution =360./1024.;
00200 maxrange = 4.1;
00201 }
00202 else if (beam_no==682) {
00203 resolution =360./1024.;
00204 maxrange = 4.1;
00205 }
00206 else if (beam_no==683) {
00207 resolution =360./1024.;
00208 maxrange = 5.5;
00209 }
00210 else {
00211 key=find("laser_front_laser_resolution");
00212 if (key!=end()){
00213 resolution=atof(key->second.front().c_str());
00214 cerr << "FRONT RES " << resolution << endl;
00215 }
00216 }
00217
00218 laser->m_beams.resize(beam_no);
00219 double center_beam=(double)beam_no/2.;
00220 uint low_index=(uint)floor(center_beam);
00221 uint up_index=(uint)ceil(center_beam);
00222 double step=resolution*M_PI/180.;
00223 double angle=beam_no%2?0:step;
00224 unsigned int i=beam_no%2?0:1;
00225 for (; i<low_index+1; i++, angle+=step){
00226 beam.span=0;
00227 beam.pose.x=0;
00228 beam.pose.y=0;
00229 beam.s = 1;
00230 beam.c = 1;
00231 beam.pose.theta=-angle;
00232 beam.maxRange=maxrange;
00233 laser->m_beams[low_index-i]=beam;
00234 beam.pose.theta=angle;
00235 laser->m_beams[up_index+i-1]=beam;
00236 }
00237 laser->updateBeamsLookup();
00238 smap.insert(make_pair(laser->getName(), laser));
00239 cerr << "front beams " << beam_no << endl;
00240 cerr << "maxrange " << maxrange << endl;
00241 }
00242
00243
00244 key=find("robot_use_laser");
00245 if (key!=end() && key->second.front()=="on"){
00246 RangeSensor* laser=new RangeSensor("ROBOTLASER1");
00247 laser->newFormat=true;
00248 cerr << "ROBOTLASER1 inserted" << endl;
00249
00250 laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
00251 key=find("robot_frontlaser_offset");
00252 if (key!=end()){
00253 laser->m_pose.x=atof(key->second.front().c_str());
00254 cerr << "FRONT OFFSET=" << laser->m_pose.x << endl;
00255 }
00256
00257 RangeSensor::Beam beam;
00258
00259
00260 unsigned int beam_no=180;
00261
00262 key=find("laser_beams");
00263 if (key!=end()){
00264 beam_no=atoi(key->second.front().c_str());
00265 cerr << "FRONT BEAMS="<< beam_no << endl;
00266 }
00267
00268 double maxrange=50;
00269 double resolution=1.;
00270
00271
00272 if (beam_no==180 || beam_no==181)
00273 resolution =1.;
00274 else if (beam_no==360 || beam_no==361)
00275 resolution =.5;
00276 else if (beam_no==540 || beam_no==541)
00277 resolution =.5;
00278 else if (beam_no==769)
00279 resolution =360./1024.;
00280 else if (beam_no==683) {
00281 resolution =360./1024.;
00282 maxrange=5.50;
00283 }
00284 else {
00285 key=find("laser_front_laser_resolution");
00286 if (key!=end()){
00287 resolution=atof(key->second.front().c_str());
00288 cerr << "FRONT RES" << resolution << endl;
00289 }
00290 }
00291
00292 laser->m_beams.resize(beam_no);
00293 double center_beam=(double)beam_no/2.;
00294 uint low_index=(uint)floor(center_beam);
00295 uint up_index=(uint)ceil(center_beam);
00296 double step=resolution*M_PI/180.;
00297 double angle=beam_no%2?0:step;
00298 unsigned int i=beam_no%2?0:1;
00299 for (; i<low_index+1; i++, angle+=step){
00300 beam.span=0;
00301 beam.pose.x=0;
00302 beam.pose.y=0;
00303 beam.s=0;
00304 beam.c=1;
00305 beam.pose.theta=-angle;
00306 beam.maxRange=maxrange;
00307 laser->m_beams[low_index-i]=beam;
00308 beam.pose.theta=angle;
00309 laser->m_beams[up_index+i-1]=beam;
00310 }
00311 laser->updateBeamsLookup();
00312 smap.insert(make_pair(laser->getName(), laser));
00313 cerr << "front beams" << beam_no << endl;
00314 }
00315
00316
00317
00318 key=find("robot_use_rear_laser");
00319
00320 if (key!=end() && key->second.front()=="on"){
00321 RangeSensor* laser=new RangeSensor("RLASER");
00322
00323
00324 laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0;
00325 laser->m_pose.theta=M_PI;
00326 key=find("robot_rearlaser_offset");
00327 if (key!=end()){
00328 laser->m_pose.x=atof(key->second.front().c_str());
00329 cerr << "REAR OFFSET = " << laser->m_pose.x << endl;
00330 }
00331
00332
00333
00334 RangeSensor::Beam beam;
00335
00336
00337 unsigned int beam_no=180;
00338
00339 key=find("rear_laser_beams");
00340 if (key!=end()){
00341 beam_no=atoi(key->second.front().c_str());
00342 cerr << "REAR BEAMS="<< beam_no << endl;
00343 }
00344
00345 double maxrange=89;
00346 double resolution=1.;
00347
00348
00349 if (beam_no==180 || beam_no==181)
00350 resolution =1.;
00351 else if (beam_no==360 || beam_no==361)
00352 resolution =.5;
00353 else if (beam_no==540 || beam_no==541)
00354 resolution =.5;
00355 else if (beam_no==769)
00356 resolution =360./1024.;
00357 else {
00358 key=find("laser_rear_laser_resolution");
00359 if (key!=end()){
00360 resolution=atof(key->second.front().c_str());
00361 cerr << "REAR RES" << resolution << endl;
00362 }
00363 }
00364
00365 laser->m_beams.resize(beam_no);
00366 double center_beam=(double)beam_no/2.;
00367 uint low_index=(uint)floor(center_beam);
00368 uint up_index=(uint)ceil(center_beam);
00369 double step=resolution*M_PI/180.;
00370 double angle=beam_no%2?0:step;
00371 unsigned int i=beam_no%2?0:1;
00372 for (; i<low_index+1; i++, angle+=step){
00373 beam.span=0;
00374 beam.pose.x=0;
00375 beam.pose.y=0;
00376 beam.s=0;
00377 beam.c=1;
00378 beam.pose.theta=-angle;
00379 beam.maxRange=maxrange;
00380 laser->m_beams[low_index-i]=beam;
00381 beam.pose.theta=angle;
00382 laser->m_beams[up_index+i-1]=beam;
00383 }
00384 laser->updateBeamsLookup();
00385 smap.insert(make_pair(laser->getName(), laser));
00386 cerr<< "rear beams" << beam_no << endl;
00387 }
00388
00389 key=find("robot_use_rear_laser");
00390 if (key!=end() && key->second.front()=="on"){
00391 RangeSensor* laser=new RangeSensor("ROBOTLASER2");
00392 laser->newFormat=true;
00393 cerr << "ROBOTLASER2 inserted" << endl;
00394
00395 laser->m_pose.x=laser->m_pose.y=0;
00396 laser->m_pose.theta=M_PI;
00397 key=find("robot_rearlaser_offset");
00398 if (key!=end()){
00399
00400 cerr << "REAR OFFSET not used" << laser->m_pose.x << endl;
00401 }
00402
00403 RangeSensor::Beam beam;
00404
00405
00406 unsigned int beam_no=180;
00407
00408 key=find("rear_laser_beams");
00409 if (key!=end()){
00410 beam_no=atoi(key->second.front().c_str());
00411 cerr << "REAR BEAMS="<< beam_no << endl;
00412 }
00413
00414 double maxrange=50;
00415 double resolution=1.;
00416
00417
00418 if (beam_no==180 || beam_no==181)
00419 resolution =1.;
00420 else if (beam_no==360 || beam_no==361)
00421 resolution =.5;
00422 else if (beam_no==540 || beam_no==541)
00423 resolution =.5;
00424 else if (beam_no==769)
00425 resolution =360./1024.;
00426 else {
00427 key=find("laser_rear_laser_resolution");
00428 if (key!=end()){
00429 resolution=atof(key->second.front().c_str());
00430 cerr << "REAR RES" << resolution << endl;
00431 }
00432 }
00433
00434 laser->m_beams.resize(beam_no);
00435 double center_beam=(double)beam_no/2.;
00436 uint low_index=(uint)floor(center_beam);
00437 uint up_index=(uint)ceil(center_beam);
00438 double step=resolution*M_PI/180.;
00439 double angle=beam_no%2?0:step;
00440 unsigned int i=beam_no%2?0:1;
00441 for (; i<low_index+1; i++, angle+=step){
00442 beam.span=0;
00443 beam.s=0;
00444 beam.c=1;
00445 beam.pose.x=0;
00446 beam.pose.y=0;
00447 beam.pose.theta=-angle;
00448 beam.maxRange=maxrange;
00449 laser->m_beams[low_index-i]=beam;
00450 beam.pose.theta=angle;
00451 laser->m_beams[up_index+i-1]=beam;
00452 }
00453 laser->updateBeamsLookup();
00454 smap.insert(make_pair(laser->getName(), laser));
00455 cerr << "rear beams" << beam_no << endl;
00456 }
00457
00458
00459 return smap;
00460 }
00461
00462 };
00463