-
Notifications
You must be signed in to change notification settings - Fork 80
/
navigation_test.cpp
133 lines (115 loc) · 3.92 KB
/
navigation_test.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
/** navigation_test.cpp
*
* Copyright (c) 2014 Navigine.
*
*/
#include <navigine/navigation-core/navigation_client.h>
#include <navigine/navigation-core/navigation_input.h>
#include <navigine/navigation-core/navigation_output.h>
#include <navigine/navigation-core/navigation_settings.h>
#include <navigine/navigation-core/level.h>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include "../tools/verification/helpers.h"
#include <navigine/navigation-core/geolevel.h>
using namespace navigine::navigation_core;
void PrintUsage(const char* program)
{
printf("Usage:\n"
"\t%s {MAP_FILE} {LOG_FILE} {SETTINGS_JSON} {NAV_BATCH_SIZE}\n",
program);
}
void BuildPdrTrack(const double pdrDistance, const double pdrHeading, double& pdrX, double& pdrY)
{
static double prevPdrDistance = 0.0, prevPdrHeading = 0.0;
if (pdrDistance != prevPdrDistance && pdrHeading != prevPdrHeading)
{
prevPdrDistance = pdrDistance;
prevPdrHeading = pdrHeading;
pdrX += pdrDistance * std::cos(pdrHeading);
pdrY += pdrDistance * std::sin(pdrHeading);
}
}
int main(int argc, char** argv)
{
if (argc < 5)
{
PrintUsage(argv[0]);
return -1;
}
std::string geojsonFile = argv[1];
std::string logFile = argv[2];
std::string settingsFile = argv[3];
const int navBatchSize = std::stoi(argv[4]); // This variable responsible for how many messages we want to parse and pass to navigation in one batch.
int errorCode = 0;
GeoLevels geolevels = ParseGeojson(geojsonFile, errorCode);
NavigationSettings navProps = CreateSettingsFromJson(settingsFile, errorCode);
if (errorCode != 0)
{
std::cerr << "error parsing json file " << errorCode << std::endl;
return errorCode;
}
std::shared_ptr<LevelCollector> levelCollector = createLevelCollector();
for (const auto& geolevel: geolevels)
{
levelCollector->addGeoLevel(*(geolevel.get()));
}
std::shared_ptr<NavigationClient> navClient = createNavigationClient(levelCollector, navProps);
double pdrX = 0.0, pdrY = 0.0;
auto navMessages = GetNavMessages(logFile);
std::vector<std::vector<Measurement>> navBatchMessages;
for(size_t i = 0; i < navMessages.size(); i += navBatchSize)
{
auto last = std::min(navMessages.size(), i + navBatchSize);
navBatchMessages.emplace_back(navMessages.begin() + i, navMessages.begin() + last);
}
long long firstTs = 0;
for (const auto& navBatchInput: navBatchMessages)
{
if (firstTs == 0) {
firstTs = navBatchInput[0].ts;
}
const std::vector<NavigationOutput> navBatchOutput = navClient->navigate(navBatchInput);
const std::vector<NavigationState> navStates = navClient->getStates();
for (std::size_t i = 0; i < navBatchOutput.size(); ++i)
{
if (navBatchOutput[i].status == NavigationStatus::OK)
{
BuildPdrTrack(navStates[i].getStepLen(), navStates[i].getHeading(), pdrX, pdrY);
}
std::string providerStr;
switch (navBatchOutput[i].provider)
{
case Provider::GNSS:
providerStr = "GNSS";
break;
case Provider::INDOOR:
providerStr = "Indoor";
break;
case Provider::FUSED:
providerStr = "Fused";
break;
case Provider::NONE:
providerStr = "none";
break;
default:
providerStr = "unknown provider";
}
std::cout << std::setprecision(10);
std::cout << std::fixed;
long long currentTs = navBatchInput[i].ts;
std::cout << ((currentTs - firstTs) / 1000) << " "
<< navBatchOutput[i].posLatitude << " "
<< navBatchOutput[i].posLongitude << " "
<< navBatchOutput[i].posAltitude << " "
<< pdrX << " "
<< pdrY << " "
<< navBatchOutput[i].posOrientation << " "
<< navBatchOutput[i].posLevel.value.c_str() << " "
<< navBatchOutput[i].status << std::endl;
}
}
return 0;
}