tutorial1.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 <OpenKarto/Mapper.h>
19 
26 {
27  karto::Dataset* pDataset = new karto::Dataset();
28 
30  // Create a laser range finder device - use SmartPointer to let karto subsystem manage memory.
31  karto::Name name("laser0");
33  pLaserRangeFinder->SetOffsetPose(karto::Pose2(1.0, 0.0, 0.0));
34  pLaserRangeFinder->SetAngularResolution(karto::math::DegreesToRadians(0.5));
35  pLaserRangeFinder->SetRangeThreshold(12.0);
36 
37  pDataset->Add(pLaserRangeFinder);
38 
40  // Create three localized range scans, all using the same range readings, but with different poses.
41  karto::LocalizedRangeScan* pLocalizedRangeScan = NULL;
42 
43  // 1. localized range scan
44 
45  // Create a vector of range readings. Simple example where all the measurements are the same value.
46  std::vector<kt_double> readings;
47  for (int i=0; i<361; i++)
48  {
49  readings.push_back(3.0);
50  }
51 
52  // create localized range scan
53  pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
54  pLocalizedRangeScan->SetOdometricPose(karto::Pose2(0.0, 0.0, 0.0));
55  pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(0.0, 0.0, 0.0));
56 
57  // Add the localized range scan to the mapper
58  pMapper->Process(pLocalizedRangeScan);
59  std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
60 
61  // Add the localized range scan to the dataset
62  pDataset->Add(pLocalizedRangeScan);
63 
64  // 2. localized range scan
65 
66  // create localized range scan
67  pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
68  pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, 0.0, 1.57));
69  pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, 0.0, 1.57));
70 
71  // Add the localized range scan to the mapper
72  pMapper->Process(pLocalizedRangeScan);
73  std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
74 
75  // Add the localized range scan to the dataset
76  pDataset->Add(pLocalizedRangeScan);
77 
78  // 3. localized range scan
79 
80  // create localized range scan
81  pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
82  pLocalizedRangeScan->SetOdometricPose(karto::Pose2(1.0, -1.0, 2.35619449));
83  pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(1.0, -1.0, 2.35619449));
84 
85  // Add the localized range scan to the mapper
86  pMapper->Process(pLocalizedRangeScan);
87  std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
88 
89  // Add the localized range scan to the dataset
90  pDataset->Add(pLocalizedRangeScan);
91 
92  return pDataset;
93 }
94 
99 {
100  std::cout << "Generating map..." << std::endl;
101 
102  // Create a map (occupancy grid) - time it
103  karto::OccupancyGrid* pOccupancyGrid = karto::OccupancyGrid::CreateFromScans(pMapper->GetAllProcessedScans(), resolution);
104 
105  return pOccupancyGrid;
106 }
107 
112 {
113  if (pOccupancyGrid != NULL)
114  {
115  // Output ASCII representation of map
116  kt_int32s width = pOccupancyGrid->GetWidth();
117  kt_int32s height = pOccupancyGrid->GetHeight();
118  karto::Vector2<kt_double> offset = pOccupancyGrid->GetCoordinateConverter()->GetOffset();
119 
120  std::cout << "width = " << width << ", height = " << height << ", scale = " << pOccupancyGrid->GetCoordinateConverter()->GetScale() << ", offset: " << offset.GetX() << ", " << offset.GetY() << std::endl;
121  for (kt_int32s y=height-1; y>=0; y--)
122  {
123  for (kt_int32s x=0; x<width; x++)
124  {
125  // Getting the value at position x,y
126  kt_int8u value = pOccupancyGrid->GetValue(karto::Vector2<kt_int32s>(x, y));
127 
128  switch (value)
129  {
131  std::cout << "*";
132  break;
134  std::cout << "X";
135  break;
137  std::cout << " ";
138  break;
139  default:
140  std::cout << "?";
141  }
142  }
143  std::cout << std::endl;
144  }
145  std::cout << std::endl;
146  }
147 }
148 
149 int main(int /*argc*/, char /***argv*/)
150 {
151  // use try/catch to catch karto exceptions that might be thrown by the karto subsystem.
153  // Get karto default mapper
154  karto::Mapper* pMapper = new karto::Mapper();
155  if (pMapper != NULL)
156  {
157  karto::OccupancyGrid* pOccupancyGrid = NULL;
158 
160  // sample code that creates a map from sample device and sample localized range scans
161 
162  std::cout << "Tutorial 1 ----------------" << std::endl << std::endl;
163 
164  // clear mapper
165  pMapper->Reset();
166 
167  // create map from created dataset
168  karto::Dataset* pDataset = CreateMap(pMapper);
169 
170  // create occupancy grid at 0.1 resolution and print grid
171  pOccupancyGrid = CreateOccupancyGrid(pMapper, 0.1);
172  PrintOccupancyGrid(pOccupancyGrid);
173  delete pOccupancyGrid;
174 
175  // delete mapper
176  delete pMapper;
177 
178  delete pDataset;
179  }
180 
181  return 0;
182 }
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
karto::OccupancyGrid * CreateOccupancyGrid(karto::Mapper *pMapper, kt_double resolution)
Definition: tutorial1.cpp:98
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
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
void SetAngularResolution(kt_double angularResolution)
Definition: Karto.h:3854
void Add(Object *pObject)
Definition: Karto.h:6131
karto::Dataset * CreateMap(karto::Mapper *pMapper)
Definition: tutorial1.cpp:25
void PrintOccupancyGrid(karto::OccupancyGrid *pOccupancyGrid)
Definition: tutorial1.cpp:111
static OccupancyGrid * CreateFromScans(const LocalizedRangeScanVector &rScans, kt_double resolution)
Definition: Karto.h:5644
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
void Reset()
Definition: Mapper.cpp:2196
kt_double GetScale() const
Definition: Karto.h:4262
int main(int, char)
Definition: tutorial1.cpp:149
const Pose2 & GetCorrectedPose() const
Definition: Karto.h:5211


open_karto
Author(s):
autogenerated on Sat Apr 6 2019 02:22:46