tutorial2.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2010 SRI International
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #include "SpaSolver.h"
19 
20 #include <OpenKarto/Mapper.h>
21 
28 {
29  karto::Dataset* pDataset = new karto::Dataset();
30 
32  // Create a laser range finder device - use SmartPointer to let karto subsystem manage memory.
33  karto::Name name("laser0");
35  pLaserRangeFinder->SetOffsetPose(karto::Pose2(1.0, 0.0, 0.0));
36  pLaserRangeFinder->SetAngularResolution(karto::math::DegreesToRadians(0.5));
37  pLaserRangeFinder->SetRangeThreshold(12.0);
38 
39  pDataset->Add(pLaserRangeFinder);
40 
42  // Create three localized range scans, all using the same range readings, but with different poses.
43  karto::LocalizedRangeScan* pLocalizedRangeScan = NULL;
44 
45  // 1. localized range scan
46 
47  // Create a vector of range readings. Simple example where all the measurements are the same value.
48  std::vector<kt_double> readings;
49  for (int i=0; i<361; i++)
50  {
51  readings.push_back(3.0);
52  }
53 
54  // create localized range scan
55  pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
56  pLocalizedRangeScan->SetOdometricPose(karto::Pose2(0.0, 0.0, 0.0));
57  pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(0.0, 0.0, 0.0));
58 
59  // Add the localized range scan to the mapper
60  pMapper->Process(pLocalizedRangeScan);
61  std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
62 
63  // Add the localized range scan to the dataset
64  pDataset->Add(pLocalizedRangeScan);
65 
66  // 2. localized range scan
67 
68  // create localized range scan
69  pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
70  pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, 0.0, 1.57));
71  pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, 0.0, 1.57));
72 
73  // Add the localized range scan to the mapper
74  pMapper->Process(pLocalizedRangeScan);
75  std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
76 
77  // Add the localized range scan to the dataset
78  pDataset->Add(pLocalizedRangeScan);
79 
80  // 3. localized range scan
81 
82  // create localized range scan
83  pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
84  pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, -1.0, 2.35619449));
85  pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, -1.0, 2.35619449));
86 
87  // Add the localized range scan to the mapper
88  pMapper->Process(pLocalizedRangeScan);
89  std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
90 
91  // Add the localized range scan to the dataset
92  pDataset->Add(pLocalizedRangeScan);
93 
94  return pDataset;
95 }
96 
101 {
102  std::cout << "Generating map..." << std::endl;
103 
104  // Create a map (occupancy grid) - time it
105  karto::OccupancyGrid* pOccupancyGrid = karto::OccupancyGrid::CreateFromScans(pMapper->GetAllProcessedScans(), resolution);
106 
107  return pOccupancyGrid;
108 }
109 
114 {
115  if (pOccupancyGrid != NULL)
116  {
117  // Output ASCII representation of map
118  kt_int32s width = pOccupancyGrid->GetWidth();
119  kt_int32s height = pOccupancyGrid->GetHeight();
120  karto::Vector2<kt_double> offset = pOccupancyGrid->GetCoordinateConverter()->GetOffset();
121 
122  std::cout << "width = " << width << ", height = " << height << ", scale = " << pOccupancyGrid->GetCoordinateConverter()->GetScale() << ", offset: " << offset.GetX() << ", " << offset.GetY() << std::endl;
123  for (kt_int32s y=height-1; y>=0; y--)
124  {
125  for (kt_int32s x=0; x<width; x++)
126  {
127  // Getting the value at position x,y
128  kt_int8u value = pOccupancyGrid->GetValue(karto::Vector2<kt_int32s>(x, y));
129 
130  switch (value)
131  {
133  std::cout << "*";
134  break;
136  std::cout << "X";
137  break;
139  std::cout << " ";
140  break;
141  default:
142  std::cout << "?";
143  }
144  }
145  std::cout << std::endl;
146  }
147  std::cout << std::endl;
148  }
149 }
150 
151 int main(int /*argc*/, char /***argv*/)
152 {
153  // use try/catch to catch karto exceptions that might be thrown by the karto subsystem.
155  // Get karto default mapper
156  karto::Mapper* pMapper = new karto::Mapper();
157  if (pMapper != NULL)
158  {
159  // set solver
160  SpaSolver* pSolver = new SpaSolver();
161  pMapper->SetScanSolver(pSolver);
162 
163  karto::OccupancyGrid* pOccupancyGrid = NULL;
164 
166  // sample code that creates a map from sample device and sample localized range scans
167 
168  // clear mapper
169  pMapper->Reset();
170 
171  // create map from created dataset
172  karto::Dataset* pDataset = CreateMap(pMapper);
173 
174  // create occupancy grid at 0.1 resolution and print grid
175  pOccupancyGrid = CreateOccupancyGrid(pMapper, 0.1);
176  PrintOccupancyGrid(pOccupancyGrid);
177  delete pOccupancyGrid;
178 
179  // delete solver
180  delete pSolver;
181 
182  // delete mapper
183  delete pMapper;
184 
185  // delete dataset and all allocated devices and device states
186  delete pDataset;
187  }
188 
189  return 0;
190 }
kt_int32s GetHeight() const
Definition: Karto.h:4564
const Pose2 & GetOdometricPose() const
Definition: Karto.h:5189
virtual kt_bool Process(LocalizedRangeScan *pScan)
Definition: Mapper.cpp:2215
int32_t kt_int32s
Definition: Types.h:51
void SetRangeThreshold(kt_double rangeThreshold)
Definition: Karto.h:3790
const T & GetY() const
Definition: Karto.h:975
uint8_t kt_int8u
Definition: Types.h:46
kt_double DegreesToRadians(kt_double degrees)
Definition: Math.h:56
karto::OccupancyGrid * CreateOccupancyGrid(karto::Mapper *pMapper, kt_double resolution)
Definition: tutorial2.cpp:100
kt_int32s GetWidth() const
Definition: Karto.h:4555
void SetOffsetPose(const Pose2 &rPose)
Definition: Karto.h:3460
const T & GetX() const
Definition: Karto.h:957
karto::Dataset * CreateMap(karto::Mapper *pMapper)
Definition: tutorial2.cpp:27
void SetAngularResolution(kt_double angularResolution)
Definition: Karto.h:3854
void Add(Object *pObject)
Definition: Karto.h:6131
void PrintOccupancyGrid(karto::OccupancyGrid *pOccupancyGrid)
Definition: tutorial2.cpp:113
static OccupancyGrid * CreateFromScans(const LocalizedRangeScanVector &rScans, kt_double resolution)
Definition: Karto.h:5644
void SetScanSolver(ScanSolver *pSolver)
Definition: Mapper.cpp:2431
virtual const LocalizedRangeScanVector GetAllProcessedScans() const
Definition: Mapper.cpp:2337
void SetOdometricPose(const Pose2 &rPose)
Definition: Karto.h:5198
static LaserRangeFinder * CreateLaserRangeFinder(LaserRangeFinderType type, const Name &rName)
Definition: Karto.h:3961
const Vector2< kt_double > & GetOffset() const
Definition: Karto.h:4280
CoordinateConverter * GetCoordinateConverter() const
Definition: Karto.h:4629
void SetCorrectedPose(const Pose2 &rPose)
Definition: Karto.h:5220
T GetValue(const Vector2< kt_int32s > &rGrid) const
Definition: Karto.h:4619
double kt_double
Definition: Types.h:67
int main(int, char)
Definition: tutorial2.cpp:151
void Reset()
Definition: Mapper.cpp:2196
kt_double GetScale() const
Definition: Karto.h:4262
const Pose2 & GetCorrectedPose() const
Definition: Karto.h:5211


open_karto
Author(s):
autogenerated on Mon Jun 10 2019 14:02:19