rx_message.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // © Copyright 2020, Septentrio NV/SA.
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // 1. Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // 2. Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // 3. Neither the name of the copyright holder nor the names of its
14 // contributors may be used to endorse or promote products derived
15 // from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 // *****************************************************************************
30 
31 #include <thread>
32 
33 #include <GeographicLib/UTMUPS.hpp>
34 
35 #include <septentrio_gnss_driver/communication/rx_message.hpp>
36 
51 using parsing_utilities::rad2deg;
52 using parsing_utilities::deg2rad;
53 using parsing_utilities::deg2radSq;
54 
55 PoseWithCovarianceStampedMsg
56 io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback()
57 {
58  PoseWithCovarianceStampedMsg msg;
59  if (settings_->septentrio_receiver_type == "gnss")
60  {
61  // Filling in the pose data
62  double yaw = 0.0;
63  if (validValue(last_atteuler_.heading))
64  yaw = last_atteuler_.heading;
65  double pitch = 0.0;
66  if (validValue(last_atteuler_.pitch))
67  pitch = last_atteuler_.pitch;
68  double roll = 0.0;
69  if (validValue(last_atteuler_.roll))
70  roll = last_atteuler_.roll;
71  msg.pose.pose.orientation = parsing_utilities::convertEulerToQuaternion(
72  deg2rad(yaw),
73  deg2rad(pitch),
74  deg2rad(roll));
75  msg.pose.pose.position.x = rad2deg(last_pvtgeodetic_.longitude);
76  msg.pose.pose.position.y = rad2deg(last_pvtgeodetic_.latitude);
77  msg.pose.pose.position.z = last_pvtgeodetic_.height;
78  // Filling in the covariance data in row-major order
79  msg.pose.covariance[0] = last_poscovgeodetic_.cov_lonlon;
80  msg.pose.covariance[1] = last_poscovgeodetic_.cov_latlon;
81  msg.pose.covariance[2] = last_poscovgeodetic_.cov_lonhgt;
82  msg.pose.covariance[6] = last_poscovgeodetic_.cov_latlon;
83  msg.pose.covariance[7] = last_poscovgeodetic_.cov_latlat;
84  msg.pose.covariance[8] = last_poscovgeodetic_.cov_lathgt;
85  msg.pose.covariance[12] = last_poscovgeodetic_.cov_lonhgt;
86  msg.pose.covariance[13] = last_poscovgeodetic_.cov_lathgt;
87  msg.pose.covariance[14] = last_poscovgeodetic_.cov_hgthgt;
88  msg.pose.covariance[21] = deg2radSq(last_attcoveuler_.cov_rollroll);
89  msg.pose.covariance[22] = deg2radSq(last_attcoveuler_.cov_pitchroll);
90  msg.pose.covariance[23] = deg2radSq(last_attcoveuler_.cov_headroll);
91  msg.pose.covariance[27] = deg2radSq(last_attcoveuler_.cov_pitchroll);
92  msg.pose.covariance[28] = deg2radSq(last_attcoveuler_.cov_pitchpitch);
93  msg.pose.covariance[29] = deg2radSq(last_attcoveuler_.cov_headpitch);
94  msg.pose.covariance[33] = deg2radSq(last_attcoveuler_.cov_headroll);
95  msg.pose.covariance[34] = deg2radSq(last_attcoveuler_.cov_headpitch);
96  msg.pose.covariance[35] = deg2radSq(last_attcoveuler_.cov_headhead);
97  }
98  if (settings_->septentrio_receiver_type == "ins")
99  {
100  msg.pose.pose.position.x = rad2deg(last_insnavgeod_.longitude);
101  msg.pose.pose.position.y = rad2deg(last_insnavgeod_.latitude);
102  msg.pose.pose.position.z = last_insnavgeod_.height;
103 
104  // Filling in the pose data
105  if((last_insnavgeod_.sb_list & 1) !=0)
106  {
107  // Pos autocov
108  msg.pose.covariance[0] = parsing_utilities::square(last_insnavgeod_.
109  longitude_std_dev);
110  msg.pose.covariance[7] = parsing_utilities::square(last_insnavgeod_.
111  latitude_std_dev);
112  msg.pose.covariance[14] = parsing_utilities::square(last_insnavgeod_.
113  height_std_dev);
114  }
115  else
116  {
117  msg.pose.covariance[0] = -1.0;
118  msg.pose.covariance[7] = -1.0;
119  msg.pose.covariance[14] = -1.0;
120  }
121  if ((last_insnavgeod_.sb_list & 2) !=0)
122  {
123  double yaw = 0.0;
124  if (validValue(last_insnavgeod_.heading))
125  yaw = last_insnavgeod_.heading;
126  double pitch = 0.0;
127  if (validValue(last_insnavgeod_.pitch))
128  pitch = last_insnavgeod_.pitch;
129  double roll = 0.0;
130  if (validValue(last_insnavgeod_.roll))
131  roll = last_insnavgeod_.roll;
132  // Attitude
133  msg.pose.pose.orientation = parsing_utilities::convertEulerToQuaternion(
134  deg2rad(yaw),
135  deg2rad(pitch),
136  deg2rad(roll));
137  }
138  else
139  {
140  msg.pose.pose.orientation.w = std::numeric_limits<double>::quiet_NaN();
141  msg.pose.pose.orientation.x = std::numeric_limits<double>::quiet_NaN();
142  msg.pose.pose.orientation.y = std::numeric_limits<double>::quiet_NaN();
143  msg.pose.pose.orientation.z = std::numeric_limits<double>::quiet_NaN();
144  }
145  if((last_insnavgeod_.sb_list & 4) !=0)
146  {
147  // Attitude autocov
148  if (validValue(last_insnavgeod_.roll_std_dev))
149  msg.pose.covariance[21] = parsing_utilities::square(deg2rad(last_insnavgeod_.
150  roll_std_dev));
151  else
152  msg.pose.covariance[21] = -1.0;
153  if (validValue(last_insnavgeod_.pitch_std_dev))
154  msg.pose.covariance[28] = parsing_utilities::square(deg2rad(last_insnavgeod_.
155  pitch_std_dev));
156  else
157  msg.pose.covariance[28] = -1.0;
158  if (validValue(last_insnavgeod_.heading_std_dev))
159  msg.pose.covariance[35] = parsing_utilities::square(deg2rad(last_insnavgeod_.
160  heading_std_dev));
161  else
162  msg.pose.covariance[35] = -1.0;
163  }
164  else
165  {
166  msg.pose.covariance[21] = -1.0;
167  msg.pose.covariance[28] = -1.0;
168  msg.pose.covariance[35] = -1.0;
169  }
170  if((last_insnavgeod_.sb_list & 32) !=0)
171  {
172  // Pos cov
173  msg.pose.covariance[1] = last_insnavgeod_.
174  latitude_longitude_cov;
175  msg.pose.covariance[2] = last_insnavgeod_.
176  longitude_height_cov;
177  msg.pose.covariance[6] = last_insnavgeod_.
178  latitude_longitude_cov;
179  msg.pose.covariance[8] = last_insnavgeod_.
180  latitude_height_cov;
181  msg.pose.covariance[12] = last_insnavgeod_.
182  longitude_height_cov;
183  msg.pose.covariance[13] = last_insnavgeod_.
184  latitude_height_cov;
185  }
186  if ((last_insnavgeod_.sb_list & 64) !=0)
187  {
188  // Attitude cov
189  msg.pose.covariance[22] = deg2radSq(last_insnavgeod_.
190  pitch_roll_cov);
191  msg.pose.covariance[23] = deg2radSq(last_insnavgeod_.
192  heading_roll_cov);
193  msg.pose.covariance[27] = deg2radSq(last_insnavgeod_.
194  pitch_roll_cov);
195 
196  msg.pose.covariance[29] = deg2radSq(last_insnavgeod_.
197  heading_pitch_cov);
198  msg.pose.covariance[33] = deg2radSq(last_insnavgeod_.
199  heading_roll_cov);
200  msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.
201  heading_pitch_cov);
202  }
203  }
204  return msg;
205 };
206 
207 DiagnosticArrayMsg io_comm_rx::RxMessage::DiagnosticArrayCallback()
208 {
209  DiagnosticArrayMsg msg;
210  std::string serialnumber(last_receiversetup_.rx_serial_number);
211  DiagnosticStatusMsg gnss_status;
212  // Constructing the "level of operation" field
213  uint16_t indicators_type_mask = static_cast<uint16_t>(255);
214  uint16_t indicators_value_mask = static_cast<uint16_t>(3840);
215  uint16_t qualityind_pos;
216  for (uint16_t i = static_cast<uint16_t>(0); i < last_qualityind_.indicators.size(); ++i)
217  {
218  if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
219  static_cast<uint16_t>(0))
220  {
221  qualityind_pos = i;
222  if (((last_qualityind_.indicators[i] & indicators_value_mask) >> 8) ==
223  static_cast<uint16_t>(0))
224  {
225  gnss_status.level = DiagnosticStatusMsg::STALE;
226  } else if (((last_qualityind_.indicators[i] & indicators_value_mask) >>
227  8) == static_cast<uint16_t>(1) ||
228  ((last_qualityind_.indicators[i] & indicators_value_mask) >>
229  8) == static_cast<uint16_t>(2))
230  {
231  gnss_status.level = DiagnosticStatusMsg::WARN;
232  } else
233  {
234  gnss_status.level = DiagnosticStatusMsg::OK;
235  }
236  break;
237  }
238  }
239  // If the ReceiverStatus's RxError field is not 0, then at least one error has
240  // been detected.
241  if (last_receiverstatus_.rx_error != static_cast<uint32_t>(0))
242  {
243  gnss_status.level = DiagnosticStatusMsg::ERROR;
244  }
245  // Creating an array of values associated with the GNSS status
246  gnss_status.values.resize(static_cast<uint16_t>(last_qualityind_.n - 1));
247  for (uint16_t i = static_cast<uint16_t>(0);
248  i != static_cast<uint16_t>(last_qualityind_.n); ++i)
249  {
250  if (i == qualityind_pos)
251  {
252  continue;
253  }
254  if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
255  static_cast<uint16_t>(1))
256  {
257  gnss_status.values[i].key = "GNSS Signals, Main Antenna";
258  gnss_status.values[i].value = std::to_string(
259  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
260  } else if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
261  static_cast<uint16_t>(2))
262  {
263  gnss_status.values[i].key = "GNSS Signals, Aux1 Antenna";
264  gnss_status.values[i].value = std::to_string(
265  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
266  } else if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
267  static_cast<uint16_t>(11))
268  {
269  gnss_status.values[i].key = "RF Power, Main Antenna";
270  gnss_status.values[i].value = std::to_string(
271  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
272  } else if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
273  static_cast<uint16_t>(12))
274  {
275  gnss_status.values[i].key = "RF Power, Aux1 Antenna";
276  gnss_status.values[i].value = std::to_string(
277  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
278  } else if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
279  static_cast<uint16_t>(21))
280  {
281  gnss_status.values[i].key = "CPU Headroom";
282  gnss_status.values[i].value = std::to_string(
283  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
284  } else if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
285  static_cast<uint16_t>(25))
286  {
287  gnss_status.values[i].key = "OCXO Stability";
288  gnss_status.values[i].value = std::to_string(
289  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
290  } else if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
291  static_cast<uint16_t>(30))
292  {
293  gnss_status.values[i].key = "Base Station Measurements";
294  gnss_status.values[i].value = std::to_string(
295  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
296  } else
297  {
298  assert((last_qualityind_.indicators[i] & indicators_type_mask) ==
299  static_cast<uint16_t>(31));
300  gnss_status.values[i].key = "RTK Post-Processing";
301  gnss_status.values[i].value = std::to_string(
302  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
303  }
304  }
305  gnss_status.hardware_id = serialnumber;
306  gnss_status.name = "gnss";
307  gnss_status.message =
308  "Quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)";
309  msg.status.push_back(gnss_status);
310  return msg;
311 };
312 
313 ImuMsg
314 io_comm_rx::RxMessage::ImuCallback()
315 {
316  ImuMsg msg;
317 
318  msg.linear_acceleration.x = last_extsensmeas_.acceleration_x;
319  msg.linear_acceleration.y = last_extsensmeas_.acceleration_y;
320  msg.linear_acceleration.z = last_extsensmeas_.acceleration_z;
321 
322  msg.angular_velocity.x = last_extsensmeas_.angular_rate_x;
323  msg.angular_velocity.y = last_extsensmeas_.angular_rate_y;
324  msg.angular_velocity.z = last_extsensmeas_.angular_rate_z;
325 
326  bool valid_orientation = true;
327  if (settings_->septentrio_receiver_type == "ins")
328  {
329  if (validValue(last_insnavgeod_.block_header.tow))
330  {
331  Timestamp tsImu = timestampSBF(last_extsensmeas_.block_header.tow, last_extsensmeas_.block_header.wnc, true);
332  Timestamp tsIns = timestampSBF(last_insnavgeod_.block_header.tow, last_insnavgeod_.block_header.wnc, true);// Filling in the oreintation data
333 
334  static int64_t maxDt = (settings_->polling_period_pvt == 0) ? 10000000 : settings_->polling_period_pvt * 1000000;
335  if ((tsImu - tsIns) > maxDt)
336  {
337  valid_orientation = false;
338  }
339  else
340  {
341  if ((last_insnavgeod_.sb_list & 2) !=0)
342  {
343  // Attitude
344  if (validValue(last_insnavgeod_.heading) &&
345  validValue(last_insnavgeod_.pitch ) &&
346  validValue(last_insnavgeod_.roll ))
347  {
348  msg.orientation = parsing_utilities::convertEulerToQuaternion(
349  deg2rad(last_insnavgeod_.heading),
350  deg2rad(last_insnavgeod_.pitch),
351  deg2rad(last_insnavgeod_.roll));
352  }
353  else
354  {
355  valid_orientation = false;
356  }
357  }
358  else
359  {
360  valid_orientation = false;
361  }
362  if((last_insnavgeod_.sb_list & 4) !=0)
363  {
364  // Attitude autocov
365  if (validValue(last_insnavgeod_.roll_std_dev ) &&
366  validValue(last_insnavgeod_.pitch_std_dev ) &&
367  validValue(last_insnavgeod_.heading_std_dev))
368  {
369  msg.orientation_covariance[0] = parsing_utilities::square(deg2rad(last_insnavgeod_.
370  roll_std_dev));
371  msg.orientation_covariance[4] = parsing_utilities::square(deg2rad(last_insnavgeod_.
372  pitch_std_dev));
373  msg.orientation_covariance[8] = parsing_utilities::square(deg2rad(last_insnavgeod_.
374  heading_std_dev));
375  }
376  else
377  {
378  valid_orientation = false;
379  }
380  }
381  else
382  {
383  valid_orientation = false;
384  }
385  if ((last_insnavgeod_.sb_list & 64) !=0)
386  {
387  // Attitude cov
388  msg.orientation_covariance[1] = deg2radSq(last_insnavgeod_.
389  pitch_roll_cov);
390  msg.orientation_covariance[2] = deg2radSq(last_insnavgeod_.
391  heading_roll_cov);
392  msg.orientation_covariance[3] = deg2radSq(last_insnavgeod_.
393  pitch_roll_cov);
394 
395  msg.orientation_covariance[5] = deg2radSq(last_insnavgeod_.
396  heading_pitch_cov);
397  msg.orientation_covariance[6] = deg2radSq(last_insnavgeod_.
398  heading_roll_cov);
399  msg.orientation_covariance[7] = deg2radSq(last_insnavgeod_.
400  heading_pitch_cov);
401  }
402  }
403  }
404  else
405  {
406  valid_orientation = false;
407  }
408  }
409  else
410  {
411  valid_orientation = false;
412  }
413 
414  if (!valid_orientation)
415  {
416  msg.orientation.w = std::numeric_limits<double>::quiet_NaN();
417  msg.orientation.x = std::numeric_limits<double>::quiet_NaN();
418  msg.orientation.y = std::numeric_limits<double>::quiet_NaN();
419  msg.orientation.z = std::numeric_limits<double>::quiet_NaN();
420  msg.orientation_covariance[0] = -1.0;
421  msg.orientation_covariance[4] = -1.0;
422  msg.orientation_covariance[8] = -1.0;
423  }
424 
425  return msg;
426 };
427 
433 LocalizationUtmMsg
434 io_comm_rx::RxMessage::LocalizationUtmCallback()
435 {
436  LocalizationUtmMsg msg;
437 
438  int zone;
439  std::string zonestring;
440  bool northernHemisphere;
441  double easting;
442  double northing;
443  double gamma = 0.0;
444  if (fixedUtmZone_)
445  {
446  double k;
447  GeographicLib::UTMUPS::DecodeZone(*fixedUtmZone_, zone, northernHemisphere);
448  GeographicLib::UTMUPS::Forward(rad2deg(last_insnavgeod_.latitude),
449  rad2deg(last_insnavgeod_.longitude),
450  zone, northernHemisphere, easting, northing, gamma, k, zone);
451  zonestring = *fixedUtmZone_;
452  }
453  else
454  {
455  double k;
456  GeographicLib::UTMUPS::Forward(rad2deg(last_insnavgeod_.latitude),
457  rad2deg(last_insnavgeod_.longitude),
458  zone, northernHemisphere, easting, northing, gamma, k);
459  zonestring = GeographicLib::UTMUPS::EncodeZone(zone, northernHemisphere);
460  }
461  if (settings_->lock_utm_zone && !fixedUtmZone_)
462  fixedUtmZone_ = std::make_shared<std::string>(zonestring);
463 
464  // UTM position (ENU)
465  if (settings_->use_ros_axis_orientation)
466  {
467  msg.pose.pose.position.x = easting;
468  msg.pose.pose.position.y = northing;
469  msg.pose.pose.position.z = last_insnavgeod_.height;
470  }
471  else // (NED)
472  {
473  msg.pose.pose.position.x = northing;
474  msg.pose.pose.position.y = easting;
475  msg.pose.pose.position.z = -last_insnavgeod_.height;
476  }
477 
478  msg.header.frame_id = "utm_" + zonestring;
479  if (settings_->ins_use_poi)
480  msg.child_frame_id = settings_->poi_frame_id; // TODO param
481  else
482  msg.child_frame_id = settings_->frame_id;
483 
484  if((last_insnavgeod_.sb_list & 1) !=0)
485  {
486  // Position autocovariance
487  msg.pose.covariance[0] = parsing_utilities::square(last_insnavgeod_.longitude_std_dev);
488  msg.pose.covariance[7] = parsing_utilities::square(last_insnavgeod_.latitude_std_dev);
489  msg.pose.covariance[14] = parsing_utilities::square(last_insnavgeod_.height_std_dev);
490  }
491  else
492  {
493  msg.pose.covariance[0] = -1.0;
494  msg.pose.covariance[7] = -1.0;
495  msg.pose.covariance[14] = -1.0;
496  }
497 
498  // Euler angles
499  double roll = 0.0;
500  if (validValue(last_insnavgeod_.roll))
501  roll = deg2rad(last_insnavgeod_.roll);
502  double pitch = 0.0;
503  if (validValue(last_insnavgeod_.pitch))
504  pitch = deg2rad(last_insnavgeod_.pitch);
505  double yaw = 0.0;
506  if (validValue(last_insnavgeod_.heading))
507  yaw = deg2rad(last_insnavgeod_.heading);
508  // gamma for conversion from true north to grid north
509  if (settings_->use_ros_axis_orientation)
510  yaw -= deg2rad(gamma);
511  else
512  yaw += deg2rad(gamma);
513 
514  Eigen::Matrix3d R_n_b = parsing_utilities::rpyToRot(roll, pitch, yaw).inverse();
515  if ((last_insnavgeod_.sb_list & 2) !=0)
516  {
517  // Attitude
518  msg.pose.pose.orientation = parsing_utilities::convertEulerToQuaternion(yaw, pitch, roll);
519  }
520  else
521  {
522  msg.pose.pose.orientation.w = std::numeric_limits<double>::quiet_NaN();
523  msg.pose.pose.orientation.x = std::numeric_limits<double>::quiet_NaN();
524  msg.pose.pose.orientation.y = std::numeric_limits<double>::quiet_NaN();
525  msg.pose.pose.orientation.z = std::numeric_limits<double>::quiet_NaN();
526  }
527  if((last_insnavgeod_.sb_list & 4) !=0)
528  {
529  // Attitude autocovariance
530  if (validValue(last_insnavgeod_.roll_std_dev))
531  msg.pose.covariance[21] = parsing_utilities::square(deg2rad(last_insnavgeod_.
532  roll_std_dev));
533  else
534  msg.pose.covariance[21] = -1.0;
535  if (validValue(last_insnavgeod_.pitch_std_dev))
536  msg.pose.covariance[28] = parsing_utilities::square(deg2rad(last_insnavgeod_.
537  pitch_std_dev));
538  else
539  msg.pose.covariance[28] = -1.0;
540  if (validValue(last_insnavgeod_.heading_std_dev))
541  msg.pose.covariance[35] = parsing_utilities::square(deg2rad(last_insnavgeod_.
542  heading_std_dev));
543  else
544  msg.pose.covariance[35] = -1.0;
545  }
546  else
547  {
548  msg.pose.covariance[21] = -1.0;
549  msg.pose.covariance[28] = -1.0;
550  msg.pose.covariance[35] = -1.0;
551  }
552  if((last_insnavgeod_.sb_list & 8) !=0)
553  {
554  // Linear velocity (ENU)
555  double ve = 0.0;
556  if (validValue(last_insnavgeod_.ve))
557  ve = last_insnavgeod_.ve;
558  double vn = 0.0;
559  if (validValue(last_insnavgeod_.vn))
560  vn = last_insnavgeod_.vn;
561  double vu = 0.0;
562  if (validValue(last_insnavgeod_.vu))
563  vu = last_insnavgeod_.vu;
564  Eigen::Vector3d vel_enu;
565  if (settings_->use_ros_axis_orientation)
566  {
567  // (ENU)
568  vel_enu << ve,
569  vn,
570  vu;
571  }
572  else
573  {
574  // (NED)
575  vel_enu << vn,
576  ve,
577  -vu;
578  }
579  // Linear velocity, rotate to body coordinates
580  Eigen::Vector3d vel_body = R_n_b * vel_enu;
581  msg.twist.twist.linear.x = vel_body(0);
582  msg.twist.twist.linear.y = vel_body(1);
583  msg.twist.twist.linear.z = vel_body(2);
584  }
585  else
586  {
587  msg.twist.twist.linear.x = std::numeric_limits<double>::quiet_NaN();
588  msg.twist.twist.linear.y = std::numeric_limits<double>::quiet_NaN();
589  msg.twist.twist.linear.z = std::numeric_limits<double>::quiet_NaN();
590  }
591  Eigen::Matrix3d Cov_vel_n;
592  if ((last_insnavgeod_.sb_list & 16) !=0)
593  {
594  // Linear velocity autocovariance
595  if (validValue(last_insnavgeod_.ve_std_dev))
596  if (settings_->use_ros_axis_orientation)
597  Cov_vel_n(0,0) = parsing_utilities::square(last_insnavgeod_.ve_std_dev);
598  else
599  Cov_vel_n(0,0) = parsing_utilities::square(last_insnavgeod_.vn_std_dev);
600  else
601  Cov_vel_n(0,0) = -1.0;
602  if (validValue(last_insnavgeod_.vn_std_dev))
603  if (settings_->use_ros_axis_orientation)
604  Cov_vel_n(1,1) = parsing_utilities::square(last_insnavgeod_.vn_std_dev);
605  else
606  Cov_vel_n(1,1) = parsing_utilities::square(last_insnavgeod_.ve_std_dev);
607  else
608  Cov_vel_n(1,1) = -1.0;
609  if (validValue(last_insnavgeod_.vu_std_dev))
610  Cov_vel_n(2,2) = parsing_utilities::square(last_insnavgeod_.vu_std_dev);
611  else
612  Cov_vel_n(2,2) = -1.0;
613  }
614  else
615  {
616  Cov_vel_n(0,0) = -1.0;
617  Cov_vel_n(1,1) = -1.0;
618  Cov_vel_n(2,2) = -1.0;
619  }
620  if((last_insnavgeod_.sb_list & 32) !=0)
621  {
622  // Position covariance
623  msg.pose.covariance[1] = last_insnavgeod_.latitude_longitude_cov;
624  msg.pose.covariance[6] = last_insnavgeod_.latitude_longitude_cov;
625 
626  if (settings_->use_ros_axis_orientation)
627  {
628  // (ENU)
629  msg.pose.covariance[2] = last_insnavgeod_.longitude_height_cov;
630  msg.pose.covariance[8] = last_insnavgeod_.latitude_height_cov;
631  msg.pose.covariance[12] = last_insnavgeod_.longitude_height_cov;
632  msg.pose.covariance[13] = last_insnavgeod_.latitude_height_cov;
633  }
634  else
635  {
636  // (NED)
637  msg.pose.covariance[2] = -last_insnavgeod_.latitude_height_cov;
638  msg.pose.covariance[8] = -last_insnavgeod_.longitude_height_cov;
639  msg.pose.covariance[12] = -last_insnavgeod_.latitude_height_cov;
640  msg.pose.covariance[13] = -last_insnavgeod_.longitude_height_cov;
641  }
642  }
643  if ((last_insnavgeod_.sb_list & 64) !=0)
644  {
645  // Attitude covariancae
646  msg.pose.covariance[22] = deg2radSq(last_insnavgeod_.pitch_roll_cov);
647  msg.pose.covariance[23] = deg2radSq(last_insnavgeod_.heading_roll_cov);
648  msg.pose.covariance[27] = deg2radSq(last_insnavgeod_.pitch_roll_cov);
649 
650  msg.pose.covariance[29] = deg2radSq(last_insnavgeod_.heading_pitch_cov);
651  msg.pose.covariance[33] = deg2radSq(last_insnavgeod_.heading_roll_cov);
652  msg.pose.covariance[34] = deg2radSq(last_insnavgeod_.heading_pitch_cov);
653 
654  if (!settings_->use_ros_axis_orientation)
655  {
656  // (NED)
657  msg.pose.covariance[33] *= -1.0;
658  msg.pose.covariance[23] *= -1.0;
659  msg.pose.covariance[22] *= -1.0;
660  msg.pose.covariance[27] *= -1.0;
661  }
662  }
663  if((last_insnavgeod_.sb_list & 128) !=0)
664  {
665  Cov_vel_n(0,1) = Cov_vel_n(1,0) = last_insnavgeod_.ve_vn_cov;
666  if (settings_->use_ros_axis_orientation)
667  {
668  Cov_vel_n(0,2) = Cov_vel_n(2,0) = last_insnavgeod_.ve_vu_cov;
669  Cov_vel_n(2,1) = Cov_vel_n(1,2) = last_insnavgeod_.vn_vu_cov;
670  }
671  else
672  {
673  Cov_vel_n(0,2) = Cov_vel_n(2,0) = -last_insnavgeod_.vn_vu_cov;
674  Cov_vel_n(2,1) = Cov_vel_n(1,2) = -last_insnavgeod_.ve_vu_cov;
675  }
676  }
677 
678  if (((last_insnavgeod_.sb_list & 16) !=0) &&
679  ((last_insnavgeod_.sb_list & 2) !=0) &&
680  ((last_insnavgeod_.sb_list & 8) !=0) &&
681  validValue(last_insnavgeod_.ve_std_dev) &&
682  validValue(last_insnavgeod_.vn_std_dev) &&
683  validValue(last_insnavgeod_.vu_std_dev))
684  {
685  // Rotate covariance matrix to body coordinates
686  Eigen::Matrix3d Cov_vel_body = R_n_b * Cov_vel_n * R_n_b.transpose();
687 
688  msg.twist.covariance[0] = Cov_vel_body(0,0);
689  msg.twist.covariance[1] = Cov_vel_body(0,1);
690  msg.twist.covariance[2] = Cov_vel_body(0,2);
691  msg.twist.covariance[6] = Cov_vel_body(1,0);
692  msg.twist.covariance[7] = Cov_vel_body(1,1);
693  msg.twist.covariance[8] = Cov_vel_body(1,2);
694  msg.twist.covariance[12] = Cov_vel_body(2,0);
695  msg.twist.covariance[13] = Cov_vel_body(1,1);
696  msg.twist.covariance[14] = Cov_vel_body(2,2);
697  }
698  else
699  {
700  msg.twist.covariance[0] = -1.0;
701  msg.twist.covariance[7] = -1.0;
702  msg.twist.covariance[14] = -1.0;
703  }
704  // Autocovariances of angular velocity
705  msg.twist.covariance[21] = -1.0;
706  msg.twist.covariance[28] = -1.0;
707  msg.twist.covariance[35] = -1.0;
708  return msg;
709 };
710 
718 NavSatFixMsg io_comm_rx::RxMessage::NavSatFixCallback()
719 {
720  NavSatFixMsg msg;
721  uint16_t mask = 15; // We extract the first four bits using this mask.
722  if (settings_->septentrio_receiver_type == "gnss")
723  {
724  uint16_t type_of_pvt = ((uint16_t)(last_pvtgeodetic_.mode)) & mask;
725  switch (type_of_pvt_map[type_of_pvt])
726  {
727  case evNoPVT:
728  {
729  msg.status.status = NavSatStatusMsg::STATUS_NO_FIX;
730  break;
731  }
732  case evStandAlone:
733  case evFixed:
734  {
735  msg.status.status = NavSatStatusMsg::STATUS_FIX;
736  break;
737  }
738  case evDGPS:
739  case evRTKFixed:
740  case evRTKFloat:
741  case evMovingBaseRTKFixed:
742  case evMovingBaseRTKFloat:
743  case evPPP:
744  {
745  msg.status.status = NavSatStatusMsg::STATUS_GBAS_FIX;
746  break;
747  }
748  case evSBAS:
749  {
750  msg.status.status = NavSatStatusMsg::STATUS_SBAS_FIX;
751  break;
752  }
753  default:
754  {
755  node_->log(LogLevel::DEBUG, "PVTGeodetic's Mode field contains an invalid type of PVT solution.");
756  break;
757  }
758  }
759  bool gps_in_pvt = false;
760  bool glo_in_pvt = false;
761  bool com_in_pvt = false;
762  bool gal_in_pvt = false;
763  uint32_t mask_2 = 1;
764  for (int bit = 0; bit != 31; ++bit)
765  {
766  bool in_use = last_pvtgeodetic_.signal_info & mask_2;
767  if (bit <= 5 && in_use)
768  {
769  gps_in_pvt = true;
770  }
771  if (8 <= bit && bit <= 12 && in_use)
772  glo_in_pvt = true;
773  if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use)
774  com_in_pvt = true;
775  if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use)
776  gal_in_pvt = true;
777  mask_2 *= 2;
778  }
779  // Below, booleans will be promoted to integers automatically.
780  uint16_t service =
781  gps_in_pvt * 1 + glo_in_pvt * 2 + com_in_pvt * 4 + gal_in_pvt * 8;
782  msg.status.service = service;
783  msg.latitude = rad2deg(last_pvtgeodetic_.latitude);
784  msg.longitude = rad2deg(last_pvtgeodetic_.longitude);
785  msg.altitude = last_pvtgeodetic_.height;
786  msg.position_covariance[0] =last_poscovgeodetic_.cov_lonlon;
787  msg.position_covariance[1] =last_poscovgeodetic_.cov_latlon;
788  msg.position_covariance[2] =last_poscovgeodetic_.cov_lonhgt;
789  msg.position_covariance[3] =last_poscovgeodetic_.cov_latlon;
790  msg.position_covariance[4] =last_poscovgeodetic_.cov_latlat;
791  msg.position_covariance[5] =last_poscovgeodetic_.cov_lathgt;
792  msg.position_covariance[6] =last_poscovgeodetic_.cov_lonhgt;
793  msg.position_covariance[7] =last_poscovgeodetic_.cov_lathgt;
794  msg.position_covariance[8] =last_poscovgeodetic_.cov_hgthgt;
795  msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN;
796  return msg;
797  }
798 
799  if (settings_->septentrio_receiver_type == "ins")
800  {
801  NavSatFixMsg msg;
802  uint16_t type_of_pvt = ((uint16_t)(last_insnavgeod_.gnss_mode)) & mask;
803  switch (type_of_pvt_map[type_of_pvt])
804  {
805  case evNoPVT:
806  {
807  msg.status.status = NavSatStatusMsg::STATUS_NO_FIX;
808  break;
809  }
810  case evStandAlone:
811  case evFixed:
812  {
813  msg.status.status = NavSatStatusMsg::STATUS_FIX;
814  break;
815  }
816  case evDGPS:
817  case evRTKFixed:
818  case evRTKFloat:
819  case evMovingBaseRTKFixed:
820  case evMovingBaseRTKFloat:
821  case evPPP:
822  {
823  msg.status.status = NavSatStatusMsg::STATUS_GBAS_FIX;
824  break;
825  }
826  case evSBAS:
827  {
828  msg.status.status = NavSatStatusMsg::STATUS_SBAS_FIX;
829  break;
830  }
831  default:
832  {
833  node_->log(LogLevel::DEBUG, "INSNavGeod's Mode field contains an invalid type of PVT solution.");
834  break;
835  }
836  }
837  bool gps_in_pvt = false;
838  bool glo_in_pvt = false;
839  bool com_in_pvt = false;
840  bool gal_in_pvt = false;
841  uint32_t mask_2 = 1;
842  for (int bit = 0; bit != 31; ++bit)
843  {
844  bool in_use = last_pvtgeodetic_.signal_info & mask_2;
845  if (bit <= 5 && in_use)
846  {
847  gps_in_pvt = true;
848  }
849  if (8 <= bit && bit <= 12 && in_use)
850  glo_in_pvt = true;
851  if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use)
852  com_in_pvt = true;
853  if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use)
854  gal_in_pvt = true;
855  mask_2 *= 2;
856  }
857  // Below, booleans will be promoted to integers automatically.
858  uint16_t service =
859  gps_in_pvt * 1 + glo_in_pvt * 2 + com_in_pvt * 4 + gal_in_pvt * 8;
860  msg.status.service = service;
861  msg.latitude = rad2deg(last_insnavgeod_.latitude);
862  msg.longitude = rad2deg(last_insnavgeod_.longitude);
863  msg.altitude = last_insnavgeod_.height;
864 
865  if((last_insnavgeod_.sb_list & 1) !=0)
866  {
867  msg.position_covariance[0] = parsing_utilities::square(last_insnavgeod_.
868  longitude_std_dev);
869  msg.position_covariance[4] = parsing_utilities::square(last_insnavgeod_.
870  latitude_std_dev);
871  msg.position_covariance[8] = parsing_utilities::square(last_insnavgeod_.
872  height_std_dev);
873  }
874  if((last_insnavgeod_.sb_list & 32) !=0)
875  {
876  msg.position_covariance[1] = last_insnavgeod_.latitude_longitude_cov;
877  msg.position_covariance[2] = last_insnavgeod_.longitude_height_cov;
878  msg.position_covariance[3] = last_insnavgeod_.latitude_longitude_cov;
879  msg.position_covariance[5] = last_insnavgeod_.latitude_height_cov;
880  msg.position_covariance[6] = last_insnavgeod_.longitude_height_cov;
881  msg.position_covariance[7] = last_insnavgeod_.latitude_height_cov;
882  }
883  msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN;
884  }
885  return msg;
886 };
887 
909 GPSFixMsg io_comm_rx::RxMessage::GPSFixCallback()
910 {
911  GPSFixMsg msg;
912  msg.status.satellites_used = static_cast<uint16_t>(last_pvtgeodetic_.nr_sv);
913 
914  // MeasEpoch Processing
915  std::vector<int32_t> cno_tracked;
916  std::vector<int32_t> svid_in_sync;
917  {
918  cno_tracked.reserve(last_measepoch_.type1.size());
919  svid_in_sync.reserve(last_measepoch_.type1.size());
920  for (const auto& measepoch_channel_type1 : last_measepoch_.type1)
921  {
922  // Define MeasEpochChannelType1 struct for the corresponding sub-block
923  svid_in_sync.push_back(
924  static_cast<int32_t>(measepoch_channel_type1.sv_id));
925  uint8_t type_mask =
926  15; // We extract the first four bits using this mask.
927  if (((measepoch_channel_type1.type & type_mask) ==
928  static_cast<uint8_t>(1)) ||
929  ((measepoch_channel_type1.type & type_mask) ==
930  static_cast<uint8_t>(2)))
931  {
932  cno_tracked.push_back(
933  static_cast<int32_t>(measepoch_channel_type1.cn0) / 4);
934  } else
935  {
936  cno_tracked.push_back(
937  static_cast<int32_t>(measepoch_channel_type1.cn0) / 4 +
938  static_cast<int32_t>(10));
939  }
940  }
941  }
942 
943  // ChannelStatus Processing
944  std::vector<int32_t> svid_in_sync_2;
945  std::vector<int32_t> elevation_tracked;
946  std::vector<int32_t> azimuth_tracked;
947  std::vector<int32_t> svid_pvt;
948  svid_pvt.clear();
949  std::vector<int32_t> ordering;
950  {
951  svid_in_sync_2.reserve(last_channelstatus_.satInfo.size());
952  elevation_tracked.reserve(last_channelstatus_.satInfo.size());
953  azimuth_tracked.reserve(last_channelstatus_.satInfo.size());
954  for (const auto& channel_sat_info : last_channelstatus_.satInfo)
955  {
956  bool to_be_added = false;
957  for (int32_t j = 0; j < static_cast<int32_t>(svid_in_sync.size()); ++j)
958  {
959  if (svid_in_sync[j] == static_cast<int32_t>(channel_sat_info.sv_id))
960  {
961  ordering.push_back(j);
962  to_be_added = true;
963  break;
964  }
965  }
966  if (to_be_added)
967  {
968  svid_in_sync_2.push_back(
969  static_cast<int32_t>(channel_sat_info.sv_id));
970  elevation_tracked.push_back(
971  static_cast<int32_t>(channel_sat_info.elev));
972  static uint16_t azimuth_mask = 511;
973  azimuth_tracked.push_back(static_cast<int32_t>(
974  (channel_sat_info.az_rise_set & azimuth_mask)));
975  }
976  svid_pvt.reserve(channel_sat_info.stateInfo.size());
977  for (const auto& channel_state_info : channel_sat_info.stateInfo)
978  {
979  // Define ChannelStateInfo struct for the corresponding sub-block
980  bool pvt_status = false;
981  uint16_t pvt_status_mask = std::pow(2, 15) + std::pow(2, 14);
982  for (int k = 15; k != -1; k -= 2)
983  {
984  uint16_t pvt_status_value =
985  (channel_state_info.pvt_status & pvt_status_mask) >> k - 1;
986  if (pvt_status_value == 2)
987  {
988  pvt_status = true;
989  }
990  if (k > 1)
991  {
992  pvt_status_mask = pvt_status_mask - std::pow(2, k) -
993  std::pow(2, k - 1) + std::pow(2, k - 2) +
994  std::pow(2, k - 3);
995  }
996  }
997  if (pvt_status)
998  {
999  svid_pvt.push_back(
1000  static_cast<int32_t>(channel_sat_info.sv_id));
1001  }
1002  }
1003  }
1004  }
1005  msg.status.satellite_used_prn =
1006  svid_pvt; // Entries such as int32[] in ROS messages are to be treated as
1007  // std::vectors.
1008  msg.status.satellites_visible = static_cast<uint16_t>(svid_in_sync.size());
1009  msg.status.satellite_visible_prn = svid_in_sync_2;
1010  msg.status.satellite_visible_z = elevation_tracked;
1011  msg.status.satellite_visible_azimuth = azimuth_tracked;
1012 
1013  // Reordering CNO vector to that of all previous arrays
1014  std::vector<int32_t> cno_tracked_reordered;
1015  if (static_cast<int32_t>(last_channelstatus_.n) != 0)
1016  {
1017  for (int32_t k = 0; k < static_cast<int32_t>(ordering.size()); ++k)
1018  {
1019  cno_tracked_reordered.push_back(cno_tracked[ordering[k]]);
1020  }
1021  }
1022  msg.status.satellite_visible_snr = cno_tracked_reordered;
1023  msg.err_time = 2 * std::sqrt(last_poscovgeodetic_.cov_bb);
1024 
1025  if (settings_->septentrio_receiver_type == "gnss")
1026  {
1027 
1028  // PVT Status Analysis
1029  uint16_t status_mask = 15; // We extract the first four bits using this mask.
1030  uint16_t type_of_pvt = ((uint16_t)(last_pvtgeodetic_.mode)) & status_mask;
1031  switch (type_of_pvt_map[type_of_pvt])
1032  {
1033  case evNoPVT:
1034  {
1035  msg.status.status = GPSStatusMsg::STATUS_NO_FIX;
1036  break;
1037  }
1038  case evStandAlone:
1039  case evFixed:
1040  {
1041  msg.status.status = GPSStatusMsg::STATUS_FIX;
1042  break;
1043  }
1044  case evDGPS:
1045  case evRTKFixed:
1046  case evRTKFloat:
1047  case evMovingBaseRTKFixed:
1048  case evMovingBaseRTKFloat:
1049  case evPPP:
1050  {
1051  msg.status.status = GPSStatusMsg::STATUS_GBAS_FIX;
1052  break;
1053  }
1054  case evSBAS:
1055  {
1056  uint16_t reference_id = last_pvtgeodetic_.reference_id;
1057  // Here come the PRNs of the 4 WAAS satellites..
1058  if (reference_id == 131 || reference_id == 133 || reference_id == 135 ||
1059  reference_id == 135)
1060  {
1061  msg.status.status = GPSStatusMsg::STATUS_WAAS_FIX;
1062  } else
1063  {
1064  msg.status.status = GPSStatusMsg::STATUS_SBAS_FIX;
1065  }
1066  break;
1067  }
1068  default:
1069  {
1070  node_->log(LogLevel::DEBUG, "PVTGeodetic's Mode field contains an invalid type of PVT solution.");
1071  break;
1072  }
1073  }
1074  // Doppler is not used when calculating the velocities of, say, mosaic-x5, hence:
1075  msg.status.motion_source = GPSStatusMsg::SOURCE_POINTS;
1076  // Doppler is not used when calculating the orientation of, say, mosaic-x5,
1077  // hence:
1078  msg.status.orientation_source = GPSStatusMsg::SOURCE_POINTS;
1079  msg.status.position_source = GPSStatusMsg::SOURCE_GPS;
1080  msg.latitude = rad2deg(last_pvtgeodetic_.latitude);
1081  msg.longitude = rad2deg(last_pvtgeodetic_.longitude);
1082  msg.altitude = last_pvtgeodetic_.height;
1083  // Note that cog is of type float32 while track is of type float64.
1084  msg.track = last_pvtgeodetic_.cog;
1085  msg.speed = std::sqrt(parsing_utilities::square(last_pvtgeodetic_.vn) +
1086  parsing_utilities::square(last_pvtgeodetic_.ve));
1087  msg.climb = last_pvtgeodetic_.vu;
1088  msg.pitch = last_atteuler_.pitch;
1089  msg.roll = last_atteuler_.roll;
1090  if (last_dop_.pdop == 0.0 ||
1091  last_dop_.tdop == 0.0)
1092  {
1093  msg.gdop = -1.0;
1094  } else
1095  {
1096  msg.gdop =
1097  std::sqrt(parsing_utilities::square(last_dop_.pdop) +
1098  parsing_utilities::square(last_dop_.tdop));
1099  }
1100  if (last_dop_.pdop == 0.0)
1101  {
1102  msg.pdop = -1.0;
1103  } else
1104  {
1105  msg.pdop = last_dop_.pdop;
1106  }
1107  if (last_dop_.hdop == 0.0)
1108  {
1109  msg.hdop = -1.0;
1110  } else
1111  {
1112  msg.hdop = last_dop_.hdop;
1113  }
1114  if (last_dop_.vdop == 0.0)
1115  {
1116  msg.vdop = -1.0;
1117  } else
1118  {
1119  msg.vdop = last_dop_.vdop;
1120  }
1121  if (last_dop_.tdop == 0.0)
1122  {
1123  msg.tdop = -1.0;
1124  } else
1125  {
1126  msg.tdop = last_dop_.tdop;
1127  }
1128  msg.time = static_cast<double>(last_pvtgeodetic_.block_header.tow) / 1000 +
1129  static_cast<double>(last_pvtgeodetic_.block_header.wnc * 7 * 24 * 60 * 60);
1130  msg.err = 2 * (std::sqrt(static_cast<double>(last_poscovgeodetic_.cov_latlat) +
1131  static_cast<double>(last_poscovgeodetic_.cov_lonlon) +
1132  static_cast<double>(last_poscovgeodetic_.cov_hgthgt)));
1133  msg.err_horz =
1134  2 * (std::sqrt(static_cast<double>(last_poscovgeodetic_.cov_latlat) +
1135  static_cast<double>(last_poscovgeodetic_.cov_lonlon)));
1136  msg.err_vert =
1137  2 * std::sqrt(static_cast<double>(last_poscovgeodetic_.cov_hgthgt));
1138  msg.err_track =
1139  2 *
1140  (std::sqrt(
1141  parsing_utilities::square(1.0 / (last_pvtgeodetic_.vn +
1142  parsing_utilities::square(last_pvtgeodetic_.ve) /
1143  last_pvtgeodetic_.vn)) *
1144  last_poscovgeodetic_.cov_lonlon +
1145  parsing_utilities::square((last_pvtgeodetic_.ve) /
1146  (parsing_utilities::square(last_pvtgeodetic_.vn) +
1147  parsing_utilities::square(last_pvtgeodetic_.ve))) *
1148  last_poscovgeodetic_.cov_latlat));
1149  msg.err_speed =
1150  2 * (std::sqrt(static_cast<double>(last_velcovgeodetic_.cov_vnvn) +
1151  static_cast<double>(last_velcovgeodetic_.cov_veve)));
1152  msg.err_climb =
1153  2 * std::sqrt(static_cast<double>(last_velcovgeodetic_.cov_vuvu));
1154  msg.err_pitch =
1155  2 * std::sqrt(static_cast<double>(last_attcoveuler_.cov_pitchpitch));
1156  msg.err_roll =
1157  2 * std::sqrt(static_cast<double>(last_attcoveuler_.cov_rollroll));
1158  msg.position_covariance[0] = last_poscovgeodetic_.cov_lonlon;
1159  msg.position_covariance[1] = last_poscovgeodetic_.cov_latlon;
1160  msg.position_covariance[2] = last_poscovgeodetic_.cov_lonhgt;
1161  msg.position_covariance[3] = last_poscovgeodetic_.cov_latlon;
1162  msg.position_covariance[4] = last_poscovgeodetic_.cov_latlat;
1163  msg.position_covariance[5] = last_poscovgeodetic_.cov_lathgt;
1164  msg.position_covariance[6] = last_poscovgeodetic_.cov_lonhgt;
1165  msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt;
1166  msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt;
1167  msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN;
1168 
1169  }
1170 
1171  if (settings_->septentrio_receiver_type == "ins")
1172  {
1173  // PVT Status Analysis
1174  uint16_t status_mask = 15; // We extract the first four bits using this mask.
1175  uint16_t type_of_pvt = ((uint16_t)(last_insnavgeod_.gnss_mode)) & status_mask;
1176  switch (type_of_pvt_map[type_of_pvt])
1177  {
1178  case evNoPVT:
1179  {
1180  msg.status.status = GPSStatusMsg::STATUS_NO_FIX;
1181  break;
1182  }
1183  case evStandAlone:
1184  case evFixed:
1185  {
1186  msg.status.status = GPSStatusMsg::STATUS_FIX;
1187  break;
1188  }
1189  case evDGPS:
1190  case evRTKFixed:
1191  case evRTKFloat:
1192  case evMovingBaseRTKFixed:
1193  case evMovingBaseRTKFloat:
1194  case evPPP:
1195  {
1196  msg.status.status = GPSStatusMsg::STATUS_GBAS_FIX;
1197  break;
1198  }
1199  case evSBAS:
1200  default:
1201  {
1202  node_->log(LogLevel::DEBUG, "INSNavGeod's Mode field contains an invalid type of PVT solution.");
1203  break;
1204  }
1205  }
1206  // Doppler is not used when calculating the velocities of, say, mosaic-x5, hence:
1207  msg.status.motion_source = GPSStatusMsg::SOURCE_POINTS;
1208  // Doppler is not used when calculating the orientation of, say, mosaic-x5,
1209  // hence:
1210  msg.status.orientation_source = GPSStatusMsg::SOURCE_POINTS;
1211  msg.status.position_source = GPSStatusMsg::SOURCE_GPS;
1212  msg.latitude = rad2deg(last_insnavgeod_.latitude);
1213  msg.longitude = rad2deg(last_insnavgeod_.longitude);
1214  msg.altitude = last_insnavgeod_.height;
1215  // Note that cog is of type float32 while track is of type float64.
1216  if ((last_insnavgeod_.sb_list & 2) !=0)
1217  {
1218  msg.track = last_insnavgeod_.heading;
1219  msg.pitch = last_insnavgeod_.pitch;
1220  msg.roll = last_insnavgeod_.roll;
1221  }
1222  if ((last_insnavgeod_.sb_list & 8) !=0)
1223  {
1224  msg.speed = std::sqrt(parsing_utilities::square(last_insnavgeod_.vn) +
1225  parsing_utilities::square(last_insnavgeod_.ve));
1226 
1227  msg.climb = last_insnavgeod_.vu;
1228  }
1229  if (last_dop_.pdop == 0.0 ||
1230  last_dop_.tdop == 0.0)
1231  {
1232  msg.gdop = -1.0;
1233  } else
1234  {
1235  msg.gdop =
1236  std::sqrt(parsing_utilities::square(last_dop_.pdop) +
1237  parsing_utilities::square(last_dop_.tdop));
1238  }
1239  if (last_dop_.pdop == 0.0)
1240  {
1241  msg.pdop = -1.0;
1242  } else
1243  {
1244  msg.pdop = last_dop_.pdop;
1245  }
1246  if (last_dop_.hdop == 0.0)
1247  {
1248  msg.hdop = -1.0;
1249  } else
1250  {
1251  msg.hdop = last_dop_.hdop;
1252  }
1253  if (last_dop_.vdop == 0.0)
1254  {
1255  msg.vdop = -1.0;
1256  } else
1257  {
1258  msg.vdop = last_dop_.vdop;
1259  }
1260  if (last_dop_.tdop == 0.0)
1261  {
1262  msg.tdop = -1.0;
1263  } else
1264  {
1265  msg.tdop = last_dop_.tdop;
1266  }
1267  msg.time = static_cast<double>(last_insnavgeod_.block_header.tow) / 1000 +
1268  static_cast<double>(last_insnavgeod_.block_header.wnc * 7 * 24 * 60 * 60);
1269  if ((last_insnavgeod_.sb_list & 1) !=0)
1270  {
1271  msg.err = 2*(std::sqrt(parsing_utilities::square(last_insnavgeod_.latitude_std_dev) +
1272  parsing_utilities::square(last_insnavgeod_.longitude_std_dev)+
1273  parsing_utilities::square(last_insnavgeod_.height_std_dev)));
1274  msg.err_horz = 2*(std::sqrt(parsing_utilities::square(last_insnavgeod_.latitude_std_dev) +
1275  parsing_utilities::square(last_insnavgeod_.longitude_std_dev)));
1276  msg.err_vert = 2*(std::sqrt(parsing_utilities::square(last_insnavgeod_.height_std_dev)));
1277  }
1278  if (((last_insnavgeod_.sb_list & 8) !=0) || ((last_insnavgeod_.sb_list & 1) !=0))
1279  {
1280  msg.err_track =
1281  2 *
1282  (std::sqrt(
1283  parsing_utilities::square(1.0 / (last_insnavgeod_.vn +
1284  parsing_utilities::square(last_insnavgeod_.ve) /
1285  last_insnavgeod_.vn)) *
1286  parsing_utilities::square(last_insnavgeod_.longitude_std_dev) +
1287  parsing_utilities::square((last_insnavgeod_.ve) /
1288  (parsing_utilities::square(last_insnavgeod_.vn) +
1289  parsing_utilities::square(last_insnavgeod_.ve))) *
1290  parsing_utilities::square(last_insnavgeod_.latitude_std_dev)));
1291  }
1292  if ((last_insnavgeod_.sb_list & 8) !=0)
1293  {
1294  msg.err_speed = 2 * (std::sqrt(parsing_utilities::square(last_insnavgeod_.vn) +
1295  parsing_utilities::square(last_insnavgeod_.ve)));
1296  msg.err_climb = 2 * std::sqrt(parsing_utilities::square(last_insnavgeod_.vn));
1297  }
1298  if ((last_insnavgeod_.sb_list & 2) !=0)
1299  {
1300  msg.err_pitch = 2 * std::sqrt(parsing_utilities::square(last_insnavgeod_.pitch));
1301  }
1302  if ((last_insnavgeod_.sb_list & 2) !=0)
1303  {
1304  msg.err_pitch = 2 * std::sqrt(parsing_utilities::square(last_insnavgeod_.roll));
1305  }
1306  if((last_insnavgeod_.sb_list & 1) !=0)
1307  {
1308  msg.position_covariance[0] = parsing_utilities::square(last_insnavgeod_.longitude_std_dev);
1309  msg.position_covariance[4] = parsing_utilities::square(last_insnavgeod_.latitude_std_dev);
1310  msg.position_covariance[8] = parsing_utilities::square(last_insnavgeod_.height_std_dev);
1311  }
1312  if((last_insnavgeod_.sb_list & 32) !=0)
1313  {
1314  msg.position_covariance[1] = last_insnavgeod_.latitude_longitude_cov;
1315  msg.position_covariance[2] = last_insnavgeod_.longitude_height_cov;
1316  msg.position_covariance[3] = last_insnavgeod_.latitude_longitude_cov;
1317  msg.position_covariance[5] = last_insnavgeod_.latitude_height_cov;
1318  msg.position_covariance[6] = last_insnavgeod_.longitude_height_cov;
1319  msg.position_covariance[7] = last_insnavgeod_.latitude_height_cov;
1320  }
1321  msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN;
1322  }
1323  return msg;
1324 };
1325 
1326 Timestamp io_comm_rx::RxMessage::timestampSBF(const uint8_t* data, bool use_gnss_time)
1327 {
1328  uint32_t tow = parsing_utilities::getTow(data);
1329  uint16_t wnc = parsing_utilities::getWnc(data);
1330 
1331  return timestampSBF(tow, wnc, use_gnss_time);
1332 }
1333 
1339 Timestamp io_comm_rx::RxMessage::timestampSBF(uint32_t tow, uint16_t wnc, bool use_gnss_time)
1340 {
1341  Timestamp time_obj;
1342  if (use_gnss_time)
1343  {
1344  // conversion from GPS time of week and week number to UTC taking leap seconds into account
1345  static uint64_t secToNSec = 1000000000;
1346  static uint64_t mSec2NSec = 1000000;
1347  static uint64_t nsOfGpsStart = 315964800 * secToNSec; // GPS week counter starts at 1980-01-06 which is 315964800 seconds since Unix epoch (1970-01-01 UTC)
1348  static uint64_t nsecPerWeek = 7 * 24 * 60 * 60 * secToNSec;
1349 
1350  time_obj = nsOfGpsStart + tow * mSec2NSec + wnc * nsecPerWeek - settings_->leap_seconds * secToNSec;
1351  }
1352  else
1353  {
1354  time_obj = recvTimestamp_;
1355  }
1356  return time_obj;
1357 }
1358 
1359 bool io_comm_rx::RxMessage::found()
1360 {
1361  if (found_)
1362  return true;
1363 
1364  // Verify header bytes
1365  if (!this->isSBF() && !this->isNMEA() && !this->isResponse() &&
1366  !(g_read_cd && this->isConnectionDescriptor()))
1367  {
1368  return false;
1369  }
1370 
1371  found_ = true;
1372  return true;
1373 }
1374 
1375 const uint8_t* io_comm_rx::RxMessage::search()
1376 {
1377  if (found_)
1378  {
1379  next();
1380  }
1381  // Search for message or a response header
1382  for (; count_ > 0; --count_, ++data_)
1383  {
1384  if (this->isSBF() || this->isNMEA() || this->isResponse() ||
1385  (g_read_cd && this->isConnectionDescriptor()))
1386  {
1387  break;
1388  }
1389  }
1390  found_ = true;
1391  return data_;
1392 }
1393 
1394 std::size_t io_comm_rx::RxMessage::messageSize()
1395 {
1396  uint16_t pos = 0;
1397  message_size_ = 0;
1398  std::size_t count_copy = count_;
1399  if (this->isResponse())
1400  {
1401  do
1402  {
1403  ++message_size_;
1404  ++pos;
1405  --count_copy;
1406  if (count_copy == 0)
1407  break;
1408  } while (!((data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED)) ||
1409  (data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED &&
1410  data_[pos + 2] == 0x20 && data_[pos + 3] == 0x20 &&
1411  data_[pos + 4] == 0x4E) ||
1412  (data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED &&
1413  data_[pos + 2] == 0x20 && data_[pos + 3] == 0x20 &&
1414  data_[pos + 4] == 0x53) ||
1415  (data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED &&
1416  data_[pos + 2] == 0x20 && data_[pos + 3] == 0x20 &&
1417  data_[pos + 4] == 0x52));
1418  } else
1419  {
1420  do
1421  {
1422  ++message_size_;
1423  ++pos;
1424  --count_copy;
1425  if (count_copy == 0)
1426  break;
1427  } while (!((data_[pos] == CARRIAGE_RETURN && data_[pos + 1] == LINE_FEED) ||
1428  data_[pos] == CARRIAGE_RETURN || data_[pos] == LINE_FEED));
1429  }
1430  return message_size_;
1431 }
1432 
1433 bool io_comm_rx::RxMessage::isMessage(const uint16_t id)
1434 {
1435  if (this->isSBF())
1436  {
1437  return (parsing_utilities::getId(data_) == static_cast<const uint16_t>(id));
1438  }
1439  else
1440  {
1441  return false;
1442  }
1443 }
1444 
1445 bool io_comm_rx::RxMessage::isMessage(std::string id)
1446 {
1447  if (this->isNMEA())
1448  {
1449  boost::char_separator<char> sep(",");
1450  typedef boost::tokenizer<boost::char_separator<char>> tokenizer;
1451  std::size_t nmea_size = this->messageSize();
1452  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
1453  tokenizer tokens(block_in_string, sep);
1454  if (*tokens.begin() == id)
1455  {
1456  return true;
1457  } else
1458  {
1459  return false;
1460  }
1461  } else
1462  {
1463  return false;
1464  }
1465 }
1466 
1467 bool io_comm_rx::RxMessage::isSBF()
1468 {
1469  if (count_ >= 2)
1470  {
1471  if (data_[0] == SBF_SYNC_BYTE_1 && data_[1] == SBF_SYNC_BYTE_2)
1472  {
1473  return true;
1474  } else
1475  {
1476  return false;
1477  }
1478  } else
1479  {
1480  return false;
1481  }
1482 }
1483 
1484 bool io_comm_rx::RxMessage::isNMEA()
1485 {
1486  if (count_ >= 2)
1487  {
1488  if ((data_[0] == NMEA_SYNC_BYTE_1 && data_[1] == NMEA_SYNC_BYTE_2_1) ||
1489  (data_[0] == NMEA_SYNC_BYTE_1 && data_[1] == NMEA_SYNC_BYTE_2_2))
1490  {
1491  return true;
1492  } else
1493  {
1494  return false;
1495  }
1496  } else
1497  {
1498  return false;
1499  }
1500 }
1501 
1502 bool io_comm_rx::RxMessage::isResponse()
1503 {
1504  if (count_ >= 2)
1505  {
1506  if (data_[0] == RESPONSE_SYNC_BYTE_1 && data_[1] == RESPONSE_SYNC_BYTE_2)
1507  {
1508  return true;
1509  } else
1510  {
1511  return false;
1512  }
1513  } else
1514  {
1515  return false;
1516  }
1517 }
1518 
1519 bool io_comm_rx::RxMessage::isConnectionDescriptor()
1520 {
1521  if (count_ >= 2)
1522  {
1523  if (data_[0] == CONNECTION_DESCRIPTOR_BYTE_1 &&
1524  data_[1] == CONNECTION_DESCRIPTOR_BYTE_2)
1525  {
1526  return true;
1527  } else
1528  {
1529  return false;
1530  }
1531  } else
1532  {
1533  return false;
1534  }
1535 }
1536 
1537 bool io_comm_rx::RxMessage::isErrorMessage()
1538 {
1539  if (count_ >= 3)
1540  {
1541  if (data_[0] == RESPONSE_SYNC_BYTE_1 && data_[1] == RESPONSE_SYNC_BYTE_2 &&
1542  data_[2] == RESPONSE_SYNC_BYTE_3)
1543  {
1544  return true;
1545  } else
1546  {
1547  return false;
1548  }
1549  } else
1550  {
1551  return false;
1552  }
1553 }
1554 
1555 std::string io_comm_rx::RxMessage::messageID()
1556 {
1557  if (this->isSBF())
1558  {
1559  std::stringstream ss;
1560  ss << parsing_utilities::getId(data_);
1561  return ss.str();
1562  }
1563  if (this->isNMEA())
1564  {
1565  boost::char_separator<char> sep(",");
1566  typedef boost::tokenizer<boost::char_separator<char>> tokenizer;
1567  std::size_t nmea_size = this->messageSize();
1568  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
1569  tokenizer tokens(block_in_string, sep);
1570  return *tokens.begin();
1571  }
1572  return std::string(); // less CPU work than return "";
1573 }
1574 
1575 const uint8_t* io_comm_rx::RxMessage::getPosBuffer() { return data_; }
1576 
1577 const uint8_t* io_comm_rx::RxMessage::getEndBuffer() { return data_ + count_; }
1578 
1579 uint16_t io_comm_rx::RxMessage::getBlockLength()
1580 {
1581  if (this->isSBF())
1582  {
1583  uint16_t block_length;
1584  // Note that static_cast<uint16_t>(data_[6]) would just take the one byte
1585  // "data_[6]" and cast it as requested, !neglecting! the byte "data_[7]".
1586  block_length = parsing_utilities::parseUInt16(data_ + 6);
1587  return block_length;
1588  } else
1589  {
1590  return 0;
1591  }
1592 }
1593 
1599 void io_comm_rx::RxMessage::next()
1600 {
1601  std::size_t jump_size;
1602  if (found())
1603  {
1604  if (this->isNMEA() || this->isResponse() ||
1605  (g_read_cd && this->isConnectionDescriptor()))
1606  {
1607  if (g_read_cd && this->isConnectionDescriptor() && g_cd_count == 2)
1608  {
1609  g_read_cd = false;
1610  }
1611  jump_size = static_cast<uint32_t>(1);
1612  }
1613  if (this->isSBF())
1614  {
1615  if (crc_check_)
1616  {
1617  jump_size = static_cast<std::size_t>(this->getBlockLength());
1618  // Some corrupted messages that survive the CRC check (this happened)
1619  // could tell ROSaic their size is 0, which would lead to an endless
1620  // while loop in the ReadCallback() method of the CallbackHandlers
1621  // class.
1622  if (jump_size == 0)
1623  jump_size = static_cast<std::size_t>(1);
1624  } else
1625  {
1626  jump_size = static_cast<std::size_t>(1);
1627  }
1628  }
1629  }
1630  found_ = false;
1631  data_ += jump_size;
1632  count_ -= jump_size;
1633  // node_->log(LogLevel::DEBUG, "Jump about to happen with jump size %li and count after jump being
1634  // %li.", jump_size, count_);
1635  return; // For readability
1636 }
1637 
1649 bool io_comm_rx::RxMessage::read(std::string message_key, bool search)
1650 {
1651  if (search)
1652  this->search();
1653  if (!found())
1654  return false;
1655  if (this->isSBF())
1656  {
1657  // If the CRC check is unsuccessful, return false
1658  crc_check_ = isValid(data_);
1659  if (!crc_check_)
1660  {
1661  node_->log(LogLevel::DEBUG, "CRC Check returned False. Not a valid data block. Retrieving full SBF block.");
1662  return false;
1663  }
1664  }
1665  switch (rx_id_map[message_key])
1666  {
1667  case evPVTCartesian: // Position and velocity in XYZ
1668  { // The curly bracket here is crucial: Declarations inside a block remain
1669  // inside, and will die at
1670  // the end of the block. Otherwise variable overloading etc.
1671  PVTCartesianMsg msg;
1672  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1673  if (!PVTCartesianParser(node_, dvec.begin(), dvec.end(), msg))
1674  {
1675  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in PVTCartesian");
1676  break;
1677  }
1678  msg.header.frame_id = settings_->frame_id;
1679  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1680  msg.header.stamp = timestampToRos(time_obj);
1681  // Wait as long as necessary (only when reading from SBF/PCAP file)
1682  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1683  {
1684  wait(time_obj);
1685  }
1686  node_->publishMessage<PVTCartesianMsg>("/pvtcartesian", msg);
1687  break;
1688  }
1689  case evPVTGeodetic: // Position and velocity in geodetic coordinate frame (ENU
1690  // frame)
1691  {
1692  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1693  if (!PVTGeodeticParser(node_, dvec.begin(), dvec.end(), last_pvtgeodetic_))
1694  {
1695  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in PVTGeodetic");
1696  break;
1697  }
1698  last_pvtgeodetic_.header.frame_id = settings_->frame_id;
1699  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1700  last_pvtgeodetic_.header.stamp = timestampToRos(time_obj);
1701  pvtgeodetic_has_arrived_gpsfix_ = true;
1702  pvtgeodetic_has_arrived_navsatfix_ = true;
1703  pvtgeodetic_has_arrived_pose_ = true;
1704  // Wait as long as necessary (only when reading from SBF/PCAP file)
1705  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1706  {
1707  wait(time_obj);
1708  }
1709  if (settings_->publish_pvtgeodetic)
1710  node_->publishMessage<PVTGeodeticMsg>("/pvtgeodetic", last_pvtgeodetic_);
1711  break;
1712  }
1713  case evPosCovCartesian:
1714  {
1715  PosCovCartesianMsg msg;
1716  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1717  if (!PosCovCartesianParser(node_, dvec.begin(), dvec.end(), msg))
1718  {
1719  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in PosCovCartesian");
1720  break;
1721  }
1722  msg.header.frame_id = settings_->frame_id;
1723  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1724  msg.header.stamp = timestampToRos(time_obj);
1725  // Wait as long as necessary (only when reading from SBF/PCAP file)
1726  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1727  {
1728  wait(time_obj);
1729  }
1730  node_->publishMessage<PosCovCartesianMsg>("/poscovcartesian", msg);
1731  break;
1732  }
1733  case evPosCovGeodetic:
1734  {
1735  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1736  if (!PosCovGeodeticParser(node_, dvec.begin(), dvec.end(), last_poscovgeodetic_))
1737  {
1738  poscovgeodetic_has_arrived_gpsfix_ = false;
1739  poscovgeodetic_has_arrived_navsatfix_ = false;
1740  poscovgeodetic_has_arrived_pose_ = false;
1741  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in PosCovGeodetic");
1742  break;
1743  }
1744  last_poscovgeodetic_.header.frame_id = settings_->frame_id;
1745  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1746  last_poscovgeodetic_.header.stamp = timestampToRos(time_obj);
1747  poscovgeodetic_has_arrived_gpsfix_ = true;
1748  poscovgeodetic_has_arrived_navsatfix_ = true;
1749  poscovgeodetic_has_arrived_pose_ = true;
1750  // Wait as long as necessary (only when reading from SBF/PCAP file)
1751  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1752  {
1753  wait(time_obj);
1754  }
1755  if (settings_->publish_poscovgeodetic)
1756  node_->publishMessage<PosCovGeodeticMsg>("/poscovgeodetic", last_poscovgeodetic_);
1757  break;
1758  }
1759  case evAttEuler:
1760  {
1761  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1762  if (!AttEulerParser(node_, dvec.begin(), dvec.end(), last_atteuler_, settings_->use_ros_axis_orientation))
1763  {
1764  atteuler_has_arrived_gpsfix_ = false;
1765  atteuler_has_arrived_pose_ = false;
1766  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in AttEuler");
1767  break;
1768  }
1769  last_atteuler_.header.frame_id = settings_->frame_id;
1770  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1771  last_atteuler_.header.stamp = timestampToRos(time_obj);
1772  atteuler_has_arrived_gpsfix_ = true;
1773  atteuler_has_arrived_pose_ = true;
1774  // Wait as long as necessary (only when reading from SBF/PCAP file)
1775  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1776  {
1777  wait(time_obj);
1778  }
1779  if (settings_->publish_atteuler)
1780  node_->publishMessage<AttEulerMsg>("/atteuler", last_atteuler_);
1781  break;
1782  }
1783  case evAttCovEuler:
1784  {
1785  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1786  if (!AttCovEulerParser(node_, dvec.begin(), dvec.end(), last_attcoveuler_, settings_->use_ros_axis_orientation))
1787  {
1788  attcoveuler_has_arrived_gpsfix_ = false;
1789  attcoveuler_has_arrived_pose_ = false;
1790  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in AttCovEuler");
1791  break;
1792  }
1793  last_attcoveuler_.header.frame_id = settings_->frame_id;
1794  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1795  last_attcoveuler_.header.stamp = timestampToRos(time_obj);
1796  attcoveuler_has_arrived_gpsfix_ = true;
1797  attcoveuler_has_arrived_pose_ = true;
1798  // Wait as long as necessary (only when reading from SBF/PCAP file)
1799  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1800  {
1801  wait(time_obj);
1802  }
1803  if (settings_->publish_attcoveuler)
1804  node_->publishMessage<AttCovEulerMsg>("/attcoveuler", last_attcoveuler_);
1805  break;
1806  }
1807  case evINSNavCart: // Position, velocity and orientation in cartesian coordinate frame (ENU
1808  // frame)
1809  {
1810  INSNavCartMsg msg;
1811  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1812  if (!INSNavCartParser(node_, dvec.begin(), dvec.end(), msg, settings_->use_ros_axis_orientation))
1813  {
1814  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in INSNavCart");
1815  break;
1816  }
1817  if (settings_->ins_use_poi)
1818  {
1819  msg.header.frame_id = settings_->poi_frame_id;
1820  }
1821  else
1822  {
1823  msg.header.frame_id = settings_->frame_id;
1824  }
1825  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1826  msg.header.stamp = timestampToRos(time_obj);
1827  // Wait as long as necessary (only when reading from SBF/PCAP file)
1828  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1829  {
1830  wait(time_obj);
1831  }
1832  node_->publishMessage<INSNavCartMsg>("/insnavcart", msg);
1833  break;
1834  }
1835  case evINSNavGeod: // Position, velocity and orientation in geodetic coordinate frame (ENU
1836  // frame)
1837  {
1838  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1839  if (!INSNavGeodParser(node_, dvec.begin(), dvec.end(), last_insnavgeod_, settings_->use_ros_axis_orientation))
1840  {
1841  insnavgeod_has_arrived_gpsfix_ = false;
1842  insnavgeod_has_arrived_navsatfix_ = false;
1843  insnavgeod_has_arrived_pose_ = false;
1844  insnavgeod_has_arrived_localization_ = false;
1845  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in INSNavGeod");
1846  break;
1847  }
1848  if (settings_->ins_use_poi)
1849  {
1850  last_insnavgeod_.header.frame_id = settings_->poi_frame_id;
1851  }
1852  else
1853  {
1854  last_insnavgeod_.header.frame_id = settings_->frame_id;
1855  }
1856  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1857  last_insnavgeod_.header.stamp = timestampToRos(time_obj);
1858  insnavgeod_has_arrived_gpsfix_ = true;
1859  insnavgeod_has_arrived_navsatfix_ = true;
1860  insnavgeod_has_arrived_pose_ = true;
1861  insnavgeod_has_arrived_localization_ = true;
1862  // Wait as long as necessary (only when reading from SBF/PCAP file)
1863  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1864  {
1865  wait(time_obj);
1866  }
1867  if (settings_->publish_insnavgeod)
1868  node_->publishMessage<INSNavGeodMsg>("/insnavgeod", last_insnavgeod_);
1869  break;
1870  }
1871 
1872  case evIMUSetup: // IMU orientation and lever arm
1873  {
1874  IMUSetupMsg msg;
1875  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1876  if (!IMUSetupParser(node_, dvec.begin(), dvec.end(), msg, settings_->use_ros_axis_orientation))
1877  {
1878  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in IMUSetup");
1879  break;
1880  }
1881  msg.header.frame_id = settings_->vehicle_frame_id;
1882  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1883  msg.header.stamp = timestampToRos(time_obj);
1884  // Wait as long as necessary (only when reading from SBF/PCAP file)
1885  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1886  {
1887  wait(time_obj);
1888  }
1889  node_->publishMessage<IMUSetupMsg>("/imusetup", msg);
1890  break;
1891  }
1892 
1893  case evVelSensorSetup: // Velocity sensor lever arm
1894  {
1895  VelSensorSetupMsg msg;
1896  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1897  if (!VelSensorSetupParser(node_, dvec.begin(), dvec.end(), msg, settings_->use_ros_axis_orientation))
1898  {
1899  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in VelSensorSetup");
1900  break;
1901  }
1902  msg.header.frame_id = settings_->vehicle_frame_id;
1903  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1904  msg.header.stamp = timestampToRos(time_obj);
1905  // Wait as long as necessary (only when reading from SBF/PCAP file)
1906  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1907  {
1908  wait(time_obj);
1909  }
1910  node_->publishMessage<VelSensorSetupMsg>("/velsensorsetup", msg);
1911  break;
1912  }
1913 
1914  case evExtEventINSNavCart: // Position, velocity and orientation in cartesian coordinate frame (ENU
1915  // frame)
1916  {
1917  INSNavCartMsg msg;
1918  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1919  if (!INSNavCartParser(node_, dvec.begin(), dvec.end(), msg, settings_->use_ros_axis_orientation))
1920  {
1921  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in ExtEventINSNavCart");
1922  break;
1923  }
1924  if (settings_->ins_use_poi)
1925  {
1926  msg.header.frame_id = settings_->poi_frame_id;
1927  }
1928  else
1929  {
1930  msg.header.frame_id = settings_->frame_id;
1931  }
1932  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1933  msg.header.stamp = timestampToRos(time_obj);
1934  // Wait as long as necessary (only when reading from SBF/PCAP file)
1935  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1936  {
1937  wait(time_obj);
1938  }
1939  node_->publishMessage<INSNavCartMsg>("/exteventinsnavcart", msg);
1940  break;
1941  }
1942 
1943  case evExtEventINSNavGeod:
1944  {
1945  INSNavGeodMsg msg;
1946  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1947  if (!INSNavGeodParser(node_, dvec.begin(), dvec.end(), msg, settings_->use_ros_axis_orientation))
1948  {
1949  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in ExtEventINSNavGeod");
1950  break;
1951  }
1952  if (settings_->ins_use_poi)
1953  {
1954  msg.header.frame_id = settings_->poi_frame_id;
1955  }
1956  else
1957  {
1958  msg.header.frame_id = settings_->frame_id;
1959  }
1960  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1961  msg.header.stamp = timestampToRos(time_obj);
1962  // Wait as long as necessary (only when reading from SBF/PCAP file)
1963  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1964  {
1965  wait(time_obj);
1966  }
1967  node_->publishMessage<INSNavGeodMsg>("/exteventinsnavgeod", msg);
1968  break;
1969  }
1970 
1971  case evExtSensorMeas:
1972  {
1973  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
1974  if (!ExtSensorMeasParser(node_, dvec.begin(), dvec.end(), last_extsensmeas_, settings_->use_ros_axis_orientation))
1975  {
1976  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in ExtSensorMeas");
1977  break;
1978  }
1979  last_extsensmeas_.header.frame_id = settings_->imu_frame_id;
1980  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
1981  last_extsensmeas_.header.stamp = timestampToRos(time_obj);
1982  // Wait as long as necessary (only when reading from SBF/PCAP file)
1983  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
1984  {
1985  wait(time_obj);
1986  }
1987  if (settings_->publish_extsensormeas)
1988  node_->publishMessage<ExtSensorMeasMsg>("/extsensormeas", last_extsensmeas_);
1989  if (settings_->publish_imu)
1990  {
1991  ImuMsg msg;
1992  try
1993  {
1994  msg = ImuCallback();
1995  } catch (std::runtime_error& e)
1996  {
1997  node_->log(LogLevel::DEBUG, "ImuMsg: " + std::string(e.what()));
1998  break;
1999  }
2000  msg.header.frame_id = settings_->imu_frame_id;
2001  msg.header.stamp = last_extsensmeas_.header.stamp;
2002  node_->publishMessage<ImuMsg>("/imu", msg);
2003  }
2004  break;
2005  }
2006 
2007  case evGPST:
2008  {
2009  TimeReferenceMsg msg;
2010  Timestamp time_obj = timestampSBF(data_, true); // We need the GPS time, hence true
2011  msg.time_ref = timestampToRos(time_obj);
2012  msg.source = "GPST";
2013  // Wait as long as necessary (only when reading from SBF/PCAP file)
2014  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2015  {
2016  wait(time_obj);
2017  }
2018  node_->publishMessage<TimeReferenceMsg>("/gpst", msg);
2019  break;
2020  }
2021  case evGPGGA:
2022  {
2023  boost::char_separator<char> sep("\r");
2024  typedef boost::tokenizer<boost::char_separator<char>> tokenizer;
2025  std::size_t nmea_size = this->messageSize();
2026  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
2027  tokenizer tokens(block_in_string, sep);
2028 
2029  std::string id = this->messageID();
2030  std::string one_message = *tokens.begin();
2031  // No kept delimiters, hence "". Also, we specify that empty tokens should
2032  // show up in the output when two delimiters are next to each other. Hence we
2033  // also append the checksum part of the GGA message to "body" below, though
2034  // it is not parsed.
2035  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens);
2036  tokenizer tokens_2(one_message, sep_2);
2037  std::vector<std::string> body;
2038  for (tokenizer::iterator tok_iter = tokens_2.begin();
2039  tok_iter != tokens_2.end(); ++tok_iter)
2040  {
2041  body.push_back(*tok_iter);
2042  }
2043  // Create NmeaSentence struct to pass to GpggaParser::parseASCII
2044  NMEASentence gga_message(id, body);
2045  GpggaMsg msg;
2046  Timestamp time_obj = node_->getTime();
2047  GpggaParser parser_obj;
2048  try
2049  {
2050  msg = parser_obj.parseASCII(gga_message, settings_->frame_id, settings_->use_gnss_time, time_obj);
2051  } catch (ParseException& e)
2052  {
2053  node_->log(LogLevel::DEBUG, "GpggaMsg: " + std::string(e.what()));
2054  break;
2055  }
2056  // Wait as long as necessary (only when reading from SBF/PCAP file)
2057  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2058  {
2059  Timestamp time_obj = timestampFromRos(msg.header.stamp);
2060  wait(time_obj);
2061  }
2062  node_->publishMessage<GpggaMsg>("/gpgga", msg);
2063  break;
2064  }
2065  case evGPRMC:
2066  {
2067  Timestamp time_obj = node_->getTime();
2068 
2069  boost::char_separator<char> sep("\r");
2070  typedef boost::tokenizer<boost::char_separator<char>> tokenizer;
2071  std::size_t nmea_size = this->messageSize();
2072  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
2073  tokenizer tokens(block_in_string, sep);
2074 
2075  std::string id = this->messageID();
2076  std::string one_message = *tokens.begin();
2077  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens);
2078  tokenizer tokens_2(one_message, sep_2);
2079  std::vector<std::string> body;
2080  for (tokenizer::iterator tok_iter = tokens_2.begin();
2081  tok_iter != tokens_2.end(); ++tok_iter)
2082  {
2083  body.push_back(*tok_iter);
2084  }
2085  // Create NmeaSentence struct to pass to GprmcParser::parseASCII
2086  NMEASentence rmc_message(id, body);
2087  GprmcMsg msg;
2088  GprmcParser parser_obj;
2089  try
2090  {
2091  msg = parser_obj.parseASCII(rmc_message, settings_->frame_id, settings_->use_gnss_time, time_obj);
2092  } catch (ParseException& e)
2093  {
2094  node_->log(LogLevel::DEBUG, "GprmcMsg: " + std::string(e.what()));
2095  break;
2096  }
2097  // Wait as long as necessary (only when reading from SBF/PCAP file)
2098  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2099  {
2100  Timestamp time_obj = timestampFromRos(msg.header.stamp);
2101  wait(time_obj);
2102  }
2103  node_->publishMessage<GprmcMsg>("/gprmc", msg);
2104  break;
2105  }
2106  case evGPGSA:
2107  {
2108  boost::char_separator<char> sep("\r");
2109  typedef boost::tokenizer<boost::char_separator<char>> tokenizer;
2110  std::size_t nmea_size = this->messageSize();
2111  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
2112  tokenizer tokens(block_in_string, sep);
2113 
2114  std::string id = this->messageID();
2115  std::string one_message = *tokens.begin();
2116  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens);
2117  tokenizer tokens_2(one_message, sep_2);
2118  std::vector<std::string> body;
2119  for (tokenizer::iterator tok_iter = tokens_2.begin();
2120  tok_iter != tokens_2.end(); ++tok_iter)
2121  {
2122  body.push_back(*tok_iter);
2123  }
2124  // Create NmeaSentence struct to pass to GpgsaParser::parseASCII
2125  NMEASentence gsa_message(id, body);
2126  GpgsaMsg msg;
2127  GpgsaParser parser_obj;
2128  try
2129  {
2130  msg = parser_obj.parseASCII(gsa_message, settings_->frame_id, settings_->use_gnss_time, node_->getTime());
2131  } catch (ParseException& e)
2132  {
2133  node_->log(LogLevel::DEBUG, "GpgsaMsg: " + std::string(e.what()));
2134  break;
2135  }
2136  if (settings_->septentrio_receiver_type == "gnss")
2137  {
2138  Timestamp time_obj;
2139  time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, last_pvtgeodetic_.block_header.wnc, settings_->use_gnss_time);
2140  msg.header.stamp = timestampToRos(time_obj);
2141  }
2142  if (settings_->septentrio_receiver_type == "ins")
2143  {
2144  Timestamp time_obj;
2145  time_obj = timestampSBF(last_insnavgeod_.block_header.tow, last_insnavgeod_.block_header.wnc, settings_->use_gnss_time);
2146  msg.header.stamp = timestampToRos(time_obj);
2147  }
2148  // Wait as long as necessary (only when reading from SBF/PCAP file)
2149  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2150  {
2151  Timestamp time_obj = timestampFromRos(msg.header.stamp);
2152  wait(time_obj);
2153  }
2154  node_->publishMessage<GpgsaMsg>("/gpgsa", msg);
2155  break;
2156  }
2157  case evGPGSV:
2158  case evGLGSV:
2159  case evGAGSV:
2160  {
2161  boost::char_separator<char> sep("\r");
2162  typedef boost::tokenizer<boost::char_separator<char>> tokenizer;
2163  std::size_t nmea_size = this->messageSize();
2164  std::string block_in_string(reinterpret_cast<const char*>(data_), nmea_size);
2165  tokenizer tokens(block_in_string, sep);
2166 
2167  std::string id = this->messageID();
2168  std::string one_message = *tokens.begin();
2169  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens);
2170  tokenizer tokens_2(one_message, sep_2);
2171  std::vector<std::string> body;
2172  for (tokenizer::iterator tok_iter = tokens_2.begin();
2173  tok_iter != tokens_2.end(); ++tok_iter)
2174  {
2175  body.push_back(*tok_iter);
2176  }
2177  // Create NmeaSentence struct to pass to GpgsvParser::parseASCII
2178  NMEASentence gsv_message(id, body);
2179  GpgsvMsg msg;
2180  GpgsvParser parser_obj;
2181  try
2182  {
2183  msg = parser_obj.parseASCII(gsv_message, settings_->frame_id, settings_->use_gnss_time, node_->getTime());
2184  } catch (ParseException& e)
2185  {
2186  node_->log(LogLevel::DEBUG, "GpgsvMsg: " + std::string(e.what()));
2187  break;
2188  }
2189  if (settings_->septentrio_receiver_type == "gnss")
2190  {
2191  Timestamp time_obj;
2192  time_obj = timestampSBF(last_pvtgeodetic_.block_header.tow, last_pvtgeodetic_.block_header.wnc, settings_->use_gnss_time);
2193  msg.header.stamp = timestampToRos(time_obj);
2194  }
2195  if (settings_->septentrio_receiver_type == "ins")
2196  {
2197  Timestamp time_obj;
2198  time_obj = timestampSBF(last_insnavgeod_.block_header.tow, last_insnavgeod_.block_header.wnc, settings_->use_gnss_time);
2199  msg.header.stamp = timestampToRos(time_obj);
2200  }
2201  // Wait as long as necessary (only when reading from SBF/PCAP file)
2202  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2203  {
2204  Timestamp time_obj = timestampFromRos(msg.header.stamp);
2205  wait(time_obj);
2206  }
2207  node_->publishMessage<GpgsvMsg>("/gpgsv", msg);
2208  break;
2209  }
2210 
2211  if (settings_->septentrio_receiver_type == "gnss")
2212  {
2213  case evNavSatFix:
2214  {
2215  NavSatFixMsg msg;
2216  try
2217  {
2218  msg = NavSatFixCallback();
2219  } catch (std::runtime_error& e)
2220  {
2221  node_->log(LogLevel::DEBUG, "NavSatFixMsg: " + std::string(e.what()));
2222  break;
2223  }
2224  msg.header.frame_id = settings_->frame_id;
2225  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
2226  msg.header.stamp = timestampToRos(time_obj);
2227  pvtgeodetic_has_arrived_navsatfix_ = false;
2228  poscovgeodetic_has_arrived_navsatfix_ = false;
2229  // Wait as long as necessary (only when reading from SBF/PCAP file)
2230  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2231  {
2232  wait(time_obj);
2233  }
2234  node_->publishMessage<NavSatFixMsg>("/navsatfix", msg);
2235  break;
2236  }
2237  }
2238  if (settings_->septentrio_receiver_type == "ins")
2239  {
2240  case evINSNavSatFix:
2241  {
2242  NavSatFixMsg msg;
2243  try
2244  {
2245  msg = NavSatFixCallback();
2246  } catch (std::runtime_error& e)
2247  {
2248  node_->log(LogLevel::DEBUG, "NavSatFixMsg: " + std::string(e.what()));
2249  break;
2250  }
2251  if (settings_->ins_use_poi)
2252  {
2253  msg.header.frame_id = settings_->poi_frame_id;
2254  }
2255  else
2256  {
2257  msg.header.frame_id = settings_->frame_id;
2258  }
2259  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
2260  msg.header.stamp = timestampToRos(time_obj);
2261  insnavgeod_has_arrived_navsatfix_ = false;
2262  // Wait as long as necessary (only when reading from SBF/PCAP file)
2263  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2264  {
2265  wait(time_obj);
2266  }
2267  node_->publishMessage<NavSatFixMsg>("/navsatfix", msg);
2268  break;
2269  }
2270  }
2271 
2272  if (settings_->septentrio_receiver_type == "gnss")
2273  {
2274  case evGPSFix:
2275  {
2276  GPSFixMsg msg;
2277  try
2278  {
2279  msg = GPSFixCallback();
2280  } catch (std::runtime_error& e)
2281  {
2282  node_->log(LogLevel::DEBUG, "GPSFixMsg: " + std::string(e.what()));
2283  break;
2284  }
2285  msg.header.frame_id = settings_->frame_id;
2286  msg.status.header.frame_id = settings_->frame_id;
2287  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
2288  msg.header.stamp = timestampToRos(time_obj);
2289  msg.status.header.stamp = timestampToRos(time_obj);
2290  ++count_gpsfix_;
2291  channelstatus_has_arrived_gpsfix_ = false;
2292  measepoch_has_arrived_gpsfix_ = false;
2293  dop_has_arrived_gpsfix_ = false;
2294  pvtgeodetic_has_arrived_gpsfix_ = false;
2295  poscovgeodetic_has_arrived_gpsfix_ = false;
2296  velcovgeodetic_has_arrived_gpsfix_ = false;
2297  atteuler_has_arrived_gpsfix_ = false;
2298  attcoveuler_has_arrived_gpsfix_ = false;
2299  // Wait as long as necessary (only when reading from SBF/PCAP file)
2300  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2301  {
2302  wait(time_obj);
2303  }
2304  node_->publishMessage<GPSFixMsg>("/gpsfix", msg);
2305  break;
2306  }
2307  }
2308  if (settings_->septentrio_receiver_type == "ins")
2309  {
2310  case evINSGPSFix:
2311  {
2312  GPSFixMsg msg;
2313  try
2314  {
2315  msg = GPSFixCallback();
2316  } catch (std::runtime_error& e)
2317  {
2318  node_->log(LogLevel::DEBUG, "GPSFixMsg: " + std::string(e.what()));
2319  break;
2320  }
2321  if (settings_->ins_use_poi)
2322  {
2323  msg.header.frame_id = settings_->poi_frame_id;
2324  }
2325  else
2326  {
2327  msg.header.frame_id = settings_->frame_id;
2328  }
2329  msg.status.header.frame_id = msg.header.frame_id;
2330  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
2331  msg.header.stamp = timestampToRos(time_obj);
2332  msg.status.header.stamp = timestampToRos(time_obj);
2333  ++count_gpsfix_;
2334  channelstatus_has_arrived_gpsfix_ = false;
2335  measepoch_has_arrived_gpsfix_ = false;
2336  dop_has_arrived_gpsfix_ = false;
2337  insnavgeod_has_arrived_gpsfix_ = false;
2338  // Wait as long as necessary (only when reading from SBF/PCAP file)
2339  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2340  {
2341  wait(time_obj);
2342  }
2343  node_->publishMessage<GPSFixMsg>("/gpsfix", msg);
2344  break;
2345  }
2346  }
2347  if (settings_->septentrio_receiver_type == "gnss")
2348  {
2349  case evPoseWithCovarianceStamped:
2350  {
2351  PoseWithCovarianceStampedMsg msg;
2352  try
2353  {
2354  msg = PoseWithCovarianceStampedCallback();
2355  } catch (std::runtime_error& e)
2356  {
2357  node_->log(LogLevel::DEBUG, "PoseWithCovarianceStampedMsg: " + std::string(e.what()));
2358  break;
2359  }
2360  msg.header.frame_id = settings_->frame_id;
2361  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
2362  msg.header.stamp = timestampToRos(time_obj);
2363  pvtgeodetic_has_arrived_pose_ = false;
2364  poscovgeodetic_has_arrived_pose_ = false;
2365  atteuler_has_arrived_pose_ = false;
2366  attcoveuler_has_arrived_pose_ = false;
2367  // Wait as long as necessary (only when reading from SBF/PCAP file)
2368  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2369  {
2370  wait(time_obj);
2371  }
2372  node_->publishMessage<PoseWithCovarianceStampedMsg>("/pose", msg);
2373  break;
2374  }
2375  }
2376  if (settings_->septentrio_receiver_type == "ins")
2377  {
2378  case evINSPoseWithCovarianceStamped:
2379  {
2380  PoseWithCovarianceStampedMsg msg;
2381  try
2382  {
2383  msg = PoseWithCovarianceStampedCallback();
2384  } catch (std::runtime_error& e)
2385  {
2386  node_->log(LogLevel::DEBUG, "PoseWithCovarianceStampedMsg: " + std::string(e.what()));
2387  break;
2388  }
2389  if (settings_->ins_use_poi)
2390  {
2391  msg.header.frame_id = settings_->poi_frame_id;
2392  }
2393  else
2394  {
2395  msg.header.frame_id = settings_->frame_id;
2396  }
2397  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
2398  msg.header.stamp = timestampToRos(time_obj);
2399  insnavgeod_has_arrived_pose_ = false;
2400  // Wait as long as necessary (only when reading from SBF/PCAP file)
2401  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2402  {
2403  wait(time_obj);
2404  }
2405  node_->publishMessage<PoseWithCovarianceStampedMsg>("/pose", msg);
2406  break;
2407  }
2408 
2409  }
2410  case evChannelStatus:
2411  {
2412  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
2413  if (!ChannelStatusParser(node_, dvec.begin(), dvec.end(), last_channelstatus_))
2414  {
2415  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in ChannelStatus");
2416  break;
2417  }
2418  channelstatus_has_arrived_gpsfix_ = true;
2419  break;
2420  }
2421  case evMeasEpoch:
2422  {
2423  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
2424  if (!MeasEpochParser(node_, dvec.begin(), dvec.end(), last_measepoch_))
2425  {
2426  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in MeasEpoch");
2427  break;
2428  }
2429  last_measepoch_.header.frame_id = settings_->frame_id;
2430  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
2431  last_measepoch_.header.stamp = timestampToRos(time_obj);
2432  measepoch_has_arrived_gpsfix_ = true;
2433  if (settings_->publish_measepoch)
2434  node_->publishMessage<MeasEpochMsg>("/measepoch", last_measepoch_);
2435  break;
2436  }
2437  case evDOP:
2438  {
2439  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
2440  if (!DOPParser(node_, dvec.begin(), dvec.end(), last_dop_))
2441  {
2442  dop_has_arrived_gpsfix_ = false;
2443  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in DOP");
2444  break;
2445  }
2446  dop_has_arrived_gpsfix_ = true;
2447  break;
2448  }
2449  case evVelCovGeodetic:
2450  {
2451  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
2452  if (!VelCovGeodeticParser(node_, dvec.begin(), dvec.end(), last_velcovgeodetic_))
2453  {
2454  velcovgeodetic_has_arrived_gpsfix_ = false;
2455  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in VelCovGeodetic");
2456  break;
2457  }
2458  last_velcovgeodetic_.header.frame_id = settings_->frame_id;
2459  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
2460  last_velcovgeodetic_.header.stamp = timestampToRos(time_obj);
2461  velcovgeodetic_has_arrived_gpsfix_ = true;
2462  // Wait as long as necessary (only when reading from SBF/PCAP file)
2463  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2464  {
2465  wait(time_obj);
2466  }
2467  if (settings_->publish_velcovgeodetic)
2468  node_->publishMessage<VelCovGeodeticMsg>("/velcovgeodetic", last_velcovgeodetic_);
2469  break;
2470  }
2471  case evDiagnosticArray:
2472  {
2473  DiagnosticArrayMsg msg;
2474  try
2475  {
2476  msg = DiagnosticArrayCallback();
2477  } catch (std::runtime_error& e)
2478  {
2479  node_->log(LogLevel::DEBUG, "DiagnosticArrayMsg: " + std::string(e.what()));
2480  break;
2481  }
2482  if (settings_->septentrio_receiver_type == "gnss")
2483  {
2484  msg.header.frame_id = settings_->frame_id;
2485  }
2486  if (settings_->septentrio_receiver_type == "ins")
2487  {
2488  if (settings_->ins_use_poi)
2489  {
2490  msg.header.frame_id = settings_->poi_frame_id;
2491  }
2492  else
2493  {
2494  msg.header.frame_id = settings_->frame_id;
2495  }
2496  }
2497  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
2498  msg.header.stamp = timestampToRos(time_obj);
2499  receiverstatus_has_arrived_diagnostics_ = false;
2500  qualityind_has_arrived_diagnostics_ = false;
2501  // Wait as long as necessary (only when reading from SBF/PCAP file)
2502  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2503  {
2504  wait(time_obj);
2505  }
2506  node_->publishMessage<DiagnosticArrayMsg>("/diagnostics", msg);
2507  break;
2508  }
2509  case evLocalization:
2510  {
2511  LocalizationUtmMsg msg;
2512  try
2513  {
2514  msg = LocalizationUtmCallback();
2515  } catch (std::runtime_error& e)
2516  {
2517  node_->log(LogLevel::DEBUG, "LocalizationMsg: " + std::string(e.what()));
2518  break;
2519  }
2520  Timestamp time_obj = timestampSBF(data_, settings_->use_gnss_time);
2521  msg.header.stamp = timestampToRos(time_obj);
2522  insnavgeod_has_arrived_localization_ = false;
2523  // Wait as long as necessary (only when reading from SBF/PCAP file)
2524  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2525  {
2526  wait(time_obj);
2527  }
2528  node_->publishMessage<LocalizationUtmMsg>("/localization", msg);
2529  if (settings_->publish_tf)
2530  node_->publishTf(msg);
2531  break;
2532  }
2533  case evReceiverStatus:
2534  {
2535  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
2536  if (!ReceiverStatusParser(node_, dvec.begin(), dvec.end(), last_receiverstatus_))
2537  {
2538  receiverstatus_has_arrived_diagnostics_ = false;
2539  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in ReceiverStatus");
2540  break;
2541  }
2542  receiverstatus_has_arrived_diagnostics_ = true;
2543  break;
2544  }
2545  case evQualityInd:
2546  {
2547  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
2548  if (!QualityIndParser(node_, dvec.begin(), dvec.end(), last_qualityind_))
2549  {
2550  qualityind_has_arrived_diagnostics_ = false;
2551  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in QualityInd");
2552  break;
2553  }
2554  qualityind_has_arrived_diagnostics_ = true;
2555  break;
2556  }
2557  case evReceiverSetup:
2558  {
2559  std::vector<uint8_t> dvec(data_, data_ + parsing_utilities::getLength(data_));
2560  if (!ReceiverSetupParser(node_, dvec.begin(), dvec.end(), last_receiversetup_))
2561  {
2562  node_->log(LogLevel::ERROR, "septentrio_gnss_driver: parse error in ReceiverSetup");
2563  break;
2564  }
2565  break;
2566  }
2567 
2568  // Many more to be implemented...
2569  }
2570  return true;
2571 }
2572 
2573 void io_comm_rx::RxMessage::wait(Timestamp time_obj)
2574 {
2575  Timestamp unix_old = unix_time_;
2576  unix_time_ = time_obj;
2577  if ((unix_old != 0) &&
2578  (unix_time_ != unix_old))
2579  {
2580  if (unix_time_ > unix_old)
2581  {
2582  auto sleep_nsec = unix_time_ - unix_old;
2583 
2584  std::stringstream ss;
2585  ss << "Waiting for " << sleep_nsec / 1000000
2586  << " milliseconds...";
2587  node_->log(LogLevel::DEBUG, ss.str());
2588 
2589  std::this_thread::sleep_for(std::chrono::nanoseconds(sleep_nsec));
2590  }
2591  }
2592 }
2593 
2594 bool io_comm_rx::RxMessage::gnss_gpsfix_complete(uint32_t id)
2595 {
2596  std::vector<bool> gpsfix_vec = {
2597  channelstatus_has_arrived_gpsfix_,
2598  measepoch_has_arrived_gpsfix_,
2599  dop_has_arrived_gpsfix_,
2600  pvtgeodetic_has_arrived_gpsfix_,
2601  poscovgeodetic_has_arrived_gpsfix_,
2602  velcovgeodetic_has_arrived_gpsfix_,
2603  atteuler_has_arrived_gpsfix_,
2604  attcoveuler_has_arrived_gpsfix_};
2605  return allTrue(gpsfix_vec, id);
2606 }
2607 
2608 bool io_comm_rx::RxMessage::ins_gpsfix_complete(uint32_t id)
2609 {
2610  std::vector<bool> gpsfix_vec = {
2611  channelstatus_has_arrived_gpsfix_,
2612  measepoch_has_arrived_gpsfix_,
2613  dop_has_arrived_gpsfix_,
2614  insnavgeod_has_arrived_gpsfix_};
2615  return allTrue(gpsfix_vec, id);
2616 }
2617 
2618 bool io_comm_rx::RxMessage::gnss_navsatfix_complete(uint32_t id)
2619 {
2620  std::vector<bool> navsatfix_vec = {
2621  pvtgeodetic_has_arrived_navsatfix_,
2622  poscovgeodetic_has_arrived_navsatfix_};
2623  return allTrue(navsatfix_vec, id);
2624 }
2625 
2626 bool io_comm_rx::RxMessage::ins_navsatfix_complete(uint32_t id)
2627 {
2628  std::vector<bool> navsatfix_vec = {
2629  insnavgeod_has_arrived_navsatfix_};
2630  return allTrue(navsatfix_vec, id);
2631 }
2632 
2633 bool io_comm_rx::RxMessage::gnss_pose_complete(uint32_t id)
2634 {
2635  std::vector<bool> pose_vec = {pvtgeodetic_has_arrived_pose_,
2636  poscovgeodetic_has_arrived_pose_,
2637  atteuler_has_arrived_pose_,
2638  attcoveuler_has_arrived_pose_};
2639  return allTrue(pose_vec, id);
2640 }
2641 
2642 bool io_comm_rx::RxMessage::ins_pose_complete(uint32_t id)
2643 {
2644  std::vector<bool> pose_vec = {insnavgeod_has_arrived_pose_};
2645  return allTrue(pose_vec, id);
2646 }
2647 
2648 bool io_comm_rx::RxMessage::diagnostics_complete(uint32_t id)
2649 {
2650  std::vector<bool> diagnostics_vec = {
2651  receiverstatus_has_arrived_diagnostics_,
2652  qualityind_has_arrived_diagnostics_};
2653  return allTrue(diagnostics_vec, id);
2654 }
2655 
2656 bool io_comm_rx::RxMessage::ins_localization_complete(uint32_t id)
2657 {
2658  std::vector<bool> loc_vec = {insnavgeod_has_arrived_localization_};
2659  return allTrue(loc_vec, id);
2660 }
2661 
2662 bool io_comm_rx::RxMessage::allTrue(std::vector<bool>& vec, uint32_t id)
2663 {
2664  vec.erase(vec.begin() + id);
2665  // Checks whether all entries in vec are true
2666  return (std::all_of(vec.begin(), vec.end(),
2667  [](bool v) { return v; }) == true);
2668 }
evVelCovGeodetic
Definition: rx_message.hpp:379
evDGPS
Definition: rx_message.hpp:335
VelCovGeodeticMsg
septentrio_gnss_driver::VelCovGeodetic VelCovGeodeticMsg
Definition: typedefs.hpp:107
rad2deg
const double rad2deg
GpgsaMsg
nmea_msgs::Gpgsa GpgsaMsg
Definition: typedefs.hpp:111
io_comm_rx::RxMessage::recvTimestamp_
Timestamp recvTimestamp_
Timestamp of receiving buffer.
Definition: rx_message.hpp:619
io_comm_rx::RxMessage::last_attcoveuler_
AttCovEulerMsg last_attcoveuler_
Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored.
Definition: rx_message.hpp:670
io_comm_rx::RxMessage::gnss_gpsfix_complete
bool gnss_gpsfix_complete(uint32_t id)
Wether all blocks from GNSS have arrived for GpsFix Message.
Definition: rx_message.cpp:2594
NMEASentence
Struct to split an NMEA sentence into its ID and its body, the latter tokenized into a vector of stri...
Definition: nmea_sentence.hpp:56
parsing_utilities::getTow
uint32_t getTow(const uint8_t *buffer)
Get the time of week in ms of the SBF message.
Definition: parsing_utilities.cpp:359
Settings::vehicle_frame_id
std::string vehicle_frame_id
The frame ID of the vehicle frame.
Definition: rx_message.hpp:318
io_comm_rx::RxMessage::isConnectionDescriptor
bool isConnectionDescriptor()
Definition: rx_message.cpp:1519
evReceiverSetup
Definition: rx_message.hpp:384
io_comm_rx::RxMessage::read
bool read(std::string message_key, bool search=false)
Performs the CRC check (if SBF) and publishes ROS messages.
Definition: rx_message.cpp:1649
io_comm_rx::RxMessage::data_
const uint8_t * data_
Pointer to the buffer of messages.
Definition: rx_message.hpp:624
io_comm_rx::RxMessage::isResponse
bool isResponse()
Determines whether data_ currently points to an NMEA message.
Definition: rx_message.cpp:1502
io_comm_rx::RxMessage::poscovgeodetic_has_arrived_gpsfix_
bool poscovgeodetic_has_arrived_gpsfix_
Definition: rx_message.hpp:768
evIMUSetup
Definition: rx_message.hpp:370
CARRIAGE_RETURN
#define CARRIAGE_RETURN
0x0D is ASCII for "Carriage Return", i.e. "Enter"
Definition: rx_message.hpp:81
DOP::pdop
double pdop
Definition: sbf_structs.hpp:201
ReceiverStatus::rx_error
uint32_t rx_error
Definition: sbf_structs.hpp:277
evINSGPSFix
Definition: rx_message.hpp:353
io_comm_rx::RxMessage::search
const uint8_t * search()
Searches the buffer for the beginning of the next message (NMEA or SBF)
Definition: rx_message.cpp:1375
io_comm_rx::RxMessage::last_poscovgeodetic_
PosCovGeodeticMsg last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored...
Definition: rx_message.hpp:658
AttCovEulerMsg
septentrio_gnss_driver::AttCovEuler AttCovEulerMsg
Definition: typedefs.hpp:100
NMEA_SYNC_BYTE_1
#define NMEA_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
Definition: rx_message.hpp:61
GpggaParser
Derived class for parsing GGA messages.
Definition: gpgga.hpp:79
evGPRMC
Definition: rx_message.hpp:357
ROSaicNodeBase::publishTf
void publishTf(const LocalizationUtmMsg &loc)
Publishing function for tf.
Definition: typedefs.hpp:266
io_comm_rx::RxMessage::insnavgeod_has_arrived_navsatfix_
bool insnavgeod_has_arrived_navsatfix_
Definition: rx_message.hpp:793
io_comm_rx::RxMessage::allTrue
bool allTrue(std::vector< bool > &vec, uint32_t id)
Wether all elements are true.
Definition: rx_message.cpp:2662
Settings::publish_attcoveuler
bool publish_attcoveuler
Whether or not to publish the AttCovEulerMsg message.
Definition: rx_message.hpp:270
io_comm_rx::RxMessage::gnss_pose_complete
bool gnss_pose_complete(uint32_t id)
Wether all blocks from GNSS have arrived for Pose Message.
Definition: rx_message.cpp:2633
evPPP
Definition: rx_message.hpp:342
Settings::publish_tf
bool publish_tf
Whether or not to publish the tf of the localization.
Definition: rx_message.hpp:300
io_comm_rx::RxMessage::messageID
std::string messageID()
Definition: rx_message.cpp:1555
Settings::read_from_sbf_log
bool read_from_sbf_log
Whether or not we are reading from an SBF file.
Definition: rx_message.hpp:324
io_comm_rx::RxMessage::wait
void wait(Timestamp time_obj)
Waits according to time when reading from file.
Definition: rx_message.cpp:2573
Settings::leap_seconds
uint32_t leap_seconds
The number of leap seconds that have been inserted into the UTC time.
Definition: rx_message.hpp:322
Settings::publish_poscovgeodetic
bool publish_poscovgeodetic
Definition: rx_message.hpp:263
io_comm_rx::RxMessage::last_pvtgeodetic_
PVTGeodeticMsg last_pvtgeodetic_
Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.
Definition: rx_message.hpp:652
evMovingBaseRTKFixed
Definition: rx_message.hpp:340
io_comm_rx::RxMessage::crc_check_
bool crc_check_
Whether the CRC check as evaluated in the read() method was successful or not is stored here...
Definition: rx_message.hpp:635
ExtSensorMeasParser
bool ExtSensorMeasParser(ROSaicNodeBase *node, It it, It itEnd, ExtSensorMeasMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "ExtSensorMeas".
Definition: sbf_structs.hpp:1505
evGPGSA
Definition: rx_message.hpp:358
DOP::tdop
double tdop
Definition: sbf_structs.hpp:202
PVTCartesianParser
bool PVTCartesianParser(ROSaicNodeBase *node, It it, It itEnd, PVTCartesianMsg &msg)
Qi based parser for the SBF block "PVTCartesian".
Definition: sbf_structs.hpp:734
IMUSetupParser
bool IMUSetupParser(ROSaicNodeBase *node, It it, It itEnd, IMUSetupMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "IMUSetup".
Definition: sbf_structs.hpp:1437
evMeasEpoch
Definition: rx_message.hpp:377
io_comm_rx::RxMessage::NavSatFixCallback
NavSatFixMsg NavSatFixCallback()
"Callback" function when constructing NavSatFix messages
Definition: rx_message.cpp:718
evGPST
Definition: rx_message.hpp:375
evFixed
Definition: rx_message.hpp:336
io_comm_rx::RxMessage::PoseWithCovarianceStampedCallback
PoseWithCovarianceStampedMsg PoseWithCovarianceStampedCallback()
"Callback" function when constructing PoseWithCovarianceStamped messages
Definition: rx_message.cpp:56
QualityIndParser
bool QualityIndParser(ROSaicNodeBase *node, It it, It itEnd, QualityInd &msg)
Qi based parser for the SBF block "QualityInd".
Definition: sbf_structs.hpp:1202
PVTGeodeticParser
bool PVTGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, PVTGeodeticMsg &msg)
Qi based parser for the SBF block "PVTGeodetic".
Definition: sbf_structs.hpp:788
PVTGeodeticMsg
septentrio_gnss_driver::PVTGeodetic PVTGeodeticMsg
Definition: typedefs.hpp:103
io_comm_rx::RxMessage::ins_gpsfix_complete
bool ins_gpsfix_complete(uint32_t id)
Wether all blocks from INS have arrived for GpsFix Message.
Definition: rx_message.cpp:2608
evChannelStatus
Definition: rx_message.hpp:376
VelSensorSetupMsg
septentrio_gnss_driver::VelSensorSetup VelSensorSetupMsg
Definition: typedefs.hpp:119
INSNavCartMsg
septentrio_gnss_driver::INSNavCart INSNavCartMsg
Definition: typedefs.hpp:116
INSNavCartParser
bool INSNavCartParser(ROSaicNodeBase *node, It it, It itEnd, INSNavCartMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "INSNavCart".
Definition: sbf_structs.hpp:922
QualityInd::indicators
std::vector< uint16_t > indicators
Definition: sbf_structs.hpp:251
parsing_utilities::square
T square(T val)
Definition: parsing_utilities.hpp:61
MeasEpochMsg
septentrio_gnss_driver::MeasEpoch MeasEpochMsg
Definition: typedefs.hpp:97
Settings::ins_use_poi
bool ins_use_poi
INS solution reference point.
Definition: rx_message.hpp:210
GprmcParser
Derived class for parsing RMC messages.
Definition: gprmc.hpp:79
evSBAS
Definition: rx_message.hpp:339
evStandAlone
Definition: rx_message.hpp:334
io_comm_rx::RxMessage::last_receiversetup_
ReceiverSetup last_receiversetup_
Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup blocks need to be stored...
Definition: rx_message.hpp:723
ExtSensorMeasMsg
septentrio_gnss_driver::ExtSensorMeas ExtSensorMeasMsg
Definition: typedefs.hpp:120
io_comm_rx::RxMessage::attcoveuler_has_arrived_pose_
bool attcoveuler_has_arrived_pose_
Definition: rx_message.hpp:808
GpgsaParser
Derived class for parsing GSA messages.
Definition: gpgsa.hpp:79
io_comm_rx::RxMessage::last_dop_
DOP last_dop_
Since GPSFix needs DOP, incoming DOP blocks need to be stored.
Definition: rx_message.hpp:699
ROSaicNodeBase::log
void log(LogLevel logLevel, const std::string &s)
Log function to provide abstraction of ROS loggers.
Definition: typedefs.hpp:208
io_comm_rx::RxMessage::next
void next()
Goes to the start of the next message based on the calculated length of current message.
Definition: rx_message.cpp:1599
Timestamp
uint64_t Timestamp
Definition: typedefs.hpp:77
evGPSFix
Definition: rx_message.hpp:352
ReceiverStatusParser
bool ReceiverStatusParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverStatus &msg)
Struct for the SBF block "ReceiverStatus".
Definition: sbf_structs.hpp:1251
AttEulerMsg
septentrio_gnss_driver::AttEuler AttEulerMsg
Definition: typedefs.hpp:101
AttCovEulerParser
bool AttCovEulerParser(ROSaicNodeBase *node, It it, It itEnd, AttCovEulerMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "AttCovEuler".
Definition: sbf_structs.hpp:885
NMEA_SYNC_BYTE_2_1
#define NMEA_SYNC_BYTE_2_1
0x47 is ASCII for G - 2nd byte to indicate NMEA-type ASCII message
Definition: rx_message.hpp:65
io_comm_rx::RxMessage::atteuler_has_arrived_pose_
bool atteuler_has_arrived_pose_
Definition: rx_message.hpp:804
IMUSetupMsg
septentrio_gnss_driver::IMUSetup IMUSetupMsg
Definition: typedefs.hpp:118
io_comm_rx::RxMessage::receiverstatus_has_arrived_diagnostics_
bool receiverstatus_has_arrived_diagnostics_
Definition: rx_message.hpp:816
io_comm_rx::RxMessage::isNMEA
bool isNMEA()
Determines whether data_ currently points to an NMEA message.
Definition: rx_message.cpp:1484
io_comm_rx::RxMessage::insnavgeod_has_arrived_localization_
bool insnavgeod_has_arrived_localization_
Definition: rx_message.hpp:824
io_comm_rx::RxMessage::getPosBuffer
const uint8_t * getPosBuffer()
Gets the current position in the read buffer.
Definition: rx_message.cpp:1575
parsing_utilities::rpyToRot
Eigen::Matrix3d rpyToRot(double roll, double pitch, double yaw)
Definition: parsing_utilities.hpp:96
GpgsvMsg
nmea_msgs::Gpgsv GpgsvMsg
Definition: typedefs.hpp:112
parsing_utilities::getLength
uint16_t getLength(const uint8_t *buffer)
Get the length of the SBF message.
Definition: parsing_utilities.cpp:354
GprmcMsg
nmea_msgs::Gprmc GprmcMsg
Definition: typedefs.hpp:113
TimeReferenceMsg
sensor_msgs::TimeReference TimeReferenceMsg
Definition: typedefs.hpp:91
RESPONSE_SYNC_BYTE_3
#define RESPONSE_SYNC_BYTE_3
Definition: rx_message.hpp:90
io_comm_rx::RxMessage::ImuCallback
ImuMsg ImuCallback()
"Callback" function when constructing ImuMsg messages
Definition: rx_message.cpp:314
io_comm_rx::RxMessage::atteuler_has_arrived_gpsfix_
bool atteuler_has_arrived_gpsfix_
For GPSFix: Whether the AttEuler block of the current epoch has arrived or not.
Definition: rx_message.hpp:775
io_comm_rx::RxMessage::diagnostics_complete
bool diagnostics_complete(uint32_t id)
Wether all blocks have arrived for Diagnostics Message.
Definition: rx_message.cpp:2648
io_comm_rx::RxMessage::last_insnavgeod_
INSNavGeodMsg last_insnavgeod_
Since NavSatFix, GPSFix, Imu and Pose. need INSNavGeod, incoming INSNavGeod blocks need to be stored...
Definition: rx_message.hpp:676
evINSNavSatFix
Definition: rx_message.hpp:351
parsing_utilities::getWnc
uint16_t getWnc(const uint8_t *buffer)
Get the GPS week counter of the SBF message.
Definition: parsing_utilities.cpp:364
rx_message.hpp
Defines a class that reads messages handed over from the circular buffer.
io_comm_rx::RxMessage::rx_id_map
RxIDMap rx_id_map
All instances of the RxMessage class shall have access to the map without reinitializing it...
Definition: rx_message.hpp:747
VelSensorSetupParser
bool VelSensorSetupParser(ROSaicNodeBase *node, It it, It itEnd, VelSensorSetupMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "VelSensorSetup".
Definition: sbf_structs.hpp:1473
evGLGSV
Definition: rx_message.hpp:360
MeasEpochParser
bool MeasEpochParser(ROSaicNodeBase *node, It it, It itEnd, MeasEpochMsg &msg)
Definition: sbf_structs.hpp:637
parsing_utilities::rad2deg
T rad2deg(T rad)
Definition: parsing_utilities.hpp:88
io_comm_rx::RxMessage::timestampSBF
Timestamp timestampSBF(const uint8_t *data, bool use_gnss_time)
Calculates the timestamp, in the Unix Epoch time format This is either done using the TOW as transmit...
Definition: rx_message.cpp:1326
GpggaMsg
nmea_msgs::Gpgga GpggaMsg
Definition: typedefs.hpp:110
ChannelStatusParser
bool ChannelStatusParser(ROSaicNodeBase *node, It it, It itEnd, ChannelStatus &msg)
Qi based parser for the SBF block "ChannelStatus".
Definition: sbf_structs.hpp:512
io_comm_rx::RxMessage::velcovgeodetic_has_arrived_gpsfix_
bool velcovgeodetic_has_arrived_gpsfix_
Definition: rx_message.hpp:772
io_comm_rx::RxMessage::gnss_navsatfix_complete
bool gnss_navsatfix_complete(uint32_t id)
Wether all blocks from GNSS have arrived for NavSatFix Message.
Definition: rx_message.cpp:2618
io_comm_rx::RxMessage::last_channelstatus_
ChannelStatus last_channelstatus_
Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks need to be stored.
Definition: rx_message.hpp:688
evExtEventINSNavGeod
Definition: rx_message.hpp:372
io_comm_rx::RxMessage::isSBF
bool isSBF()
Determines whether data_ currently points to an SBF block.
Definition: rx_message.cpp:1467
parsing_utilities::parseUInt16
uint16_t parseUInt16(const uint8_t *buffer)
Converts a 2-byte-buffer into an unsigned 16-bit integer.
Definition: parsing_utilities.cpp:176
io_comm_rx::RxMessage::settings_
Settings * settings_
Settings struct.
Definition: rx_message.hpp:884
Settings::polling_period_pvt
uint32_t polling_period_pvt
Polling period for PVT-related SBF blocks.
Definition: rx_message.hpp:158
g_cd_count
uint32_t g_cd_count
Definition: communication_core.cpp:120
Settings::publish_insnavgeod
bool publish_insnavgeod
Whether or not to publish the INSNavGeodMsg message.
Definition: rx_message.hpp:274
AttEulerParser
bool AttEulerParser(ROSaicNodeBase *node, It it, It itEnd, AttEulerMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "AttEuler".
Definition: sbf_structs.hpp:842
evVelSensorSetup
Definition: rx_message.hpp:371
io_comm_rx::RxMessage::ins_pose_complete
bool ins_pose_complete(uint32_t id)
Wether all blocks from INS have arrived for Pose Message.
Definition: rx_message.cpp:2642
io_comm_rx::RxMessage::ins_navsatfix_complete
bool ins_navsatfix_complete(uint32_t id)
Wether all blocks from INS have arrived for NavSatFix Message.
Definition: rx_message.cpp:2626
DOP::vdop
double vdop
Definition: sbf_structs.hpp:204
timestampFromRos
Timestamp timestampFromRos(const TimestampRos &tsr)
Convert ROS timestamp to nsec timestamp.
Definition: typedefs.hpp:139
evINSNavCart
Definition: rx_message.hpp:368
evExtEventINSNavCart
Definition: rx_message.hpp:373
Settings::use_gnss_time
bool use_gnss_time
Definition: rx_message.hpp:306
DOP::hdop
double hdop
Definition: sbf_structs.hpp:203
io_comm_rx::RxMessage::count_
std::size_t count_
Number of unread bytes in the buffer.
Definition: rx_message.hpp:629
PosCovGeodeticParser
bool PosCovGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, PosCovGeodeticMsg &msg)
Qi based parser for the SBF block "PosCovGeodetic".
Definition: sbf_structs.hpp:1100
SBF_SYNC_BYTE_1
static const uint8_t SBF_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each message
Definition: sbf_structs.hpp:113
evPoseWithCovarianceStamped
Definition: rx_message.hpp:354
INSNavGeodParser
bool INSNavGeodParser(ROSaicNodeBase *node, It it, It itEnd, INSNavGeodMsg &msg, bool use_ros_axis_orientation)
Qi based parser for the SBF block "INSNavGeod".
Definition: sbf_structs.hpp:1292
ParseException
Class to declare error message format when parsing, derived from the public class "std::runtime_error...
Definition: parse_exception.hpp:87
Settings::publish_measepoch
bool publish_measepoch
Whether or not to publish the MeasEpoch message.
Definition: rx_message.hpp:252
io_comm_rx::RxMessage::type_of_pvt_map
TypeOfPVTMap type_of_pvt_map
All instances of the RxMessage class shall have access to the map without reinitializing it...
Definition: rx_message.hpp:733
io_comm_rx::RxMessage::ins_localization_complete
bool ins_localization_complete(uint32_t id)
Wether all blocks have arrived for Localization Message.
Definition: rx_message.cpp:2656
io_comm_rx::RxMessage::node_
ROSaicNodeBase * node_
Pointer to the node.
Definition: rx_message.hpp:614
evGPGGA
Definition: rx_message.hpp:356
ROSaicNodeBase::publishMessage
void publishMessage(const std::string &topic, const M &msg)
Publishing function.
Definition: typedefs.hpp:247
Settings::publish_extsensormeas
bool publish_extsensormeas
Whether or not to publish the ExtSensorMeasMsg message.
Definition: rx_message.hpp:284
Settings::poi_frame_id
std::string poi_frame_id
The frame ID used in the header of published ROS Localization message if poi is used.
Definition: rx_message.hpp:312
io_comm_rx::RxMessage::messageSize
std::size_t messageSize()
Definition: rx_message.cpp:1394
evPosCovCartesian
Definition: rx_message.hpp:364
io_comm_rx::RxMessage::count_gpsfix_
uint32_t count_gpsfix_
Number of times the GPSFixMsg message has been published.
Definition: rx_message.hpp:646
evReceiverStatus
Definition: rx_message.hpp:382
ReceiverSetupParser
bool ReceiverSetupParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverSetup &msg)
Qi based parser for the SBF block "ReceiverSetup".
Definition: sbf_structs.hpp:677
ImuMsg
sensor_msgs::Imu ImuMsg
Definition: typedefs.hpp:92
INSNavGeodMsg
septentrio_gnss_driver::INSNavGeod INSNavGeodMsg
Definition: typedefs.hpp:117
CONNECTION_DESCRIPTOR_BYTE_1
#define CONNECTION_DESCRIPTOR_BYTE_1
Definition: rx_message.hpp:95
Settings::septentrio_receiver_type
std::string septentrio_receiver_type
Septentrio receiver type, either "gnss" or "ins".
Definition: rx_message.hpp:302
timestampToRos
TimestampRos timestampToRos(Timestamp ts)
Convert nsec timestamp to ROS timestamp.
Definition: typedefs.hpp:127
io_comm_rx::RxMessage::qualityind_has_arrived_diagnostics_
bool qualityind_has_arrived_diagnostics_
Definition: rx_message.hpp:820
io_comm_rx::RxMessage::poscovgeodetic_has_arrived_pose_
bool poscovgeodetic_has_arrived_pose_
Definition: rx_message.hpp:800
Settings::read_from_pcap
bool read_from_pcap
Whether or not we are reading from a PCAP file.
Definition: rx_message.hpp:326
Settings::publish_velcovgeodetic
bool publish_velcovgeodetic
Definition: rx_message.hpp:266
SBF_SYNC_BYTE_2
static const uint8_t SBF_SYNC_BYTE_2
0x40 is ASCII for @ - 2nd byte to indicate SBF block
Definition: sbf_structs.hpp:115
DiagnosticStatusMsg
diagnostic_msgs::DiagnosticStatus DiagnosticStatusMsg
Definition: typedefs.hpp:83
PosCovCartesianParser
bool PosCovCartesianParser(ROSaicNodeBase *node, It it, It itEnd, PosCovCartesianMsg &msg)
Qi based parser for the SBF block "PosCovCartesian".
Definition: sbf_structs.hpp:1066
Settings::lock_utm_zone
bool lock_utm_zone
Wether the UTM zone of the localization is locked.
Definition: rx_message.hpp:320
io_comm_rx::RxMessage::getBlockLength
uint16_t getBlockLength()
Gets the length of the SBF block.
Definition: rx_message.cpp:1579
io_comm_rx::RxMessage::last_velcovgeodetic_
VelCovGeodeticMsg last_velcovgeodetic_
Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks need to be stored.
Definition: rx_message.hpp:705
PosCovGeodeticMsg
septentrio_gnss_driver::PosCovGeodetic PosCovGeodeticMsg
Definition: typedefs.hpp:105
Settings::publish_pvtgeodetic
bool publish_pvtgeodetic
Whether or not to publish the PVTGeodeticMsg message.
Definition: rx_message.hpp:257
io_comm_rx::RxMessage::message_size_
std::size_t message_size_
Helps to determine size of response message / NMEA message / SBF block.
Definition: rx_message.hpp:641
Settings::imu_frame_id
std::string imu_frame_id
The frame ID used in the header of published ROS Imu message.
Definition: rx_message.hpp:310
PosCovCartesianMsg
septentrio_gnss_driver::PosCovCartesian PosCovCartesianMsg
Definition: typedefs.hpp:104
evINSPoseWithCovarianceStamped
Definition: rx_message.hpp:355
io_comm_rx::RxMessage::last_receiverstatus_
ReceiverStatus last_receiverstatus_
Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus blocks need to be stored...
Definition: rx_message.hpp:711
io_comm_rx::RxMessage::DiagnosticArrayCallback
DiagnosticArrayMsg DiagnosticArrayCallback()
"Callback" function when constructing DiagnosticArrayMsg messages
Definition: rx_message.cpp:207
evMovingBaseRTKFloat
Definition: rx_message.hpp:341
io_comm_rx::RxMessage::pvtgeodetic_has_arrived_navsatfix_
bool pvtgeodetic_has_arrived_navsatfix_
Definition: rx_message.hpp:785
Settings::publish_imu
bool publish_imu
Whether or not to publish the ImuMsg message.
Definition: rx_message.hpp:296
io_comm_rx::RxMessage::dop_has_arrived_gpsfix_
bool dop_has_arrived_gpsfix_
For GPSFix: Whether the DOP block of the current epoch has arrived or not.
Definition: rx_message.hpp:761
validValue
bool validValue(T s)
Check if value is not set to Do-Not-Use -2e10.
Definition: sbf_structs.hpp:326
parsing_utilities::convertEulerToQuaternion
QuaternionMsg convertEulerToQuaternion(double yaw, double pitch, double roll)
Transforms Euler angles to a quaternion.
Definition: parsing_utilities.cpp:309
io_comm_rx::RxMessage::getEndBuffer
const uint8_t * getEndBuffer()
Gets the end position in the read buffer.
Definition: rx_message.cpp:1577
ChannelStatus::satInfo
std::vector< ChannelSatInfo > satInfo
Definition: sbf_structs.hpp:189
NMEA_SYNC_BYTE_2_2
#define NMEA_SYNC_BYTE_2_2
0x50 is ASCII for P - 2nd byte to indicate proprietary ASCII message
Definition: rx_message.hpp:69
DiagnosticArrayMsg
diagnostic_msgs::DiagnosticArray DiagnosticArrayMsg
Definition: typedefs.hpp:82
evRTKFixed
Definition: rx_message.hpp:337
evExtSensorMeas
Definition: rx_message.hpp:374
evAttCovEuler
Definition: rx_message.hpp:367
io_comm_rx::RxMessage::LocalizationUtmCallback
LocalizationUtmMsg LocalizationUtmCallback()
"Callback" function when constructing LocalizationUtmMsg messages
Definition: rx_message.cpp:434
GprmcParser::parseASCII
GprmcMsg parseASCII(const NMEASentence &sentence, const std::string &frame_id, bool use_gnss_time, Timestamp time_obj) noexcept(false) override
Parses one RMC message.
Definition: gprmc.cpp:58
evRTKFloat
Definition: rx_message.hpp:338
RESPONSE_SYNC_BYTE_1
#define RESPONSE_SYNC_BYTE_1
0x24 is ASCII for $ - 1st byte in each response from the Rx
Definition: rx_message.hpp:73
io_comm_rx::RxMessage::measepoch_has_arrived_gpsfix_
bool measepoch_has_arrived_gpsfix_
For GPSFix: Whether the MeasEpoch block of the current epoch has arrived or not.
Definition: rx_message.hpp:758
ROSaicNodeBase::getTime
Timestamp getTime()
Gets current timestamp.
Definition: typedefs.hpp:236
evPosCovGeodetic
Definition: rx_message.hpp:365
evGPGSV
Definition: rx_message.hpp:359
LocalizationUtmMsg
nav_msgs::Odometry LocalizationUtmMsg
Definition: typedefs.hpp:93
evQualityInd
Definition: rx_message.hpp:383
io_comm_rx::RxMessage::last_measepoch_
MeasEpochMsg last_measepoch_
Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks need to be stored.
Definition: rx_message.hpp:694
ReceiverSetup::rx_serial_number
std::string rx_serial_number
Definition: sbf_structs.hpp:221
io_comm_rx::RxMessage::pvtgeodetic_has_arrived_pose_
bool pvtgeodetic_has_arrived_pose_
Definition: rx_message.hpp:796
Settings::publish_atteuler
bool publish_atteuler
Whether or not to publish the AttEulerMsg message.
Definition: rx_message.hpp:268
io_comm_rx::RxMessage::pvtgeodetic_has_arrived_gpsfix_
bool pvtgeodetic_has_arrived_gpsfix_
For GPSFix: Whether the PVTGeodetic block of the current epoch has arrived or not.
Definition: rx_message.hpp:764
GpggaParser::parseASCII
GpggaMsg parseASCII(const NMEASentence &sentence, const std::string &frame_id, bool use_gnss_time, Timestamp time_obj) noexcept(false) override
Parses one GGA message.
Definition: gpgga.cpp:54
io_comm_rx::RxMessage::fixedUtmZone_
std::shared_ptr< std::string > fixedUtmZone_
Fixed UTM zone.
Definition: rx_message.hpp:889
evLocalization
Definition: rx_message.hpp:381
io_comm_rx::RxMessage::GPSFixCallback
GPSFixMsg GPSFixCallback()
"Callback" function when constructing GPSFix messages
Definition: rx_message.cpp:909
evGAGSV
Definition: rx_message.hpp:361
evAttEuler
Definition: rx_message.hpp:366
evNoPVT
Definition: rx_message.hpp:333
io_comm_rx::RxMessage::last_atteuler_
AttEulerMsg last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
Definition: rx_message.hpp:664
evPVTCartesian
Definition: rx_message.hpp:362
io_comm_rx::RxMessage::last_qualityind_
QualityInd last_qualityind_
Since DiagnosticArray needs QualityInd, incoming QualityInd blocks need to be stored.
Definition: rx_message.hpp:717
io_comm_rx::RxMessage::unix_time_
Timestamp unix_time_
Definition: rx_message.hpp:751
deg2rad
const double deg2rad
GpgsvParser::parseASCII
GpgsvMsg parseASCII(const NMEASentence &sentence, const std::string &frame_id, bool use_gnss_time, Timestamp time_obj) noexcept(false) override
Parses one GSV message.
Definition: gpgsv.cpp:54
evINSNavGeod
Definition: rx_message.hpp:369
io_comm_rx::RxMessage::insnavgeod_has_arrived_gpsfix_
bool insnavgeod_has_arrived_gpsfix_
For GPSFix: Whether the INSNavGeod block of the current epoch has arrived or not. ...
Definition: rx_message.hpp:781
io_comm_rx::RxMessage::found
bool found()
Has an NMEA message, SBF block or command reply been found in the buffer?
Definition: rx_message.cpp:1359
parsing_utilities::deg2radSq
T deg2radSq(T deg)
Definition: parsing_utilities.hpp:79
isValid
bool isValid(const uint8_t *block)
Validates whether the calculated CRC of the SBF block at hand matches the CRC field of the streamed S...
Definition: crc.cpp:63
PVTCartesianMsg
septentrio_gnss_driver::PVTCartesian PVTCartesianMsg
Definition: typedefs.hpp:102
LINE_FEED
#define LINE_FEED
0x0A is ASCII for "Line Feed", i.e. "New Line"
Definition: rx_message.hpp:85
io_comm_rx::RxMessage::attcoveuler_has_arrived_gpsfix_
bool attcoveuler_has_arrived_gpsfix_
For GPSFix: Whether the AttCovEuler block of the current epoch has arrived or not.
Definition: rx_message.hpp:778
io_comm_rx::RxMessage::insnavgeod_has_arrived_pose_
bool insnavgeod_has_arrived_pose_
Definition: rx_message.hpp:812
parsing_utilities::getId
uint16_t getId(const uint8_t *buffer)
Get the ID of the SBF message.
Definition: parsing_utilities.cpp:344
io_comm_rx::RxMessage::isErrorMessage
bool isErrorMessage()
Definition: rx_message.cpp:1537
GpgsvParser
Derived class for parsing GSV messages.
Definition: gpgsv.hpp:79
RESPONSE_SYNC_BYTE_2
#define RESPONSE_SYNC_BYTE_2
0x52 is ASCII for R (for "Response") - 2nd byte in each response from the Rx
Definition: rx_message.hpp:77
NavSatFixMsg
sensor_msgs::NavSatFix NavSatFixMsg
Definition: typedefs.hpp:89
VelCovGeodeticParser
bool VelCovGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, VelCovGeodeticMsg &msg)
Qi based parser for the SBF block "VelCovGeodetic".
Definition: sbf_structs.hpp:1168
GPSFixMsg
gps_common::GPSFix GPSFixMsg
Definition: typedefs.hpp:87
evDiagnosticArray
Definition: rx_message.hpp:380
g_read_cd
bool g_read_cd
Definition: communication_core.cpp:115
io_comm_rx::RxMessage::channelstatus_has_arrived_gpsfix_
bool channelstatus_has_arrived_gpsfix_
Definition: rx_message.hpp:755
Settings::use_ros_axis_orientation
bool use_ros_axis_orientation
ROS axis orientation, body: front-left-up, geographic: ENU.
Definition: rx_message.hpp:178
PoseWithCovarianceStampedMsg
geometry_msgs::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg
Definition: typedefs.hpp:85
GpgsaParser::parseASCII
GpgsaMsg parseASCII(const NMEASentence &sentence, const std::string &frame_id, bool use_gnss_time, Timestamp time_obj) noexcept(false) override
Parses one GSA message.
Definition: gpgsa.cpp:54
DOPParser
bool DOPParser(ROSaicNodeBase *node, It it, It itEnd, DOP &msg)
Qi based parser for the SBF block "DOP".
Definition: sbf_structs.hpp:549
evDOP
Definition: rx_message.hpp:378
io_comm_rx::RxMessage::poscovgeodetic_has_arrived_navsatfix_
bool poscovgeodetic_has_arrived_navsatfix_
Definition: rx_message.hpp:789
Settings::frame_id
std::string frame_id
The frame ID used in the header of every published ROS message.
Definition: rx_message.hpp:308
evNavSatFix
Definition: rx_message.hpp:350
io_comm_rx::RxMessage::last_extsensmeas_
ExtSensorMeasMsg last_extsensmeas_
Since Imu needs ExtSensorMeas, incoming ExtSensorMeas blocks need to be stored.
Definition: rx_message.hpp:682
parsing_utilities::deg2rad
T deg2rad(T deg)
Definition: parsing_utilities.hpp:70
CONNECTION_DESCRIPTOR_BYTE_2
#define CONNECTION_DESCRIPTOR_BYTE_2
Definition: rx_message.hpp:100
evPVTGeodetic
Definition: rx_message.hpp:363
io_comm_rx::RxMessage::found_
bool found_
Whether or not a message has been found.
Definition: rx_message.hpp:568
io_comm_rx::RxMessage::isMessage
bool isMessage(const uint16_t ID)
Determines whether data_ points to the SBF block with ID "ID", e.g. 5003.
Definition: rx_message.cpp:1433

septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Thu Jun 23 2022 02:11:38