Skip to content

Commit 2617c34

Browse files
marschrmarschr
committed
modelV2 output now works
1 parent 3fde57e commit 2617c34

File tree

9 files changed

+407
-25
lines changed

9 files changed

+407
-25
lines changed

‎.vscode/settings.json

+59-1
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,64 @@
44
"qmapbox": "cpp",
55
"qmapboxgl": "cpp",
66
"qthread": "cpp",
7-
"qguiapplication": "cpp"
7+
"qguiapplication": "cpp",
8+
"array": "cpp",
9+
"atomic": "cpp",
10+
"bit": "cpp",
11+
"*.tcc": "cpp",
12+
"cctype": "cpp",
13+
"chrono": "cpp",
14+
"clocale": "cpp",
15+
"cmath": "cpp",
16+
"condition_variable": "cpp",
17+
"cstdarg": "cpp",
18+
"cstddef": "cpp",
19+
"cstdint": "cpp",
20+
"cstdio": "cpp",
21+
"cstdlib": "cpp",
22+
"cstring": "cpp",
23+
"ctime": "cpp",
24+
"cwchar": "cpp",
25+
"cwctype": "cpp",
26+
"deque": "cpp",
27+
"list": "cpp",
28+
"map": "cpp",
29+
"set": "cpp",
30+
"unordered_map": "cpp",
31+
"vector": "cpp",
32+
"exception": "cpp",
33+
"algorithm": "cpp",
34+
"functional": "cpp",
35+
"iterator": "cpp",
36+
"memory": "cpp",
37+
"memory_resource": "cpp",
38+
"numeric": "cpp",
39+
"optional": "cpp",
40+
"random": "cpp",
41+
"ratio": "cpp",
42+
"string": "cpp",
43+
"string_view": "cpp",
44+
"system_error": "cpp",
45+
"tuple": "cpp",
46+
"type_traits": "cpp",
47+
"utility": "cpp",
48+
"fstream": "cpp",
49+
"future": "cpp",
50+
"initializer_list": "cpp",
51+
"iomanip": "cpp",
52+
"iosfwd": "cpp",
53+
"iostream": "cpp",
54+
"istream": "cpp",
55+
"limits": "cpp",
56+
"mutex": "cpp",
57+
"new": "cpp",
58+
"ostream": "cpp",
59+
"sstream": "cpp",
60+
"stdexcept": "cpp",
61+
"streambuf": "cpp",
62+
"thread": "cpp",
63+
"cinttypes": "cpp",
64+
"typeinfo": "cpp",
65+
"variant": "cpp"
866
}
967
}

‎geojson.qrc

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
<RCC>
2+
<qresource prefix="/">
3+
<file>test2.geojson</file>
4+
</qresource>
5+
</RCC>

‎location.cpp

+73-21
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,11 @@
66

77
#include "location.h"
88

