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

Commit

Permalink
* Prepared interface for Applanix
Browse files Browse the repository at this point in the history 
Signed-off-by: Christian Berger <[email protected]>
  • Loading branch information
@chrberger
chrberger committed Feb 24, 2018
1 parent 9f96fd6 commit e7baba1
Show file tree
Hide file tree
Showing 4 changed files with 58 additions and 84 deletions.
57 changes: 33 additions & 24 deletions src/opendlv-device-gps-pos.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,38 +43,47 @@ int32_t main(int32_t argc, char **argv) {
[](auto){}
};

// Interface to OxTS unit providing data in POS format.
const std::string POS_ADDRESS((commandlineArguments.count("pos_ip") == 0) ? "0.0.0.0" : commandlineArguments["pos_ip"]);
const uint32_t POS_PORT(std::stoi(commandlineArguments["pos_port"]));
POSDecoder posDecoder;
cluon::UDPReceiver fromDevice(POS_ADDRESS, POS_PORT,
[&od4Session = od4, &decoder = posDecoder, senderStamp = ID, VERBOSE](std::string &&d, std::string &&/*from*/, std::chrono::system_clock::time_point &&tp) noexcept {
auto retVal = decoder.decode(d);
if (retVal.first) {
cluon::data::TimeStamp sampleTime = cluon::time::convert(tp);

opendlv::proxy::GeodeticWgs84Reading msg1 = retVal.second.first;
od4Session.send(msg1, sampleTime, senderStamp);

opendlv::proxy::GeodeticHeadingReading msg2 = retVal.second.second;
od4Session.send(msg2, sampleTime, senderStamp);
POSDecoder posDecoder{
[&od4Session = od4, senderStamp = ID, VERBOSE](const double &latitude, const double &longitude, const std::chrono::system_clock::time_point &tp) {
opendlv::proxy::GeodeticWgs84Reading m;
m.latitude(latitude).longitude(longitude);
od4Session.send(m, cluon::time::convert(tp), senderStamp);

// Print values on console.
if (VERBOSE) {
std::stringstream buffer;
msg1.accept([](uint32_t, const std::string &, const std::string &) {},
[&buffer](uint32_t, std::string &&, std::string &&n, auto v) { buffer << n << " = " << v << '\n'; },
[]() {});
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() << std::endl;
}
},
[&od4Session = od4, senderStamp = ID, VERBOSE](const float &heading, const std::chrono::system_clock::time_point &tp) {
opendlv::proxy::GeodeticHeadingReading m;
m.northHeading(heading);
od4Session.send(m, cluon::time::convert(tp), senderStamp);

std::stringstream buffer2;
msg2.accept([](uint32_t, const std::string &, const std::string &) {},
[&buffer2](uint32_t, std::string &&, std::string &&n, auto v) { buffer2 << n << " = " << v << '\n'; },
[]() {});
std::cout << buffer2.str() << std::endl;
// 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() << std::endl;
}
}
});
};

// Interface to a Trimble unit providing data in POS format.
const std::string POS_ADDRESS(commandlineArguments["pos_ip"]);
const uint16_t POS_PORT(std::stoi(commandlineArguments["pos_port"]));

cluon::TCPConnection fromDevice{POS_ADDRESS, POS_PORT,
[&decoder = posDecoder](std::string &&d, std::chrono::system_clock::time_point &&tp) noexcept {
decoder.decode(d, std::move(tp));
},
[&argv](){ std::cerr << "[" << argv[0] << "] Connection lost." << std::endl; exit(1); }
};

// Just sleep as this microservice is data driven.
using namespace std::literals::chrono_literals;
Expand Down
62 changes: 9 additions & 53 deletions src/pos-decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,58 +24,14 @@
#include <sstream>
#include <string>

