tutorial1.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 <OpenKarto/Mapper.h>
00019 
00025 karto::Dataset* CreateMap(karto::Mapper* pMapper)
00026 {
00027   karto::Dataset* pDataset = new karto::Dataset();
00028 
00030   // Create a laser range finder device - use SmartPointer to let karto subsystem manage memory.
00031   karto::Name name("laser0");
00032   karto::LaserRangeFinder* pLaserRangeFinder = karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, name);
00033   pLaserRangeFinder->SetOffsetPose(karto::Pose2(1.0, 0.0, 0.0));
00034   pLaserRangeFinder->SetAngularResolution(karto::math::DegreesToRadians(0.5));
00035   pLaserRangeFinder->SetRangeThreshold(12.0);
00036 
00037   pDataset->Add(pLaserRangeFinder);
00038 
00040   // Create three localized range scans, all using the same range readings, but with different poses. 
00041   karto::LocalizedRangeScan* pLocalizedRangeScan = NULL;
00042 
00043   // 1. localized range scan
00044 
00045   // Create a vector of range readings. Simple example where all the measurements are the same value.
00046   std::vector<kt_double> readings;
00047   for (int i=0; i<361; i++)
00048   {
00049     readings.push_back(3.0);
00050   }
00051   
00052   // create localized range scan
00053   pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
00054   pLocalizedRangeScan->SetOdometricPose(karto::Pose2(0.0, 0.0, 0.0));
00055   pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(0.0, 0.0, 0.0));
00056 
00057   // Add the localized range scan to the mapper
00058   pMapper->Process(pLocalizedRangeScan);
00059   std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
00060 
00061   // Add the localized range scan to the dataset
00062   pDataset->Add(pLocalizedRangeScan);
00063 
00064   // 2. localized range scan
00065 
00066   // create localized range scan
00067   pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
00068   pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, 0.0, 1.57));
00069   pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, 0.0, 1.57));
00070 
00071   // Add the localized range scan to the mapper
00072   pMapper->Process(pLocalizedRangeScan);
00073   std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
00074 
00075   // Add the localized range scan to the dataset
00076   pDataset->Add(pLocalizedRangeScan);
00077 
00078   // 3. localized range scan
00079 
00080   // create localized range scan
00081   pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
00082   pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, -1.0, 2.35619449));
00083   pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, -1.0, 2.35619449));
00084 
00085   // Add the localized range scan to the mapper
00086   pMapper->Process(pLocalizedRangeScan);
00087   std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
00088 
00089   // Add the localized range scan to the dataset
00090   pDataset->Add(pLocalizedRangeScan);
00091 
00092   return pDataset;
00093 }
00094 
00098 karto::OccupancyGrid* CreateOccupancyGrid(karto::Mapper* pMapper, kt_double resolution)
00099 {
00100   std::cout << "Generating map..." << std::endl;
00101 
00102   // Create a map (occupancy grid) - time it
00103   karto::OccupancyGrid* pOccupancyGrid = karto::OccupancyGrid::CreateFromScans(pMapper->GetAllProcessedScans(), resolution);
00104 
00105   return pOccupancyGrid;
00106 }
00107 
00111 void PrintOccupancyGrid(karto::OccupancyGrid* pOccupancyGrid)
00112 {
00113   if (pOccupancyGrid != NULL)
00114   {
00115     // Output ASCII representation of map
00116     kt_int32s width = pOccupancyGrid->GetWidth();
00117     kt_int32s height = pOccupancyGrid->GetHeight();
00118     karto::Vector2<kt_double> offset = pOccupancyGrid->GetCoordinateConverter()->GetOffset();
00119 
00120     std::cout << "width = " << width << ", height = " << height << ", scale = " << pOccupancyGrid->GetCoordinateConverter()->GetScale() << ", offset: " << offset.GetX() << ", " << offset.GetY() << std::endl;
00121     for (kt_int32s y=height-1; y>=0; y--)
00122     {
00123       for (kt_int32s x=0; x<width; x++) 
00124       {
00125         // Getting the value at position x,y
00126         kt_int8u value = pOccupancyGrid->GetValue(karto::Vector2<kt_int32s>(x, y));
00127 
00128         switch (value)
00129         {
00130         case karto::GridStates_Unknown:
00131           std::cout << "*";
00132           break;
00133         case karto::GridStates_Occupied:
00134           std::cout << "X";
00135           break;
00136         case karto::GridStates_Free:
00137           std::cout << " ";
00138           break;
00139         default:
00140           std::cout << "?";
00141         }
00142       }
00143       std::cout << std::endl;
00144     }
00145     std::cout << std::endl;
00146   }
00147 }
00148 
00149 int main(int /*argc*/, char /***argv*/)
00150 {
00151   // use try/catch to catch karto exceptions that might be thrown by the karto subsystem. 
00153   // Get karto default mapper
00154   karto::Mapper* pMapper = new karto::Mapper();
00155   if (pMapper != NULL)
00156   {
00157     karto::OccupancyGrid* pOccupancyGrid = NULL;
00158 
00160     // sample code that creates a map from sample device and sample localized range scans
00161 
00162     std::cout << "Tutorial 1 ----------------" << std::endl << std::endl;
00163 
00164     // clear mapper
00165     pMapper->Reset();
00166 
00167     // create map from created dataset
00168     karto::Dataset* pDataset = CreateMap(pMapper);
00169 
00170     // create occupancy grid at 0.1 resolution and print grid
00171     pOccupancyGrid = CreateOccupancyGrid(pMapper, 0.1);
00172     PrintOccupancyGrid(pOccupancyGrid);
00173     delete pOccupancyGrid;
00174 
00175     // delete mapper
00176     delete pMapper;
00177 
00178     delete pDataset;
00179   }
00180 
00181   return 0;
00182 }


open_karto
Author(s):
autogenerated on Tue May 2 2017 02:41:15