60 #ifndef SBF_SYNC_BYTE_1 61 #define SBF_SYNC_BYTE_1 0x24 63 #ifndef SBF_SYNC_BYTE_2 65 #define SBF_SYNC_BYTE_2 0x40 67 #ifndef NMEA_SYNC_BYTE_1 69 #define NMEA_SYNC_BYTE_1 0x24 71 #ifndef NMEA_SYNC_BYTE_2_1 73 #define NMEA_SYNC_BYTE_2_1 0x47 75 #ifndef NMEA_SYNC_BYTE_2_2 77 #define NMEA_SYNC_BYTE_2_2 0x50 79 #ifndef RESPONSE_SYNC_BYTE_1 81 #define RESPONSE_SYNC_BYTE_1 0x24 83 #ifndef RESPONSE_SYNC_BYTE_2 85 #define RESPONSE_SYNC_BYTE_2 0x52 87 #ifndef CARRIAGE_RETURN 89 #define CARRIAGE_RETURN 0x0D 93 #define LINE_FEED 0x0A 95 #ifndef RESPONSE_SYNC_BYTE_3 97 #define RESPONSE_SYNC_BYTE_3 0x3F 99 #ifndef CONNECTION_DESCRIPTOR_BYTE_1 101 #define CONNECTION_DESCRIPTOR_BYTE_1 0x49 103 #ifndef CONNECTION_DESCRIPTOR_BYTE_2 105 #define CONNECTION_DESCRIPTOR_BYTE_2 0x50 116 #include <boost/tokenizer.hpp> 117 #include <boost/call_traits.hpp> 118 #include <boost/format.hpp> 119 #include <boost/math/constants/constants.hpp> 122 #include <rosaic/Gpgga.h> 123 #include <rosaic/Gprmc.h> 124 #include <rosaic/Gpgsa.h> 125 #include <rosaic/Gpgsv.h> 126 #include <sensor_msgs/NavSatFix.h> 127 #include <sensor_msgs/TimeReference.h> 128 #include <geometry_msgs/PoseWithCovarianceStamped.h> 129 #include <gps_common/GPSFix.h> 138 #include <rosaic/PVTCartesian.h> 139 #include <rosaic/PVTGeodetic.h> 140 #include <rosaic/PosCovCartesian.h> 141 #include <rosaic/PosCovGeodetic.h> 142 #include <rosaic/AttEuler.h> 143 #include <rosaic/AttCovEuler.h> 147 #ifndef MOSAIC_MESSAGE_HPP 148 #define MOSAIC_MESSAGE_HPP 160 extern std::vector<int32_t>
svid_pvt;
163 enum NMEA_ID_Enum {
evNavSatFix,
evGPSFix,
evPoseWithCovarianceStamped,
evGPGGA,
evGPRMC,
evGPGSA,
evGPGSV,
evGLGSV,
evGAGSV,
evPVTCartesian,
evPVTGeodetic,
evPosCovCartesian,
evPosCovGeodetic,
evAttEuler,
evAttCovEuler,
evGPST,
evChannelStatus,
evMeasEpoch,
evDOP,
evVelCovGeodetic};
169 StringValues.insert(std::make_pair(
"NavSatFix",
evNavSatFix));
170 StringValues.insert(std::make_pair(
"GPSFix",
evGPSFix));
172 StringValues.insert(std::make_pair(
"$GPGGA",
evGPGGA));
173 StringValues.insert(std::make_pair(
"$GPRMC",
evGPRMC));
174 StringValues.insert(std::make_pair(
"$GPGSA",
evGPGSA));
175 StringValues.insert(std::make_pair(
"$GPGSV",
evGPGSV));
176 StringValues.insert(std::make_pair(
"$GLGSV",
evGLGSV));
177 StringValues.insert(std::make_pair(
"$GAGSV",
evGAGSV));
182 StringValues.insert(std::make_pair(
"5938",
evAttEuler));
184 StringValues.insert(std::make_pair(
"GPST",
evGPST));
186 StringValues.insert(std::make_pair(
"4027",
evMeasEpoch));
187 StringValues.insert(std::make_pair(
"4001",
evDOP));
260 const uint8_t*
Pos();
266 const uint8_t*
End();
277 const uint8_t*
Next();
283 template <
typename T>
284 bool Read(
typename boost::call_traits<T>::reference message, std::string message_key,
bool search =
false);
431 template <
typename T>
432 bool mosaicMessage::Read(
typename boost::call_traits<T>::reference message, std::string message_key,
bool search)
435 if (search) this->
Search();
436 if (!
Found())
return false;
443 throw std::runtime_error(
"CRC Check returned False. Not a valid data block, perhaps noisy. Ignore..");
448 throw std::runtime_error(
"Not a valid data block, parts of the SBF block are not yet received. Ignore..");
451 switch(StringValues[message_key])
455 rosaic::PVTCartesianPtr msg = boost::make_shared<rosaic::PVTCartesian>();
457 memcpy(&pvtcartesian,
data_,
sizeof(pvtcartesian));
460 uint32_t TOW = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
463 msg->header.stamp.sec = time_obj.sec;
464 msg->header.stamp.nsec = time_obj.nsec;
465 msg->Block_Header.ID = 4006;
466 memcpy(&message, msg.get(),
sizeof(*msg));
471 rosaic::PVTGeodeticPtr msg = boost::make_shared<rosaic::PVTGeodetic>();
475 uint32_t TOW = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
478 msg->header.stamp.sec = time_obj.sec;
479 msg->header.stamp.nsec = time_obj.nsec;
480 msg->Block_Header.ID = 4007;
481 memcpy(&message, msg.get(),
sizeof(*msg));
486 rosaic::PosCovCartesianPtr msg = boost::make_shared<rosaic::PosCovCartesian>();
488 memcpy(&poscovcartesian,
data_,
sizeof(poscovcartesian));
491 uint32_t TOW = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
494 msg->header.stamp.sec = time_obj.sec;
495 msg->header.stamp.nsec = time_obj.nsec;
496 msg->Block_Header.ID = 5905;
497 memcpy(&message, msg.get(),
sizeof(*msg));
502 rosaic::PosCovGeodeticPtr msg = boost::make_shared<rosaic::PosCovGeodetic>();
506 uint32_t TOW = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
509 msg->header.stamp.sec = time_obj.sec;
510 msg->header.stamp.nsec = time_obj.nsec;
511 msg->Block_Header.ID = 5906;
512 memcpy(&message, msg.get(),
sizeof(*msg));
517 rosaic::AttEulerPtr msg = boost::make_shared<rosaic::AttEuler>();
521 uint32_t TOW = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
524 msg->header.stamp.sec = time_obj.sec;
525 msg->header.stamp.nsec = time_obj.nsec;
526 msg->Block_Header.ID = 5938;
527 memcpy(&message, msg.get(),
sizeof(*msg));
532 rosaic::AttCovEulerPtr msg = boost::make_shared<rosaic::AttCovEuler>();
536 uint32_t TOW = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
539 msg->header.stamp.sec = time_obj.sec;
540 msg->header.stamp.nsec = time_obj.nsec;
541 msg->Block_Header.ID = 5939;
542 memcpy(&message, msg.get(),
sizeof(*msg));
547 sensor_msgs::TimeReferencePtr msg = boost::make_shared<sensor_msgs::TimeReference>();
548 uint32_t TOW = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
551 msg->time_ref.sec = time_obj.sec;
552 msg->time_ref.nsec = time_obj.nsec;
553 msg->source =
"GPST";
554 memcpy(&message, msg.get(),
sizeof(*msg));
559 boost::char_separator<char> sep(
"\r");
560 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
561 std::size_t nmea_size = std::min(this->
SegmentEnd(), static_cast<std::size_t>(89));
562 std::string block_in_string(reinterpret_cast<const char*>(
data_), nmea_size);
563 tokenizer tokens(block_in_string, sep);
566 std::string one_message = *tokens.begin();
567 boost::char_separator<char> sep_2(
",*",
"", boost::keep_empty_tokens);
569 tokenizer tokens_2(one_message, sep_2);
570 std::vector<std::string> body;
571 for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter)
573 body.push_back(*tok_iter);
577 rosaic::GpggaPtr gpgga_ros_message_ptr;
581 gpgga_ros_message_ptr = parser_obj.
ParseASCII(gga_message);
585 throw std::runtime_error(e.what());
588 memcpy(&message, gpgga_ros_message_ptr.get(),
sizeof(*gpgga_ros_message_ptr));
593 boost::char_separator<char> sep(
"\r");
594 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
595 std::size_t nmea_size = std::min(this->
SegmentEnd(), static_cast<std::size_t>(89));
596 std::string block_in_string(reinterpret_cast<const char*>(
data_), nmea_size);
597 tokenizer tokens(block_in_string, sep);
600 std::string one_message = *tokens.begin();
601 boost::char_separator<char> sep_2(
",*",
"", boost::keep_empty_tokens);
603 tokenizer tokens_2(one_message, sep_2);
604 std::vector<std::string> body;
605 for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter)
607 body.push_back(*tok_iter);
611 rosaic::GprmcPtr msg = boost::make_shared<rosaic::Gprmc>();
619 throw std::runtime_error(e.what());
623 memcpy(&message, msg.get(),
sizeof(*msg));
628 boost::char_separator<char> sep(
"\r");
629 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
630 std::size_t nmea_size = std::min(this->
SegmentEnd(), static_cast<std::size_t>(89));
631 std::string block_in_string(reinterpret_cast<const char*>(
data_), nmea_size);
632 tokenizer tokens(block_in_string, sep);
635 std::string one_message = *tokens.begin();
636 boost::char_separator<char> sep_2(
",*",
"", boost::keep_empty_tokens);
638 tokenizer tokens_2(one_message, sep_2);
639 std::vector<std::string> body;
640 for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter)
642 body.push_back(*tok_iter);
646 rosaic::GpgsaPtr msg = boost::make_shared<rosaic::Gpgsa>();
654 throw std::runtime_error(e.what());
659 msg->header.stamp.sec = time_obj.sec;
660 msg->header.stamp.nsec = time_obj.nsec;
663 memcpy(&message, msg.get(),
sizeof(*msg));
668 boost::char_separator<char> sep(
"\r");
669 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
670 std::size_t nmea_size = std::min(this->
SegmentEnd(), static_cast<std::size_t>(89));
671 std::string block_in_string(reinterpret_cast<const char*>(
data_), nmea_size);
672 tokenizer tokens(block_in_string, sep);
675 std::string one_message = *tokens.begin();
676 boost::char_separator<char> sep_2(
",*",
"", boost::keep_empty_tokens);
678 tokenizer tokens_2(one_message, sep_2);
679 std::vector<std::string> body;
680 for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter)
682 body.push_back(*tok_iter);
686 rosaic::GpgsvPtr msg = boost::make_shared<rosaic::Gpgsv>();
694 throw std::runtime_error(e.what());
699 msg->header.stamp.sec = time_obj.sec;
700 msg->header.stamp.nsec = time_obj.nsec;
703 memcpy(&message, msg.get(),
sizeof(*msg));
708 sensor_msgs::NavSatFixPtr msg = boost::make_shared<sensor_msgs::NavSatFix>();
713 catch (std::runtime_error& e)
715 throw std::runtime_error(e.what());
721 msg->header.stamp.sec = time_obj.sec;
722 msg->header.stamp.nsec = time_obj.nsec;
723 memcpy(&message, msg.get(),
sizeof(*msg));
728 gps_common::GPSFixPtr msg = boost::make_shared<gps_common::GPSFix>();
733 catch (std::runtime_error& e)
735 throw std::runtime_error(e.what());
739 msg->status.header.frame_id =
frame_id;
743 msg->header.stamp.sec = time_obj.sec;
744 msg->status.header.stamp.sec = time_obj.sec;
745 msg->header.stamp.nsec = time_obj.nsec;
746 msg->status.header.stamp.nsec = time_obj.nsec;
747 memcpy(&message, msg.get(),
sizeof(*msg));
753 geometry_msgs::PoseWithCovarianceStampedPtr msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
758 catch (std::runtime_error& e)
760 throw std::runtime_error(e.what());
766 msg->header.stamp.sec = time_obj.sec;
767 msg->header.stamp.nsec = time_obj.nsec;
768 memcpy(&message, msg.get(),
sizeof(*msg));
797 #endif // for MOSAIC_MESSAGE_HPP
static PosCovGeodetic last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
Struct for the SBF block "AttCovEuler".
Derived class for parsing GSA messages.
const uint8_t * Next()
Goes to the start of the next message based on the calculated length of current message.
std::size_t segment_size_
Helps to determine size of response message / NMEA message / SBF block.
bool found_
Whether or not a message has been found.
std::string MessageID()
Returns the MessageID of the message where data_ is pointing at at the moment, SBF identifiers embell...
const uint8_t * Search()
Searches the buffer for the beginning of the next message (NMEA or SBF)
mosaicMessage(const uint8_t *data, std::size_t &size)
Constructor of the mosaicMessage class.
rosaic::AttCovEulerPtr AttCovEulerCallback(AttCovEuler &data)
Callback function when reading AttCovEuler blocks.
static DOP last_dop_
Since GPSFix etc. need DOP, incoming DOP blocks need to be stored.
Derived class for parsing GSA messages.
static void StringValues_Initialize()
Derived class for parsing GGA messages.
const uint8_t * End()
Gets the end position in the read buffer.
Derived class for parsing GSV messages.
uint16_t BlockLength()
Gets the length of the SBF block.
Struct for the SBF block "VelCovGeodetic".
rosaic::PVTGeodeticPtr PVTGeodeticCallback(PVTGeodetic &data)
Callback function when reading PVTGeodetic blocks.
bool IsErrorMessage()
Determines whether data_ currently points to an error message reply from mosaic.
uint32_t count_
The number of bytes in the buffer, decremented as the buffer is read (so it is the buffer's size in t...
rosaic::AttEulerPtr AttEulerCallback(AttEuler &data)
Callback function when reading AttEuler blocks.
static VelCovGeodetic last_velcovgeodetic_
Since GPSFix etc. need VelCovGeodetic (for signal-to-noise ratios), incoming VelCovGeodetic blocks ne...
Struct for the SBF block "PosCovGeodetic".
Declares the functions to compute and validate the CRC of a buffer.
static AttCovEuler last_attcoveuler_
Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored.
bool CRCIsValid(const void *Block)
This function validates whether the calculated CRC of the SBF block at hand matches the CRC field tha...
static PVTGeodetic last_pvtgeodetic_
Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.
std::vector< int32_t > svid_pvt
uint32_t leap_seconds
The number of leap seconds that have been inserted into the UTC time.
rosaic::PosCovCartesianPtr PosCovCartesianCallback(PosCovCartesian &data)
Callback function when reading PosCovCartesian blocks.
gps_common::GPSFixPtr GPSFixCallback()
"Callback" function when constructing GPSFix messages
bool Found()
Has an NMEA message or SBF block been found in the buffer?
uint32_t GetCount()
Returns the count_ variable.
bool IsResponse()
Determines whether data_ currently points to an NMEA message.
Struct for the SBF block "PVTGeodetic".
Declares and defines structs into which SBF blocks are unpacked then shipped to handler functions...
static AttEuler last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
Struct for the SBF block "AttEuler".
const uint8_t * Pos()
Gets the current position in the read buffer.
Derived class for parsing GGA messages.
geometry_msgs::PoseWithCovarianceStampedPtr PoseWithCovarianceStampedCallback()
"Callback" function when constructing PoseWithCovarianceStamped messages
static ChannelStatus last_channelstatus_
Since GPSFix etc. need ChannelStatus, incoming ChannelStatus blocks need to be stored.
bool CRCcheck_
Whether the CRC check as evaluated in the read() method was successful or not is stored here...
Declares lower-level string utility functions used when parsing messages.
uint32_t cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
Struct for the SBF block "PVTCartesian".
Derived class for parsing RMC messages.
bool IsNMEA()
Determines whether data_ currently points to an NMEA message.
static std::map< std::string, NMEA_ID_Enum > StringValues
Struct for the SBF block "PosCovCartesian".
sensor_msgs::NavSatFixPtr NavSatFixCallback()
"Callback" function when constructing NavSatFix messages
rosaic::PVTCartesianPtr PVTCartesianCallback(PVTCartesian &data)
Callback function when reading PVTCartesian blocks.
ros::Time TimestampSBF(uint32_t TOW, bool use_GNSS)
Calculates the timestamp, in the Unix Epoch time format, either using the TOW as transmitted with the...
bool Read(typename boost::call_traits< T >::reference message, std::string message_key, bool search=false)
Performs the CRC check (if SBF block) and populates message with the necessary content (mapped 1-to-1...
bool IsSBF()
Determines whether data_ currently points to an SBF block.
Struct for the SBF block "ChannelStatus".
const uint8_t * data_
The pointer to the buffer of messages.
Struct for the SBF block "MeasEpoch".
std::string frame_id
The frame ID used in the header of every published ROS message.
bool IsConnectionDescriptor()
Determines whether data_ currently points to a connection descriptor (right after initiating TCP conn...
Struct for the SBF block "DOP".
Can search buffer for messages, read/parse them, and so on.
Derived class for parsing RMC messages.
rosaic::GpggaPtr ParseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GGA message.
Struct to split an NMEA sentence into its ID (e.g. the standardized "$GPGGA" or proprietary "$PSSN...
Ćlass to declare error message format when parsing, derived from the public class "std::runtime_error...
Derived class for parsing GSV messages.
bool read_cd
Whether or not we still want to read the connection descriptor, which we only want in the very beginn...
static MeasEpoch last_measepoch_
Since GPSFix etc. need MeasEpoch (for signal-to-noise ratios), incoming MeasEpoch blocks need to be s...
rosaic::PosCovGeodeticPtr PosCovGeodeticCallback(PosCovGeodetic &data)
Callback function when reading PosCovGeodetic blocks.
static uint32_t count_gpsfix_
Number of times the gps_common::GPSFix message has been published.
rosaic::GpgsaPtr ParseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GSA message.
std::size_t SegmentEnd()
Determines size of message that data_ is currently pointing at.
Defines a struct NMEASentence, into which NMEA sentences - both standardized and proprietary ones - s...
rosaic::GprmcPtr ParseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one RMC message.
rosaic::GpgsvPtr ParseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GSV message.
bool IsMessage(const uint16_t ID)
Determines whether data_ points to the SBF block with ID "ID", e.g. 5003.