Skip to content
This repository has been archived by the owner on Mar 25, 2024. It is now read-only.

Commit

Permalink
* Added Grp10001,2,3,9
Browse files Browse the repository at this point in the history 
Signed-off-by: Christian Berger <[email protected]>
  • Loading branch information
@chrberger
chrberger committed Jan 29, 2019
1 parent 4b4c8ab commit b60a38f
Show file tree
Hide file tree
Showing 4 changed files with 142 additions and 10 deletions.
48 changes: 48 additions & 0 deletions src/opendlv-device-gps-pos.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,54 @@ int32_t main(int32_t argc, char **argv) {
[&od4Session = od4, senderStamp = ID, VERBOSE](opendlv::device::gps::pos::Grp4Data m, const cluon::data::TimeStamp &sampleTime) {
od4Session.send(m, sampleTime, senderStamp);

// Print values on console.
if (VERBOSE) {
std::stringstream buffer;
m.accept([](uint32_t, const std::string &, const std::string &) {},
[&buffer](uint32_t, std::string &&, std::string &&n, auto v) { buffer << n << " = " << v << '\n'; },
[]() {});
std::cout << buffer.str() << " at " << sampleTime.seconds() << "." << sampleTime.microseconds() << std::endl;
}
},
[&od4Session = od4, senderStamp = ID, VERBOSE](opendlv::device::gps::pos::Grp10001Data m, const cluon::data::TimeStamp &sampleTime) {
od4Session.send(m, sampleTime, senderStamp);

// Print values on console.
if (VERBOSE) {
std::stringstream buffer;
m.accept([](uint32_t, const std::string &, const std::string &) {},
[&buffer](uint32_t, std::string &&, std::string &&n, auto v) { buffer << n << " = " << v << '\n'; },
[]() {});
std::cout << buffer.str() << " at " << sampleTime.seconds() << "." << sampleTime.microseconds() << std::endl;
}
},
[&od4Session = od4, senderStamp = ID, VERBOSE](opendlv::device::gps::pos::Grp10002Data m, const cluon::data::TimeStamp &sampleTime) {
od4Session.send(m, sampleTime, senderStamp);

// Print values on console.
if (VERBOSE) {
std::stringstream buffer;
m.accept([](uint32_t, const std::string &, const std::string &) {},
[&buffer](uint32_t, std::string &&, std::string &&n, auto v) { buffer << n << " = " << v << '\n'; },
[]() {});
std::cout << buffer.str() << " at " << sampleTime.seconds() << "." << sampleTime.microseconds() << std::endl;
}
},
[&od4Session = od4, senderStamp = ID, VERBOSE](opendlv::device::gps::pos::Grp10003Data m, const cluon::data::TimeStamp &sampleTime) {
od4Session.send(m, sampleTime, senderStamp);

// Print values on console.
if (VERBOSE) {
std::stringstream buffer;
m.accept([](uint32_t, const std::string &, const std::string &) {},
[&buffer](uint32_t, std::string &&, std::string &&n, auto v) { buffer << n << " = " << v << '\n'; },
[]() {});
std::cout << buffer.str() << " at " << sampleTime.seconds() << "." << sampleTime.microseconds() << std::endl;
}
},
[&od4Session = od4, senderStamp = ID, VERBOSE](opendlv::device::gps::pos::Grp10009Data m, const cluon::data::TimeStamp &sampleTime) {
od4Session.send(m, sampleTime, senderStamp);

// Print values on console.
if (VERBOSE) {
std::stringstream buffer;
Expand Down
52 changes: 46 additions & 6 deletions src/pos-decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,13 +30,21 @@ POSDecoder::POSDecoder(std::function<void(const double &latitude, const double &
std::function<void(opendlv::device::gps::pos::Grp1Data d, const cluon::data::TimeStamp &sampleTime)> delegateGrp1Data,
std::function<void(opendlv::device::gps::pos::Grp2Data d, const cluon::data::TimeStamp &sampleTime)> delegateGrp2Data,
std::function<void(opendlv::device::gps::pos::Grp3Data d, const cluon::data::TimeStamp &sampleTime)> delegateGrp3Data,
std::function<void(opendlv::device::gps::pos::Grp4Data d, const cluon::data::TimeStamp &sampleTime)> delegateGrp4Data) noexcept
std::function<void(opendlv::device::gps::pos::Grp4Data d, const cluon::data::TimeStamp &sampleTime)> delegateGrp4Data,
std::function<void(opendlv::device::gps::pos::Grp10001Data d, const cluon::data::TimeStamp &sampleTime)> d10001,
std::function<void(opendlv::device::gps::pos::Grp10002Data d, const cluon::data::TimeStamp &sampleTime)> d10002,
std::function<void(opendlv::device::gps::pos::Grp10003Data d, const cluon::data::TimeStamp &sampleTime)> d10003,
std::function<void(opendlv::device::gps::pos::Grp10009Data d, const cluon::data::TimeStamp &sampleTime)> d10009) noexcept
: m_delegateLatitudeLongitude(std::move(delegateLatitudeLongitude))
, m_delegateHeading(std::move(delegateHeading))
, m_delegateGrp1Data(std::move(delegateGrp1Data))
, m_delegateGrp2Data(std::move(delegateGrp2Data))
, m_delegateGrp3Data(std::move(delegateGrp3Data))
, m_delegateGrp4Data(std::move(delegateGrp4Data)) {
, m_delegateGrp4Data(std::move(delegateGrp4Data))
, m_delegateGrp10001Data(std::move(d10001))
, m_delegateGrp10002Data(std::move(d10002))
, m_delegateGrp10003Data(std::move(d10003))
, m_delegateGrp10009Data(std::move(d10009)) {
m_dataBuffer = new uint8_t[POSDecoder::BUFFER_SIZE];

// Calculate offset between GPS and UTC.
Expand Down Expand Up @@ -211,22 +219,54 @@ size_t POSDecoder::parseBuffer(uint8_t *buffer, const size_t size, std::chrono::
else if (POSDecoder::GRP10001 == groupNumber) {
// Decode Applanix GRP10001.
opendlv::device::gps::pos::Grp10001Data g10001Data{getGRP10001(b, messageSize)};
(void)g10001Data;

// Use timestamp from GPS if available.
if (1 == g10001Data.timeDistance().time1Type()) {
sampleTimeStamp = extractTimeDistance(g10001Data.timeDistance().time1());
}

if (nullptr != m_delegateGrp10001Data) {
m_delegateGrp10001Data(g10001Data, sampleTimeStamp);
}
}
else if (POSDecoder::GRP10002 == groupNumber) {
// Decode Applanix GRP10002.
opendlv::device::gps::pos::Grp10002Data g10002Data{getGRP10002(b, messageSize)};
(void)g10002Data;

// Use timestamp from GPS if available.
if (1 == g10002Data.timeDistance().time1Type()) {
sampleTimeStamp = extractTimeDistance(g10002Data.timeDistance().time1());
}

if (nullptr != m_delegateGrp10002Data) {
m_delegateGrp10002Data(g10002Data, sampleTimeStamp);
}
}
else if (POSDecoder::GRP10003 == groupNumber) {
// Decode Applanix GRP10003.
opendlv::device::gps::pos::Grp10003Data g10003Data{getGRP10003(b)};
(void)g10003Data;

// Use timestamp from GPS if available.
if (1 == g10003Data.timeDistance().time1Type()) {
sampleTimeStamp = extractTimeDistance(g10003Data.timeDistance().time1());
}

if (nullptr != m_delegateGrp10003Data) {
m_delegateGrp10003Data(g10003Data, sampleTimeStamp);
}
}
else if (POSDecoder::GRP10009 == groupNumber) {
// Decode Applanix GRP10009.
opendlv::device::gps::pos::Grp10009Data g10009Data{getGRP10009(b, messageSize)};
(void)g10009Data;

// Use timestamp from GPS if available.
if (1 == g10009Data.timeDistance().time1Type()) {
sampleTimeStamp = extractTimeDistance(g10009Data.timeDistance().time1());
}

if (nullptr != m_delegateGrp10009Data) {
m_delegateGrp10009Data(g10009Data, sampleTimeStamp);
}
}

// We have consumed the message, move offset accordingly.
Expand Down
10 changes: 9 additions & 1 deletion src/pos-decoder.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,11 @@ class POSDecoder {
std::function<void(opendlv::device::gps::pos::Grp1Data d, const cluon::data::TimeStamp &sampleTime)> d1,
std::function<void(opendlv::device::gps::pos::Grp2Data d, const cluon::data::TimeStamp &sampleTime)> d2,
std::function<void(opendlv::device::gps::pos::Grp3Data d, const cluon::data::TimeStamp &sampleTime)> d3,
std::function<void(opendlv::device::gps::pos::Grp4Data d, const cluon::data::TimeStamp &sampleTime)> d4) noexcept;
std::function<void(opendlv::device::gps::pos::Grp4Data d, const cluon::data::TimeStamp &sampleTime)> d4,
std::function<void(opendlv::device::gps::pos::Grp10001Data d, const cluon::data::TimeStamp &sampleTime)> d10001,
std::function<void(opendlv::device::gps::pos::Grp10002Data d, const cluon::data::TimeStamp &sampleTime)> d10002,
std::function<void(opendlv::device::gps::pos::Grp10003Data d, const cluon::data::TimeStamp &sampleTime)> d10003,
std::function<void(opendlv::device::gps::pos::Grp10009Data d, const cluon::data::TimeStamp &sampleTime)> d10009) noexcept;
~POSDecoder();

public:
Expand All @@ -76,6 +80,10 @@ class POSDecoder {
std::function<void(opendlv::device::gps::pos::Grp2Data d, const cluon::data::TimeStamp &sampleTime)> m_delegateGrp2Data{};
std::function<void(opendlv::device::gps::pos::Grp3Data d, const cluon::data::TimeStamp &sampleTime)> m_delegateGrp3Data{};
std::function<void(opendlv::device::gps::pos::Grp4Data d, const cluon::data::TimeStamp &sampleTime)> m_delegateGrp4Data{};
std::function<void(opendlv::device::gps::pos::Grp10001Data d, const cluon::data::TimeStamp &sampleTime)> m_delegateGrp10001Data{};
std::function<void(opendlv::device::gps::pos::Grp10002Data d, const cluon::data::TimeStamp &sampleTime)> m_delegateGrp10002Data{};
std::function<void(opendlv::device::gps::pos::Grp10003Data d, const cluon::data::TimeStamp &sampleTime)> m_delegateGrp10003Data{};
std::function<void(opendlv::device::gps::pos::Grp10009Data d, const cluon::data::TimeStamp &sampleTime)> m_delegateGrp10009Data{};

private:
opendlv::device::gps::pos::TimeDistance getTimeDistance(std::stringstream &buffer);
Expand Down
42 changes: 39 additions & 3 deletions test/tests-pos-decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12539,6 +12539,10 @@ TEST_CASE("Test POSDecoder with empty payload.") {
bool grp2Called{false};
bool grp3Called{false};
bool grp4Called{false};
bool grp10001Called{false};
bool grp10002Called{false};
bool grp10003Called{false};
bool grp10009Called{false};

const std::string DATA;

Expand All @@ -12548,7 +12552,11 @@ TEST_CASE("Test POSDecoder with empty payload.") {
[&grp1Called](opendlv::device::gps::pos::Grp1Data, const cluon::data::TimeStamp &){ grp1Called = true; },
[&grp2Called](opendlv::device::gps::pos::Grp2Data, const cluon::data::TimeStamp &){ grp2Called = true; },
[&grp3Called](opendlv::device::gps::pos::Grp3Data, const cluon::data::TimeStamp &){ grp3Called = true; },
[&grp4Called](opendlv::device::gps::pos::Grp4Data, const cluon::data::TimeStamp &){ grp4Called = true; }
[&grp4Called](opendlv::device::gps::pos::Grp4Data, const cluon::data::TimeStamp &){ grp4Called = true; },
[&grp10001Called](opendlv::device::gps::pos::Grp10001Data, const cluon::data::TimeStamp &){ grp10001Called = true; },
[&grp10002Called](opendlv::device::gps::pos::Grp10002Data, const cluon::data::TimeStamp &){ grp10002Called = true; },
[&grp10003Called](opendlv::device::gps::pos::Grp10003Data, const cluon::data::TimeStamp &){ grp10003Called = true; },
[&grp10009Called](opendlv::device::gps::pos::Grp10009Data, const cluon::data::TimeStamp &){ grp10009Called = true; }
};
d.decode(DATA, std::chrono::system_clock::time_point());

Expand All @@ -12558,6 +12566,10 @@ TEST_CASE("Test POSDecoder with empty payload.") {
REQUIRE(!grp2Called);
REQUIRE(!grp3Called);
REQUIRE(!grp4Called);
REQUIRE(!grp10001Called);
REQUIRE(!grp10002Called);
REQUIRE(!grp10003Called);
REQUIRE(!grp10009Called);
}

TEST_CASE("Test POSDecoder with faulty payload.") {
Expand All @@ -12567,6 +12579,10 @@ TEST_CASE("Test POSDecoder with faulty payload.") {
bool grp2Called{false};
bool grp3Called{false};
bool grp4Called{false};
bool grp10001Called{false};
bool grp10002Called{false};
bool grp10003Called{false};
bool grp10009Called{false};

const std::string DATA{"Hello World"};

Expand All @@ -12576,7 +12592,11 @@ TEST_CASE("Test POSDecoder with faulty payload.") {
[&grp1Called](opendlv::device::gps::pos::Grp1Data, const cluon::data::TimeStamp &){ grp1Called = true; },
[&grp2Called](opendlv::device::gps::pos::Grp2Data, const cluon::data::TimeStamp &){ grp2Called = true; },
[&grp3Called](opendlv::device::gps::pos::Grp3Data, const cluon::data::TimeStamp &){ grp3Called = true; },
[&grp4Called](opendlv::device::gps::pos::Grp4Data, const cluon::data::TimeStamp &){ grp4Called = true; }
[&grp4Called](opendlv::device::gps::pos::Grp4Data, const cluon::data::TimeStamp &){ grp4Called = true; },
[&grp10001Called](opendlv::device::gps::pos::Grp10001Data, const cluon::data::TimeStamp &){ grp10001Called = true; },
[&grp10002Called](opendlv::device::gps::pos::Grp10002Data, const cluon::data::TimeStamp &){ grp10002Called = true; },
[&grp10003Called](opendlv::device::gps::pos::Grp10003Data, const cluon::data::TimeStamp &){ grp10003Called = true; },
[&grp10009Called](opendlv::device::gps::pos::Grp10009Data, const cluon::data::TimeStamp &){ grp10009Called = true; }
};
d.decode(DATA, std::chrono::system_clock::time_point());

Expand All @@ -12586,13 +12606,21 @@ TEST_CASE("Test POSDecoder with faulty payload.") {
REQUIRE(!grp2Called);
REQUIRE(!grp3Called);
REQUIRE(!grp4Called);
REQUIRE(!grp10001Called);
REQUIRE(!grp10002Called);
REQUIRE(!grp10003Called);
REQUIRE(!grp10009Called);
}

TEST_CASE("Test POSDecoder with sample payload.") {
bool grp1Called{false};
bool grp2Called{false};
bool grp3Called{false};
bool grp4Called{false};
bool grp10001Called{false};
bool grp10002Called{false};
bool grp10003Called{false};
bool grp10009Called{false};
const std::string DATA(reinterpret_cast<const char*>(POS_DUMP.data()), POS_DUMP.size());

std::vector<std::pair<double, double> > listOfGPS{};
Expand All @@ -12608,7 +12636,11 @@ TEST_CASE("Test POSDecoder with sample payload.") {
[&grp1Called](opendlv::device::gps::pos::Grp1Data, const cluon::data::TimeStamp &){ grp1Called = true; },
[&grp2Called](opendlv::device::gps::pos::Grp2Data, const cluon::data::TimeStamp &){ grp2Called = true; },
[&grp3Called](opendlv::device::gps::pos::Grp3Data, const cluon::data::TimeStamp &){ grp3Called = true; },
[&grp4Called](opendlv::device::gps::pos::Grp4Data, const cluon::data::TimeStamp &){ grp4Called = true; }
[&grp4Called](opendlv::device::gps::pos::Grp4Data, const cluon::data::TimeStamp &){ grp4Called = true; },
[&grp10001Called](opendlv::device::gps::pos::Grp10001Data, const cluon::data::TimeStamp &){ grp10001Called = true; },
[&grp10002Called](opendlv::device::gps::pos::Grp10002Data, const cluon::data::TimeStamp &){ grp10002Called = true; },
[&grp10003Called](opendlv::device::gps::pos::Grp10003Data, const cluon::data::TimeStamp &){ grp10003Called = true; },
[&grp10009Called](opendlv::device::gps::pos::Grp10009Data, const cluon::data::TimeStamp &){ grp10009Called = true; }
};

uint32_t overallCounter{0};
Expand Down Expand Up @@ -12648,5 +12680,9 @@ TEST_CASE("Test POSDecoder with sample payload.") {
REQUIRE(grp2Called);
REQUIRE(!grp3Called);
REQUIRE(grp4Called);
REQUIRE(grp10001Called);
REQUIRE(!grp10002Called);
REQUIRE(!grp10003Called);
REQUIRE(!grp10009Called);
}

0 comments on commit b60a38f

Please sign in to comment.