std::pair<bool, std::pair<opendlv::proxy::GeodeticWgs84Reading, opendlv::proxy::GeodeticHeadingReading> > POSDecoder::decode(const std::string &data) noexcept {
bool retVal{false};
opendlv::proxy::GeodeticWgs84Reading gps;
opendlv::proxy::GeodeticHeadingReading heading;

constexpr std::size_t POS_PACKET_LENGTH{72};
constexpr uint8_t POS_FIRST_BYTE{0xE7};
if ( (POS_PACKET_LENGTH == data.size()) && (POS_FIRST_BYTE == static_cast<uint8_t>(data.at(0))) ) {
std::stringstream buffer{data};

// Decode latitude/longitude.
{
double latitude{0.0};
double longitude{0.0};

// Move to where latitude/longitude are encoded.
constexpr uint32_t START_OF_LAT_LON{23};
buffer.seekg(START_OF_LAT_LON);
buffer.read(reinterpret_cast<char*>(&latitude), sizeof(double));
buffer.read(reinterpret_cast<char*>(&longitude), sizeof(double));

gps.latitude(latitude / M_PI * 180.0).longitude(longitude / M_PI * 180.0);
}

// Decode heading.
{
float northHeading{0.0f};

// Move to where heading is encoded.
constexpr uint32_t START_OF_HEADING{52};
buffer.seekg(START_OF_HEADING);

// Extract only three bytes from POS.
std::array<char, 4> tmp{0, 0, 0, 0};
buffer.read(tmp.data(), 3);
uint32_t value{0};
std::memcpy(&value, tmp.data(), 4);
value = le32toh(value);
northHeading = value * 1e-6f;

// Normalize between -M_PI .. M_PI.
while (northHeading < -M_PI) {
northHeading += 2.0f * static_cast<float>(M_PI);
}
while (northHeading > M_PI) {
northHeading -= 2.0f * static_cast<float>(M_PI);
}

heading.northHeading(northHeading);
}
retVal = true;
}
return std::make_pair(retVal, std::make_pair(gps, heading));
POSDecoder::POSDecoder(std::function<void(const double &latitude, const double &longitude, const std::chrono::system_clock::time_point &tp)> delegateLatitudeLongitude,
std::function<void(const float &heading, const std::chrono::system_clock::time_point &tp)> delegateHeading) noexcept
: m_delegateLatitudeLongitude(std::move(delegateLatitudeLongitude))
, m_delegateHeading(std::move(delegateHeading)) {}

void POSDecoder::decode(const std::string &data, std::chrono::system_clock::time_point &&tp) noexcept {
const std::chrono::system_clock::time_point timestamp{tp};
(void)timestamp;
(void)data;
}

13 changes: 10 additions & 3 deletions src/pos-decoder.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,9 @@

#include "opendlv-standard-message-set.hpp"

#include <chrono>
#include <functional>
#include <string>
#include <utility>

class POSDecoder {
private:
Expand All @@ -31,12 +32,18 @@ class POSDecoder {
POSDecoder &operator=(POSDecoder &&) = delete;

public:
POSDecoder() = default;
POSDecoder(std::function<void(const double &latitude, const double &longitude, const std::chrono::system_clock::time_point &tp)> delegateLatitudeLongitude,
std::function<void(const float &heading, const std::chrono::system_clock::time_point &tp)> delegateHeading) noexcept;
~POSDecoder() = default;

public:
std::pair<bool, std::pair<opendlv::proxy::GeodeticWgs84Reading, opendlv::proxy::GeodeticHeadingReading> > decode(const std::string &data) noexcept;
void decode(const std::string &data, std::chrono::system_clock::time_point &&tp) noexcept;

private:
std::function<void(const double &latitude, const double &longitude, const std::chrono::system_clock::time_point &tp)> m_delegateLatitudeLongitude{};
std::function<void(const float &heading, const std::chrono::system_clock::time_point &tp)> m_delegateHeading{};
};


#endif

10 changes: 6 additions & 4 deletions test/tests-pos-decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,14 +27,15 @@
#include <vector>

TEST_CASE("Test POSDecoder with empty payload.") {
const std::string DATA;
// const std::string DATA;

POSDecoder d;
auto retVal = d.decode(DATA);
// POSDecoder d;
// auto retVal = d.decode(DATA);

REQUIRE(!retVal.first);
// REQUIRE(!retVal.first);
}

#if 0
TEST_CASE("Test POSDecoder with faulty payload.") {
const std::string DATA{"Hello World"};

Expand Down Expand Up @@ -73,4 +74,5 @@ TEST_CASE("Test POSDecoder with sample payload.") {

REQUIRE(2.1584727764 == Approx(msg2.northHeading()));
}
#endif

0 comments on commit e7baba1

Please sign in to comment.