55 std::make_pair(static_cast<uint16_t>(0),
evNoPVT),
57 std::make_pair(static_cast<uint16_t>(2),
evDGPS),
58 std::make_pair(static_cast<uint16_t>(3),
evFixed),
59 std::make_pair(static_cast<uint16_t>(4),
evRTKFixed),
60 std::make_pair(static_cast<uint16_t>(5),
evRTKFloat),
61 std::make_pair(static_cast<uint16_t>(6),
evSBAS),
64 std::make_pair(static_cast<uint16_t>(10),
evPPP)
75 std::make_pair(
"$GPGGA",
evGPGGA),
76 std::make_pair(
"$GPRMC",
evGPRMC),
77 std::make_pair(
"$GPGSA",
evGPGSA),
78 std::make_pair(
"$GPGSV",
evGPGSV),
79 std::make_pair(
"$GLGSV",
evGLGSV),
80 std::make_pair(
"$GAGSV",
evGAGSV),
87 std::make_pair(
"GPST",
evGPST),
90 std::make_pair(
"4001",
evDOP),
102 rosaic::PVTGeodeticPtr msg = boost::make_shared<rosaic::PVTGeodetic>();
108 msg->block_header.tow = data.
tow;
109 msg->block_header.wnc = data.
wnc;
110 msg->mode = data.
mode;
111 msg->error = data.
error;
114 msg->height = data.
height;
123 msg->datum = data.
datum;
124 msg->nr_sv = data.
nr_sv;
135 msg->misc = data.
misc;
142 rosaic::PVTCartesianPtr msg = boost::make_shared<rosaic::PVTCartesian>();
148 msg->block_header.tow = data.
tow;
149 msg->block_header.wnc = data.
wnc;
150 msg->mode = data.
mode;
151 msg->error = data.
error;
163 msg->datum = data.
datum;
164 msg->nr_sv = data.
nr_sv;
175 msg->misc = data.
misc;
181 rosaic::PosCovCartesianPtr msg = boost::make_shared<rosaic::PosCovCartesian>();
187 msg->block_header.tow = data.
tow;
188 msg->block_header.wnc = data.
wnc;
189 msg->mode = data.
mode;
190 msg->error = data.
error;
191 msg->cov_xx = data.
cov_xx;
192 msg->cov_yy = data.
cov_yy;
193 msg->cov_zz = data.
cov_zz;
194 msg->cov_bb = data.
cov_bb;
195 msg->cov_xy = data.
cov_xy;
196 msg->cov_xz = data.
cov_xz;
197 msg->cov_xb = data.
cov_xb;
198 msg->cov_yz = data.
cov_yz;
199 msg->cov_yb = data.
cov_yb;
200 msg->cov_zb = data.
cov_zb;
207 rosaic::PosCovGeodeticPtr msg = boost::make_shared<rosaic::PosCovGeodetic>();
213 msg->block_header.tow = data.
tow;
214 msg->block_header.wnc = data.
wnc;
215 msg->mode = data.
mode;
216 msg->error = data.
error;
220 msg->cov_bb = data.
cov_bb;
226 msg->cov_hb = data.
cov_hb;
232 rosaic::AttEulerPtr msg = boost::make_shared<rosaic::AttEuler>();
238 msg->block_header.tow = data.
tow;
239 msg->block_header.wnc = data.
wnc;
240 msg->nr_sv = data.
nr_sv;
241 msg->error = data.
error;
242 msg->mode = data.
mode;
244 msg->pitch = data.
pitch;
245 msg->roll = data.
roll;
254 rosaic::AttCovEulerPtr msg = boost::make_shared<rosaic::AttCovEuler>();
260 msg->block_header.tow = data.
tow;
261 msg->block_header.wnc = data.
wnc;
262 msg->error = data.
error;
284 geometry_msgs::PoseWithCovarianceStampedPtr msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
289 msg->pose.pose.position.y =
static_cast<double>(
last_pvtgeodetic_.
latitude)*360/(2*boost::math::constants::pi<double>());
295 msg->pose.covariance[3] = 0;
296 msg->pose.covariance[4] = 0;
297 msg->pose.covariance[5] = 0;
301 msg->pose.covariance[9] = 0;
302 msg->pose.covariance[10] = 0;
303 msg->pose.covariance[11] = 0;
307 msg->pose.covariance[15] = 0;
308 msg->pose.covariance[16] = 0;
309 msg->pose.covariance[17] = 0;
310 msg->pose.covariance[18] = 0;
311 msg->pose.covariance[19] = 0;
312 msg->pose.covariance[20] = 0;
316 msg->pose.covariance[24] = 0;
317 msg->pose.covariance[25] = 0;
318 msg->pose.covariance[26] = 0;
322 msg->pose.covariance[30] = 0;
323 msg->pose.covariance[31] = 0;
324 msg->pose.covariance[32] = 0;
334 diagnostic_msgs::DiagnosticArrayPtr msg = boost::make_shared<diagnostic_msgs::DiagnosticArray>();
336 diagnostic_msgs::DiagnosticStatusPtr gnss_status = boost::make_shared<diagnostic_msgs::DiagnosticStatus>();
338 uint16_t indicators_type_mask =
static_cast<uint16_t
>(255);
339 uint16_t indicators_value_mask =
static_cast<uint16_t
>(3840);
340 uint16_t qualityind_pos;
348 gnss_status->level = diagnostic_msgs::DiagnosticStatus::STALE;
353 gnss_status->level = diagnostic_msgs::DiagnosticStatus::WARN;
357 gnss_status->level = diagnostic_msgs::DiagnosticStatus::OK;
365 gnss_status->level = diagnostic_msgs::DiagnosticStatus::ERROR;
369 for(uint16_t i = static_cast<uint16_t>(0); i !=
static_cast<uint16_t
>(
last_qualityind_.
n); ++i)
371 if(i == qualityind_pos)
377 gnss_status->values[i].key =
"GNSS Signals, Main Antenna";
382 gnss_status->values[i].key =
"GNSS Signals, Aux1 Antenna";
387 gnss_status->values[i].key =
"RF Power, Main Antenna";
392 gnss_status->values[i].key =
"RF Power, Aux1 Antenna";
397 gnss_status->values[i].key =
"CPU Headroom";
402 gnss_status->values[i].key =
"OCXO Stability";
407 gnss_status->values[i].key =
"Base Station Measurements";
413 gnss_status->values[i].key =
"RTK Post-Processing";
417 gnss_status->hardware_id = serialnumber;
418 gnss_status->name =
"GNSS";
419 gnss_status->message =
"Quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)";
420 msg->status.push_back(*gnss_status);
432 sensor_msgs::NavSatFixPtr msg = boost::make_shared<sensor_msgs::NavSatFix>();
439 msg->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
444 msg->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
449 msg->status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX;
454 msg->status.status = sensor_msgs::NavSatStatus::STATUS_SBAS_FIX;
459 throw std::runtime_error(
"PVTGeodetic's Mode field contains an invalid type of PVT solution.");
462 bool gps_in_pvt =
false;
463 bool glo_in_pvt =
false;
464 bool com_in_pvt =
false;
465 bool gal_in_pvt =
false;
467 for(
int bit = 0; bit != 31; ++bit)
470 if (bit <= 5 && in_use)
474 if (8 <= bit && bit <= 12 && in_use) glo_in_pvt =
true;
475 if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use) com_in_pvt =
true;
476 if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use) gal_in_pvt =
true;
480 uint16_t service = gps_in_pvt*1+glo_in_pvt*2+com_in_pvt*4+gal_in_pvt*8;
481 msg->status.service = service;
494 msg->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
521 gps_common::GPSFixPtr msg = boost::make_shared<gps_common::GPSFix>();
526 std::vector<int32_t> cno_tracked;
527 std::vector<int32_t> svid_in_sync;
538 svid_in_sync.push_back(static_cast<int32_t>(measepoch_channel_type1->
sv_id));
539 uint8_t type_mask = 15;
540 if (((measepoch_channel_type1->
type & type_mask) == static_cast<uint8_t>(1)) ||
541 ((measepoch_channel_type1->
type & type_mask) == static_cast<uint8_t>(2)))
543 cno_tracked.push_back(static_cast<int32_t>(measepoch_channel_type1->
cn0)/4);
547 cno_tracked.push_back(static_cast<int32_t>(measepoch_channel_type1->
cn0)/4+static_cast<int32_t>(10));
550 for (int32_t j = 0; j < static_cast<int32_t>(measepoch_channel_type1->
n_type2); j++)
558 std::vector<int32_t> svid_in_sync_2;
559 std::vector<int32_t> elevation_tracked;
560 std::vector<int32_t> azimuth_tracked;
561 std::vector<int32_t> svid_pvt;
563 std::vector<int32_t> ordering;
571 uint16_t azimuth_mask = 511;
577 bool to_be_added =
false;
578 for (int32_t j = 0; j < static_cast<int32_t>(svid_in_sync.size()); ++j)
580 if (svid_in_sync[j] == static_cast<int32_t>(channel_sat_info->
sv_id))
582 ordering.push_back(j);
589 svid_in_sync_2.push_back(static_cast<int32_t>(channel_sat_info->
sv_id));
590 elevation_tracked.push_back(static_cast<int32_t>(channel_sat_info->
elev));
591 azimuth_tracked.push_back(static_cast<int32_t>((channel_sat_info->
az_rise_set & azimuth_mask)));
594 for (int32_t j = 0; j < static_cast<int32_t>(channel_sat_info->
n2); j++)
599 bool pvt_status =
false;
600 uint16_t pvt_status_mask = std::pow(2,15)+std::pow(2,14);
601 for(
int k = 15; k != -1; k -= 2)
603 uint16_t pvt_status_value = (channel_state_info->
pvt_status & pvt_status_mask) >> k-1;
604 if (pvt_status_value == 2)
610 pvt_status_mask = pvt_status_mask - std::pow(2,k) - std::pow(2,k-1)
611 + std::pow(2,k-2) + std::pow(2,k-3);
616 svid_pvt.push_back(static_cast<int32_t>(channel_sat_info->
sv_id));
622 msg->status.satellite_used_prn = svid_pvt;
623 msg->status.satellites_visible =
static_cast<uint16_t
>(svid_in_sync.size());
624 msg->status.satellite_visible_prn = svid_in_sync_2;
625 msg->status.satellite_visible_z = elevation_tracked;
626 msg->status.satellite_visible_azimuth = azimuth_tracked;
629 std::vector<int32_t> cno_tracked_reordered;
632 for (int32_t k = 0; k < static_cast<int32_t>(ordering.size()); ++k)
634 cno_tracked_reordered.push_back(cno_tracked[ordering[k]]);
637 msg->status.satellite_visible_snr = cno_tracked_reordered;
640 uint16_t status_mask = 15;
646 msg->status.status = gps_common::GPSStatus::STATUS_NO_FIX;
651 msg->status.status = gps_common::GPSStatus::STATUS_FIX;
656 msg->status.status = gps_common::GPSStatus::STATUS_GBAS_FIX;
663 if (reference_id == 131 || reference_id == 133 || reference_id == 135 || reference_id == 135)
665 msg->status.status = gps_common::GPSStatus::STATUS_WAAS_FIX;
669 msg->status.status = gps_common::GPSStatus::STATUS_SBAS_FIX;
675 throw std::runtime_error(
"PVTGeodetic's Mode field contains an invalid type of PVT solution.");
679 msg->status.motion_source = gps_common::GPSStatus::SOURCE_POINTS;
681 msg->status.orientation_source = gps_common::GPSStatus::SOURCE_POINTS;
682 msg->status.position_source = gps_common::GPSStatus::SOURCE_GPS;
695 msg->gdop =
static_cast<double>(-1);
699 msg->gdop = std::sqrt(std::pow(static_cast<double>(
last_dop_.
pdop)/100, 2) +
704 msg->pdop =
static_cast<double>(-1);
712 msg->hdop =
static_cast<double>(-1);
720 msg->vdop =
static_cast<double>(-1);
728 msg->tdop =
static_cast<double>(-1);
740 msg->err_track = 2*(std::sqrt(std::pow(static_cast<double>(1)/(static_cast<double>(
last_pvtgeodetic_.
vn) +
760 msg->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;
772 uint16_t hh = (tow%(1000*60*60*24))/(60*60*1000);
773 uint16_t mm = ((tow%(1000*60*60*24))-hh*(60*60*1000))/(60*1000);
774 uint16_t ss = ((tow%(1000*60*60*24))-hh*(60*60*1000)-mm*(60*1000))/(1000);
775 uint16_t hs = ((tow%(1000*60*60*24))-hh*(60*60*1000)-mm*(60*1000)-ss*1000)/10;
803 boost::format fmt = boost::format(
"%02u%02u%02u.%02u") % hh % mm % ss % hs;
804 std::string utc_string = fmt.str();
810 uint32_t unix_time_nanoseconds = (
static_cast<uint32_t
>(utc_double*100)%100)*10000000;
811 ros::Time time_obj(unix_time_seconds, unix_time_nanoseconds);
816 return ros::Time::now();
856 std::size_t count_copy =
count_;
864 if (count_copy == 0)
break;
870 &&
data_[pos+4] == 0x52));
879 if (count_copy == 0)
break;
890 uint16_t mask = 8191;
891 if (*(reinterpret_cast<const uint16_t *>(
data_ + 4)) & mask == static_cast<const uint16_t>(
id))
913 boost::char_separator<char> sep(
",");
914 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
916 std::string block_in_string(reinterpret_cast<const char*>(
data_), nmea_size);
917 tokenizer tokens(block_in_string,sep);
918 if (*tokens.begin() == id)
1035 uint16_t mask = 8191;
1037 uint16_t value = (*(
reinterpret_cast<const uint16_t*
>(
data_+4))) & mask;
1038 std::stringstream ss;
1044 boost::char_separator<char> sep(
",");
1045 typedef boost::tokenizer<boost::char_separator<char> > tokenizer;
1047 std::string block_in_string(reinterpret_cast<const char*>(
data_), nmea_size);
1048 tokenizer tokens(block_in_string,sep);
1049 return *tokens.begin();
1051 return std::string();
1070 uint16_t block_length;
1073 block_length = *(
reinterpret_cast<const uint16_t*
>(
data_ + 6));
1074 return block_length;
1088 std::size_t jump_size;
1097 jump_size =
static_cast<uint32_t
>(1);
1106 if (jump_size == 0) jump_size =
static_cast<std::size_t
>(1);
1110 jump_size =
static_cast<std::size_t
>(1);
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".
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
#define CARRIAGE_RETURN
0x0D is ASCII for "Carriage Return", i.e. "Enter"
#define NMEA_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
uint32_t g_leap_seconds
The number of leap seconds that have been inserted into the UTC time.
static PosCovGeodetic last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
BlockHeader_t block_header
rosaic::PVTGeodeticPtr PVTGeodeticCallback(PVTGeodetic &data)
Callback function when reading PVTGeodetic blocks.
bool toDouble(const std::string &string, double &value)
Interprets the contents of "string" as a floating point number of type double It stores the "string"'...
static DOP last_dop_
Since GPSFix needs DOP, incoming DOP blocks need to be stored.
BlockHeader_t block_header
static MeasEpoch last_measepoch_
Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks need to be stored.
bool isResponse()
Determines whether data_ currently points to an NMEA message.
rosaic::PosCovCartesianPtr PosCovCartesianCallback(PosCovCartesian &data)
Callback function when reading PosCovCartesian blocks.
char rx_serial_number[20]
Struct for the SBF block "VelCovGeodetic".
Struct for the SBF sub-block "MeasEpochChannelType1".
Struct for the SBF block "ReceiverSetup".
#define SBF_SYNC_BYTE_2
0x40 is ASCII for @ - 2nd byte to indicate SBF block
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".
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.
#define NMEA_SYNC_BYTE_2_1
0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message
Struct for the SBF block "PVTGeodetic".
#define RESPONSE_SYNC_BYTE_3
0x3F is ASCII for ? - 3rd byte in the response message from the Rx in case the command was invalid ...
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".
BlockHeader_t block_header
Defines a class that reads messages handed over from the circular buffer.
rosaic::AttEulerPtr AttEulerCallback(AttEuler &data)
Callback function when reading AttEuler blocks.
bool isErrorMessage()
Determines whether data_ currently points to an error message reply from the Rx.
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".
BlockHeader_t block_header
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.
Struct for the SBF sub-block "ChannelStateInfo".
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...
Struct for the SBF block "PVTCartesian".
uint16_t getBlockLength()
Gets the length of the SBF block.
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...
Struct for the SBF block "PosCovCartesian".
std::size_t messageSize()
Determines size of the message (also command reply) that data_ is currently pointing at...
#define CONNECTION_DESCRIPTOR_BYTE_1
0x49 is ASCII for I - 1st character of connection descriptor sent by the Rx after initiating TCP conn...
rosaic::AttCovEulerPtr AttCovEulerCallback(AttCovEuler &data)
Callback function when reading AttCovEuler blocks.
const uint8_t * getEndBuffer()
Gets the end position in the read buffer.
BlockHeader_t block_header
static uint32_t count_gpsfix_
Number of times the gps_common::GPSFix message has been published.
Struct for the SBF sub-block "ChannelSatInfo".
Struct for the SBF block "ChannelStatus".
Struct for the SBF block "MeasEpoch".
std::pair< uint16_t, TypeOfPVT_Enum > type_of_pvt_pairs[]
Pair of iterators to facilitate initialization of the map.
#define NMEA_SYNC_BYTE_2_2
0x50 is ASCII for P - 2nd byte to indicate proprietary ASCII message
diagnostic_msgs::DiagnosticArrayPtr DiagnosticArrayCallback()
"Callback" function when constructing diagnostic_msgs::DiagnosticArray messages
BlockHeader_t block_header
Struct for the SBF block "DOP".
#define SBF_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
#define RESPONSE_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each response from the Rx
std::pair< std::string, RxID_Enum > rx_id_pairs[]
Pair of iterators to facilitate initialization of the map.
time_t convertUTCtoUnix(double utc_double)
Converts UTC time from the without-colon-delimiter format to Unix Epoch time (a number-of-seconds-sin...
uint8_t data[((80+1) *sizeof(MeasEpochChannelType1)+(((80+1)) *(((7) *(3)) -1)) *sizeof(MeasEpochChannelType2))]
geometry_msgs::PoseWithCovarianceStampedPtr PoseWithCovarianceStampedCallback()
"Callback" function when constructing PoseWithCovarianceStamped messages
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.
bool found_
Whether or not a message has been found.
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)
BlockHeader_t block_header
uint8_t data[(80+1) *sizeof(ChannelSatInfo)+((80+1) *4) *sizeof(ChannelStateInfo)]
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.
#define LINE_FEED
0x0A is ASCII for "Line Feed", i.e. "New Line"
#define RESPONSE_SYNC_BYTE_2
0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx
BlockHeader_t block_header
uint32_t g_cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
geometry_msgs::Quaternion convertEulerToQuaternion(double yaw, double pitch, double roll)
Transforms Euler angles to a quaternion.
#define CONNECTION_DESCRIPTOR_BYTE_2
0x50 is ASCII for P - 2nd character of connection descriptor sent by the Rx after initiating TCP conn...
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".