openslam_gmapping: carmenconfiguration.cpp Source File

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                 //this is a workaround for carmen log files
00040                 //the number lf laser beams should be specofoed in the config
00041                 //part of the log
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         //this boring stuff is for retrieving the parameters from the loaded tokens
00099         
00100         SensorMap smap;
00101         //odometry
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         //sonars
00108         const_iterator key=find("robot_use_sonar");
00109         if (key!=end() && key->second.front()=="on"){
00110                 RangeSensor* sonar=new RangeSensor("SONAR");
00111 
00112                 //the center of the sonar is the center of the base
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         //laser
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                 //by default the center of the robot is the center of the laser
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                 //double angle=-.5*M_PI;
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                 //by default the center of the robot is the center of the laser
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                 //double angle=-.5*M_PI;
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         //vertical laser
00318         key=find("robot_use_rear_laser");
00319 
00320         if (key!=end() && key->second.front()=="on"){
00321                 RangeSensor* laser=new RangeSensor("RLASER");
00322                 
00323                 //by default the center of the robot is the center of the laser
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                 //double angle=-.5*M_PI;
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                 //by default the center of the robot is the center of the laser
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                         // laser->m_pose.x==atof(key->second.front().c_str());
00400                         cerr << "REAR OFFSET not used" << laser->m_pose.x << endl;
00401                 }
00402                 
00403                 RangeSensor::Beam beam;
00404                         
00405                 //double angle=-.5*M_PI;
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