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 115 #include <boost/tokenizer.hpp> 116 #include <boost/call_traits.hpp> 117 #include <boost/format.hpp> 118 #include <boost/math/constants/constants.hpp> 120 #include <sensor_msgs/NavSatFix.h> 121 #include <sensor_msgs/TimeReference.h> 122 #include <geometry_msgs/PoseWithCovarianceStamped.h> 123 #include <diagnostic_msgs/DiagnosticArray.h> 124 #include <diagnostic_msgs/DiagnosticStatus.h> 125 #include <gps_common/GPSFix.h> 133 #include <rosaic/PVTCartesian.h> 134 #include <rosaic/PVTGeodetic.h> 135 #include <rosaic/PosCovCartesian.h> 136 #include <rosaic/PosCovGeodetic.h> 137 #include <rosaic/AttEuler.h> 138 #include <rosaic/AttCovEuler.h> 140 #ifndef RX_MESSAGE_HPP 141 #define RX_MESSAGE_HPP 169 extern boost::shared_ptr<ros::NodeHandle>
g_nh;
285 template <
typename T>
286 bool read(
typename boost::call_traits<T>::reference message, std::string message_key,
bool search =
false);
384 typedef std::map<std::string, RxID_Enum>
RxIDMap;
476 template <
typename T>
479 if (search) this->
search();
480 if (!
found())
return false;
487 throw std::runtime_error(
"CRC Check returned False. Not a valid data block, perhaps noisy. Ignore..");
495 rosaic::PVTCartesianPtr msg = boost::make_shared<rosaic::PVTCartesian>();
497 memcpy(&pvtcartesian,
data_,
sizeof(pvtcartesian));
500 uint32_t tow = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
503 msg->header.stamp.sec = time_obj.sec;
504 msg->header.stamp.nsec = time_obj.nsec;
505 msg->block_header.id = 4006;
506 memcpy(&message, msg.get(),
sizeof(*msg));
507 static ros::Publisher publisher =
g_nh->advertise<rosaic::PVTCartesian>(
"/pvtcartesian",
g_ROS_QUEUE_SIZE);
508 publisher.publish(*msg);
513 rosaic::PVTGeodeticPtr msg = boost::make_shared<rosaic::PVTGeodetic>();
517 uint32_t tow = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
520 msg->header.stamp.sec = time_obj.sec;
521 msg->header.stamp.nsec = time_obj.nsec;
522 msg->block_header.id = 4007;
523 memcpy(&message, msg.get(),
sizeof(*msg));
527 static ros::Publisher publisher =
g_nh->advertise<rosaic::PVTGeodetic>(
"/pvtgeodetic",
g_ROS_QUEUE_SIZE);
528 publisher.publish(*msg);
533 rosaic::PosCovCartesianPtr msg = boost::make_shared<rosaic::PosCovCartesian>();
535 memcpy(&poscovcartesian,
data_,
sizeof(poscovcartesian));
538 uint32_t tow = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
541 msg->header.stamp.sec = time_obj.sec;
542 msg->header.stamp.nsec = time_obj.nsec;
543 msg->block_header.id = 5905;
544 memcpy(&message, msg.get(),
sizeof(*msg));
545 static ros::Publisher publisher =
g_nh->advertise<rosaic::PosCovCartesian>(
"/poscovcartesian",
g_ROS_QUEUE_SIZE);
546 publisher.publish(*msg);
551 rosaic::PosCovGeodeticPtr msg = boost::make_shared<rosaic::PosCovGeodetic>();
555 uint32_t tow = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
558 msg->header.stamp.sec = time_obj.sec;
559 msg->header.stamp.nsec = time_obj.nsec;
560 msg->block_header.id = 5906;
561 memcpy(&message, msg.get(),
sizeof(*msg));
565 static ros::Publisher publisher =
g_nh->advertise<rosaic::PosCovGeodetic>(
"/poscovgeodetic",
g_ROS_QUEUE_SIZE);
566 publisher.publish(*msg);
571 rosaic::AttEulerPtr msg = boost::make_shared<rosaic::AttEuler>();
575 uint32_t tow = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
578 msg->header.stamp.sec = time_obj.sec;
579 msg->header.stamp.nsec = time_obj.nsec;
580 msg->block_header.id = 5938;
581 memcpy(&message, msg.get(),
sizeof(*msg));
584 static ros::Publisher publisher =
g_nh->advertise<rosaic::AttEuler>(
"/atteuler",
g_ROS_QUEUE_SIZE);
585 publisher.publish(*msg);
590 rosaic::AttCovEulerPtr msg = boost::make_shared<rosaic::AttCovEuler>();
594 uint32_t tow = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
597 msg->header.stamp.sec = time_obj.sec;
598 msg->header.stamp.nsec = time_obj.nsec;
599 msg->block_header.id = 5939;
600 memcpy(&message, msg.get(),
sizeof(*msg));
603 static ros::Publisher publisher =
g_nh->advertise<rosaic::AttCovEuler>(
"/attcoveuler",
g_ROS_QUEUE_SIZE);
604 publisher.publish(*msg);
609 sensor_msgs::TimeReferencePtr msg = boost::make_shared<sensor_msgs::TimeReference>();
610 uint32_t tow = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
613 msg->time_ref.sec = time_obj.sec;
614 msg->time_ref.nsec = time_obj.nsec;
615 msg->source =
"GPST";
616 memcpy(&message, msg.get(),
sizeof(*msg));
617 static ros::Publisher publisher =
g_nh->advertise<sensor_msgs::TimeReference>(
"/gpst",
g_ROS_QUEUE_SIZE);
618 publisher.publish(*msg);
623 boost::char_separator<char> sep(
"\r");
624 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
626 std::string block_in_string(reinterpret_cast<const char*>(
data_), nmea_size);
627 tokenizer tokens(block_in_string, sep);
630 std::string one_message = *tokens.begin();
634 boost::char_separator<char> sep_2(
",*",
"", boost::keep_empty_tokens);
635 tokenizer tokens_2(one_message, sep_2);
636 std::vector<std::string> body;
637 for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter)
639 body.push_back(*tok_iter);
643 rosaic::GpggaPtr msg = boost::make_shared<rosaic::Gpgga>();
651 throw std::runtime_error(e.what());
653 memcpy(&message, msg.get(),
sizeof(*msg));
655 publisher.publish(*msg);
660 boost::char_separator<char> sep(
"\r");
661 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
663 std::string block_in_string(reinterpret_cast<const char*>(
data_), nmea_size);
664 tokenizer tokens(block_in_string, sep);
667 std::string one_message = *tokens.begin();
668 boost::char_separator<char> sep_2(
",*",
"", boost::keep_empty_tokens);
669 tokenizer tokens_2(one_message, sep_2);
670 std::vector<std::string> body;
671 for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter)
673 body.push_back(*tok_iter);
677 rosaic::GprmcPtr msg = boost::make_shared<rosaic::Gprmc>();
685 throw std::runtime_error(e.what());
687 memcpy(&message, msg.get(),
sizeof(*msg));
689 publisher.publish(*msg);
694 boost::char_separator<char> sep(
"\r");
695 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
697 std::string block_in_string(reinterpret_cast<const char*>(
data_), nmea_size);
698 tokenizer tokens(block_in_string, sep);
701 std::string one_message = *tokens.begin();
702 boost::char_separator<char> sep_2(
",*",
"", boost::keep_empty_tokens);
703 tokenizer tokens_2(one_message, sep_2);
704 std::vector<std::string> body;
705 for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter)
707 body.push_back(*tok_iter);
711 rosaic::GpgsaPtr msg = boost::make_shared<rosaic::Gpgsa>();
719 throw std::runtime_error(e.what());
724 msg->header.stamp.sec = time_obj.sec;
725 msg->header.stamp.nsec = time_obj.nsec;
726 memcpy(&message, msg.get(),
sizeof(*msg));
728 publisher.publish(*msg);
733 boost::char_separator<char> sep(
"\r");
734 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
736 std::string block_in_string(reinterpret_cast<const char*>(
data_), nmea_size);
737 tokenizer tokens(block_in_string, sep);
740 std::string one_message = *tokens.begin();
741 boost::char_separator<char> sep_2(
",*",
"", boost::keep_empty_tokens);
742 tokenizer tokens_2(one_message, sep_2);
743 std::vector<std::string> body;
744 for (tokenizer::iterator tok_iter = tokens_2.begin(); tok_iter != tokens_2.end(); ++tok_iter)
746 body.push_back(*tok_iter);
750 rosaic::GpgsvPtr msg = boost::make_shared<rosaic::Gpgsv>();
758 throw std::runtime_error(e.what());
763 msg->header.stamp.sec = time_obj.sec;
764 msg->header.stamp.nsec = time_obj.nsec;
765 memcpy(&message, msg.get(),
sizeof(*msg));
767 publisher.publish(*msg);
772 sensor_msgs::NavSatFixPtr msg = boost::make_shared<sensor_msgs::NavSatFix>();
777 catch (std::runtime_error& e)
779 throw std::runtime_error(e.what());
782 uint32_t tow = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
785 msg->header.stamp.sec = time_obj.sec;
786 msg->header.stamp.nsec = time_obj.nsec;
787 memcpy(&message, msg.get(),
sizeof(*msg));
790 static ros::Publisher publisher =
g_nh->advertise<sensor_msgs::NavSatFix>(
"/navsatfix",
g_ROS_QUEUE_SIZE);
791 publisher.publish(*msg);
796 gps_common::GPSFixPtr msg = boost::make_shared<gps_common::GPSFix>();
801 catch (std::runtime_error& e)
803 throw std::runtime_error(e.what());
808 uint32_t tow = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
811 msg->header.stamp.sec = time_obj.sec;
812 msg->status.header.stamp.sec = time_obj.sec;
813 msg->header.stamp.nsec = time_obj.nsec;
814 msg->status.header.stamp.nsec = time_obj.nsec;
815 memcpy(&message, msg.get(),
sizeof(*msg));
825 static ros::Publisher publisher =
g_nh->advertise<gps_common::GPSFix>(
"/gpsfix",
g_ROS_QUEUE_SIZE);
826 publisher.publish(*msg);
831 geometry_msgs::PoseWithCovarianceStampedPtr msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
836 catch (std::runtime_error& e)
838 throw std::runtime_error(e.what());
841 uint32_t tow = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
844 msg->header.stamp.sec = time_obj.sec;
845 msg->header.stamp.nsec = time_obj.nsec;
846 memcpy(&message, msg.get(),
sizeof(*msg));
851 static ros::Publisher publisher =
g_nh->advertise<geometry_msgs::PoseWithCovarianceStamped>(
"/pose",
g_ROS_QUEUE_SIZE);
852 publisher.publish(*msg);
881 diagnostic_msgs::DiagnosticArrayPtr msg = boost::make_shared<diagnostic_msgs::DiagnosticArray>();
886 catch (std::runtime_error& e)
888 throw std::runtime_error(e.what());
891 uint32_t tow = *(
reinterpret_cast<const uint32_t *
>(
data_ + 8));
894 msg->header.stamp.sec = time_obj.sec;
895 msg->header.stamp.nsec = time_obj.nsec;
896 memcpy(&message, msg.get(),
sizeof(*msg));
899 static ros::Publisher publisher =
g_nh->advertise<diagnostic_msgs::DiagnosticArray>(
"/diagnostics",
g_ROS_QUEUE_SIZE);
900 publisher.publish(*msg);
926 #endif // for RX_MESSAGE_HPP
bool found()
Has an NMEA message, SBF block or command reply been found in the buffer?
static PVTGeodetic last_pvtgeodetic_
Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.
Struct for the SBF block "AttCovEuler".
Derived class for parsing GSA messages.
std::string g_frame_id
The frame ID used in the header of every published ROS message.
std::map< uint16_t, TypeOfPVT_Enum > TypeOfPVTMap
Shorthand for the map responsible for matching PVTGeodetic's Mode field to an enum value...
sensor_msgs::NavSatFixPtr NavSatFixCallback()
"Callback" function when constructing NavSatFix messages
std::size_t getCount()
Returns the count_ variable.
rosaic::GpggaPtr parseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GGA message.
static PosCovGeodetic last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
rosaic::PVTGeodeticPtr PVTGeodeticCallback(PVTGeodetic &data)
Callback function when reading PVTGeodetic blocks.
RxMessage(const uint8_t *data, std::size_t &size)
Constructor of the RxMessage class.
bool g_dop_has_arrived_gpsfix
For GPSFix: Whether the DOP block of the current epoch has arrived or not.
static DOP last_dop_
Since GPSFix needs DOP, incoming DOP blocks need to be stored.
bool g_measepoch_has_arrived_gpsfix
For GPSFix: Whether the MeasEpoch block of the current epoch has arrived or not.
Derived class for parsing GSA messages.
static MeasEpoch last_measepoch_
Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks need to be stored.
Derived class for parsing GGA messages.
bool isResponse()
Determines whether data_ currently points to an NMEA message.
rosaic::PosCovCartesianPtr PosCovCartesianCallback(PosCovCartesian &data)
Callback function when reading PosCovCartesian blocks.
bool g_attcoveuler_has_arrived_pose
For PoseWithCovarianceStamped: Whether the AttCovEuler block of the current epoch has arrived or not...
bool g_poscovgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PosCovGeodetic block of the current epoch has arrived or not.
Derived class for parsing GSV messages.
Struct for the SBF block "VelCovGeodetic".
bool g_pvtgeodetic_has_arrived_navsatfix
For NavSatFix: Whether the PVTGeodetic block of the current epoch has arrived or not.
Struct for the SBF block "ReceiverSetup".
rosaic::PosCovGeodeticPtr PosCovGeodeticCallback(PosCovGeodetic &data)
Callback function when reading PosCovGeodetic blocks.
gps_common::GPSFixPtr GPSFixCallback()
"Callback" function when constructing GPSFix messages
Struct for the SBF block "PosCovGeodetic".
Declares the functions to compute and validate the CRC of a buffer.
bool g_atteuler_has_arrived_gpsfix
For GPSFix: Whether the AttEuler block of the current epoch has arrived or not.
static ChannelStatus last_channelstatus_
Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks need to be stored.
rosaic::PVTCartesianPtr PVTCartesianCallback(PVTCartesian &data)
Callback function when reading PVTCartesian blocks.
void next()
Goes to the start of the next message based on the calculated length of current message.
uint32_t g_cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
bool g_receiverstatus_has_arrived_diagnostics
For DiagnosticArray: Whether the ReceiverStatus block of the current epoch has arrived or not...
Struct for the SBF block "PVTGeodetic".
static AttEuler last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
std::map< std::string, RxID_Enum > RxIDMap
Shorthand for the map responsible for matching ROS message identifiers to an enum value...
Struct for the SBF block "AttEuler".
rosaic::AttEulerPtr AttEulerCallback(AttEuler &data)
Callback function when reading AttEuler blocks.
Can search buffer for messages, read/parse them, and so on.
Derived class for parsing GGA messages.
bool g_attcoveuler_has_arrived_gpsfix
For GPSFix: Whether the AttCovEuler block of the current epoch has arrived or not.
bool isErrorMessage()
Determines whether data_ currently points to an error message reply from the Rx.
bool g_poscovgeodetic_has_arrived_navsatfix
For NavSatFix: Whether the PosCovGeodetic block of the current epoch has arrived or not...
std::size_t count_
Number of unread bytes in the buffer.
bool isNMEA()
Determines whether data_ currently points to an NMEA message.
Struct for the SBF block "QualityInd".
static VelCovGeodetic last_velcovgeodetic_
Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks need to be stored.
bool isMessage(const uint16_t ID)
Determines whether data_ points to the SBF block with ID "ID", e.g. 5003.
static TypeOfPVTMap type_of_pvt_map
All instances of the RxMessage class shall have access to the map without reinitializing it...
static ReceiverStatus last_receiverstatus_
Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus blocks need to be stored...
Declares lower-level string utility functions used when parsing messages.
Struct for the SBF block "PVTCartesian".
uint16_t getBlockLength()
Gets the length of the SBF block.
Derived class for parsing RMC messages.
ros::Time timestampSBF(uint32_t tow, bool use_gnss)
Calculates the timestamp, in the Unix Epoch time format This is either done using the TOW as transmit...
uint32_t g_leap_seconds
The number of leap seconds that have been inserted into the UTC time.
Struct for the SBF block "PosCovCartesian".
std::size_t messageSize()
Determines size of the message (also command reply) that data_ is currently pointing at...
rosaic::GpgsaPtr parseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GSA message.
boost::shared_ptr< ros::NodeHandle > g_nh
rosaic::AttCovEulerPtr AttCovEulerCallback(AttCovEuler &data)
Callback function when reading AttCovEuler blocks.
bool g_atteuler_has_arrived_pose
For PoseWithCovarianceStamped: Whether the AttEuler block of the current epoch has arrived or not...
const uint8_t * getEndBuffer()
Gets the end position in the read buffer.
bool g_channelstatus_has_arrived_gpsfix
For GPSFix: Whether the ChannelStatus block of the current epoch has arrived or not.
static uint32_t count_gpsfix_
Number of times the gps_common::GPSFix message has been published.
Struct for the SBF block "ChannelStatus".
TypeOfPVT_Enum
Enum for NavSatFix's status.status field, which is obtained from PVTGeodetic's Mode field...
bool g_velcovgeodetic_has_arrived_gpsfix
For GPSFix: Whether the VelCovGeodetic block of the current epoch has arrived or not.
Struct for the SBF block "MeasEpoch".
diagnostic_msgs::DiagnosticArrayPtr DiagnosticArrayCallback()
"Callback" function when constructing diagnostic_msgs::DiagnosticArray messages
Struct for the SBF block "DOP".
bool g_pvtgeodetic_has_arrived_pose
For PoseWithCovarianceStamped: Whether the PVTGeodetic block of the current epoch has arrived or not...
rosaic::GpgsvPtr parseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one GSV message.
bool g_poscovgeodetic_has_arrived_pose
For PoseWithCovarianceStamped: Whether the PosCovGeodetic block of the current epoch has arrived or n...
Derived class for parsing RMC messages.
geometry_msgs::PoseWithCovarianceStampedPtr PoseWithCovarianceStampedCallback()
"Callback" function when constructing PoseWithCovarianceStamped messages
Struct to split an NMEA sentence into its ID and its body, the latter tokenized into a vector of stri...
Class to declare error message format when parsing, derived from the public class "std::runtime_error...
static ReceiverSetup last_receiversetup_
Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup blocks need to be stored...
bool isSBF()
Determines whether data_ currently points to an SBF block.
const uint32_t g_ROS_QUEUE_SIZE
Queue size for ROS publishers.
bool found_
Whether or not a message has been found.
Derived class for parsing GSV messages.
const uint8_t * getPosBuffer()
Gets the current position in the read buffer.
std::size_t message_size_
Helps to determine size of response message / NMEA message / SBF block.
const uint8_t * search()
Searches the buffer for the beginning of the next message (NMEA or SBF)
bool g_pvtgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PVTGeodetic block of the current epoch has arrived or not.
bool read(typename boost::call_traits< T >::reference message, std::string message_key, bool search=false)
Performs the CRC check (if SBF) and populates ROS message "message" with the necessary content...
bool isConnectionDescriptor()
static QualityInd last_qualityind_
Since DiagnosticArray needs QualityInd, incoming QualityInd blocks need to be stored.
const uint8_t * data_
Pointer to the buffer of messages.
static RxIDMap rx_id_map
All instances of the RxMessage class shall have access to the map without reinitializing it...
static AttCovEuler last_attcoveuler_
Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored.
bool g_qualityind_has_arrived_diagnostics
For DiagnosticArray: Whether the QualityInd block of the current epoch has arrived or not...
bool isValid(const void *block)
Validates whether the calculated CRC of the SBF block at hand matches the CRC field of the streamed S...
rosaic::GprmcPtr parseASCII(const NMEASentence &sentence) noexcept(false) override
Parses one RMC message.
bool crc_check_
Whether the CRC check as evaluated in the read() method was successful or not is stored here...
Struct for the SBF block "ReceiverStatus".