9+
#define TRAJECTORY_SIZE 33
10+
911
Location::Location(){
1012
qDebug() << "Location constuct()!";
11-
m_sm = new SubMaster({"controlsState", "gpsLocationExternal"});
13+
m_sm = new SubMaster({"modelV2", "controlsState", "gpsLocationExternal"});
1214
// m_sm = new SubMaster({"thermal","controlsState"});
1315

1416
}
@@ -21,35 +23,85 @@ void Location::handle_message(){
2123
SubMaster sm = *(m_sm);
2224

2325
// GpsLocationData
26+
27+
28+
float edgeX[TRAJECTORY_SIZE];
29+
float edgeY[TRAJECTORY_SIZE];
30+
float edgeZ[TRAJECTORY_SIZE];
31+
32+
long frameIdx;
2433

2534

2635
while (1){
36+
2737
if (sm.update(0) > 0){
28-
qDebug() << "got update";
29-
if (sm.updated("controlsState")) {
38+
39+
// qDebug() << "got update: while";
40+
if (sm.updated("modelV2")) {
41+
// qDebug() << "got update: modelV2";
3042

31-
// cereal::Event::Reader
32-
auto event = sm["controlsState"];
33-
// event.getControlsState();
34-
controls_state = event.getControlsState();
35-
vel = controls_state.getVEgo();
43+
auto event = sm["modelV2"];
44+
model = event.getModelV2();
45+
46+
for (int re_idx = 0; re_idx < 2; re_idx++) {
47+
if (model.getRoadEdgeStds().size() > re_idx) {
48+
road_edge_std[re_idx] = model.getRoadEdgeStds()[re_idx];
49+
50+
// qDebug() << "RE IF";
3651

52+
road_edges = model.getRoadEdges()[re_idx];
53+
for (int i = 0; i < TRAJECTORY_SIZE; i++) {
54+
edgeX[i] = road_edges.getX()[i];
55+
edgeY[i] = road_edges.getY()[i];
56+
edgeZ[i] = road_edges.getZ()[i];
57+
// qDebug() << "re_idx: " << re_idx << "; x: " << road_edges.getX()[i] << "; y: " << road_edges.getY()[i] << "; z: " << road_edges.getZ()[i];
58+
59+
}
60+
} else {
61+
qDebug() << "got update: road_edge_std1.0";
62+
road_edge_std[re_idx] = 1.0;
63+
}
64+
}
3765

38-
// getVEgo()
39-
emit newMsg();
40-
// auto data = sm["radarState"].getRadarState();
41-
// scene.lead_data[0] = data.getLeadOne();
42-
// scene.lead_data[1] = data.getLeadTwo();
43-
}
66+
//csv printer
67+
frameIdx++;
68+
for (int i = 0; i < TRAJECTORY_SIZE; i++) {
69+
qDebug() << frameIdx << "," << edgeX[i] << "," << edgeY[i] << "," << edgeZ[i];
70+
}
4471

45-
if (sm.updated("gpsLocationExternal")){
46-
auto event = sm["gpsLocationExternal"];
47-
gps_state = event.getGpsLocationExternal();
48-
lat = gps_state.getLatitude();
49-
lon = gps_state.getLongitude();
50-
bea = gps_state.getBearing();
51-
qDebug() << "got GPS data!";
72+
73+
5274
}
75+
76+
77+
78+
// if (sm.updated("controlsState")) {
79+
// // qDebug() << "got update: controlsState";
80+
81+
// // cereal::Event::Reader
82+
// auto event = sm["controlsState"];
83+
// // event.getControlsState();
84+
// controls_state = event.getControlsState();
85+
// vel = controls_state.getVEgo();
86+
87+
88+
// // getVEgo()
89+
// emit newMsg();
90+
// // auto data = sm["radarState"].getRadarState();
91+
// // scene.lead_data[0] = data.getLeadOne();
92+
// // scene.lead_data[1] = data.getLeadTwo();
93+
// }
94+
95+
// if (sm.updated("gpsLocationExternal")){
96+
// // qDebug() << "got update: gpsLocationExternal";
97+
98+
// auto event = sm["gpsLocationExternal"];
99+
// gps_state = event.getGpsLocationExternal();
100+
// lat = gps_state.getLatitude();
101+
// lon = gps_state.getLongitude();
102+
// bea = gps_state.getBearing();
103+
// qDebug() << "got GPS data!";
104+
// }
53105
}
54106
}
55107
}

‎location.h

+5
Original file line numberDiff line numberDiff line change
@@ -20,12 +20,17 @@ Q_OBJECT
2020

2121
cereal::ControlsState::Reader controls_state;
2222
cereal::GpsLocationData::Reader gps_state;
23+
24+
cereal::ModelDataV2::Reader model;
2325

2426
float lat;
2527
float lon;
2628
float bea;
2729
float vel;
2830

31+
cereal::ModelDataV2::XYZTData::Reader road_edges;
32+
float road_edge_std[2];
33+
2934
public slots:
3035
void handle_message();
3136

‎mapwindow.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -89,7 +89,7 @@ void MapWindow::keyPressEvent(QKeyEvent *ev)
8989
// Not in all styles, but will work on streets
9090
QString before = "waterway-label";
9191

92-
QFile geojson(":source2.geojson");
92+
QFile geojson(":test2.geojson");
9393
geojson.open(QIODevice::ReadOnly);
9494

9595
// The data source for the route line and markers
@@ -467,7 +467,7 @@ void MapWindow::initializeGL()
467467
connect(m_map.data(), SIGNAL(needsRendering()), this, SLOT(update()));
468468

469469
// Set default location to Helsinki.
470-
m_map->setCoordinateZoom(QMapbox::Coordinate(60.170448, 24.942046), 14);
470+
m_map->setCoordinateZoom(QMapbox::Coordinate(37.721044, -122.472303), 14);
471471

472472
QString styleUrl = qgetenv("MAPBOX_STYLE_URL");
473473
if (styleUrl.isEmpty()) {

0 commit comments

Comments
 (0)