| Note: This is the first tutorial to use Karto mapping library. This tutorial assumes that you have completed the previous tutorials: ROS tutorials. |
| |
How to use Karto stand-alone (tutorial1.cpp)
Description: This tutorial presents how to create a localized range scanKeywords: navigation, SLAM, mapping
Tutorial Level: INTERMEDIATE
Contents
What does it do ?
This tutorial shows you how:
- to create basic Karto objects (devices and range scans)
- to create a mapper object
- to add the range scans to the mapper and getting back a corrected pose
- to get the occupancy grid and displaying it
- to store Karto objects into a file for post processing or debugging.
Show me the code ?
The entire code for this tutorial is in the samples directory and is appropriately called tutorial1.cpp
The different classes and modules
Laser Range Finder
First, you need to define a device that produces a laser scan (we only support 2D laser scan at the moment).
1 // Create a laser range finder device - use SmartPointer to let karto subsystem manage memory.
2 karto::Name name("laser0");
3 karto::LaserRangeFinder* pLaserRangeFinder = karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, name);
4 pLaserRangeFinder->SetOffsetPose(karto::Pose2(1.0, 0.0, 0.0));
5 pLaserRangeFinder->SetAngularResolution(karto::math::DegreesToRadians(0.5));
6 pLaserRangeFinder->SetRangeThreshold(12.0);
This code defines a custom laser, that is called laser0 and has an angular resolution of 0.5 degrees, 180 degres field of view (since it's not specify this is the default), and has a maximum useable range of 12m.
In addition, this laser is mounted 1m forward in respect to the reference position of the localization system. The localization system could be as simple as an odometric pose, or as complex as a kalman filter that integrates IMU and GPS.
LocalizedRangeScan
Karto has its own laser data structure called a LocalizedRangeScan (a Range Scan with a position attached).
1 // 1. localized range scan
2
3 // Create a vector of range readings. Simple example where all the measurements are the same value.
4 std::vector<kt_double> readings;
5 for (int i=0; i<361; i++)
6 {
7 readings.push_back(3.0);
8 }
9
10 // create localized range scan
11 pLocalizedRangeScan = new karto::LocalizedRangeScan(name, readings);
12 pLocalizedRangeScan->SetOdometricPose(karto::Pose2(0.0, 0.0, 0.0));
13 pLocalizedRangeScan->SetCorrectedPose(karto::Pose2(0.0, 0.0, 0.0));
Calling the SLAM
Once you have defined both a device and a localizedrangescan, you can simply call the mapper to get the corrected position:
1 // Get karto default mapper
2 karto::Mapper* pMapper = new karto::Mapper();
3 ....
4 // Create the localized range scan as described above
5 ....
6 // Add the localized range scan to the mapper
7 pMapper->Process(pLocalizedRangeScan);
8 std::cout << "Pose: " << pLocalizedRangeScan->GetOdometricPose() << " Corrected Pose: " << pLocalizedRangeScan->GetCorrectedPose() << std::endl;
How to get the map ?
Once you have give several scans to the mapper, you can ask the mapper for an occupancy grid:
1 karto::OccupancyGrid* pOccupancyGrid = karto::OccupancyGrid::CreateFromScans(pMapper->GetAllProcessedScans(), resolution);
The resolution is in meter per pixel. The most common value is 0.0254 (or one inch per pixel).
To display the map, here is a very basic (text-based) method:
1 // Output ASCII representation of map
2 kt_int32s width = pOccupancyGrid->GetWidth();
3 kt_int32s height = pOccupancyGrid->GetHeight();
4 karto::Vector2<kt_double> offset = pOccupancyGrid->GetCoordinateConverter()->GetOffset();
5
6 std::cout << "width = " << width << ", height = " << height << ", scale = " << pOccupancyGrid->GetCoordinateConverter()->GetScale() << ", offset: " << offset.GetX() << ", " << offset.GetY() << std::endl;
7 for (kt_int32s y=height-1; y>=0; y--)
8 {
9 for (kt_int32s x=0; x<width; x++)
10 {
11 // Getting the value at position x,y
12 kt_int8u value = pOccupancyGrid->GetValue(karto::Vector2<kt_int32s>(x, y));
13
14 switch (value)
15 {
16 case karto::GridStates_Unknown:
17 std::cout << "*";
18 break;
19 case karto::GridStates_Occupied:
20 std::cout << "X";
21 break;
22 case karto::GridStates_Free:
23 std::cout << " ";
24 break;
25 default:
26 std::cout << "?";
27 }
28 }
29 std::cout << std::endl;
30 }
31 std::cout << std::endl;
32 }
Datasets
In Karto, you can store the logs in our XML format, so you can post process it. It's also extremely useful when you want to send us any bug reports, please send us the dataset that exhibits the problem. It's a lot easier to fix your bug, if we can reproduce it.
To create a dataset:
1 // create an empty dataset
2 karto::Dataset* pDataset = new karto::Dataset();
3 ....
4 // Create the localized range scan as described above
5 ....
6 // Add the localized range scan to the dataset
7 pDataset->Add(pLocalizedRangeScan);
8
9 // write dataset to file
10 WriteDataset(pDataset, "tutorial1.kxd");
What is the correct output of tutorial1 ?
$ ./tutorial1 Tutorial 1 ---------------- Registering sensor: laser0 Pose: 0 0 0 Corrected Pose: 0 0 0 Pose: 1 0 1.57 Corrected Pose: 0.799804 -0.156715 1.48971 Pose: 1 -1 2.35619 Corrected Pose: 0.756943 -1.03575 2.27242 Generating map... width = 69, height = 68, scale = 10, offset: -2.8885, -3 ****************************XXX XXXX******************** *************************XXX XXX****************** ***********************XXX * * * XX**************** **********************XX * *XX************** ********************XX* *X************* *******************X* * * * *XX*********** ******************X* * * *XX********** *****************X* * * XXXXXX * * * *XX********* ****************X* * * XXXXX * * XX******** ***************X* * * * * XXX * * XX******* **************X* * * *XXXXXXXXXXXXXXX XXX * *X******* *************X* * * XXXX XXXX * XXX* * *X****** *************X XXX XX XX *X***** ************X* XX XX XX* * X***** ***********XX XX * XX *XX *X**** ***********XXXX XX XX* X**** **********XXX XX XX X*** **********XX XX XX X*** *********XX XX X * X** ********XX * X *X X** *******XXX XX XX X** ******XX X X* X* ******X*XX X X* *****X* X XXX* ****XX X XX* ****X* *X XX* ***X* *X XX ***X * * XX **XX* * * XX **X * * ** * X **X * X *X * X *X * X *X *X * X * ** X *** X **** X ****** X ******* X ******** X ********* X ********** X X ************ X X ************* X XX * ************** * * X *X *************** *XX *X **************** * X* *X * * ****************** * * X* *XX* ******************* X** **X * ******************** * * X** **X * * * ********************* * * * X*** ***X ********************** * * * *X*** ***X ************************ * * * * X**** ****X * * ************************* * * XX**** ****X* * * ************************** * * * * * *X***** *****X * *************************** * * * *X****** *****XX * **************************** * * *X******* ******X* ****************************** * * *X******** *******X******************************* * * *XX******** *************************************** * *X********** *************************************** * ** *X*********** *************************************** *XX************ *************************************** ** XX************** *************************************** * * XXX*************** *************************************** XXXX***************** *************************************** XXXXX******************** ***************************************XXXXXX************************