-
Notifications
You must be signed in to change notification settings - Fork 1
/
main.cpp
417 lines (370 loc) · 17 KB
/
main.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
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
//
// Created by christophpilz on 15.09.2021.
//
// Authors:
// - Christoph Pilz ([email protected])
//
// reference: https://bytefreaks.net/programming-2/c/asn1c-full-working-example-of-asn-1-in-cc
//
// List of possible improvements
// [] Implement sample implementation of generationDeltaTime
// [] Implement Hardcoded Hex/Base64 from cmd line
// [] Implement sending Hex/Base64 lines from cmd line
// [] Implement full creation of DENM
// [] Implement full creation of CAM
// [] Implement full creation of POI
// [] Implement full creation of SPATEM
// [] Implement full creation of MAPEM
// [] Implement full creation of IVIM
// [] Implement full creation of EV_RSR
// [] Implement full creation of TISTPGTRANSACTION
// [] Implement full creation of SREM
// [] Implement full creation of SSEM
// [] Implement full creation of EVCSN
// [] Implement full creation of SAEM
// [] Implement full creation of RTCMEM
//
#include <chrono>
#include <iostream>
#include <thread>
#include <string>
#include <csignal>
#include "zmq.hpp"
extern "C" {
#include "vcits/cam/CAM.h"
#include "vcits/denm/DENM.h"
#include "vcits/parser/Encoder.h"
}
// have means of clear exit on Ctrl+C
sig_atomic_t stopFlag = 0;
void signalHandler(int) {
stopFlag = 1;
}
// prototypes
long getGenerationDeltaTime();
void create_msg_from_hex_dump(zmq::message_t &msg, std::string dump, size_t size);
void create_V2X_msg(zmq::message_t &msg, int msg_id);
void create_denm(zmq::message_t &msg);
void create_cam(zmq::message_t &msg);
void create_poi(zmq::message_t &msg);
void create_spatem(zmq::message_t &msg);
void create_mapem(zmq::message_t &msg);
void create_ivim(zmq::message_t &msg);
void create_ev_rsr(zmq::message_t &msg);
void create_tistpgtransaction(zmq::message_t &msg);
void create_srem(zmq::message_t &msg);
void create_ssem(zmq::message_t &msg);
void create_evcsn(zmq::message_t &msg);
void create_saem(zmq::message_t &msg);
void create_rtcmem(zmq::message_t &msg);
// helper functions
// Source https://stackoverflow.com/questions/17261798/converting-a-hex-string-to-a-byte-array
int char2int(char input)
{
if(input >= '0' && input <= '9')
return input - '0';
if(input >= 'A' && input <= 'F')
return input - 'A' + 10;
if(input >= 'a' && input <= 'f')
return input - 'a' + 10;
throw std::invalid_argument("Invalid input string");
}
// Source: https://stackoverflow.com/questions/17261798/converting-a-hex-string-to-a-byte-array
// This function assumes src to be a zero terminated sanitized string with
// an even number of [0-9a-f] characters, and target to be sufficiently large
void hex2bin(const char* src, char* target)
{
while(*src && src[1])
{
*(target++) = char2int(*src)*16 + char2int(src[1]);
src += 2;
}
}
// main
int main(int argc, char* argv[]) {
std::cout << "main()" << std::endl;
// have means of clear exit on Ctrl+C
signal(SIGINT, &signalHandler);
// variables
int ret = 0;
zmq::context_t zmq_context(1);
zmq::socket_t zmq_socket_out(zmq_context, ZMQ_PUB);
zmq::message_t envelope;
zmq::message_t msg;
std::string zmq_connect_string;
std::string zmq_envelope;
int msg_id = 0;
int frequency_hz = 0;
int cycle_count = 0;
std::string dump = "";
size_t size = 0;
if(argc == 5) {
zmq_connect_string.append("tcp://");
zmq_connect_string.append(argv[1]);
zmq_connect_string.append(":");
zmq_connect_string.append(argv[2]);
msg_id = atoi(argv[3]);
frequency_hz = atoi(argv[4]);
} else {
std::cout << "Usage: " << argv[0] << " <ip> <port> <msg_id> <frequency (1/10Hz)>" << std::endl;
std::cout << "Message Types:" << std::endl
<< "ItsPduHeader__messageID_denm = 1" << std::endl
<< "ItsPduHeader__messageID_cam = 2" << std::endl
<< "ItsPduHeader__messageID_poi = 3" << std::endl
<< "ItsPduHeader__messageID_spatem = 4" << std::endl
<< "ItsPduHeader__messageID_mapem = 5" << std::endl
<< "ItsPduHeader__messageID_ivim = 6" << std::endl
<< "ItsPduHeader__messageID_ev_rsr = 7" << std::endl
<< "ItsPduHeader__messageID_tistpgtransaction = 8" << std::endl
<< "ItsPduHeader__messageID_srem = 9" << std::endl
<< "ItsPduHeader__messageID_ssem = 10" << std::endl
<< "ItsPduHeader__messageID_evcsn = 11" << std::endl
<< "ItsPduHeader__messageID_saem = 12" << std::endl
<< "ItsPduHeader__messageID_rtcmem = 13" << std::endl;
std::cout << std::endl;
std::cout << "--- --- General Tipps --- ---" << std::endl;
std::cout << "1. Use CLion or a similar IDe -> it helps stepping through the C type structs" << std::endl;
std::cout << "2. Read carefully what is OPTIONAL -> sometimes it is still required, because you have to set the present flag to 0" << std::endl;
std::cout << "3. Read carefully what are the value ranges! Else the message won't parse" << std::endl;
std::cout << "4. Some choice objects still require set values -> may fail on encoding" << std::endl;
std::cout << "5. You may also use the variables dump&size for hardcoded stuff" << std::endl;
return 0;
}
// connect to ZMQ
std::cout << "main(): " << "connect ZMQ (" << zmq_connect_string << ")" << std::endl;
zmq_socket_out.connect(zmq_connect_string);
// sending config
zmq_envelope = "v2x.";
std::cout << std::endl;
std::cout << "main(): " << "main loop" << std::endl;
while(!stopFlag) {
// create v2x_msg
create_V2X_msg(msg, msg_id);
// send v2x_msg
if(msg.size() > 0){
// send envelope for message
envelope = zmq::message_t(zmq_envelope);
zmq_socket_out.send(envelope, zmq::send_flags::sndmore);
// ZMQ will swap the msg for an empty message
zmq_socket_out.send(msg, zmq::send_flags::none);
std::cout << "main(): " << "sent msg #" << cycle_count << std::endl;
cycle_count++;
}
else {
std::cout << "main(): " << "no message created for msg_id: " << msg_id << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000/frequency_hz));
}
std::cout << "done" << std::endl;
return ret;
}
// helpers
long getGenerationDeltaTime(){
// ETSI EN 302 637-2 V1.3.1 (Annex B3)
// - Time corresponding to the time of the reference position in the CAM, considered as time of the CAM generation.
// -> meaning the timestamp of the situation, i.e., the exact time matching the GPS position, when the CAM was generated
// -> in other words: the timestamp matching the GPS position
// - calculation: generationDeltaTime = TimestampIts mod 65 536
// -> TimestampIts represents an integer value in milliseconds since 2004-01-01T00:00:00:000Z as defined in ETSI TS 102 894-2 [2].
// ... TimestampIts = zulu_time_now - 2004-01-01T00:00:00:000Z
std::cout << "getGenerationDeltaTime(): not yet implemented - read comments in code" << std::endl;
return 0;
}
void create_msg_from_hex_dump(zmq::message_t &msg, std::string dump, size_t size) {
// variables
assert(size = dump.size()/2); // just to be sure its hex encoded properly
void *final_buffer = nullptr;
ssize_t final_size = 0;
// buffer
final_size = size;
final_buffer = (char*) malloc(final_size);
hex2bin(dump.c_str(), (char*) final_buffer);
if(final_size > 0){
std::cout << "MSG created successfully with size " << final_size << std::endl;
msg = zmq::message_t(final_buffer, final_size);
free(final_buffer);
} else {
std::cout << std::endl;
std::cout << "MSG creation failed - you should probably stop the program" << std::endl;
}
}
void create_V2X_msg(zmq::message_t &msg, int msg_id) {
std::cout << std::endl;
std::cout << "*** *** V2X Message Creation *** ***" << std::endl;
switch (msg_id) {
case ItsPduHeader__messageID_denm:
create_denm(msg);
break;
case ItsPduHeader__messageID_cam:
create_cam(msg);
break;
case ItsPduHeader__messageID_poi:
create_poi(msg);
break;
case ItsPduHeader__messageID_spatem:
create_spatem(msg);
break;
case ItsPduHeader__messageID_mapem:
create_mapem(msg);
break;
case ItsPduHeader__messageID_ivim:
create_ivim(msg);
break;
case ItsPduHeader__messageID_ev_rsr:
create_ev_rsr(msg);
break;
case ItsPduHeader__messageID_tistpgtransaction:
create_tistpgtransaction(msg);
break;
case ItsPduHeader__messageID_srem:
create_srem(msg);
break;
case ItsPduHeader__messageID_ssem:
create_ssem(msg);
break;
case ItsPduHeader__messageID_evcsn:
create_evcsn(msg);
break;
case ItsPduHeader__messageID_saem:
create_saem(msg);
break;
case ItsPduHeader__messageID_rtcmem:
create_rtcmem(msg);
break;
default:
std::cout << "Message not supported by this demo" << std::endl;
break;
}
}
// dedicated creating functions
void create_denm(zmq::message_t &msg){
std::cout << "--- --- --- DENM --- --- ---" << std::endl;
// variables
DENM_t denm;
long generationDeltaTime = getGenerationDeltaTime();
void *final_denm_buffer = nullptr;
ssize_t final_denm_size = 0;
// reset data structure
memset((void *)&denm, 0, sizeof(denm));
// set header
denm.header.protocolVersion = 2; // ?
denm.header.messageID = 1;
denm.header.stationID = 1111; // ?
if(final_denm_size > 0){
std::cout << "DENM created successfully with size " << final_denm_size << std::endl;
msg = zmq::message_t(final_denm_buffer, final_denm_size);
free(final_denm_buffer);
} else {
std::cout << std::endl;
std::cout << "DENM creation failed - you should probably stop the program" << std::endl;
}
}
void create_cam(zmq::message_t &msg){
std::cout << "--- --- --- CAM --- --- ---" << std::endl;
// variables
CAM_t cam;
long generationDeltaTime = getGenerationDeltaTime();
void *final_cam_buffer = nullptr;
ssize_t final_cam_size = 0;
// reset data structure
memset((void *)&cam, 0, sizeof(cam));
// set header
cam.header.protocolVersion = 2; // V2 is most recent CDD header
cam.header.messageID = 2; // CAM
cam.header.stationID = 1337;
cam.cam.generationDeltaTime = generationDeltaTime; // TimestampIts mod 65 536; TimestampIts = integer value in milliseconds since 2004-01-01T00:00:00:000Z as defined in ETSI TS 102 894-2
cam.cam.camParameters.basicContainer.stationType = StationType_unknown; // StationType enum -> important: Optional parameters get mandatory, depending on station type
cam.cam.camParameters.basicContainer.referencePosition.latitude = Latitude_unavailable; // enum Latitude -> Latitude_oneMicrodegreeNorth = 10
cam.cam.camParameters.basicContainer.referencePosition.longitude = Longitude_unavailable; // enum Longitude -> Longitude_oneMicrodegreeEast = 10
cam.cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
cam.cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
cam.cam.camParameters.basicContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
cam.cam.camParameters.basicContainer.referencePosition.altitude.altitudeValue = AltitudeValue_unavailable;
cam.cam.camParameters.basicContainer.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable;
cam.cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
// IMPORTANT! present can not just be HighFrequencyContainer_PR_NOTHING, because encoding will fail
// -> there is a bug in the ASN.1, which makes the basicVehicleContainerHighFrequency needed
// -> rsuContainerHighFrequency has a subvalue that is a pointer. it is thus not needed to be implemented
// -> the highFrequencyContainer has the following default parameters that will cause an "encoding failed -> check if values are out of range" error
cam.cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.heading.headingConfidence = HeadingConfidence_unavailable;
cam.cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.speed.speedConfidence = SpeedConfidence_unavailable;
cam.cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleLength.vehicleLengthValue = VehicleLengthValue_unavailable;
cam.cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.vehicleWidth = VehicleWidth_unavailable;
// OPTIONALs are all set to zero with memset earlier
// cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.accelerationControl
// cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.lanePosition
// cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.steeringWheelAngle
// cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.lateralAcceleration
// cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.verticalAcceleration
// cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.performanceClass
// cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency.cenDsrcTollingZone
// HighFrequencyContainer could also be HighFrequencyContainer_PR_rsuContainerHighFrequency
// -> not implemented as example
cam.cam.camParameters.lowFrequencyContainer = nullptr; // OPTIONAL not implemented in demo
cam.cam.camParameters.specialVehicleContainer = nullptr; // OPTIONAL not implemented in demo
Encoder::validate_constraints(&asn_DEF_CAM, &cam);
final_cam_size = Encoder::encode(&asn_DEF_CAM, nullptr, &cam, &final_cam_buffer);
if(final_cam_size > 0){
std::cout << "CAM created successfully with size " << final_cam_size << std::endl;
msg = zmq::message_t(final_cam_buffer, final_cam_size);
free(final_cam_buffer);
} else {
std::cout << std::endl;
std::cout << "CAM creation failed - you should probably stop the program" << std::endl;
}
}
void create_poi(zmq::message_t &msg){
std::cout << "--- --- --- POI --- --- ---" << std::endl;
if(msg.size() == 0)
std::cout << "create_poi(): " << "not yet implemented" << std::endl;
}
void create_spatem(zmq::message_t &msg){
std::cout << "--- --- --- SPATEM --- --- ---" << std::endl;
if(msg.size() == 0)
std::cout << "create_spatem(): " << "not yet implemented" << std::endl;
}
void create_mapem(zmq::message_t &msg){
std::cout << "--- --- --- MAPEM --- --- ---" << std::endl;
if(msg.size() == 0)
std::cout << "create_mapem(): " << "not yet implemented" << std::endl;
}
void create_ivim(zmq::message_t &msg){
std::cout << "--- --- --- IVIM --- --- ---" << std::endl;
if(msg.size() == 0)
std::cout << "create_ivim(): " << "not yet implemented" << std::endl;
}
void create_ev_rsr(zmq::message_t &msg){
std::cout << "--- --- --- EV RSR --- --- ---" << std::endl;
if(msg.size() == 0)
std::cout << "create_ev_rsr(): " << "not yet implemented" << std::endl;
}
void create_tistpgtransaction(zmq::message_t &msg){
std::cout << "--- --- --- TISTPGTRANSACTION --- --- ---" << std::endl;
if(msg.size() == 0)
std::cout << "create_tistpgtransaction(): " << "not yet implemented" << std::endl;
}
void create_srem(zmq::message_t &msg){
std::cout << "--- --- --- SREM --- --- ---" << std::endl;
if(msg.size() == 0)
std::cout << "create_srem(): " << "not yet implemented" << std::endl;
}
void create_ssem(zmq::message_t &msg){
std::cout << "--- --- --- SSEM --- --- ---" << std::endl;
if(msg.size() == 0)
std::cout << "create_ssem(): " << "not yet implemented" << std::endl;
}
void create_evcsn(zmq::message_t &msg){
std::cout << "--- --- --- EVCSN --- --- ---" << std::endl;
if(msg.size() == 0)
std::cout << "create_evcsn(): " << "not yet implemented" << std::endl;
}
void create_saem(zmq::message_t &msg){
std::cout << "--- --- --- SAEM --- --- ---" << std::endl;
if(msg.size() == 0)
std::cout << "create_saem(): " << "not yet implemented" << std::endl;
}
void create_rtcmem(zmq::message_t &msg){
std::cout << "--- --- --- RTCMEM --- --- ---" << std::endl;
if(msg.size() == 0)
std::cout << "create_rtcmem(): " << "not yet implemented" << std::endl;
}