IPI/cpp/examples/library/v2x_roundtrip.cpp at main · Croquembouche/IPI · GitHub

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
#include "ipi/v2x/j2735_messages.hpp"
#include "ipi/v2x/uper_codec.hpp"

#include <iomanip>
#include <iostream>
#include <vector>

namespace {

void print_bytes(const std::vector<std::uint8_t>& bytes) {
    std::cout << "[";
    for (std::size_t i = 0; i < bytes.size(); ++i) {
        if (i) std::cout << ' ';
        std::cout << std::hex << std::setw(2) << std::setfill('0')
                  << static_cast<int>(bytes[i]);
    }
    std::cout << std::dec << "]\n";
}

} // namespace

int main() {
    using namespace ipi::j2735;

    try {
        ipi::v2x::UperCodec codec;
        BasicSafetyMessage bsm{};
        bsm.vehicleId = 0xABCDEF01;
        bsm.latitude = 37.3365;
        bsm.longitude = -121.8899;
        bsm.speedMps = 12.5F;
        bsm.headingDeg = 92.0F;
        bsm.accelerationMps2 = 0.5F;
        bsm.laneId = 4;

        auto bsmBytes = codec.encode(bsm);
        std::cout << "BSM send: " << bsm.to_string() << '\n';
        print_bytes(bsmBytes);
        auto bsmRecv = codec.decode_bsm(bsmBytes);
        std::cout << "BSM receive: " << bsmRecv.to_string() << "\n\n";

        PersonalSafetyMessage psm{};
        psm.basicType = PersonalDeviceUserType::Pedestrian;
        psm.secondMarkMs = 12345;
        psm.messageCount = 7;
        psm.temporaryId = 0x10203040;
        psm.latitude = 37.3366;
        psm.longitude = -121.8898;
        psm.horizontalAccuracyM = 1.5F;
        psm.speedMps = 1.4F;
        psm.headingDeg = 90.0F;
        psm.pathPrediction = PersonalPathPrediction{25, 90};
        psm.propulsion = PersonalPropelledInformation{PersonalPropulsionKind::Human, 2};

        auto psmBytes = codec.encode(psm);
        std::cout << "PSM send: " << psm.to_string() << '\n';
        print_bytes(psmBytes);
        auto psmRecv = codec.decode_psm(psmBytes);
        std::cout << "PSM receive: " << psmRecv.to_string() << "\n\n";

        MapMessage map{};
        map.intersectionId = 101;
        map.revision = 3;
        map.name = "Main & 1st";
        map.lanes = {{1, true}, {2, false}, {3, true}};
        auto mapBytes = codec.encode(map);
        std::cout << "MAP send: " << map.to_string() << '\n';
        print_bytes(mapBytes);
        auto mapRecv = codec.decode_map(mapBytes);
        std::cout << "MAP receive: " << mapRecv.to_string() << "\n\n";

        SpatMessage spat{};
        spat.intersectionId = 101;
        spat.timestampMs = 12345678;
        spat.phases = {{8, MovementPhaseState::Proceed, 5000},
                       {9, MovementPhaseState::StopAndRemain, std::nullopt}};
        auto spatBytes = codec.encode(spat);
        std::cout << "SPaT send: " << spat.to_string() << '\n';
        print_bytes(spatBytes);
        auto spatRecv = codec.decode_spat(spatBytes);
        std::cout << "SPaT receive: " << spatRecv.to_string() << "\n\n";

        SignalRequestMessage srm{};
        srm.requestId = 77;
        srm.vehicleId = 0xABCDEF01;
        srm.intersectionId = 101;
        srm.requestedSignalGroup = 8;
        srm.estimatedArrivalMs = 3200;
        srm.priorityLevel = 1;
        auto srmBytes = codec.encode(srm);
        std::cout << "SRM send: " << srm.to_string() << '\n';
        print_bytes(srmBytes);
        auto srmRecv = codec.decode_srm(srmBytes);
        std::cout << "SRM receive: " << srmRecv.to_string() << "\n\n";

        SignalStatusMessage ssm{};
        ssm.requestId = 77;
        ssm.intersectionId = 101;
        ssm.grantedSignalGroup = 8;
        ssm.granted = true;
        ssm.estimatedServedTimeMs = 4000;
        auto ssmBytes = codec.encode(ssm);
        std::cout << "SSM send: " << ssm.to_string() << '\n';
        print_bytes(ssmBytes);
        auto ssmRecv = codec.decode_ssm(ssmBytes);
        std::cout << "SSM receive: " << ssmRecv.to_string() << "\n\n";

    } catch (const std::exception& ex) {
        std::cerr << "Error: " << ex.what() << '\n';
        return 1;
    }

    return 0;
}