tutorial2.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2010 SRI International
00003  *
00004  * This program is free software: you can redistribute it and/or modify
00005  * it under the terms of the GNU Lesser General Public License as published by
00006  * the Free Software Foundation, either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 #include "SpaSolver.h"
00019 
00020 #include <OpenKarto/Mapper.h>
00021 
00027 karto::Dataset* CreateMap(karto::Mapper* pMapper)
00028 {
00029   karto::Dataset* pDataset = new karto::Dataset();
00030 
00032   // Create a laser range finder device - use SmartPointer to let karto subsystem manage memory.
00033   karto::Name name("laser0");
00034   karto::LaserRangeFinder* pLaserRangeFinder = karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, name);
00035   pLaserRangeFinder->SetOffsetPose(karto::Pose2(1.0, 0.0, 0.0));
00036   pLaserRangeFinder->SetAngularResolution(karto::math::DegreesToRadians(0.5));
00037   pLaserRangeFinder->SetRangeThreshold(12.0);
00038 
00039   pDataset->Add(pLaserRangeFinder);
00040 
00042   // Create three localized range scans, all using the same range readings, but with different poses. 
00043   karto::LocalizedRangeScan* pLocalizedRangeScan = NULL;
00044 
00045   // 1. localized range scan
00046 
00047   // Create a vector of range readings. Simple example where all the measurements are the same value.
00048   std::vector<kt_double> readings;
00049   for (int i=0; i<361; i++)
00050   {
00051     readings.push_back(3.0);
00052   }
00053   
00054   // create localized range scan
00055   pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
00056   pLocalizedRangeScan->SetOdometricPose(karto::Pose2(0.0, 0.0, 0.0));
00057   pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(0.0, 0.0, 0.0));
00058 
00059   // Add the localized range scan to the mapper
00060   pMapper->Process(pLocalizedRangeScan);
00061   std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
00062 
00063   // Add the localized range scan to the dataset
00064   pDataset->Add(pLocalizedRangeScan);
00065 
00066   // 2. localized range scan
00067 
00068   // create localized range scan
00069   pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
00070   pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, 0.0, 1.57));
00071   pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, 0.0, 1.57));
00072 
00073   // Add the localized range scan to the mapper
00074   pMapper->Process(pLocalizedRangeScan);
00075   std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
00076 
00077   // Add the localized range scan to the dataset
00078   pDataset->Add(pLocalizedRangeScan);
00079 
00080   // 3. localized range scan
00081 
00082   // create localized range scan
00083   pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
00084   pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, -1.0, 2.35619449));
00085   pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, -1.0, 2.35619449));
00086 
00087   // Add the localized range scan to the mapper
00088   pMapper->Process(pLocalizedRangeScan);
00089   std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
00090 
00091   // Add the localized range scan to the dataset
00092   pDataset->Add(pLocalizedRangeScan);
00093 
00094   return pDataset;
00095 }
00096 
00100 karto::OccupancyGrid* CreateOccupancyGrid(karto::Mapper* pMapper, kt_double resolution)
00101 {
00102   std::cout << "Generating map..." << std::endl;
00103 
00104   // Create a map (occupancy grid) - time it
00105   karto::OccupancyGrid* pOccupancyGrid = karto::OccupancyGrid::CreateFromScans(pMapper->GetAllProcessedScans(), resolution);
00106 
00107   return pOccupancyGrid;
00108 }
00109 
00113 void PrintOccupancyGrid(karto::OccupancyGrid* pOccupancyGrid)
00114 {
00115   if (pOccupancyGrid != NULL)
00116   {
00117     // Output ASCII representation of map
00118     kt_int32s width = pOccupancyGrid->GetWidth();
00119     kt_int32s height = pOccupancyGrid->GetHeight();
00120     karto::Vector2<kt_double> offset = pOccupancyGrid->GetCoordinateConverter()->GetOffset();
00121 
00122     std::cout << "width = " << width << ", height = " << height << ", scale = " << pOccupancyGrid->GetCoordinateConverter()->GetScale() << ", offset: " << offset.GetX() << ", " << offset.GetY() << std::endl;
00123     for (kt_int32s y=height-1; y>=0; y--)
00124     {
00125       for (kt_int32s x=0; x<width; x++) 
00126       {
00127         // Getting the value at position x,y
00128         kt_int8u value = pOccupancyGrid->GetValue(karto::Vector2<kt_int32s>(x, y));
00129 
00130         switch (value)
00131         {
00132         case karto::GridStates_Unknown:
00133           std::cout << "*";
00134           break;
00135         case karto::GridStates_Occupied:
00136           std::cout << "X";
00137           break;
00138         case karto::GridStates_Free:
00139           std::cout << " ";
00140           break;
00141         default:
00142           std::cout << "?";
00143         }
00144       }
00145       std::cout << std::endl;
00146     }
00147     std::cout << std::endl;
00148   }
00149 }
00150 
00151 int main(int /*argc*/, char /***argv*/)
00152 {
00153   // use try/catch to catch karto exceptions that might be thrown by the karto subsystem. 
00155   // Get karto default mapper
00156   karto::Mapper* pMapper = new karto::Mapper();
00157   if (pMapper != NULL)
00158   {
00159     // set solver
00160     SpaSolver* pSolver = new SpaSolver();
00161     pMapper->SetScanSolver(pSolver);
00162 
00163     karto::OccupancyGrid* pOccupancyGrid = NULL;
00164 
00166     // sample code that creates a map from sample device and sample localized range scans
00167 
00168     // clear mapper
00169     pMapper->Reset();
00170 
00171     // create map from created dataset
00172     karto::Dataset* pDataset = CreateMap(pMapper);
00173 
00174     // create occupancy grid at 0.1 resolution and print grid
00175     pOccupancyGrid = CreateOccupancyGrid(pMapper, 0.1);
00176     PrintOccupancyGrid(pOccupancyGrid);
00177     delete pOccupancyGrid;
00178 
00179     // delete solver
00180     delete pSolver;
00181 
00182     // delete mapper
00183     delete pMapper;
00184 
00185     // delete dataset and all allocated devices and device states
00186     delete pDataset;
00187   }
00188 
00189   return 0;
00190 }


open_karto
Author(s):
autogenerated on Thu Jun 6 2019 21:02:56