April 15, 2013 - By Charles Calkins, OCI Senior Software Engineer
Middleware News Brief (MNB) features news and technical information about Open Source middleware technologies.
INTRODUCTION
In part I [1] of this article, we used Boost libraries under both Windows and Linux to communicate with a GPS receiver connected to a serial port. Boost Asio [2] was used for asynchronous I/O to read data from the receiver in a platform-independent manner, with other Boost libraries in supporting roles. This article continues the interaction with the GPS receiver by parsing the GPS data, and publishing it via OpenDDS [3].
The source code of the applications presented here is available in the code archive that accompanies this article. This code has been tested under 64-bit Windows 7 with Visual Studio 2010 and Boost 1.51, as well as under 64-bit Ubuntu 12.10 with gcc 4.7.2 and Boost 1.49. OpenDDS 3.4 was used under both operating systems. It also requires MPC [4] for its build system, which is included with OpenDDS.
PARSE GPS SENTENCES
Now that GPS data can be obtained from the serial port, as shown in part I, the next step is to parse the data. The NMEA 0183 specification [5] defines a set of sentences that are produced by a GPS (or GLONASS [6]) device, sent at 4800 baud, 8 data bits, no parity, 1 stop bit, and with no handshaking. Although the official specification is not freely available, the protocol has been documented in a number of places, such as [7] and [8]. For more information on the GPS system itself and the data that is obtained, please see [9].
A sentence from a GPS receiver consists of a comma-delimited list of fields. The first field begins with a dollar sign, followed by the characters GP (a talker ID of GP means that the sentence was produced from a GPS receiver — a GLONASS device would use GL), followed by three letters indicating the type of sentence. The data provided in the sentence immediately follows. After the comma-delimited fields, sentences may also contain a checksum expressed as two hexidecimal digits, preceded by an asterisk. The checksum is the 8-bit exclusive OR of the ASCII values of all of the characters between the $ and *. All sentences, whether or not they contain a checksum, end with a CR-LF pair.
| $ | GP | xxx | , | data field | , | data field | , | data field | ... | *nn | CR LF |
The Pharos GPS-360 that is used in these articles produces the following sentence types:
- GGA - 3D position and accuracy data
- GLL - Geographic latitude and longitude
- RMC - NMEA's own "Recommended Minimum" GPS data
- GSV - GPS satellites in view
- GSA - GPS dilution of precision and active satellites
We will create a new library, GPSLib, to contain the code that parses these sentences.
Once again, we start with needed Boost libraries (here, ones we have encountered before in part I), and another generated file that contains export macros.
// GPSLib/GPSSentenceDecoder.h
#include <vector>
#include <boost/asio.hpp>
#include <boost/function.hpp>
#include <boost/date_time.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <boost/date_time/gregorian/gregorian.hpp>
#include "GPSLib_Export.h"We need a structure to store satellite info as obtained from the GSV sentence, which we will need later.
namespace GPSLib {
class SatelliteInfo {
public:
int _prn, _elevation, _azimuth, _snr;
SatelliteInfo(int prn, int elevation, int azimuth, int snr) :
_prn(prn), _elevation(elevation), _azimuth(azimuth),
_snr(snr) {}
};The GPSSentenceDecoder class consists of a buffer to store incoming bytes, a mutex to protect it, and a pointer to a boost::asio::strand to be used for message decoding. Work posted directly to an io_service object may be executed in any order, but work posted through a strand is guaranteed to be executed in the order it was posted. In our case, this guarantees that messages are decoded and processed in the same order that they arrive by invocations of the Decode() method. Lastly, a number of callback functions are provided — one for each of the message types that are decoded, plus one, OnInvalidSentence(), that is called on a decode error.
class GPSLib_Export GPSSentenceDecoder : public
boost::enable_shared_from_this<GPSSentenceDecoder> {
std::string _buffer;
boost::mutex _bufferMutex;
boost::shared_ptr<boost::asio::strand> _decodeStrand;
void Decode(boost::asio::io_service &ios,
const std::string &s);
public:
void AddBytes(boost::asio::io_service &ios,
const std::vector<unsigned char> &buffer,
size_t bufferSize = -1);
boost::function<void (boost::asio::io_service &,
const std::string &)> OnInvalidSentence;
boost::function<void (boost::asio::io_service &,
boost::posix_time::time_duration, double, double, int,
int, double, double)> OnGGA;
boost::function<void (boost::asio::io_service &,
boost::posix_time::time_duration, double, double,
const std::string &)> OnGLL;
boost::function<void (boost::asio::io_service &,
boost::posix_time::time_duration, double, double, double,
double, boost::gregorian::date,
const std::string &)> OnRMC;
boost::function<void (boost::asio::io_service &,
int, int, int, const std::vector<SatelliteInfo> &)> OnGSV;
boost::function<void (boost::asio::io_service &,
const std::string &, int, const std::vector<int> &,
double, double, double)> OnGSA;
};
}The implementation of GPSSentenceDecoder, as always, begins with the inclusion of various header files. For the message decoding, we will use both the Boost Tokenizer [10] and Regex [11] libraries. Tokenizer is useful for splitting strings based on particular characters, such as commas, while Regex is used for interpreting formatted strings. Boost.Bind was described in Part I.
// GPSLib/GPSSentenceDecoder.cpp
#include "GPSSentenceDecoder.h"
#include "Util.h"
#include <cctype>
#include <boost/bind.hpp>
#include <boost/tokenizer.hpp>
#include <boost/regex.hpp>The AddBytes() method is used to accumulate bytes until an entire sentence is read. Serial port reads may only contain a few bytes at a time, so bytes need to be stored until a full sentence exists in the buffer. As the protocol contains only non-whitespace ASCII data, only printable characters are added to the buffer, in case line noise or other problems cause obviously corrupted data to be received.
As sentences end with a CR-LF pair, once a CR-LF pair is found, all bytes from the beginning of the buffer up to and including the CR-LF are extracted. A call to the Decode() method, with the sentence as an argument, is posted through the strand. It is possible that some sentences may decode more quickly than others, so this preserves message order. Also, posting the decode operation allows the thread that called AddBytes() to return to its normal processing without needing to wait for one or more messages to be decoded.
void GPSLib::GPSSentenceDecoder::AddBytes(boost::asio::io_service &ios,
const std::vector<unsigned char> &bufferToAdd, size_t bufferSize) {
boost::mutex::scoped_lock lock(_bufferMutex);
if (_decodeStrand == 0)
_decodeStrand = boost::shared_ptr<boost::asio::strand>(
new boost::asio::strand(ios));
// pass bufferSize in case buffer has size greater than the amount
// of meaningful data in it
std::for_each(bufferToAdd.begin(), (bufferSize == -1) ?
bufferToAdd.end() : (bufferToAdd.begin() + bufferSize),
[this](const unsigned char &c) {
// a sentence is ASCII plus CR-LF, ignore anything out of range
if (std::isprint(c) || (c=='\r') || (c == '\n'))
_buffer += c;
});
// tokenize on \r\n which ends a string
size_t pos;
while ( (pos = _buffer.find("\r\n")) != std::string::npos) {
const std::string s(_buffer.substr(0, pos+2)); // +2 for \r\n
_buffer.erase(0, pos+2);
_decodeStrand->post(boost::bind(&GPSSentenceDecoder::Decode,
shared_from_this(), boost::ref(ios), s));
}
}We need a few functions to assist with the decoding. These seemed utilitarian enough to be included in an anonymous namespace, rather than being methods of GPSSentenceDecoder, but tastes may vary. The first of these methods is TokenizeSentence, which uses the Boost Tokenizer library to split the GPS sentence into its component parts. First, the beginning and end of the sentence is found, and if either are missing, or are too close together to contain legitimate data, false is returned to indicate a parsing error.
namespace {
bool TokenizeSentence(const std::string &s,
std::vector<std::string> &tokens) {
tokens.clear();
const std::string::size_type dollarPos = s.find("$");
const std::string::size_type CRLFPos = s.find("\r\n");
// fail if can't find sentence boundary, or sentence too short
if ((dollarPos == std::string::npos) ||
(CRLFPos == std::string::npos) ||
(CRLFPos - dollarPos < 5)) {
return false;
}Next, the sentence is examined to see if it has a checksum, and, if it does, test if the checksum is valid. If the calculated checksum does not match the one supplied in the sentence, false is returned to indicate failure.
std::string::size_type textEndPos = CRLFPos;
const std::string::size_type starPos = s.rfind("*");
if (starPos != std::string::npos) {
// have a checksum, so validate it
// checksum is just prior to the CRLF
const char expected_checksum_bytes[] = {
s[starPos+1], s[starPos+2], 0 };
const unsigned char expected_checksum =
boost::lexical_cast<GPSLib::byte_from_hex>(
expected_checksum_bytes);
// +1, since only chars between $ *
unsigned char calculated_checksum = 0;
std::for_each(s.begin()+dollarPos+1, s.begin()+starPos,
[&calculated_checksum](char c) {
calculated_checksum^=c; });
// fail if checksum mismatch
if (calculated_checksum != expected_checksum)
return false;
textEndPos = starPos;
}Although lexical_cast<> itself does not convert between bases, a converter can be supplied [12].
// GPSLib/Util.h
class byte_from_hex {
unsigned int value;
public:
operator unsigned char() const { return value & 0xFF; }
friend std::istream &operator>>(std::istream &in,
byte_from_hex &out) {
in >> std::hex >> out.value;
return in;
}
};We now split the sentence into pieces by tokenizing on the comma character. The definition of the separator must include boost::keep_empty_tokens, as, by default, empty fields are not preserved. GPS sentences, especially when GPS devices are still initializing and are not yet tracking satellites, can contain empty fields. As decoding sentences relies on the position of fields within the string, not including empty fields causes positions to change and the parse to fail.
We only tokenize the string starting from the dollar sign through the end of the comma-delimited fields — the tokenization range ends at either the character before the asterisk if the sentence contains a checksum, or the character before the CR-LF if it does not.
Once the string is tokenized, the tokens are copied into a buffer to return to the caller, and the function returns true if at least one token was found.
const boost::char_separator<char> sep(",", 0,
boost::keep_empty_tokens);
const boost::tokenizer<boost::char_separator<char>> t(
s.begin()+dollarPos, s.begin()+textEndPos, sep);
std::copy(t.begin(), t.end(), std::back_inserter(tokens));
// parse is good if there is at least one token
return tokens.size()>0;
}As there are multiple sentences which contain elements of similar structure, we can implement decoders for these that can be reused.
The first of these parses time-of-day sentence elements. The parameter to the DecodeTime() function is an iterator into the list of tokens, where dereferencing the iterator yields the string token itself. The Boost Regex library is used to parse the token because a time is represented in a fixed format: 2 digits for the hour, 2 digits for the minute, 2 digits for the second, and a variable (at least, among different GPS receivers) precision of fractional seconds. As some receivers only include time to the second, this field may be optional, but the Pharos GPS-360 seems to always provide three digits to the right of the decimal point.
The regular expression object is created outside of the function as a speed optimization to avoid re-parsing it on each function invocation. boost::regex_match is passed three arguments: the string to parse, an instance of boost::smatch to store successful match results, and the regex pattern to compare against. Upon a successful match, the iterator is incremented, so the next token is ready to be parsed. The boost::smatch object is then consulted for the match results. The 0th match is the entire matched string, followed by the capture groups starting at index 1, so, in our case, the hour is in group 1, minute in group 2, second in group 3, and milliseconds in group 4. boost::lexical_cast<> from the Boost.Lexical_Cast library is used to convert each capture group from its string representation to integer. The Boost Date_Time library is used to represent the time as a composite boost::posix_time::time_duration that is returned to the caller.
const boost::regex hms("(\\d{2})(\\d{2})(\\d{2})(?:(.\\d*))?");
boost::posix_time::time_duration DecodeTime(
std::vector<std::string>::iterator &i) {
boost::smatch m;
if (boost::regex_match(*i, m, hms)) {
i++; // consume the match
const boost::posix_time::hours hr(
boost::lexical_cast<int>(m[1].str()));
const boost::posix_time::minutes min(
boost::lexical_cast<int>(m[2].str()));
const boost::posix_time::seconds sec(
boost::lexical_cast<int>(m[3].str()));
const boost::lexical_cast<int>(
boost::lexical_cast<double>(m[4].str())*1000));
return boost::posix_time::time_duration(hr+min+sec+ms);
}
else
throw std::invalid_argument(*i + " is not hms");
}Functions similar to the above are used to parse latitude and longitude fields, as well as date fields, the implementation of which is not shown here.
double DecodeLatLng(std::vector<std::string>::iterator &i) {
...
}
boost::gregorian::date DecodeDate(
std::vector<std::string>::iterator &i) {
...
}
}The Decode() method is passed a GPS sentence to decode, and calls TokenizeSentence() to validate it and divide it into pieces. If the tokenization fails, and the user has provided an OnInvalidSentence() callback, the callback is invoked with the string that failed the parse as one of its arguments.
void GPSLib::GPSSentenceDecoder::Decode(boost::asio::io_service &ios,
const std::string &s) {
try {
std::vector<std::string> v;
if (!TokenizeSentence(s, v)) {
if (OnInvalidSentence)
OnInvalidSentence(ios, s);
return;
}Decode() then compares the first token to see if it matches a known sentence type, and, if so, to decode that sentence. The first test is for the GGA sentence, which looks like:
$GPGGA,191630.609,3848.2905,N,09018.4239,W,1,06,1.3,132.0,M,-33.7,M,0.0,0000*48If the iterator, initially at the start of the token list, matches $GPGGA, it is incremented., The time, latitude, and longitude are then parsed via the helper functions described above. Next the fix quality, number of satellites used, horizontal dilution, and altitude are converted to numbers from the string representation using the lexical_cast_default<> helper function. If the user has provided an OnGGA() callback function, it is invoked with the decoded parameters.
std::vector<std::string>::iterator i = v.begin();
if (*i == "$GPGGA") {
i++; // consume the $GPGGA token
const boost::posix_time::time_duration time(DecodeTime(i));
const double lat = DecodeLatLng(i);
const double lng = DecodeLatLng(i);
const int quality = lexical_cast_default<int>(*i++, 0);
const int numSatellites =
lexical_cast_default<int>(*i++, 0);
const double horizontalDilution =
lexical_cast_default<double>(*i++, 0);
const double altitude =
lexical_cast_default<double>(*i++, 0);
if (OnGGA)
OnGGA(ios, time, lat, lng, quality, numSatellites,
horizontalDilution, altitude);
return;
}The other sentences are parsed similarly, and if the sentence didn't match any of the choices, OnInvalidSentence(), if provided, is invoked.
if (*i == "$GPGLL") {
...
}
if (*i == "$GPRMC") {
...
}
if (*i == "$GPGSV") {
...
}
if (*i == "$GPGSA") {
...
}
// didn't parse anything
if (OnInvalidSentence)
OnInvalidSentence(ios, s);
} catch (const std::exception &e) {
std::cout << "Error while decoding >" << s << "< : " <<
e.what() << std::endl;
throw;
}
}The lexical_cast_default<> function simply returns a default value if the provided string does not successfully parse, such as when the field is empty.
// GPSLib/Util.h
template<typename Target>
inline Target lexical_cast_default(const std::string &arg,
const Target &def) {
try {
return boost::lexical_cast<Target>(arg);
} catch (boost::bad_lexical_cast &) {
return def;
}
}TEST THE GPS SENTENCE PARSER
With the GPS sentence parser written, it is important to test the parser to ensure that it operates correctly. For this, we can use the Boost Test library [13] with tests implemented in the Tests project.
We will rely on the test framework that can be automatically provided, rather than defining our own, by using the Boost Test macros. The first macro is BOOST_AUTO_TEST_MAIN which causes a main() function and test runner to be compiled into the test executable. Following this macro, we again include a number of headers to Boost libraries that we have seen before, but the Boost Filesystem library [15] makes its first appearance. The Filesystem library includes features such as iteration over directory contents, path construction, and other file operations.
// Tests/GPSSentences.cpp
#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/filesystem.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <fstream>
#include "../GPSLib/GPSSentenceDecoder.h"
#include "../GPSLib/Util.h"A number of tests are included, but two are highlighted here. The first is a test of a particular GGA sentence. The BOOST_AUTO_TEST_CASE macro is used to configure the test case and make it available to the test runner.
BOOST_AUTO_TEST_CASE(GGATest)
{
const std::string s(
"$GPGGA,191630.609,3848.2905,N,09018.4239,W,1,06,1.3,132.0,M,-33.7,M,0.0,0000*48\r\n");
const boost::shared_ptr<GPSLib::GPSSentenceDecoder> d(
new GPSLib::GPSSentenceDecoder);To test the sentence decoding, we create variables, each defaulting to false, representing the invocation of each of the sentence callback functions. A condition of the test is that only the callback function for the GGA sentence should have been called, and no other.
bool onInvalidSentenceCalled = false, onGGACalled = false,
onGLLCalled = false, onRMCCalled = false,
onGSVCalled = false, onGSACalled = false;
d->OnInvalidSentence = [&](boost::asio::io_service &ios,
const std::string &s) {
onInvalidSentenceCalled = true;
};For the GGA sentence, we must ensure that the fields were parsed correctly. Boost Test macros are used for the test assertions. BOOST_REQUIRE_x macros cause the test to fail if the condition is not met. Other macros are available: BOOST_WARN_x logs a warning, and BOOST_CHECK_x logs an error, but in both cases the test continues to run. We desire tests to halt on error, so we use BOOST_REQUIRE_x.
The BOOST_REQUIRE_EQUAL macro enforces equality between expected and actual values, while the BOOST_REQUIRE_CLOSE macro allows the values to differ within a user-specified percent tolerance, which is necessary for the comparison of floating point values.
d->OnGGA = [&](boost::asio::io_service &ios,
boost::posix_time::time_duration time, double latitude, double
longitude, int quality, int numSatellites,
double horizontalDilution, double altitude) {
onGGACalled = true;
BOOST_REQUIRE_EQUAL(
boost::posix_time::duration_from_string("19:16:30.609"),
time);
BOOST_REQUIRE_CLOSE(38 +(48.2905/60.0), latitude, 0.01);
BOOST_REQUIRE_CLOSE(- (90 + (18.4239/60.0)), longitude, 0.01);
BOOST_REQUIRE_EQUAL(1, quality);
BOOST_REQUIRE_EQUAL(6, numSatellites);
BOOST_REQUIRE_CLOSE(1.3, horizontalDilution, 0.01);
BOOST_REQUIRE_CLOSE(132.0, altitude, 0.01);
};After establishing the remaining callbacks, the string is fed to the decoder, and run() is called on the io_service object to perform the decoding.
d->OnGLL = [&](boost::asio::io_service &ios,
boost::posix_time::time_duration time, double latitude,
double longitude, const std::string &validity) {
onGLLCalled = true;
};
d->OnRMC = [&](boost::asio::io_service &ios,
boost::posix_time::time_duration time, double latitude,
double longitude, double speed, double course,
boost::gregorian::date date,
const std::string &validity) {
onRMCCalled = true;
};
d->OnGSV = [&](boost::asio::io_service &ios,
int totalMessages, int messageNumber,
int totalSatellitesInView,
const std::vector<GPSLib::SatelliteInfo> &satelliteInfo) {
onGSVCalled = true;
};
d->OnGSA = [&](boost::asio::io_service &ios,
const std::string &mode, int fix,
const std::vector<int> &satellitesInView,
double pdop, double hdop, double vdop) {
onGSACalled = true;
};
boost::asio::io_service ios;
d->AddBytes(ios, std::vector<unsigned char>(s.begin(), s.end()));
ios.run();BOOST_REQUIRE takes a boolean predicate, and fails the test if the predicate is false. We confirm here that only the GGA decoding function was called, and no other. If these were placed in the body of each function, and no callbacks were called, the test as a whole would still pass as no assertions to fail the test would be executed.
BOOST_REQUIRE(!onInvalidSentenceCalled);
BOOST_REQUIRE(onGGACalled);
BOOST_REQUIRE(!onGLLCalled);
BOOST_REQUIRE(!onRMCCalled);
BOOST_REQUIRE(!onGSVCalled);
BOOST_REQUIRE(!onGSACalled);
}Many other tests are written as the one above, where a particular sentence is provided and the test ensures that the sentence is decoded correctly. The AllSentencesTest is closer to a functional test than a unit test. The contents of the pre-recorded GPS sentence archive are played back, and the test confirms that no invalid sentences are identified. This ensures that out of a large sample of sentences, no sentences were recorded that are not covered by the GPS sentence decoder.
Only the OnInvalidSentence() callback is needed, and in addition to setting a boolean indicating an invalid sentence was found, it is also displayed to the console so the developer can update the parser accordingly.
BOOST_AUTO_TEST_CASE(AllSentencesTest)
{
const boost::shared_ptr<GPSLib::GPSSentenceDecoder> d(
new GPSLib::GPSSentenceDecoder);
bool onInvalidSentenceCalled = false;
d->OnInvalidSentence = [&](boost::asio::io_service &ios,
const std::string &s) {
std::cerr << "Invalid: >" << s << "<" << std::endl;
onInvalidSentenceCalled = true;
};We wish to be able to run the test application from any current working directory, but to do so, the test must be able to locate the GPS data archive file. The data file is located in the Tests directory, while the test executable is located in either the Output/Debug or Output/Release directories, depending on how it was built.
The Boost Filesystem library is used to obtain the full path to the test runner executable. The relative path to the executable is in argv[0], which is obtained from the Boost test framework because command-line arguments are not directly available to individual test functions.
char **argv =
boost::unit_test::framework::master_test_suite().argv;The function boost::filesystem::system_complete() is used to obtain the complete path from the contents of argv[0]. The Tests directory is two levels above the location of the test runner executable, so parent_path() is called three times to find the root directory (once to strip the filename, again to remove Debug or Release, and once more to remove Output). The /= operator is overloaded to perform path concatenation using the correct path separator for the operating system. The Tests directory is appended, and then the name of the archive file.
// EXE is <root>/Output/Debug/<filename> or
// <root>/Output/Release/<filename>
// get the parent path 3 times to get <root>
// add on the path to the data
boost::filesystem::path dataPath(
boost::filesystem::system_complete(argv[0])
.parent_path().parent_path().parent_path());
dataPath /= "Tests";
dataPath /= "gps_2013-01-15_0106";The archive is then read, with the data passed to the decoder.
std::ifstream in(dataPath.c_str());
boost::archive::text_iarchive ia(in);
uint64_t us;
boost::asio::io_service ios;
while (true) {
try {
std::vector<unsigned char> buffer;
ia >> us >> buffer;
d->AddBytes(ios, buffer);
} catch (const std::exception &) {
// end of archive - ignore exception
break;
}
}After the decoding process is complete, the test ensures that no invalid sentences were identified.
ios.run();
BOOST_REQUIRE(!onInvalidSentenceCalled);
}PUBLISH THE DATA WITH OPENDDS
With the sentences decoded, we can publish the data as OpenDDS data samples. The data sample structures are expressed in IDL, with the code generated from the IDL compiled in the GPSDDSLib library.
All data samples are identified by the sensor that published them. The date and time the sample was acquired, if available, is also given, followed by the sensor values. DCPS_DATA_TYPE identifies that this struct is to be used as an OpenDDS data sample, and DCPS_DATA_KEY specifies that the sensor ID is to be used for instance determination.
The definition of the PositionData sample type is as follows, with AltitudeData and CourseData defined in a similar manner.
// GPSLib/GPS.idl
#include "tao/LongSeq.pidl"
module GPS {
#pragma DCPS_DATA_TYPE "GPS::PositionData"
#pragma DCPS_DATA_KEY "GPS::PositionData sensor_id"
struct PositionData {
string sensor_id;
unsigned long long date;
double latitude;
double longitude;
};
#pragma DCPS_DATA_TYPE "GPS::AltitudeData"
#pragma DCPS_DATA_KEY "GPS::AltitudeData sensor_id"
struct AltitudeData {
string sensor_id;
unsigned long long date;
double altitude;
};
#pragma DCPS_DATA_TYPE "GPS::CourseData"
#pragma DCPS_DATA_KEY "GPS::CourseData sensor_id"
struct CourseData {
string sensor_id;
unsigned long long date;
double speed;
double course;
};The SatelliteInfoData structure contains a list of satellites and their attributes, such as a satellite identifier, its position, and the signal-to-noise ratio of the GPS signal. This is represented as a sequence of SatelliteInfo structures.
struct SatelliteInfo {
long prn;
long elevation;
long azimuth;
long snr;
};
typedef sequence<SatelliteInfo> SatelliteInfoSeq;
#pragma DCPS_DATA_TYPE "GPS::SatelliteInfoData"
#pragma DCPS_DATA_KEY "GPS::SatelliteInfoData sensor_id"
struct SatelliteInfoData {
string sensor_id;
SatelliteInfoSeq satelliteInfo;
};The ActiveSatellitesData structure also contains a sequence, but the pre-existing CORBA::LongSeq can be used, rather than defining our own.
#pragma DCPS_DATA_TYPE "GPS::ActiveSatellitesData"
#pragma DCPS_DATA_KEY "GPS::ActiveSatellitesData sensor_id"
struct ActiveSatellitesData {
string sensor_id;
CORBA::LongSeq activeSatellites;
double pdop;
double hdop;
double vdop;
};
};The code generated by the compilation of the IDL is referenced by the GPSPublisher application, as well as code that we have seen above.
// GPSPublisher/GPSPublisher.cpp
#include "../GPSDDSLib/GPSTypeSupportC.h"
#include "../GPSDDSLib/GPSC.h"
#include "../GPSDDSLib/GPSTypeSupportImpl.h"
#include "../GPSDDSLib/DDSException.h"
#include <dds/DCPS/Marked_Default_Qos.h>
#include <dds/DCPS/Service_Participant.h>
#include "../ASIOLib/Executor.h"
#include "../ASIOLib/SerialPort.h"
#include "../GPSLib/GPSSentenceDecoder.h"
#include <boost/program_options.hpp>
#include <boost/thread.hpp>
boost::mutex cout_lock;
void Log(const std::string &msg) {
boost::mutex::scoped_lock lock(cout_lock);
std::cout << "[" << boost::this_thread::get_id() << "] " << msg <<
std::endl;
}Class GPSPublisher is similar to SerialReader in that it obtains data from the serial port. Additionally, it also holds OpenDDS entities for each of the sample data types.
class GPSPublisher : public
boost::enable_shared_from_this<GPSPublisher> {
boost::shared_ptr<ASIOLib::SerialPort> _serialPort;
const std::string _portName;
const unsigned int _baudRate;
const boost::shared_ptr<GPSLib::GPSSentenceDecoder> _decoder;
const GPS::PositionDataDataWriter_var _positionWriter;
const GPS::AltitudeDataDataWriter_var _altitudeWriter;
const GPS::CourseDataDataWriter_var _courseWriter;
const GPS::SatelliteInfoDataDataWriter_var _satelliteInfoWriter;
const GPS::ActiveSatellitesDataDataWriter_var
_activeSatellitesWriter;
const boost::posix_time::ptime _epoch;To uniquely identify a sensor, we will use the hostname of the machine that has the serial port, combined with the identifier of the port itself. boost::asio::ip::host_name() returns the host name of the current machine as would be used with TCP/IP communication. This generates a name such as cko:\\.\COM16 or ako:/dev/ttyUSB0.
std::string GetSensorID() {
boost::system::error_code ec;
std::string hostName = boost::asio::ip::host_name(ec);
if (ec)
hostName = "<UNKNOWN>";
return hostName + ":" + _portName;
}For each of the data types, a helper function assembles the data sample structure, and then publishes it by calling write() on the OpenDDS DataWriter that is associated with the sample type. The date provided in the sample is encoded as the number of milliseconds between an epoch date of January 1, 1970 and the time that the sample was obtained.
void PublishPosition(boost::posix_time::ptime date,
double latitude, double longitude) {
GPS::PositionData sample;
sample.sensor_id = GetSensorID().c_str();
sample.date = (date-_epoch).total_milliseconds();
sample.latitude = latitude;
sample.longitude = longitude;
if (_positionWriter->write(sample, DDS::HANDLE_NIL) !=
DDS::RETCODE_OK)
throw DDSException("position write() failed");
}The helpers to publish altitude and course information are similar, and not shown here. The satellite information helper copies values from a std::vector<> into an OpenDDS sequence. First, the sequence length must be set, and then each element is assigned. When complete, the sample is published. The helper to publish active satellite information is similar.
void PublishSatelliteInfo(
const std::vector<GPSLib::SatelliteInfo> &satelliteInfo) {
GPS::SatelliteInfoData sample;
sample.sensor_id = GetSensorID().c_str();
sample.satelliteInfo.length(satelliteInfo.size());
for (size_t i=0; i<satelliteInfo.size(); i++) {
sample.satelliteInfo[i].prn = satelliteInfo[i]._prn;
sample.satelliteInfo[i].azimuth =
satelliteInfo[i]._azimuth;
sample.satelliteInfo[i].elevation =
satelliteInfo[i]._elevation;
sample.satelliteInfo[i].snr = satelliteInfo[i]._snr;
}
if (_satelliteInfoWriter->write(sample, DDS::HANDLE_NIL) !=
DDS::RETCODE_OK)
throw DDSException("satelliteInfo write() failed");
}The remainder of the GPSPublisher class is similar to that of SerialReader as developed in Part I of this article, so is not shown here in detail, save the OnRead() callback. The data received by OnRead() is sent to a GPSSentenceDecoder object, and as the sentences are decoded and their callbacks are invoked, relevant data samples are published. For example, the GGA sentence publishes position and altitude data samples:
void OnRead(boost::asio::io_service &ios,
const std::vector<unsigned char> &buffer, size_t bytesRead) {
_decoder->OnGGA = [&](boost::asio::io_service &ios,
boost::posix_time::time_duration time, double latitude,
double longitude, int quality, int /*numSatellites*/,
double /*horizontalDilution*/, double altitude) {
if (quality != 0) { // only post if valid
PublishPosition(
boost::posix_time::ptime(_epoch + time), latitude,
longitude);
PublishAltitude(
boost::posix_time::ptime(_epoch + time), altitude);
}
};The other sentence types are handled similarly.
_decoder->OnGLL = [&](boost::asio::io_service &ios,
boost::posix_time::time_duration time, double latitude,
double longitude, const std::string &validity) {
if (validity == "A") // only post if valid
PublishPosition(
boost::posix_time::ptime(_epoch +
time), latitude, longitude);
};
_decoder->OnRMC = [&](boost::asio::io_service &ios,
boost::posix_time::time_duration time,
double latitude, double longitude, double speed,
double course, boost::gregorian::date date,
const std::string &validity) {
if (validity == "A") { // only post if valid
PublishPosition(
boost::posix_time::ptime(date,
time), latitude, longitude);
PublishCourse(
boost::posix_time::ptime(date,
time), speed, course);
}
};
_decoder->OnGSV = [&](boost::asio::io_service &ios,
int /*totalMessages*/, int /*messageNumber*/,
int /*totalSatellitesInView*/,
const std::vector<GPSLib::SatelliteInfo>
&satelliteInfo) {
PublishSatelliteInfo(satelliteInfo);
};
_decoder->OnGSA = [&](boost::asio::io_service &ios,
const std::string &mode, int fix,
const std::vector<int> &satellitesInView,
double pdop, double hdop, double vdop) {
PublishActiveSatellites(satellitesInView, pdop,
hdop, vdop);
};
_decoder->AddBytes(ios,
std::vector<unsigned char>(buffer.begin(),
buffer.begin()+bytesRead));
}
public:
GPSPublisher( /*...*/ ) {}
void Create(boost::asio::io_service &ios) {
...
}
};The creation of the OpenDDS entities for each of the data sample types is done within this template function. The type support, type support implementation, and data writer classes for each type are provided, as well as the name of the associated topic, and the topic, publisher, and data writer are created. The entities are returned as a Boost tuple. For details on OpenDDS entities, please see the OpenDDS Developer's Guide [15].
template <typename TTypeSupport, typename TTypeSupportImpl,
typename TDataWriter>
TAO_Objref_Var_T<TDataWriter> CreateWriter(
DDS::DomainParticipant_ptr dp, const char *topicName) {
TAO_Objref_Var_T<TTypeSupport> ts = new TTypeSupportImpl;
if (ts->register_type(dp, "") != DDS::RETCODE_OK)
throw DDSException("reigster_type() failed");
CORBA::String_var typeName(ts->get_type_name());
DDS::Topic_var topic = dp->create_topic(topicName, typeName,
TOPIC_QOS_DEFAULT, 0, OpenDDS::DCPS::DEFAULT_STATUS_MASK);
if (0 == topic)
throw DDSException("create_topic() failed");
DDS::Publisher_var pub = dp->create_publisher(
PUBLISHER_QOS_DEFAULT, 0,
OpenDDS::DCPS::DEFAULT_STATUS_MASK);
if (0 == pub)
throw DDSException("create_publisher() failed");
DDS::DataWriterQos dw_qos;
pub->get_default_datawriter_qos(dw_qos);
DDS::DataWriter_var dw = pub->create_datawriter(topic, dw_qos, 0,
OpenDDS::DCPS::DEFAULT_STATUS_MASK);
if (0 == dw)
throw DDSException("create_datawriter() failed");
TAO_Objref_Var_T<TDataWriter> ndw = TDataWriter::_narrow(dw);
if (0 == ndw)
throw DDSException("writer _narrow() failed");
return ndw;
}In main(), we parse command-line arguments as we have done before, although rather than using boost::program_options::parse_command_line(), boost::program_options::command_line_parser() is used. This allows arguments that are unrecognized by the parser, such as ones applicable to OpenDDS, to not cause argument parsing errors.
int main(int argc, char *argv[]) {
DDS::DomainParticipantFactory_var dpf;
DDS::DomainParticipant_var dp;
try {
std::string portName, file;
int baudRate;
boost::program_options::options_description desc("Options");
desc.add_options()
("help,h", "help")
("port,p", boost::program_options::value<std::string>(
&portName)->required(), "port name (required)")
("baud,b", boost::program_options::value<int>(
&baudRate)->required(), "baud rate (required)")
;
boost::program_options::variables_map vm;
boost::program_options::store(
boost::program_options::command_line_parser(argc, argv)
.options(desc).allow_unregistered().run(), vm);
if (vm.empty() || vm.count("help")) {
std::cout << desc << "\n";
return -1;
}
boost::program_options::notify(vm);We create an OpenDDS domain participant, and using it, create the additional entities for each sample type.
dpf = TheParticipantFactoryWithArgs(argc, argv);
dp = dpf->create_participant(42, PARTICIPANT_QOS_DEFAULT, 0,
OpenDDS::DCPS::DEFAULT_STATUS_MASK);
if (0 == dp)
throw DDSException("create_participant() failed");
const GPS::PositionDataDataWriter_var positionWriter =
CREATE_WRITER(GPS::PositionData)(dp, "GPS_Position");
const GPS::AltitudeDataDataWriter_var altitudeWriter =
CREATE_WRITER(GPS::AltitudeData)(dp, "GPS_Altitude");
const GPS::CourseDataDataWriter_var courseWriter =
CREATE_WRITER(GPS::CourseData)(dp, "GPS_Course");
const GPS::SatelliteInfoDataDataWriter_var
satelliteInfoWriter =
CREATE_WRITER(GPS::SatelliteInfoData)(dp,
"GPS_SatelliteInfo");
const GPS::ActiveSatellitesDataDataWriter_var
activeSatellitesInfoWriter =
CREATE_WRITER(GPS::ActiveSatellitesData)(dp,
"GPS_ActiveSatellites");Code is made more readable by using a macro to specify the template parameters. (Versions of OpenDDS newer than May 10th, 2013, can obtain these types via nested typedefs instead of by macro concatenation — see the ManyTopicTest of OpenDDS for details.)
#define CREATE_WRITER(X) CreateWriter<X##TypeSupport, X##TypeSupportImpl, X##DataWriter>The publisher concludes by starting the Executor to begin reading data on the serial port.
ASIOLib::Executor e;
e.OnWorkerThreadError = [](boost::asio::io_service &,
boost::system::error_code ec) {
Log(std::string("GPSPublisher error (asio): ") +
boost::lexical_cast<std::string>(ec));
};
e.OnWorkerThreadException = [](boost::asio::io_service &,
const std::exception &ex) {
Log(std::string("GPSPublisher exception (asio): ") +
ex.what());
};
boost::shared_ptr<GPSPublisher> spd(new GPSPublisher(portName,
baudRate, positionWriter, altitudeWriter, courseWriter,
satelliteInfoWriter, activeSatellitesInfoWriter));
e.OnRun = boost::bind(&GPSPublisher::Create, spd, _1);
e.Run();
} catch (const std::exception &e) {
std::cout << "GPSPublisher exception (main): " << e.what() <<
std::endl;
return -1;
} catch (const CORBA::Exception &e) {
e._tao_print_exception("GPSPublisher exception (main): ");
return -1;
}
if (0 != dp)
dp->delete_contained_entities();
if (0 != dpf)
dpf->delete_participant(dp);
TheServiceParticipant->shutdown();
return 0;
}The GPSSubscriber application is analagous to GPSPublisher. Command-line arguments are parsed, and subscriber-side OpenDDS entities are created to receive the published data. Each OpenDDS DataReader shares the same DataReaderListenerImpl that is invoked whenever a data sample arrives:
// GPSSubscriber/GPSSubscriber.cpp
DDS::DataReaderListener_var listener(
new DataReaderListenerImpl);
const GPS::PositionDataDataReader_var positonReader =
CREATE_READER(GPS::PositionData)(dp, listener,
"GPS_Position");
const GPS::AltitudeDataDataReader_var altitudeReader =
CREATE_READER(GPS::AltitudeData)(dp, listener,
"GPS_Altitude");
const GPS::CourseDataDataReader_var courseReader =
CREATE_READER(GPS::CourseData)(dp, listener,
"GPS_Course");
const GPS::SatelliteInfoDataDataReader_var
satelliteInfoReader =
CREATE_READER(GPS::SatelliteInfoData)(dp, listener,
"GPS_SatelliteInfo");
const GPS::ActiveSatellitesDataDataReader_var
activeSatellitesInfoReader =
CREATE_READER(GPS::ActiveSatellitesData)(dp, listener,
"GPS_ActiveSatellites");The on_data_available() method of DataReaderListenerImpl narrows the supplied DataReader pointer to each of the sample types in turn, and only if the call to _narrow() was successful is the sample displayed. This allows the one method to service all sample types.
void DataReaderListenerImpl::on_data_available(
DDS::DataReader_ptr reader) {
const boost::posix_time::ptime epoch(
boost::gregorian::date(1970, 1, 1));
DDS::SampleInfo info;
GPS::PositionDataDataReader_var positionReader =
GPS::PositionDataDataReader::_narrow(reader);
if (positionReader) {
GPS::PositionData sample;
DDS::ReturnCode_t error =
positionReader->take_next_sample(sample, info);
if ((error == DDS::RETCODE_OK) && info.valid_data) {
boost::posix_time::ptime time(epoch +
boost::posix_time::time_duration(
boost::posix_time::milliseconds(sample.date)));
std::cout << "Position: " << sample.sensor_id << " " <<
time << " " << sample.latitude << " " <<
sample.longitude << std::endl;
}
return;
}
GPS::AltitudeDataDataReader_var altitudeReader =
GPS::AltitudeDataDataReader::_narrow(reader);
if (altitudeReader) {
...The run_test.pl script in the Tests directory can be used to demonstrate the system. The tester must ensure that the current directory is either Output/Debug or Output/Release as appropriate, and then invoke the run_test.pl script. If run_test.pl is run with no arguments, as follows:
> ..\..\Tests\run_test.pl
A Windows-based system with the com0com null-modem emulator installed is assumed, as introduced in Part I of this article. An instance of SerialWriter is created to play back the recorded GPS data to \\.\CNCA0, an instance of GPSPublisher is created to read serial data from \\.\CNCB0, and an instance of GPSSubscriber created to display sample data as it arrives.
The script can take two parameters which will be used as the writing and reading ports. Running the script under Linux with /dev/pts/4 and /dev/pts/6 arguments, for instance:
$ ../../Tests/run_test.pl /dev/pts/4 /dev/pts/6
will perform the same test with the data sent through socat, which, as described in the first part of this article, can be invoked in a different terminal window as:
$ socat -d -d pty,raw,echo=0 pty,raw,echo=0 2013/01/18 13:29:42 socat[3378] N PTY is /dev/pts/4 2013/01/18 13:29:42 socat[3378] N PTY is /dev/pts/6 2013/01/18 13:29:42 socat[3378] N starting data transfer loop with FDs [4,4] and [6,6]
Using "none" in place of the first port:
$ ../../Tests/run_test.pl none /dev/ttyUSB0
instructs the script to not play back data using SerialWriter, allowing data from the real device to be used.
For example, running under Windows without any arguments (so the GPS archive file is replayed) produces output that includes the following:
... Position: cko:\\.\CNCB0 1970-Jan-01 19:16:45.608000 38.8062 -90.3046 Altitude: cko:\\.\CNCB0 1970-Jan-01 19:16:45.608000 127.9 Position: cko:\\.\CNCB0 1970-Jan-01 19:16:45.608000 38.8062 -90.3046 Active Satellites: cko:\\.\CNCB0 2.2 1.3 1.8 [18 15 29 21 6 9 ] Satellite info: cko:\\.\CNCB0 [18,311,62,37][15,49,47,40][14,219,17,26][29,186,11,30] Satellite info: cko:\\.\CNCB0 [22,282,30,0][21,221,85,35][24,119,34,23][6,295,24,33] Satellite info: cko:\\.\CNCB0 [9,86,34,36][137,0,0,0] Position: cko:\\.\CNCB0 2013-Jan-15 19:16:45.608000 38.8062 -90.3046 Course: cko:\\.\CNCB0 2013-Jan-15 19:16:45.608000 35.5016 55.95 ...
where the January 1, 1970 epoch given earlier is used for samples that only include time of day information.
CONCLUSION
This article series has shown that writing platform-independent code that interacts with the serial port is possible using Boost, as is the ability to post serial-based sensor data as OpenDDS data samples. This concept can be applied to any type of sensor — data is read using Boost Asio, and then published via OpenDDS.
Boost libraries greatly aided the code developed in this article. While fourteen Boost libraries were used explicitly, Boost provides over a hundred others [16], giving much support to a C++ programmer.
REFERENCES
- [1] Multi-Platform Serial Interfacing using Boost: A GPS Sensor and OpenDDS Part I.
http://mnb.ociweb.com/mnb/MiddlewareNewsBrief-201103.html - [2] Boost.Asio
http://www.boost.org/doc/libs/1_52_0/doc/html/boost_asio.html - [3] OpenDDS
https://objectcomputing.com/products/opendds - [4] MPC (The Makefile, Project, and Workspace Creator
https://objectcomputing.com/products/mpc - [5] NMEA 0183
http://en.wikipedia.org/wiki/NMEA_0183 - [6] GLONASS
http://en.wikipedia.org/wiki/GLONASS - [7] GPS - NMEA sentence information
http://aprs.gids.nl/nmea - [8] NMEA data
http://www.gpsinformation.org/dale/nmea.htm - [9] GPS-System
http://www.kowoma.de/en/gps/index.htm - [10] tokenizer
http://www.boost.org/doc/libs/1_52_0/libs/tokenizer/index.html - [11] Boost.Regex
http://www.boost.org/doc/libs/1_52_0/libs/regex/doc/html/index.html - [12] C++ convert hex string to signed integer
http://stackoverflow.com/questions/1070497/c-convert-hex-string-to-signed-integer - [13] Boost Test Library
http://www.boost.org/doc/libs/1_52_0/libs/test/doc/html/index.html - [14] Filesystem Library Version3
http://www.boost.org/doc/libs/1_52_0/libs/filesystem/doc/index.htm - [15] OpenDDS Developer's Guide
http://downloads.ociweb.com/OpenDDS/OpenDDS-latest.pdf - [16] Libraries Listed Alphabetically
http://www.boost.org/doc/libs/1_52_0/libs/libraries.htm#Alphabetically
