message_handler.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 <GeographicLib/UTMUPS.hpp>
32 #include <boost/tokenizer.hpp>
33 #include <septentrio_gnss_driver/communication/message_handler.hpp>
34 #include <thread>
35 
50 using parsing_utilities::convertEulerToQuaternionMsg;
51 using parsing_utilities::deg2rad;
52 using parsing_utilities::deg2radSq;
53 using parsing_utilities::rad2deg;
54 using parsing_utilities::square;
55 
56 namespace io {
57 
58  void MessageHandler::assemblePoseWithCovarianceStamped()
59  {
60  if (!settings_->publish_pose)
61  return;
62 
63  thread_local auto last_ins_tow = last_insnavgeod_.block_header.tow;
64 
65  PoseWithCovarianceStampedMsg msg;
66  if (settings_->septentrio_receiver_type == "ins")
67  {
68  if (!validValue(last_insnavgeod_.block_header.tow) ||
69  (last_insnavgeod_.block_header.tow == last_ins_tow))
70  return;
71  last_ins_tow = last_insnavgeod_.block_header.tow;
72 
73  msg.header = last_insnavgeod_.header;
74 
75  msg.pose.pose.position.x = rad2deg(last_insnavgeod_.longitude);
76  msg.pose.pose.position.y = rad2deg(last_insnavgeod_.latitude);
77  msg.pose.pose.position.z = last_insnavgeod_.height;
78 
79  // Filling in the pose data
80  if ((last_insnavgeod_.sb_list & 1) != 0)
81  {
82  // Pos autocov
83  msg.pose.covariance[0] = square(last_insnavgeod_.longitude_std_dev);
84  msg.pose.covariance[7] = square(last_insnavgeod_.latitude_std_dev);
85  msg.pose.covariance[14] = square(last_insnavgeod_.height_std_dev);
86  } else
87  {
88  msg.pose.covariance[0] = -1.0;
89  msg.pose.covariance[7] = -1.0;
90  msg.pose.covariance[14] = -1.0;
91  }
92  if ((last_insnavgeod_.sb_list & 2) != 0)
93  {
94  double yaw = deg2rad(last_insnavgeod_.heading);
95  double pitch = deg2rad(last_insnavgeod_.pitch);
96  double roll = deg2rad(last_insnavgeod_.roll);
97  // Attitude
98  if (validValue(last_insnavgeod_.heading) &&
99  validValue(last_insnavgeod_.pitch) &&
100  validValue(last_insnavgeod_.roll))
101  {
102  msg.pose.pose.orientation =
103  convertEulerToQuaternionMsg(roll, pitch, yaw);
104  } else if (validValue(last_insnavgeod_.heading))
105  {
106  msg.pose.pose.orientation =
107  convertEulerToQuaternionMsg(0.0, 0.0, yaw);
108  }
109  } else
110  {
111  parsing_utilities::setQuaternionNaN(msg.pose.pose.orientation);
112  }
113  if ((last_insnavgeod_.sb_list & 4) != 0)
114  {
115  // Attitude autocov
116  msg.pose.covariance[21] = parsing_utilities::convertAutoCovariance(
117  last_insnavgeod_.roll_std_dev);
118  msg.pose.covariance[28] = parsing_utilities::convertAutoCovariance(
119  last_insnavgeod_.pitch_std_dev);
120  msg.pose.covariance[35] = parsing_utilities::convertAutoCovariance(
121  last_insnavgeod_.heading_std_dev);
122 
123  } else
124  {
125  msg.pose.covariance[21] = -1.0;
126  msg.pose.covariance[28] = -1.0;
127  msg.pose.covariance[35] = -1.0;
128  }
129  if ((last_insnavgeod_.sb_list & 32) != 0)
130  {
131  // Pos cov
132  msg.pose.covariance[1] = last_insnavgeod_.latitude_longitude_cov;
133  msg.pose.covariance[2] = last_insnavgeod_.longitude_height_cov;
134  msg.pose.covariance[6] = last_insnavgeod_.latitude_longitude_cov;
135  msg.pose.covariance[8] = last_insnavgeod_.latitude_height_cov;
136  msg.pose.covariance[12] = last_insnavgeod_.longitude_height_cov;
137  msg.pose.covariance[13] = last_insnavgeod_.latitude_height_cov;
138  }
139  if ((last_insnavgeod_.sb_list & 64) != 0)
140  {
141  // Attitude cov
142  msg.pose.covariance[22] = parsing_utilities::convertCovariance(
143  last_insnavgeod_.pitch_roll_cov);
144  msg.pose.covariance[23] = parsing_utilities::convertCovariance(
145  last_insnavgeod_.heading_roll_cov);
146  msg.pose.covariance[27] = parsing_utilities::convertCovariance(
147  last_insnavgeod_.pitch_roll_cov);
148 
149  msg.pose.covariance[29] = parsing_utilities::convertCovariance(
150  last_insnavgeod_.heading_pitch_cov);
151  msg.pose.covariance[33] = parsing_utilities::convertCovariance(
152  last_insnavgeod_.heading_roll_cov);
153  msg.pose.covariance[34] = parsing_utilities::convertCovariance(
154  last_insnavgeod_.heading_pitch_cov);
155  }
156  } else
157  {
158  if ((!validValue(last_pvtgeodetic_.block_header.tow)) ||
159  (last_pvtgeodetic_.block_header.tow !=
160  last_atteuler_.block_header.tow) ||
161  (last_pvtgeodetic_.block_header.tow !=
162  last_poscovgeodetic_.block_header.tow) ||
163  (last_pvtgeodetic_.block_header.tow !=
164  last_attcoveuler_.block_header.tow))
165  return;
166 
167  msg.header = last_pvtgeodetic_.header;
168 
169  // Filling in the pose data
170  double yaw = last_atteuler_.heading;
171  double pitch = last_atteuler_.pitch;
172  double roll = last_atteuler_.roll;
173 
174  roll = std::isnan(roll) ? 0.0 : roll;
175  pitch = std::isnan(pitch) ? 0.0 : pitch;
176 
177  msg.pose.pose.orientation = convertEulerToQuaternionMsg(
178  deg2rad(roll), deg2rad(pitch), deg2rad(yaw));
179  msg.pose.pose.position.x = rad2deg(last_pvtgeodetic_.longitude);
180  msg.pose.pose.position.y = rad2deg(last_pvtgeodetic_.latitude);
181  msg.pose.pose.position.z = last_pvtgeodetic_.height;
182  // Filling in the covariance data in row-major order
183  msg.pose.covariance[0] = last_poscovgeodetic_.cov_lonlon;
184  msg.pose.covariance[1] = last_poscovgeodetic_.cov_latlon;
185  msg.pose.covariance[2] = last_poscovgeodetic_.cov_lonhgt;
186  msg.pose.covariance[6] = last_poscovgeodetic_.cov_latlon;
187  msg.pose.covariance[7] = last_poscovgeodetic_.cov_latlat;
188  msg.pose.covariance[8] = last_poscovgeodetic_.cov_lathgt;
189  msg.pose.covariance[12] = last_poscovgeodetic_.cov_lonhgt;
190  msg.pose.covariance[13] = last_poscovgeodetic_.cov_lathgt;
191  msg.pose.covariance[14] = last_poscovgeodetic_.cov_hgthgt;
192  msg.pose.covariance[21] = parsing_utilities::convertAutoCovariance(
193  last_attcoveuler_.cov_rollroll);
194  msg.pose.covariance[22] = parsing_utilities::convertCovariance(
195  last_attcoveuler_.cov_pitchroll);
196  msg.pose.covariance[23] =
197  parsing_utilities::convertCovariance(last_attcoveuler_.cov_headroll);
198  msg.pose.covariance[27] = parsing_utilities::convertCovariance(
199  last_attcoveuler_.cov_pitchroll);
200  msg.pose.covariance[28] = parsing_utilities::convertAutoCovariance(
201  last_attcoveuler_.cov_pitchpitch);
202  msg.pose.covariance[29] = parsing_utilities::convertCovariance(
203  last_attcoveuler_.cov_headpitch);
204  msg.pose.covariance[33] =
205  parsing_utilities::convertCovariance(last_attcoveuler_.cov_headroll);
206  msg.pose.covariance[34] = parsing_utilities::convertCovariance(
207  last_attcoveuler_.cov_headpitch);
208  msg.pose.covariance[35] = parsing_utilities::convertAutoCovariance(
209  last_attcoveuler_.cov_headhead);
210  }
211  publish<PoseWithCovarianceStampedMsg>("pose", msg);
212  };
213 
214  void MessageHandler::assembleDiagnosticArray(
215  const std::shared_ptr<Telegram>& telegram)
216  {
217  if (last_receiverstatus_.rx_error & (1 << 9))
218  node_->log(log_level::DEBUG, " RX has reported CPU overload!");
219 
220  if (!settings_->publish_diagnostics)
221  return;
222 
223  DiagnosticArrayMsg msg;
224  if (!validValue(last_receiverstatus_.block_header.tow) ||
225  (last_receiverstatus_.block_header.tow !=
226  last_qualityind_.block_header.tow))
227  return;
228  std::string serialnumber;
229  if (validValue(last_receiversetup_.block_header.tow))
230  serialnumber = last_receiversetup_.rx_serial_number;
231  else
232  serialnumber = "unknown";
233  DiagnosticStatusMsg gnss_status;
234  // Constructing the "level of operation" field
235  uint16_t indicators_type_mask = static_cast<uint16_t>(255);
236  uint16_t indicators_value_mask = static_cast<uint16_t>(3840);
237  uint16_t qualityind_pos;
238  for (uint16_t i = static_cast<uint16_t>(0);
239  i < last_qualityind_.indicators.size(); ++i)
240  {
241  if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
242  static_cast<uint16_t>(0))
243  {
244  qualityind_pos = i;
245  if (((last_qualityind_.indicators[i] & indicators_value_mask) >>
246  8) == static_cast<uint16_t>(0))
247  {
248  gnss_status.level = DiagnosticStatusMsg::STALE;
249  } else if (((last_qualityind_.indicators[i] &
250  indicators_value_mask) >>
251  8) == static_cast<uint16_t>(1) ||
252  ((last_qualityind_.indicators[i] &
253  indicators_value_mask) >>
254  8) == static_cast<uint16_t>(2))
255  {
256  gnss_status.level = DiagnosticStatusMsg::WARN;
257  } else
258  {
259  gnss_status.level = DiagnosticStatusMsg::OK;
260  }
261  break;
262  }
263  }
264  // If the ReceiverStatus's RxError field is not 0, then at least one error
265  // has been detected.
266  if (last_receiverstatus_.rx_error != static_cast<uint32_t>(0))
267  {
268  gnss_status.level = DiagnosticStatusMsg::ERROR;
269  }
270  // Creating an array of values associated with the GNSS status
271  gnss_status.values.resize(static_cast<uint16_t>(last_qualityind_.n - 1));
272  for (uint16_t i = static_cast<uint16_t>(0);
273  i != static_cast<uint16_t>(last_qualityind_.n); ++i)
274  {
275  if (i == qualityind_pos)
276  {
277  continue;
278  }
279  if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
280  static_cast<uint16_t>(1))
281  {
282  gnss_status.values[i].key = "GNSS Signals, Main Antenna";
283  gnss_status.values[i].value = std::to_string(
284  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
285  } else if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
286  static_cast<uint16_t>(2))
287  {
288  gnss_status.values[i].key = "GNSS Signals, Aux1 Antenna";
289  gnss_status.values[i].value = std::to_string(
290  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
291  } else if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
292  static_cast<uint16_t>(11))
293  {
294  gnss_status.values[i].key = "RF Power, Main Antenna";
295  gnss_status.values[i].value = std::to_string(
296  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
297  } else if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
298  static_cast<uint16_t>(12))
299  {
300  gnss_status.values[i].key = "RF Power, Aux1 Antenna";
301  gnss_status.values[i].value = std::to_string(
302  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
303  } else if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
304  static_cast<uint16_t>(21))
305  {
306  gnss_status.values[i].key = "CPU Headroom";
307  gnss_status.values[i].value = std::to_string(
308  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
309  } else if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
310  static_cast<uint16_t>(25))
311  {
312  gnss_status.values[i].key = "OCXO Stability";
313  gnss_status.values[i].value = std::to_string(
314  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
315  } else if ((last_qualityind_.indicators[i] & indicators_type_mask) ==
316  static_cast<uint16_t>(30))
317  {
318  gnss_status.values[i].key = "Base Station Measurements";
319  gnss_status.values[i].value = std::to_string(
320  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
321  } else
322  {
323  assert((last_qualityind_.indicators[i] & indicators_type_mask) ==
324  static_cast<uint16_t>(31));
325  gnss_status.values[i].key = "RTK Post-Processing";
326  gnss_status.values[i].value = std::to_string(
327  (last_qualityind_.indicators[i] & indicators_value_mask) >> 8);
328  }
329  }
330  gnss_status.hardware_id = serialnumber;
331  gnss_status.name = "septentrio_driver: Quality indicators";
332  gnss_status.message =
333  "GNSS quality Indicators (from 0 for low quality to 10 for high quality, 15 if unknown)";
334  msg.status.push_back(gnss_status);
335  DiagnosticStatusMsg receiver_status;
336  receiver_status.hardware_id = serialnumber;
337  receiver_status.name = "septentrio_driver: receiver status";
338  receiver_status.message = "Receiver status";
339  receiver_status.values.resize(5);
340  receiver_status.values[0].key = "ExtError";
341  receiver_status.values[0].value =
342  std::to_string(last_receiverstatus_.ext_error);
343  receiver_status.values[1].key = "RxError";
344  receiver_status.values[1].value =
345  std::to_string(last_receiverstatus_.rx_error);
346  receiver_status.values[2].key = "RxStatus";
347  receiver_status.values[2].value =
348  std::to_string(last_receiverstatus_.rx_status);
349  receiver_status.values[3].key = "Uptime in s";
350  receiver_status.values[3].value =
351  std::to_string(last_receiverstatus_.up_time);
352  receiver_status.values[4].key = "CPU load in %";
353  receiver_status.values[4].value =
354  std::to_string(last_receiverstatus_.cpu_load);
355  if ((last_receiverstatus_.rx_error & (1 << 9)))
356  receiver_status.level = DiagnosticStatusMsg::ERROR;
357  else if ((last_receiverstatus_.rx_status & (1 << 8)))
358  receiver_status.level = DiagnosticStatusMsg::WARN;
359  else
360  receiver_status.level = DiagnosticStatusMsg::OK;
361  msg.status.push_back(receiver_status);
362  std::string frame_id;
363  if (settings_->septentrio_receiver_type == "gnss")
364  {
365  frame_id = settings_->frame_id;
366  }
367  if (settings_->septentrio_receiver_type == "ins")
368  {
369  if (settings_->ins_use_poi)
370  {
371  frame_id = settings_->poi_frame_id;
372  } else
373  {
374  frame_id = settings_->frame_id;
375  }
376  }
377  assembleHeader(frame_id, telegram, msg);
378  publish<DiagnosticArrayMsg>("/diagnostics", msg);
379  };
380 
381  void MessageHandler::assembleOsnmaDiagnosticArray()
382  {
383  DiagnosticArrayMsg msg;
384  DiagnosticStatusMsg diagOsnma;
385 
386  diagOsnma.hardware_id = last_receiversetup_.rx_serial_number;
387  diagOsnma.name = "septentrio_driver: OSNMA";
388  diagOsnma.message = "Current status of the OSNMA authentication";
389 
390  diagOsnma.values.resize(6);
391  diagOsnma.values[0].key = "status";
392  switch (last_gal_auth_status_.osnma_status & 7)
393  {
394  case 0:
395  {
396  diagOsnma.values[0].value = "Disabled";
397  break;
398  }
399  case 1:
400  {
401  uint16_t percent = (last_gal_auth_status_.osnma_status >> 3) & 127;
402  diagOsnma.values[0].value =
403  "Initializing " + std::to_string(percent) + " %";
404  break;
405  }
406  case 2:
407  {
408  diagOsnma.values[0].value = "Waiting on NTP";
409  break;
410  }
411  case 3:
412  {
413  diagOsnma.values[0].value = "Init failed - inconsistent time";
414  break;
415  }
416  case 4:
417  {
418  diagOsnma.values[0].value = "Init failed - KROOT signature invalid";
419  break;
420  }
421  case 5:
422  {
423  diagOsnma.values[0].value = "Init failed - invalid param received";
424  break;
425  }
426  case 6:
427  {
428  diagOsnma.values[0].value = "Authenticating";
429  break;
430  }
431 
432  default:
433  break;
434  }
435 
436  diagOsnma.values[1].key = "trusted_time_delta";
437  if (validValue(last_gal_auth_status_.trusted_time_delta))
438  diagOsnma.values[1].value =
439  std::to_string(last_gal_auth_status_.trusted_time_delta);
440  else
441  diagOsnma.values[1].value = "N/A";
442 
443  std::bitset<64> gal_active = last_gal_auth_status_.gal_active_mask;
444  std::bitset<64> gal_auth = last_gal_auth_status_.gal_authentic_mask;
445  uint8_t gal_authentic = (gal_auth & gal_active).count();
446  uint8_t gal_spoofed = (~gal_auth & gal_active).count();
447  diagOsnma.values[2].key = "Galileo authentic";
448  diagOsnma.values[2].value = std::to_string(gal_authentic);
449  diagOsnma.values[3].key = "Galileo spoofed";
450  diagOsnma.values[3].value = std::to_string(gal_spoofed);
451 
452  std::bitset<64> gps_active = last_gal_auth_status_.gps_active_mask;
453  std::bitset<64> gps_auth = last_gal_auth_status_.gps_authentic_mask;
454  uint8_t gps_authentic = (gps_auth & gps_active).count();
455  uint8_t gps_spoofed = (~gps_auth & gps_active).count();
456  diagOsnma.values[4].key = "GPS authentic";
457  diagOsnma.values[4].value = std::to_string(gps_authentic);
458  diagOsnma.values[5].key = "GPS spoofed";
459  diagOsnma.values[5].value = std::to_string(gps_spoofed);
460 
461  if ((gal_spoofed + gps_spoofed) == 0)
462  diagOsnma.level = DiagnosticStatusMsg::OK;
463  else if ((gal_authentic + gps_authentic) > 0)
464  diagOsnma.level = DiagnosticStatusMsg::WARN;
465  else
466  diagOsnma.level = DiagnosticStatusMsg::ERROR;
467 
468  msg.status.push_back(diagOsnma);
469  msg.header = last_gal_auth_status_.header;
470 
471  publish<DiagnosticArrayMsg>("/diagnostics", msg);
472  }
473 
474  void MessageHandler::assembleAimAndDiagnosticArray()
475  {
476  AimPlusStatusMsg aimMsg;
477  DiagnosticArrayMsg msg;
478  DiagnosticStatusMsg diagRf;
479  diagRf.hardware_id = last_receiversetup_.rx_serial_number;
480  diagRf.name = "septentrio_driver: AIM+ status";
481  diagRf.message =
482  "Current status of the AIM+ interference and spoofing mitigation";
483 
484  diagRf.values.resize(2);
485  diagRf.values[0].key = "interference";
486  bool mitigated = false;
487  bool detected = false;
488  for (auto rfband : last_rf_status_.rfband)
489  {
490  std::bitset<8> info = rfband.info;
491  if (info.test(1))
492  mitigated = true;
493  if (info.test(3))
494  {
495  detected = true;
496  break;
497  }
498  }
499  if (detected)
500  {
501  diagRf.values[0].value = "present";
502  aimMsg.interference = AimPlusStatusMsg::INTERFERENCE_PRESENT;
503  } else if (mitigated)
504  {
505  diagRf.values[0].value = "mitigated";
506  aimMsg.interference = AimPlusStatusMsg::INTERFERENCE_MITIGATED;
507  } else
508  {
509  diagRf.values[0].value = "spectrum clean";
510  aimMsg.interference = AimPlusStatusMsg::SPECTRUM_CLEAN;
511  }
512 
513  diagRf.values[1].key = "spoofing";
514  bool spoofed = false;
515  std::bitset<8> flags = last_rf_status_.flags;
516  if (flags.test(0) && flags.test(1))
517  {
518  diagRf.values[1].value = "detected by OSNMA and authenticity test";
519  aimMsg.spoofing =
520  AimPlusStatusMsg::SPOOFING_DETECTED_BY_OSNMA_AND_AUTHENTCITY_TEST;
521  spoofed = true;
522  } else if (flags.test(0))
523  {
524  diagRf.values[1].value = "detected by authenticity test";
525  aimMsg.spoofing =
526  AimPlusStatusMsg::SPOOFING_DETECTED_BY_AUTHENTCITY_TEST;
527  spoofed = true;
528  } else if (flags.test(1))
529  {
530  diagRf.values[1].value = "detected by OSNMA";
531  aimMsg.spoofing = AimPlusStatusMsg::SPOOFING_DETECTED_BY_OSNMA;
532  spoofed = true;
533  } else
534  {
535  diagRf.values[1].value = "none detected";
536  aimMsg.spoofing = AimPlusStatusMsg::NONE_DETECTED;
537  }
538  if (osnma_info_available_)
539  {
540  aimMsg.osnma_authenticating =
541  ((last_gal_auth_status_.osnma_status & 7) == 6);
542  std::bitset<64> gal_active = last_gal_auth_status_.gal_active_mask;
543  std::bitset<64> gal_auth = last_gal_auth_status_.gal_authentic_mask;
544  aimMsg.galileo_authentic = (gal_auth & gal_active).count();
545  aimMsg.galileo_spoofed = (~gal_auth & gal_active).count();
546  std::bitset<64> gps_active = last_gal_auth_status_.gps_active_mask;
547  std::bitset<64> gps_auth = last_gal_auth_status_.gps_authentic_mask;
548  aimMsg.gps_authentic = (gps_auth & gps_active).count();
549  aimMsg.gps_spoofed = (~gps_auth & gps_active).count();
550  } else
551  {
552  aimMsg.osnma_authenticating = false;
553  aimMsg.galileo_authentic = 0;
554  aimMsg.galileo_spoofed = 0;
555  aimMsg.gps_authentic = 0;
556  aimMsg.gps_spoofed = 0;
557  }
558  aimMsg.header = last_rf_status_.header;
559  aimMsg.tow = last_rf_status_.block_header.tow;
560  aimMsg.wnc = last_rf_status_.block_header.wnc;
561  publish<AimPlusStatusMsg>("aimplusstatus", aimMsg);
562 
563  if (spoofed || detected)
564  diagRf.level = DiagnosticStatusMsg::ERROR;
565  else if (mitigated)
566  diagRf.level = DiagnosticStatusMsg::WARN;
567  else
568  diagRf.level = DiagnosticStatusMsg::OK;
569 
570  msg.status.push_back(diagRf);
571  msg.header = last_rf_status_.header;
572 
573  publish<DiagnosticArrayMsg>("/diagnostics", msg);
574  }
575 
576  void MessageHandler::assembleImu()
577  {
578  // INS tow and extsens meas tow have the same time scale
579  ImuMsg msg;
580 
581  msg.header = last_extsensmeas_.header;
582 
583  msg.linear_acceleration.x = last_extsensmeas_.acceleration_x;
584  msg.linear_acceleration.y = last_extsensmeas_.acceleration_y;
585  msg.linear_acceleration.z = last_extsensmeas_.acceleration_z;
586 
587  msg.angular_velocity.x = deg2rad(last_extsensmeas_.angular_rate_x);
588  msg.angular_velocity.y = deg2rad(last_extsensmeas_.angular_rate_y);
589  msg.angular_velocity.z = deg2rad(last_extsensmeas_.angular_rate_z);
590 
591  Timestamp tsImu = timestampSBF(last_extsensmeas_.block_header.tow,
592  last_extsensmeas_.block_header.wnc);
593  Timestamp tsIns = timestampSBF(last_insnavgeod_.block_header.tow,
594  last_insnavgeod_.block_header.wnc);
595 
596  bool valid_orientation = false;
597  if (validValue(last_insnavgeod_.block_header.tow) && (tsImu == tsIns))
598  {
599  if ((last_insnavgeod_.sb_list & 2) != 0)
600  {
601  // Attitude
602  if (validValue(last_insnavgeod_.heading) &&
603  validValue(last_insnavgeod_.pitch) &&
604  validValue(last_insnavgeod_.roll))
605  {
606  msg.orientation = convertEulerToQuaternionMsg(
607  deg2rad(last_insnavgeod_.roll),
608  deg2rad(last_insnavgeod_.pitch),
609  deg2rad(last_insnavgeod_.heading));
610  valid_orientation = true;
611  } else if (validValue(last_insnavgeod_.heading))
612  {
613  msg.orientation = convertEulerToQuaternionMsg(
614  0.0, 0.0, deg2rad(last_insnavgeod_.heading));
615  valid_orientation = true;
616  }
617  }
618  if ((last_insnavgeod_.sb_list & 4) != 0)
619  {
620  // Attitude autocov
621  if (validValue(last_insnavgeod_.roll_std_dev) &&
622  validValue(last_insnavgeod_.pitch_std_dev) &&
623  validValue(last_insnavgeod_.heading_std_dev))
624  {
625  msg.orientation_covariance[0] =
626  parsing_utilities::convertAutoCovariance(
627  last_insnavgeod_.roll_std_dev);
628  msg.orientation_covariance[4] =
629  parsing_utilities::convertAutoCovariance(
630  last_insnavgeod_.pitch_std_dev);
631  msg.orientation_covariance[8] =
632  parsing_utilities::convertAutoCovariance(
633  last_insnavgeod_.heading_std_dev);
634 
635  if ((last_insnavgeod_.sb_list & 64) != 0)
636  {
637  // Attitude cov
638  msg.orientation_covariance[1] =
639  parsing_utilities::convertCovariance(
640  last_insnavgeod_.pitch_roll_cov);
641  msg.orientation_covariance[2] =
642  parsing_utilities::convertCovariance(
643  last_insnavgeod_.heading_roll_cov);
644  msg.orientation_covariance[3] =
645  parsing_utilities::convertCovariance(
646  last_insnavgeod_.pitch_roll_cov);
647 
648  msg.orientation_covariance[5] =
649  parsing_utilities::convertCovariance(
650  last_insnavgeod_.heading_pitch_cov);
651  msg.orientation_covariance[6] =
652  parsing_utilities::convertCovariance(
653  last_insnavgeod_.heading_roll_cov);
654  msg.orientation_covariance[7] =
655  parsing_utilities::convertCovariance(
656  last_insnavgeod_.heading_pitch_cov);
657  }
658  } else
659  {
660  msg.orientation_covariance[0] = -1.0;
661  msg.orientation_covariance[4] = -1.0;
662  msg.orientation_covariance[8] = -1.0;
663  }
664  }
665  }
666 
667  if (!valid_orientation)
668  {
669  parsing_utilities::setQuaternionNaN(msg.orientation);
670  }
671 
672  publish<ImuMsg>("imu", msg);
673  };
674 
675  void MessageHandler::assembleTwist(bool fromIns /* = false*/)
676  {
677  if (!settings_->publish_twist)
678  return;
679  TwistWithCovarianceStampedMsg msg;
680 
681  // Autocovariances of angular velocity
682  msg.twist.covariance[21] = -1.0;
683  msg.twist.covariance[28] = -1.0;
684  msg.twist.covariance[35] = -1.0;
685  // Set angular velocities to NaN
686  parsing_utilities::setVector3NaN(msg.twist.twist.angular);
687 
688  if (fromIns)
689  {
690  msg.header = last_insnavgeod_.header;
691 
692  if ((last_insnavgeod_.sb_list & 8) != 0)
693  {
694  // Linear velocity in navigation frame
695  double ve = last_insnavgeod_.ve;
696  double vn = last_insnavgeod_.vn;
697  double vu = last_insnavgeod_.vu;
698  Eigen::Vector3d vel;
699  if (settings_->use_ros_axis_orientation)
700  {
701  // (ENU)
702  vel << ve, vn, vu;
703  } else
704  {
705  // (NED)
706  vel << vn, ve, -vu;
707  }
708  // Linear velocity
709  msg.twist.twist.linear.x = vel(0);
710  msg.twist.twist.linear.y = vel(1);
711  msg.twist.twist.linear.z = vel(2);
712  } else
713  {
714  parsing_utilities::setVector3NaN(msg.twist.twist.linear);
715  }
716 
717  if (((last_insnavgeod_.sb_list & 16) != 0) &&
718  ((last_insnavgeod_.sb_list & 2) != 0) &&
719  ((last_insnavgeod_.sb_list & 8) != 0))
720  {
721  Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero();
722  if ((last_insnavgeod_.sb_list & 128) != 0)
723  {
724  // Linear velocity covariance
725  if (validValue(last_insnavgeod_.ve_std_dev))
726  if (settings_->use_ros_axis_orientation)
727  covVel_local(0, 0) = square(last_insnavgeod_.ve_std_dev);
728  else
729  covVel_local(1, 1) = square(last_insnavgeod_.ve_std_dev);
730  else
731  covVel_local(0, 0) = -1.0;
732  if (validValue(last_insnavgeod_.vn_std_dev))
733  if (settings_->use_ros_axis_orientation)
734  covVel_local(1, 1) = square(last_insnavgeod_.vn_std_dev);
735  else
736  covVel_local(0, 0) = square(last_insnavgeod_.vn_std_dev);
737  else
738  covVel_local(1, 1) = -1.0;
739  if (validValue(last_insnavgeod_.vu_std_dev))
740  covVel_local(2, 2) = square(last_insnavgeod_.vu_std_dev);
741  else
742  covVel_local(2, 2) = -1.0;
743 
744  if (validValue(last_insnavgeod_.ve_vn_cov))
745  covVel_local(0, 1) = covVel_local(1, 0) =
746  last_insnavgeod_.ve_vn_cov;
747  if (settings_->use_ros_axis_orientation)
748  {
749  if (validValue(last_insnavgeod_.ve_vu_cov))
750  covVel_local(0, 2) = covVel_local(2, 0) =
751  last_insnavgeod_.ve_vu_cov;
752  if (validValue(last_insnavgeod_.vn_vu_cov))
753  covVel_local(2, 1) = covVel_local(1, 2) =
754  last_insnavgeod_.vn_vu_cov;
755  } else
756  {
757  if (validValue(last_insnavgeod_.vn_vu_cov))
758  covVel_local(0, 2) = covVel_local(2, 0) =
759  -last_insnavgeod_.vn_vu_cov;
760  if (validValue(last_insnavgeod_.ve_vu_cov))
761  covVel_local(2, 1) = covVel_local(1, 2) =
762  -last_insnavgeod_.ve_vu_cov;
763  }
764  } else
765  {
766  covVel_local(0, 0) = -1.0;
767  covVel_local(1, 1) = -1.0;
768  covVel_local(2, 2) = -1.0;
769  }
770 
771  msg.twist.covariance[0] = covVel_local(0, 0);
772  msg.twist.covariance[1] = covVel_local(0, 1);
773  msg.twist.covariance[2] = covVel_local(0, 2);
774  msg.twist.covariance[6] = covVel_local(1, 0);
775  msg.twist.covariance[7] = covVel_local(1, 1);
776  msg.twist.covariance[8] = covVel_local(1, 2);
777  msg.twist.covariance[12] = covVel_local(2, 0);
778  msg.twist.covariance[13] = covVel_local(2, 1);
779  msg.twist.covariance[14] = covVel_local(2, 2);
780  } else
781  {
782  msg.twist.covariance[0] = -1.0;
783  msg.twist.covariance[7] = -1.0;
784  msg.twist.covariance[14] = -1.0;
785  }
786 
787  publish<TwistWithCovarianceStampedMsg>("twist_ins", msg);
788  } else
789  {
790  if ((!validValue(last_pvtgeodetic_.block_header.tow)) ||
791  (last_pvtgeodetic_.block_header.tow !=
792  last_velcovgeodetic_.block_header.tow))
793  return;
794  msg.header = last_pvtgeodetic_.header;
795 
796  if (last_pvtgeodetic_.error == 0)
797  {
798  // Linear velocity in navigation frame
799  double ve = last_pvtgeodetic_.ve;
800  double vn = last_pvtgeodetic_.vn;
801  double vu = last_pvtgeodetic_.vu;
802  Eigen::Vector3d vel;
803  if (settings_->use_ros_axis_orientation)
804  {
805  // (ENU)
806  vel << ve, vn, vu;
807  } else
808  {
809  // (NED)
810  vel << vn, ve, -vu;
811  }
812  // Linear velocity
813  msg.twist.twist.linear.x = vel(0);
814  msg.twist.twist.linear.y = vel(1);
815  msg.twist.twist.linear.z = vel(2);
816  } else
817  {
818  parsing_utilities::setVector3NaN(msg.twist.twist.linear);
819  }
820 
821  if (last_velcovgeodetic_.error == 0)
822  {
823  Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero();
824  // Linear velocity covariance in navigation frame
825  if (validValue(last_velcovgeodetic_.cov_veve))
826  if (settings_->use_ros_axis_orientation)
827  covVel_local(0, 0) = last_velcovgeodetic_.cov_veve;
828  else
829  covVel_local(1, 1) = last_velcovgeodetic_.cov_veve;
830  else
831  covVel_local(0, 0) = -1.0;
832  if (validValue(last_velcovgeodetic_.cov_vnvn))
833  if (settings_->use_ros_axis_orientation)
834  covVel_local(1, 1) = last_velcovgeodetic_.cov_vnvn;
835  else
836  covVel_local(0, 0) = last_velcovgeodetic_.cov_vnvn;
837  else
838  covVel_local(1, 1) = -1.0;
839  if (validValue(last_velcovgeodetic_.cov_vuvu))
840  covVel_local(2, 2) = last_velcovgeodetic_.cov_vuvu;
841  else
842  covVel_local(2, 2) = -1.0;
843 
844  covVel_local(0, 1) = covVel_local(1, 0) =
845  last_velcovgeodetic_.cov_vnve;
846  if (settings_->use_ros_axis_orientation)
847  {
848  if (validValue(last_velcovgeodetic_.cov_vevu))
849  covVel_local(0, 2) = covVel_local(2, 0) =
850  last_velcovgeodetic_.cov_vevu;
851  if (validValue(last_velcovgeodetic_.cov_vnvu))
852  covVel_local(2, 1) = covVel_local(1, 2) =
853  last_velcovgeodetic_.cov_vnvu;
854  } else
855  {
856  if (validValue(last_velcovgeodetic_.cov_vnvu))
857  covVel_local(0, 2) = covVel_local(2, 0) =
858  -last_velcovgeodetic_.cov_vnvu;
859  if (validValue(last_velcovgeodetic_.cov_vevu))
860  covVel_local(2, 1) = covVel_local(1, 2) =
861  -last_velcovgeodetic_.cov_vevu;
862  }
863 
864  msg.twist.covariance[0] = covVel_local(0, 0);
865  msg.twist.covariance[1] = covVel_local(0, 1);
866  msg.twist.covariance[2] = covVel_local(0, 2);
867  msg.twist.covariance[6] = covVel_local(1, 0);
868  msg.twist.covariance[7] = covVel_local(1, 1);
869  msg.twist.covariance[8] = covVel_local(1, 2);
870  msg.twist.covariance[12] = covVel_local(2, 0);
871  msg.twist.covariance[13] = covVel_local(2, 1);
872  msg.twist.covariance[14] = covVel_local(2, 2);
873  } else
874  {
875  msg.twist.covariance[0] = -1.0;
876  msg.twist.covariance[7] = -1.0;
877  msg.twist.covariance[14] = -1.0;
878  }
879 
880  publish<TwistWithCovarianceStampedMsg>("twist_gnss", msg);
881  }
882  };
883 
890  void MessageHandler::assembleLocalizationUtm()
891  {
892  if (!settings_->publish_localization && !settings_->publish_tf)
893  return;
894 
895  LocalizationMsg msg;
896 
897  int zone;
898  std::string zonestring;
899  bool northernHemisphere;
900  double easting;
901  double northing;
902  double meridian_convergence = 0.0;
903  if (fixedUtmZone_)
904  {
905  try
906  {
907  GeographicLib::UTMUPS::DecodeZone(*fixedUtmZone_, zone,
908  northernHemisphere);
909  double k;
910  GeographicLib::UTMUPS::Forward(
911  rad2deg(last_insnavgeod_.latitude),
912  rad2deg(last_insnavgeod_.longitude), zone, northernHemisphere,
913  easting, northing, meridian_convergence, k, zone);
914  } catch (const std::exception& e)
915  {
916  node_->log(log_level::DEBUG,
917  "UTMUPS conversion exception: " + std::string(e.what()));
918  return;
919  }
920  zonestring = *fixedUtmZone_;
921  } else
922  {
923  try
924  {
925  double k;
926  GeographicLib::UTMUPS::Forward(rad2deg(last_insnavgeod_.latitude),
927  rad2deg(last_insnavgeod_.longitude),
928  zone, northernHemisphere, easting,
929  northing, meridian_convergence, k);
930  zonestring =
931  GeographicLib::UTMUPS::EncodeZone(zone, northernHemisphere);
932  } catch (const std::exception& e)
933  {
934  node_->log(log_level::DEBUG,
935  "UTMUPS conversion exception: " + std::string(e.what()));
936  return;
937  }
938  }
939  if (settings_->lock_utm_zone && !fixedUtmZone_)
940  fixedUtmZone_ = std::make_shared<std::string>(zonestring);
941 
942  // UTM position (ENU)
943  if (settings_->use_ros_axis_orientation)
944  {
945  msg.pose.pose.position.x = easting;
946  msg.pose.pose.position.y = northing;
947  msg.pose.pose.position.z = last_insnavgeod_.height;
948  } else // (NED)
949  {
950  msg.pose.pose.position.x = northing;
951  msg.pose.pose.position.y = easting;
952  msg.pose.pose.position.z = -last_insnavgeod_.height;
953  }
954 
955  msg.header.frame_id = "utm_" + zonestring;
956  msg.header.stamp = last_insnavgeod_.header.stamp;
957  if (settings_->ins_use_poi)
958  msg.child_frame_id = settings_->poi_frame_id;
959  else
960  msg.child_frame_id = settings_->frame_id;
961 
962  Eigen::Matrix3d P_pos = -Eigen::Matrix3d::Identity();
963  if ((last_insnavgeod_.sb_list & 1) != 0)
964  {
965  // Position autocovariance
966  P_pos(0, 0) = square(last_insnavgeod_.longitude_std_dev);
967  P_pos(1, 1) = square(last_insnavgeod_.latitude_std_dev);
968  P_pos(2, 2) = square(last_insnavgeod_.height_std_dev);
969  }
970 
971  // Euler angles
972  double roll = 0.0;
973  double pitch = 0.0;
974  double yaw = 0.0;
975  if (validValue(last_insnavgeod_.roll))
976  roll = deg2rad(last_insnavgeod_.roll);
977  if (validValue(last_insnavgeod_.pitch))
978  pitch = deg2rad(last_insnavgeod_.pitch);
979  if (validValue(last_insnavgeod_.heading))
980  yaw = deg2rad(last_insnavgeod_.heading);
981  // meridian_convergence for conversion from true north to grid north
982  if (settings_->use_ros_axis_orientation)
983  yaw += deg2rad(meridian_convergence);
984  else
985  yaw -= deg2rad(meridian_convergence);
986 
987  if ((last_insnavgeod_.sb_list & 2) != 0)
988  {
989  // Attitude
990  msg.pose.pose.orientation =
991  convertEulerToQuaternionMsg(roll, pitch, yaw);
992  } else
993  {
994  parsing_utilities::setQuaternionNaN(msg.pose.pose.orientation);
995  }
996  if ((last_insnavgeod_.sb_list & 4) != 0)
997  {
998  // Attitude autocovariance
999  msg.pose.covariance[21] = parsing_utilities::convertAutoCovariance(
1000  last_insnavgeod_.roll_std_dev);
1001  msg.pose.covariance[28] = parsing_utilities::convertAutoCovariance(
1002  last_insnavgeod_.pitch_std_dev);
1003  msg.pose.covariance[35] = parsing_utilities::convertAutoCovariance(
1004  last_insnavgeod_.heading_std_dev);
1005  } else
1006  {
1007  msg.pose.covariance[21] = -1.0;
1008  msg.pose.covariance[28] = -1.0;
1009  msg.pose.covariance[35] = -1.0;
1010  }
1011 
1012  if ((last_insnavgeod_.sb_list & 32) != 0)
1013  {
1014  // Position covariance
1015  P_pos(0, 1) = last_insnavgeod_.latitude_longitude_cov;
1016  P_pos(1, 0) = last_insnavgeod_.latitude_longitude_cov;
1017 
1018  if (settings_->use_ros_axis_orientation)
1019  {
1020  // (ENU)
1021  P_pos(0, 2) = last_insnavgeod_.longitude_height_cov;
1022  P_pos(1, 2) = last_insnavgeod_.latitude_height_cov;
1023  P_pos(2, 0) = last_insnavgeod_.longitude_height_cov;
1024  P_pos(2, 1) = last_insnavgeod_.latitude_height_cov;
1025  } else
1026  {
1027  // (NED): down = -height
1028  P_pos(0, 2) = -last_insnavgeod_.latitude_height_cov;
1029  P_pos(1, 2) = -last_insnavgeod_.longitude_height_cov;
1030  P_pos(2, 0) = -last_insnavgeod_.latitude_height_cov;
1031  P_pos(2, 1) = -last_insnavgeod_.longitude_height_cov;
1032  }
1033  }
1034 
1035  if ((meridian_convergence != 0.0) && (last_insnavgeod_.sb_list & 1))
1036  {
1037  double cg = std::cos(meridian_convergence);
1038  double sg = std::sin(meridian_convergence);
1039  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
1040  R(0, 0) = cg;
1041  R(0, 1) = -sg;
1042  R(1, 0) = sg;
1043  R(1, 1) = cg;
1044  P_pos = (R * P_pos * R.transpose()).eval();
1045  }
1046 
1047  msg.pose.covariance[0] = P_pos(0, 0);
1048  msg.pose.covariance[1] = P_pos(0, 1);
1049  msg.pose.covariance[2] = P_pos(0, 2);
1050  msg.pose.covariance[6] = P_pos(1, 0);
1051  msg.pose.covariance[7] = P_pos(1, 1);
1052  msg.pose.covariance[8] = P_pos(1, 2);
1053  msg.pose.covariance[12] = P_pos(2, 0);
1054  msg.pose.covariance[13] = P_pos(2, 1);
1055  msg.pose.covariance[14] = P_pos(2, 2);
1056 
1057  if ((last_insnavgeod_.sb_list & 64) != 0)
1058  {
1059  // Attitude covariancae
1060  msg.pose.covariance[22] = parsing_utilities::convertCovariance(
1061  last_insnavgeod_.pitch_roll_cov);
1062  msg.pose.covariance[23] = parsing_utilities::convertCovariance(
1063  last_insnavgeod_.heading_roll_cov);
1064  msg.pose.covariance[27] = parsing_utilities::convertCovariance(
1065  last_insnavgeod_.pitch_roll_cov);
1066 
1067  msg.pose.covariance[29] = parsing_utilities::convertCovariance(
1068  last_insnavgeod_.heading_pitch_cov);
1069  msg.pose.covariance[33] = parsing_utilities::convertCovariance(
1070  last_insnavgeod_.heading_roll_cov);
1071  msg.pose.covariance[34] = parsing_utilities::convertCovariance(
1072  last_insnavgeod_.heading_pitch_cov);
1073  }
1074 
1075  assembleLocalizationMsgTwist(roll, pitch, yaw, msg);
1076 
1077  if (settings_->publish_localization)
1078  publish<LocalizationMsg>("localization", msg);
1079  if (settings_->publish_tf)
1080  publishTf(msg);
1081  };
1082 
1088  void MessageHandler::assembleLocalizationEcef()
1089  {
1090  if (!settings_->publish_localization_ecef && !settings_->publish_tf_ecef)
1091  return;
1092 
1093  if ((!validValue(last_insnavcart_.block_header.tow)) ||
1094  (last_insnavcart_.block_header.tow != last_insnavgeod_.block_header.tow))
1095  return;
1096 
1097  LocalizationMsg msg;
1098 
1099  msg.header.frame_id = "ecef";
1100  msg.header.stamp = last_insnavcart_.header.stamp;
1101  if (settings_->ins_use_poi)
1102  msg.child_frame_id = settings_->poi_frame_id;
1103  else
1104  msg.child_frame_id = settings_->frame_id;
1105 
1106  // ECEF position
1107  msg.pose.pose.position.x = last_insnavcart_.x;
1108  msg.pose.pose.position.y = last_insnavcart_.y;
1109  msg.pose.pose.position.z = last_insnavcart_.z;
1110 
1111  if ((last_insnavgeod_.sb_list & 1) != 0)
1112  {
1113  // Position autocovariance
1114  msg.pose.covariance[0] = square(last_insnavcart_.x_std_dev);
1115  msg.pose.covariance[7] = square(last_insnavcart_.y_std_dev);
1116  msg.pose.covariance[14] = square(last_insnavcart_.z_std_dev);
1117  } else
1118  {
1119  msg.pose.covariance[0] = -1.0;
1120  msg.pose.covariance[7] = -1.0;
1121  msg.pose.covariance[14] = -1.0;
1122  }
1123  if ((last_insnavcart_.sb_list & 32) != 0)
1124  {
1125  // Position covariance
1126  msg.pose.covariance[1] = last_insnavcart_.xy_cov;
1127  msg.pose.covariance[6] = last_insnavcart_.xy_cov;
1128  msg.pose.covariance[2] = last_insnavcart_.xz_cov;
1129  msg.pose.covariance[8] = last_insnavcart_.yz_cov;
1130  msg.pose.covariance[12] = last_insnavcart_.xz_cov;
1131  msg.pose.covariance[13] = last_insnavcart_.yz_cov;
1132  }
1133 
1134  // Euler angles
1135  double roll = 0.0;
1136  double pitch = 0.0;
1137  double yaw = 0.0;
1138  if (validValue(last_insnavcart_.roll))
1139  roll = deg2rad(last_insnavcart_.roll);
1140  if (validValue(last_insnavcart_.pitch))
1141  pitch = deg2rad(last_insnavcart_.pitch);
1142  if (validValue(last_insnavcart_.heading))
1143  yaw = deg2rad(last_insnavcart_.heading);
1144 
1145  if ((last_insnavcart_.sb_list & 2) != 0)
1146  {
1147  // Attitude
1148  Eigen::Quaterniond q_local_ecef;
1149  if (settings_->use_ros_axis_orientation)
1150  q_local_ecef = parsing_utilities::q_enu_ecef(
1151  last_insnavgeod_.latitude, last_insnavgeod_.longitude);
1152  else
1153  q_local_ecef = parsing_utilities::q_ned_ecef(
1154  last_insnavgeod_.latitude, last_insnavgeod_.longitude);
1155  Eigen::Quaterniond q_b_local =
1156  parsing_utilities::convertEulerToQuaternion(roll, pitch, yaw);
1157 
1158  Eigen::Quaterniond q_b_ecef = q_local_ecef * q_b_local;
1159 
1160  msg.pose.pose.orientation =
1161  parsing_utilities::quaternionToQuaternionMsg(q_b_ecef);
1162  } else
1163  {
1164  parsing_utilities::setQuaternionNaN(msg.pose.pose.orientation);
1165  }
1166  Eigen::Matrix3d covAtt_local = Eigen::Matrix3d::Zero();
1167  bool covAttValid = true;
1168  if ((last_insnavgeod_.sb_list & 4) != 0)
1169  {
1170  // Attitude autocovariance
1171  covAtt_local(0, 0) = parsing_utilities::convertAutoCovariance(
1172  last_insnavgeod_.roll_std_dev);
1173  covAtt_local(1, 1) = parsing_utilities::convertAutoCovariance(
1174  last_insnavgeod_.pitch_std_dev);
1175  covAtt_local(2, 2) = parsing_utilities::convertAutoCovariance(
1176  last_insnavgeod_.heading_std_dev);
1177  covAttValid = !std::isnan(last_insnavgeod_.roll_std_dev) &&
1178  !std::isnan(last_insnavgeod_.pitch_std_dev) &&
1179  !std::isnan(last_insnavgeod_.heading_std_dev);
1180  } else
1181  {
1182  covAtt_local(0, 0) = -1.0;
1183  covAtt_local(1, 1) = -1.0;
1184  covAtt_local(2, 2) = -1.0;
1185  covAttValid = false;
1186  }
1187 
1188  if (covAttValid)
1189  {
1190  if ((last_insnavcart_.sb_list & 64) != 0)
1191  {
1192  // Attitude covariancae
1193  covAtt_local(0, 1) = parsing_utilities::convertCovariance(
1194  last_insnavcart_.pitch_roll_cov);
1195  covAtt_local(0, 2) = parsing_utilities::convertCovariance(
1196  last_insnavcart_.heading_roll_cov);
1197  covAtt_local(1, 0) = parsing_utilities::convertCovariance(
1198  last_insnavcart_.pitch_roll_cov);
1199  covAtt_local(2, 1) = parsing_utilities::convertCovariance(
1200  last_insnavcart_.heading_pitch_cov);
1201  covAtt_local(2, 0) = parsing_utilities::convertCovariance(
1202  last_insnavcart_.heading_roll_cov);
1203  covAtt_local(1, 2) = parsing_utilities::convertCovariance(
1204  last_insnavcart_.heading_pitch_cov);
1205  }
1206 
1207  Eigen::Matrix3d R_local_ecef;
1208  if (settings_->use_ros_axis_orientation)
1209  R_local_ecef = parsing_utilities::R_enu_ecef(
1210  last_insnavgeod_.latitude, last_insnavgeod_.longitude);
1211  else
1212  R_local_ecef = parsing_utilities::R_ned_ecef(
1213  last_insnavgeod_.latitude, last_insnavgeod_.longitude);
1214  // Rotate attitude covariance matrix to ecef coordinates
1215  Eigen::Matrix3d covAtt_ecef =
1216  R_local_ecef * covAtt_local * R_local_ecef.transpose();
1217 
1218  msg.pose.covariance[21] = covAtt_ecef(0, 0);
1219  msg.pose.covariance[22] = covAtt_ecef(0, 1);
1220  msg.pose.covariance[23] = covAtt_ecef(0, 2);
1221  msg.pose.covariance[27] = covAtt_ecef(1, 0);
1222  msg.pose.covariance[28] = covAtt_ecef(1, 1);
1223  msg.pose.covariance[29] = covAtt_ecef(1, 2);
1224  msg.pose.covariance[33] = covAtt_ecef(2, 0);
1225  msg.pose.covariance[34] = covAtt_ecef(2, 1);
1226  msg.pose.covariance[35] = covAtt_ecef(2, 2);
1227  } else
1228  {
1229  msg.pose.covariance[21] = -1.0;
1230  msg.pose.covariance[28] = -1.0;
1231  msg.pose.covariance[35] = -1.0;
1232  }
1233 
1234  assembleLocalizationMsgTwist(roll, pitch, yaw, msg);
1235 
1236  if (settings_->publish_localization_ecef)
1237  publish<LocalizationMsg>("localization_ecef", msg);
1238  if (settings_->publish_tf_ecef)
1239  publishTf(msg);
1240  };
1241 
1242  void MessageHandler::assembleLocalizationMsgTwist(double roll, double pitch,
1243  double yaw,
1244  LocalizationMsg& msg) const
1245  {
1246 
1247  parsing_utilities::setVector3NaN(msg.twist.twist.angular);
1248  Eigen::Matrix3d R_local_body =
1249  parsing_utilities::rpyToRot(roll, pitch, yaw).inverse();
1250  if ((last_insnavgeod_.sb_list & 8) != 0)
1251  {
1252  // Linear velocity (ENU)
1253  double ve = last_insnavgeod_.ve;
1254  double vn = last_insnavgeod_.vn;
1255  double vu = last_insnavgeod_.vu;
1256  Eigen::Vector3d vel_local;
1257  if (settings_->use_ros_axis_orientation)
1258  {
1259  // (ENU)
1260  vel_local << ve, vn, vu;
1261  } else
1262  {
1263  // (NED)
1264  vel_local << vn, ve, -vu;
1265  }
1266  // Linear velocity, rotate to body coordinates
1267  Eigen::Vector3d vel_body = R_local_body * vel_local;
1268  msg.twist.twist.linear.x = vel_body(0);
1269  msg.twist.twist.linear.y = vel_body(1);
1270  msg.twist.twist.linear.z = vel_body(2);
1271  } else
1272  {
1273  parsing_utilities::setVector3NaN(msg.twist.twist.linear);
1274  }
1275  Eigen::Matrix3d covVel_local = Eigen::Matrix3d::Zero();
1276  if ((last_insnavgeod_.sb_list & 16) != 0)
1277  {
1278  // Linear velocity autocovariance
1279  if (validValue(last_insnavgeod_.ve_std_dev))
1280  if (settings_->use_ros_axis_orientation)
1281  covVel_local(0, 0) = square(last_insnavgeod_.ve_std_dev);
1282  else
1283  covVel_local(0, 0) = square(last_insnavgeod_.vn_std_dev);
1284  else
1285  covVel_local(0, 0) = -1.0;
1286  if (validValue(last_insnavgeod_.vn_std_dev))
1287  if (settings_->use_ros_axis_orientation)
1288  covVel_local(1, 1) = square(last_insnavgeod_.vn_std_dev);
1289  else
1290  covVel_local(1, 1) = square(last_insnavgeod_.ve_std_dev);
1291  else
1292  covVel_local(1, 1) = -1.0;
1293  if (validValue(last_insnavgeod_.vu_std_dev))
1294  covVel_local(2, 2) = square(last_insnavgeod_.vu_std_dev);
1295  else
1296  covVel_local(2, 2) = -1.0;
1297  } else
1298  {
1299  covVel_local(0, 0) = -1.0;
1300  covVel_local(1, 1) = -1.0;
1301  covVel_local(2, 2) = -1.0;
1302  }
1303 
1304  if ((last_insnavgeod_.sb_list & 128) != 0)
1305  {
1306  covVel_local(0, 1) = covVel_local(1, 0) = last_insnavgeod_.ve_vn_cov;
1307  if (settings_->use_ros_axis_orientation)
1308  {
1309  covVel_local(0, 2) = covVel_local(2, 0) = last_insnavgeod_.ve_vu_cov;
1310  covVel_local(2, 1) = covVel_local(1, 2) = last_insnavgeod_.vn_vu_cov;
1311  } else
1312  {
1313  covVel_local(0, 2) = covVel_local(2, 0) =
1314  -last_insnavgeod_.vn_vu_cov;
1315  covVel_local(2, 1) = covVel_local(1, 2) =
1316  -last_insnavgeod_.ve_vu_cov;
1317  }
1318  }
1319 
1320  if (((last_insnavgeod_.sb_list & 16) != 0) &&
1321  ((last_insnavgeod_.sb_list & 2) != 0) &&
1322  ((last_insnavgeod_.sb_list & 8) != 0) &&
1323  validValue(last_insnavgeod_.ve_std_dev) &&
1324  validValue(last_insnavgeod_.vn_std_dev) &&
1325  validValue(last_insnavgeod_.vu_std_dev))
1326  {
1327  // Rotate velocity covariance matrix to body coordinates
1328  Eigen::Matrix3d covVel_body =
1329  R_local_body * covVel_local * R_local_body.transpose();
1330 
1331  msg.twist.covariance[0] = covVel_body(0, 0);
1332  msg.twist.covariance[1] = covVel_body(0, 1);
1333  msg.twist.covariance[2] = covVel_body(0, 2);
1334  msg.twist.covariance[6] = covVel_body(1, 0);
1335  msg.twist.covariance[7] = covVel_body(1, 1);
1336  msg.twist.covariance[8] = covVel_body(1, 2);
1337  msg.twist.covariance[12] = covVel_body(2, 0);
1338  msg.twist.covariance[13] = covVel_body(2, 1);
1339  msg.twist.covariance[14] = covVel_body(2, 2);
1340  } else
1341  {
1342  msg.twist.covariance[0] = -1.0;
1343  msg.twist.covariance[7] = -1.0;
1344  msg.twist.covariance[14] = -1.0;
1345  }
1346  // Autocovariances of angular velocity
1347  msg.twist.covariance[21] = -1.0;
1348  msg.twist.covariance[28] = -1.0;
1349  msg.twist.covariance[35] = -1.0;
1350  }
1351 
1352  void MessageHandler::setStatus(uint8_t mode, NavSatFixMsg& msg)
1353  {
1354  switch (mode & 15)
1355  {
1356  case evNoPVT:
1357  {
1358  msg.status.status = NavSatStatusMsg::STATUS_NO_FIX;
1359  break;
1360  }
1361  case evStandAlone:
1362  {
1363  msg.status.status = NavSatStatusMsg::STATUS_FIX;
1364  break;
1365  }
1366  case evFixed:
1367  {
1368  msg.status.status = NavSatStatusMsg::STATUS_FIX;
1369  break;
1370  }
1371  case evDGPS:
1372  {
1373  msg.status.status = NavSatStatusMsg::STATUS_FIX;
1374  break;
1375  }
1376  case evRTKFixed:
1377  {
1378  msg.status.status = NavSatStatusMsg::STATUS_FIX;
1379  break;
1380  }
1381  case evRTKFloat:
1382  {
1383  msg.status.status = NavSatStatusMsg::STATUS_FIX;
1384  break;
1385  }
1386  case evMovingBaseRTKFixed:
1387  {
1388  msg.status.status = NavSatStatusMsg::STATUS_FIX;
1389  break;
1390  }
1391  case evMovingBaseRTKFloat:
1392  {
1393  msg.status.status = NavSatStatusMsg::STATUS_FIX;
1394  break;
1395  }
1396  case evPPP:
1397  {
1398  msg.status.status = NavSatStatusMsg::STATUS_FIX;
1399  break;
1400  }
1401  case evSBAS:
1402  {
1403  msg.status.status = NavSatStatusMsg::STATUS_SBAS_FIX;
1404  break;
1405  }
1406  default:
1407  {
1408  msg.status.status = NavSatStatusMsg::STATUS_NO_FIX;
1409  node_->log(
1410  log_level::DEBUG,
1411  "PVTGeodetic's Mode field contains an invalid type of PVT solution.");
1412  break;
1413  }
1414  }
1415  }
1416 
1424  void MessageHandler::assembleNavSatFix()
1425  {
1426  if (!settings_->publish_navsatfix)
1427  return;
1428 
1429  thread_local auto last_ins_tow = last_insnavgeod_.block_header.tow;
1430 
1431  NavSatFixMsg msg;
1432  if (settings_->septentrio_receiver_type == "gnss")
1433  {
1434  if ((!validValue(last_pvtgeodetic_.block_header.tow)) ||
1435  (last_pvtgeodetic_.block_header.tow !=
1436  last_poscovgeodetic_.block_header.tow))
1437  return;
1438 
1439  msg.header = last_pvtgeodetic_.header;
1440 
1441  setStatus(last_pvtgeodetic_.mode, msg);
1442 
1443  bool gps_in_pvt = false;
1444  bool glo_in_pvt = false;
1445  bool com_in_pvt = false;
1446  bool gal_in_pvt = false;
1447  uint32_t mask_2 = 1;
1448  for (int bit = 0; bit != 31; ++bit)
1449  {
1450  bool in_use = last_pvtgeodetic_.signal_info & mask_2;
1451  if (bit <= 5 && in_use)
1452  {
1453  gps_in_pvt = true;
1454  }
1455  if (8 <= bit && bit <= 12 && in_use)
1456  glo_in_pvt = true;
1457  if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use)
1458  com_in_pvt = true;
1459  if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use)
1460  gal_in_pvt = true;
1461  mask_2 *= 2;
1462  }
1463  // Below, booleans will be promoted to integers automatically.
1464  uint16_t service =
1465  gps_in_pvt * 1 + glo_in_pvt * 2 + com_in_pvt * 4 + gal_in_pvt * 8;
1466  msg.status.service = service;
1467  msg.latitude = rad2deg(last_pvtgeodetic_.latitude);
1468  msg.longitude = rad2deg(last_pvtgeodetic_.longitude);
1469  msg.altitude = last_pvtgeodetic_.height;
1470  msg.position_covariance[0] = last_poscovgeodetic_.cov_lonlon;
1471  msg.position_covariance[1] = last_poscovgeodetic_.cov_latlon;
1472  msg.position_covariance[2] = last_poscovgeodetic_.cov_lonhgt;
1473  msg.position_covariance[3] = last_poscovgeodetic_.cov_latlon;
1474  msg.position_covariance[4] = last_poscovgeodetic_.cov_latlat;
1475  msg.position_covariance[5] = last_poscovgeodetic_.cov_lathgt;
1476  msg.position_covariance[6] = last_poscovgeodetic_.cov_lonhgt;
1477  msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt;
1478  msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt;
1479  msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN;
1480  } else if (settings_->septentrio_receiver_type == "ins")
1481  {
1482  if ((!validValue(last_insnavgeod_.block_header.tow)) ||
1483  (last_insnavgeod_.block_header.tow == last_ins_tow))
1484  {
1485  return;
1486  }
1487  last_ins_tow = last_insnavgeod_.block_header.tow;
1488 
1489  msg.header = last_insnavgeod_.header;
1490 
1491  setStatus(last_insnavgeod_.gnss_mode, msg);
1492 
1493  bool gps_in_pvt = false;
1494  bool glo_in_pvt = false;
1495  bool com_in_pvt = false;
1496  bool gal_in_pvt = false;
1497  uint32_t mask_2 = 1;
1498  for (int bit = 0; bit != 31; ++bit)
1499  {
1500  bool in_use = last_pvtgeodetic_.signal_info & mask_2;
1501  if (bit <= 5 && in_use)
1502  {
1503  gps_in_pvt = true;
1504  }
1505  if (8 <= bit && bit <= 12 && in_use)
1506  glo_in_pvt = true;
1507  if (((13 <= bit && bit <= 14) || (28 <= bit && bit <= 30)) && in_use)
1508  com_in_pvt = true;
1509  if ((bit == 17 || (19 <= bit && bit <= 22)) && in_use)
1510  gal_in_pvt = true;
1511  mask_2 *= 2;
1512  }
1513  // Below, booleans will be promoted to integers automatically.
1514  uint16_t service =
1515  gps_in_pvt * 1 + glo_in_pvt * 2 + com_in_pvt * 4 + gal_in_pvt * 8;
1516  msg.status.service = service;
1517  msg.latitude = rad2deg(last_insnavgeod_.latitude);
1518  msg.longitude = rad2deg(last_insnavgeod_.longitude);
1519  msg.altitude = last_insnavgeod_.height;
1520 
1521  if ((last_insnavgeod_.sb_list & 1) != 0)
1522  {
1523  msg.position_covariance[0] =
1524  square(last_insnavgeod_.longitude_std_dev);
1525  msg.position_covariance[4] =
1526  square(last_insnavgeod_.latitude_std_dev);
1527  msg.position_covariance[8] = square(last_insnavgeod_.height_std_dev);
1528  }
1529  if ((last_insnavgeod_.sb_list & 32) != 0)
1530  {
1531  msg.position_covariance[1] = last_insnavgeod_.latitude_longitude_cov;
1532  msg.position_covariance[2] = last_insnavgeod_.longitude_height_cov;
1533  msg.position_covariance[3] = last_insnavgeod_.latitude_longitude_cov;
1534  msg.position_covariance[5] = last_insnavgeod_.latitude_height_cov;
1535  msg.position_covariance[6] = last_insnavgeod_.longitude_height_cov;
1536  msg.position_covariance[7] = last_insnavgeod_.latitude_height_cov;
1537  }
1538  msg.position_covariance_type =
1539  NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN;
1540  }
1541  publish<NavSatFixMsg>("navsatfix", msg);
1542  };
1543 
1544  void MessageHandler::setStatus(uint8_t mode, GpsFixMsg& msg)
1545  {
1546  switch (mode & 15)
1547  {
1548  case evNoPVT:
1549  {
1550  msg.status.status = GpsStatusMsg::STATUS_NO_FIX;
1551  break;
1552  }
1553  case evStandAlone:
1554  {
1555  msg.status.status = GpsStatusMsg::STATUS_FIX;
1556  break;
1557  }
1558  case evFixed:
1559  {
1560  msg.status.status = GpsStatusMsg::STATUS_FIX;
1561  break;
1562  }
1563  case evDGPS:
1564  {
1565  msg.status.status = GpsStatusMsg::STATUS_DGPS_FIX;
1566  break;
1567  }
1568  case evRTKFixed:
1569  {
1570  msg.status.status = GpsStatusMsg::STATUS_DGPS_FIX;
1571  break;
1572  }
1573  case evRTKFloat:
1574  {
1575  msg.status.status = GpsStatusMsg::STATUS_DGPS_FIX;
1576  break;
1577  }
1578  case evMovingBaseRTKFixed:
1579  {
1580  msg.status.status = GpsStatusMsg::STATUS_DGPS_FIX;
1581  break;
1582  }
1583  case evMovingBaseRTKFloat:
1584  {
1585  msg.status.status = GpsStatusMsg::STATUS_DGPS_FIX;
1586  break;
1587  }
1588  case evPPP:
1589  {
1590  msg.status.status = GpsStatusMsg::STATUS_DGPS_FIX;
1591  break;
1592  }
1593  case evSBAS:
1594  {
1595  uint16_t reference_id = last_pvtgeodetic_.reference_id;
1596  // Here come the PRNs of the 4 WAAS satellites..
1597  if (reference_id == 131 || reference_id == 133 || reference_id == 135 ||
1598  reference_id == 135)
1599  {
1600  msg.status.status = GpsStatusMsg::STATUS_WAAS_FIX;
1601  } else
1602  {
1603  msg.status.status = GpsStatusMsg::STATUS_SBAS_FIX;
1604  }
1605  break;
1606  }
1607  default:
1608  {
1609  msg.status.status = GpsStatusMsg::STATUS_NO_FIX;
1610  node_->log(
1611  log_level::DEBUG,
1612  "PVTGeodetic's Mode field contains an invalid type of PVT solution.");
1613  break;
1614  }
1615  }
1616  }
1617 
1639  void MessageHandler::assembleGpsFix()
1640  {
1641  if (!settings_->publish_gpsfix)
1642  return;
1643 
1644  if (settings_->septentrio_receiver_type == "gnss")
1645  {
1646  if (!validValue(last_measepoch_.block_header.tow) ||
1647  !validValue(last_channelstatus_.block_header.tow) ||
1648  !validValue(last_dop_.block_header.tow) ||
1649  (last_measepoch_.block_header.tow !=
1650  last_pvtgeodetic_.block_header.tow) ||
1651  (last_measepoch_.block_header.tow !=
1652  last_poscovgeodetic_.block_header.tow) ||
1653  (last_measepoch_.block_header.tow !=
1654  last_atteuler_.block_header.tow) ||
1655  (last_measepoch_.block_header.tow !=
1656  last_attcoveuler_.block_header.tow) ||
1657  (last_measepoch_.block_header.tow != last_dop_.block_header.tow) ||
1658  (last_measepoch_.block_header.tow !=
1659  last_channelstatus_.block_header.tow))
1660  return;
1661  } else if (settings_->septentrio_receiver_type == "ins")
1662  {
1663  if (!validValue(last_measepoch_.block_header.tow) ||
1664  !validValue(last_channelstatus_.block_header.tow) ||
1665  !validValue(last_dop_.block_header.tow))
1666  return;
1667  }
1668 
1669  GpsFixMsg msg;
1670  msg.status.satellites_used = static_cast<uint16_t>(last_pvtgeodetic_.nr_sv);
1671 
1672  // MeasEpoch Processing
1673  std::vector<int32_t> cno_tracked;
1674  std::vector<int32_t> svid_in_sync;
1675  {
1676  cno_tracked.reserve(last_measepoch_.type1.size());
1677  svid_in_sync.reserve(last_measepoch_.type1.size());
1678  for (const auto& measepoch_channel_type1 : last_measepoch_.type1)
1679  {
1680  // Define MeasEpochChannelType1 struct for the corresponding
1681  // sub-block
1682  svid_in_sync.push_back(
1683  static_cast<int32_t>(measepoch_channel_type1.sv_id));
1684  uint8_t type_mask =
1685  15; // We extract the first four bits using this mask.
1686  if (((measepoch_channel_type1.type & type_mask) ==
1687  static_cast<uint8_t>(1)) ||
1688  ((measepoch_channel_type1.type & type_mask) ==
1689  static_cast<uint8_t>(2)))
1690  {
1691  cno_tracked.push_back(
1692  static_cast<int32_t>(measepoch_channel_type1.cn0) / 4);
1693  } else
1694  {
1695  cno_tracked.push_back(
1696  static_cast<int32_t>(measepoch_channel_type1.cn0) / 4 +
1697  static_cast<int32_t>(10));
1698  }
1699  }
1700  }
1701 
1702  // ChannelStatus Processing
1703  std::vector<int32_t> svid_in_sync_2;
1704  std::vector<int32_t> elevation_tracked;
1705  std::vector<int32_t> azimuth_tracked;
1706  std::vector<int32_t> svid_pvt;
1707  svid_pvt.clear();
1708  std::vector<int32_t> ordering;
1709  {
1710  svid_in_sync_2.reserve(last_channelstatus_.satInfo.size());
1711  elevation_tracked.reserve(last_channelstatus_.satInfo.size());
1712  azimuth_tracked.reserve(last_channelstatus_.satInfo.size());
1713  for (const auto& channel_sat_info : last_channelstatus_.satInfo)
1714  {
1715  bool to_be_added = false;
1716  for (int32_t j = 0; j < static_cast<int32_t>(svid_in_sync.size());
1717  ++j)
1718  {
1719  if (svid_in_sync[j] ==
1720  static_cast<int32_t>(channel_sat_info.sv_id))
1721  {
1722  ordering.push_back(j);
1723  to_be_added = true;
1724  break;
1725  }
1726  }
1727  if (to_be_added)
1728  {
1729  svid_in_sync_2.push_back(
1730  static_cast<int32_t>(channel_sat_info.sv_id));
1731  elevation_tracked.push_back(
1732  static_cast<int32_t>(channel_sat_info.elev));
1733  constexpr uint16_t azimuth_mask = 511;
1734  azimuth_tracked.push_back(static_cast<int32_t>(
1735  (channel_sat_info.az_rise_set & azimuth_mask)));
1736  }
1737  svid_pvt.reserve(channel_sat_info.stateInfo.size());
1738  for (const auto& channel_state_info : channel_sat_info.stateInfo)
1739  {
1740  // Define ChannelStateInfo struct for the corresponding sub-block
1741  bool pvt_status = false;
1742  uint16_t pvt_status_mask = std::pow(2, 15) + std::pow(2, 14);
1743  for (int k = 15; k != -1; k -= 2)
1744  {
1745  uint16_t pvt_status_value =
1746  (channel_state_info.pvt_status & pvt_status_mask) >>
1747  k - 1;
1748  if (pvt_status_value == 2)
1749  {
1750  pvt_status = true;
1751  }
1752  if (k > 1)
1753  {
1754  pvt_status_mask = pvt_status_mask - std::pow(2, k) -
1755  std::pow(2, k - 1) +
1756  std::pow(2, k - 2) +
1757  std::pow(2, k - 3);
1758  }
1759  }
1760  if (pvt_status)
1761  {
1762  svid_pvt.push_back(
1763  static_cast<int32_t>(channel_sat_info.sv_id));
1764  }
1765  }
1766  }
1767  }
1768  msg.status.satellite_used_prn =
1769  svid_pvt; // Entries such as int32[] in ROS messages are to be treated as
1770  // std::vectors.
1771  msg.status.satellites_visible = static_cast<uint16_t>(svid_in_sync.size());
1772  msg.status.satellite_visible_prn = svid_in_sync_2;
1773  msg.status.satellite_visible_z = elevation_tracked;
1774  msg.status.satellite_visible_azimuth = azimuth_tracked;
1775 
1776  // Reordering CNO vector to that of all previous arrays
1777  std::vector<int32_t> cno_tracked_reordered;
1778  if (static_cast<int32_t>(last_channelstatus_.n) != 0)
1779  {
1780  for (int32_t k = 0; k < static_cast<int32_t>(ordering.size()); ++k)
1781  {
1782  cno_tracked_reordered.push_back(cno_tracked[ordering[k]]);
1783  }
1784  }
1785  msg.status.satellite_visible_snr = cno_tracked_reordered;
1786  msg.err_time = 2 * std::sqrt(last_poscovgeodetic_.cov_bb);
1787 
1788  if (settings_->septentrio_receiver_type == "gnss")
1789  {
1790  msg.header = last_pvtgeodetic_.header;
1791 
1792  setStatus(last_pvtgeodetic_.mode, msg);
1793 
1794  // Doppler is not used when calculating the velocities of, say,
1795  // mosaic-x5, hence:
1796  msg.status.motion_source = GpsStatusMsg::SOURCE_POINTS;
1797  // Doppler is not used when calculating the orientation of, say,
1798  // mosaic-x5, hence:
1799  msg.status.orientation_source = GpsStatusMsg::SOURCE_POINTS;
1800  msg.status.position_source = GpsStatusMsg::SOURCE_GPS;
1801  msg.latitude = rad2deg(last_pvtgeodetic_.latitude);
1802  msg.longitude = rad2deg(last_pvtgeodetic_.longitude);
1803  msg.altitude = last_pvtgeodetic_.height;
1804  // Note that cog is of type float32 while track is of type float64.
1805  msg.track = last_pvtgeodetic_.cog;
1806  msg.speed = std::sqrt(square(last_pvtgeodetic_.vn) +
1807  square(last_pvtgeodetic_.ve));
1808  msg.climb = last_pvtgeodetic_.vu;
1809 
1810  msg.roll = last_atteuler_.roll;
1811  msg.pitch = last_atteuler_.pitch;
1812  msg.dip = last_atteuler_.heading;
1813 
1814  if (last_dop_.pdop == 0.0 || last_dop_.tdop == 0.0)
1815  {
1816  msg.gdop = -1.0;
1817  } else
1818  {
1819  msg.gdop =
1820  std::sqrt(square(last_dop_.pdop) + square(last_dop_.tdop));
1821  }
1822  if (last_dop_.pdop == 0.0)
1823  {
1824  msg.pdop = -1.0;
1825  } else
1826  {
1827  msg.pdop = last_dop_.pdop;
1828  }
1829  if (last_dop_.hdop == 0.0)
1830  {
1831  msg.hdop = -1.0;
1832  } else
1833  {
1834  msg.hdop = last_dop_.hdop;
1835  }
1836  if (last_dop_.vdop == 0.0)
1837  {
1838  msg.vdop = -1.0;
1839  } else
1840  {
1841  msg.vdop = last_dop_.vdop;
1842  }
1843  if (last_dop_.tdop == 0.0)
1844  {
1845  msg.tdop = -1.0;
1846  } else
1847  {
1848  msg.tdop = last_dop_.tdop;
1849  }
1850  msg.time =
1851  static_cast<double>(last_pvtgeodetic_.block_header.tow) / 1000 +
1852  static_cast<double>(last_pvtgeodetic_.block_header.wnc * 604800);
1853  // position
1854  msg.err =
1855  2 *
1856  (std::sqrt(static_cast<double>(last_poscovgeodetic_.cov_latlat) +
1857  static_cast<double>(last_poscovgeodetic_.cov_lonlon) +
1858  static_cast<double>(last_poscovgeodetic_.cov_hgthgt)));
1859  msg.err_horz =
1860  2 *
1861  (std::sqrt(static_cast<double>(last_poscovgeodetic_.cov_latlat) +
1862  static_cast<double>(last_poscovgeodetic_.cov_lonlon)));
1863  msg.err_vert =
1864  2 * std::sqrt(static_cast<double>(last_poscovgeodetic_.cov_hgthgt));
1865  // motion
1866  msg.err_track =
1867  2 * (std::sqrt(square(1.0 / (last_pvtgeodetic_.vn +
1868  square(last_pvtgeodetic_.ve) /
1869  last_pvtgeodetic_.vn)) *
1870  last_poscovgeodetic_.cov_lonlon +
1871  square((last_pvtgeodetic_.ve) /
1872  (square(last_pvtgeodetic_.vn) +
1873  square(last_pvtgeodetic_.ve))) *
1874  last_poscovgeodetic_.cov_latlat));
1875  msg.err_speed =
1876  2 * (std::sqrt(static_cast<double>(last_velcovgeodetic_.cov_vnvn) +
1877  static_cast<double>(last_velcovgeodetic_.cov_veve)));
1878  msg.err_climb =
1879  2 * std::sqrt(static_cast<double>(last_velcovgeodetic_.cov_vuvu));
1880  // attitude
1881  msg.err_roll =
1882  2 * std::sqrt(static_cast<double>(last_attcoveuler_.cov_rollroll));
1883  msg.err_pitch =
1884  2 * std::sqrt(static_cast<double>(last_attcoveuler_.cov_pitchpitch));
1885  msg.err_dip =
1886  2 * std::sqrt(static_cast<double>(last_attcoveuler_.cov_headhead));
1887 
1888  msg.position_covariance[0] = last_poscovgeodetic_.cov_lonlon;
1889  msg.position_covariance[1] = last_poscovgeodetic_.cov_latlon;
1890  msg.position_covariance[2] = last_poscovgeodetic_.cov_lonhgt;
1891  msg.position_covariance[3] = last_poscovgeodetic_.cov_latlon;
1892  msg.position_covariance[4] = last_poscovgeodetic_.cov_latlat;
1893  msg.position_covariance[5] = last_poscovgeodetic_.cov_lathgt;
1894  msg.position_covariance[6] = last_poscovgeodetic_.cov_lonhgt;
1895  msg.position_covariance[7] = last_poscovgeodetic_.cov_lathgt;
1896  msg.position_covariance[8] = last_poscovgeodetic_.cov_hgthgt;
1897  msg.position_covariance_type = NavSatFixMsg::COVARIANCE_TYPE_KNOWN;
1898  } else if (settings_->septentrio_receiver_type == "ins")
1899  {
1900  msg.header = last_insnavgeod_.header;
1901 
1902  setStatus(last_insnavgeod_.gnss_mode, msg);
1903 
1904  // Doppler is not used when calculating the velocities of, say,
1905  // mosaic-x5, hence:
1906  msg.status.motion_source = GpsStatusMsg::SOURCE_POINTS;
1907  // Doppler is not used when calculating the orientation of, say,
1908  // mosaic-x5, hence:
1909  msg.status.orientation_source = GpsStatusMsg::SOURCE_POINTS;
1910  msg.status.position_source = GpsStatusMsg::SOURCE_GPS;
1911  msg.latitude = rad2deg(last_insnavgeod_.latitude);
1912  msg.longitude = rad2deg(last_insnavgeod_.longitude);
1913  msg.altitude = last_insnavgeod_.height;
1914  // Note that cog is of type float32 while track is of type float64.
1915  if ((last_insnavgeod_.sb_list & 2) != 0)
1916  {
1917  msg.pitch = last_insnavgeod_.pitch;
1918  msg.roll = last_insnavgeod_.roll;
1919  msg.dip = last_insnavgeod_.heading;
1920  }
1921  if ((last_insnavgeod_.sb_list & 8) != 0)
1922  {
1923  msg.speed = std::sqrt(square(last_insnavgeod_.vn) +
1924  square(last_insnavgeod_.ve));
1925 
1926  msg.climb = last_insnavgeod_.vu;
1927  msg.track = std::atan2(last_insnavgeod_.vn, last_insnavgeod_.ve);
1928  }
1929  if (last_dop_.pdop == 0.0 || last_dop_.tdop == 0.0)
1930  {
1931  msg.gdop = -1.0;
1932  } else
1933  {
1934  msg.gdop =
1935  std::sqrt(square(last_dop_.pdop) + square(last_dop_.tdop));
1936  }
1937  if (last_dop_.pdop == 0.0)
1938  {
1939  msg.pdop = -1.0;
1940  } else
1941  {
1942  msg.pdop = last_dop_.pdop;
1943  }
1944  if (last_dop_.hdop == 0.0)
1945  {
1946  msg.hdop = -1.0;
1947  } else
1948  {
1949  msg.hdop = last_dop_.hdop;
1950  }
1951  if (last_dop_.vdop == 0.0)
1952  {
1953  msg.vdop = -1.0;
1954  } else
1955  {
1956  msg.vdop = last_dop_.vdop;
1957  }
1958  if (last_dop_.tdop == 0.0)
1959  {
1960  msg.tdop = -1.0;
1961  } else
1962  {
1963  msg.tdop = last_dop_.tdop;
1964  }
1965  msg.time =
1966  static_cast<double>(last_insnavgeod_.block_header.tow) / 1000 +
1967  static_cast<double>(last_insnavgeod_.block_header.wnc * 604800);
1968  if ((last_insnavgeod_.sb_list & 1) != 0)
1969  {
1970  msg.err = 2 * (std::sqrt(square(last_insnavgeod_.latitude_std_dev) +
1971  square(last_insnavgeod_.longitude_std_dev) +
1972  square(last_insnavgeod_.height_std_dev)));
1973  msg.err_horz =
1974  2 * (std::sqrt(square(last_insnavgeod_.latitude_std_dev) +
1975  square(last_insnavgeod_.longitude_std_dev)));
1976  msg.err_vert = 2 * last_insnavgeod_.height_std_dev;
1977  }
1978  if (((last_insnavgeod_.sb_list & 8) != 0) ||
1979  ((last_insnavgeod_.sb_list & 1) != 0))
1980  {
1981  msg.err_track =
1982  2 * (std::sqrt(square(1.0 / (last_insnavgeod_.vn +
1983  square(last_insnavgeod_.ve) /
1984  last_insnavgeod_.vn)) *
1985  square(last_insnavgeod_.longitude_std_dev) +
1986  square((last_insnavgeod_.ve) /
1987  (square(last_insnavgeod_.vn) +
1988  square(last_insnavgeod_.ve))) *
1989  square(last_insnavgeod_.latitude_std_dev)));
1990  }
1991  if ((last_insnavgeod_.sb_list & 8) != 0)
1992  {
1993  msg.err_speed = 2 * (std::sqrt(square(last_insnavgeod_.ve_std_dev) +
1994  square(last_insnavgeod_.vn_std_dev)));
1995  msg.err_climb = 2 * last_insnavgeod_.vu_std_dev;
1996  }
1997  if ((last_insnavgeod_.sb_list & 2) != 0)
1998  {
1999  msg.err_pitch = 2 * last_insnavgeod_.pitch_std_dev;
2000  msg.err_roll = 2 * last_insnavgeod_.roll_std_dev;
2001  msg.err_dip = 2 * last_insnavgeod_.heading_std_dev;
2002  }
2003  if ((last_insnavgeod_.sb_list & 1) != 0)
2004  {
2005  msg.position_covariance[0] =
2006  square(last_insnavgeod_.longitude_std_dev);
2007  msg.position_covariance[4] =
2008  square(last_insnavgeod_.latitude_std_dev);
2009  msg.position_covariance[8] = square(last_insnavgeod_.height_std_dev);
2010  }
2011  if ((last_insnavgeod_.sb_list & 32) != 0)
2012  {
2013  msg.position_covariance[1] = last_insnavgeod_.latitude_longitude_cov;
2014  msg.position_covariance[2] = last_insnavgeod_.longitude_height_cov;
2015  msg.position_covariance[3] = last_insnavgeod_.latitude_longitude_cov;
2016  msg.position_covariance[5] = last_insnavgeod_.latitude_height_cov;
2017  msg.position_covariance[6] = last_insnavgeod_.longitude_height_cov;
2018  msg.position_covariance[7] = last_insnavgeod_.latitude_height_cov;
2019  }
2020  msg.position_covariance_type =
2021  NavSatFixMsg::COVARIANCE_TYPE_DIAGONAL_KNOWN;
2022  }
2023  publish<GpsFixMsg>("gpsfix", msg);
2024  }
2025 
2026  void
2027  MessageHandler::assembleTimeReference(const std::shared_ptr<Telegram>& telegram)
2028  {
2029  TimeReferenceMsg msg;
2030  Timestamp time_obj = timestampSBF(telegram->message);
2031  msg.time_ref = timestampToRos(time_obj);
2032  msg.source = "GPST";
2033  assembleHeader(settings_->frame_id, telegram, msg);
2034  publish<TimeReferenceMsg>("gpst", msg);
2035  }
2036 
2037  template <typename T>
2038  void MessageHandler::assembleHeader(const std::string& frameId,
2039  const std::shared_ptr<Telegram>& telegram,
2040  T& msg) const
2041  {
2042  Timestamp time_obj = settings_->use_gnss_time
2043  ? timestampSBF(telegram->message)
2044  : telegram->stamp;
2045  msg.header.frame_id = frameId;
2046 
2047  if (!settings_->use_gnss_time && settings_->latency_compensation)
2048  {
2049  if constexpr (std::is_same<INSNavCartMsg, T>::value ||
2050  std::is_same<INSNavGeodMsg, T>::value)
2051  {
2052  time_obj -= msg.latency * 100000ul; // from 0.0001 s to ns
2053  } else if constexpr (std::is_same<PVTCartesianMsg, T>::value ||
2054  std::is_same<PVTGeodeticMsg, T>::value)
2055  {
2056  last_pvt_latency_ = msg.latency * 100000ul; // from 0.0001 s to ns
2057  time_obj -= last_pvt_latency_;
2058  } else if constexpr (std::is_same<PosCovCartesianMsg, T>::value ||
2059  std::is_same<PosCovGeodeticMsg, T>::value ||
2060  std::is_same<VelCovCartesianMsg, T>::value ||
2061  std::is_same<VelCovGeodeticMsg, T>::value ||
2062  std::is_same<AttEulerMsg, T>::value ||
2063  std::is_same<AttCovEulerMsg, T>::value ||
2064  std::is_same<BaseVectorCartMsg, T>::value ||
2065  std::is_same<BaseVectorGeodMsg, T>::value)
2066  {
2067  time_obj -= last_pvt_latency_;
2068  }
2069  }
2070 
2071  msg.header.stamp = timestampToRos(time_obj);
2072  }
2073 
2074  Timestamp MessageHandler::timestampSBF(const std::vector<uint8_t>& message) const
2075  {
2076  uint32_t tow = parsing_utilities::getTow(message);
2077  uint16_t wnc = parsing_utilities::getWnc(message);
2078 
2079  if (!validValue(tow) || !validValue(wnc))
2080  return 0;
2081 
2082  return timestampSBF(tow, wnc);
2083  }
2084 
2090  Timestamp MessageHandler::timestampSBF(uint32_t tow, uint16_t wnc) const
2091  {
2092  Timestamp time_obj;
2093 
2094  // conversion from GPS time of week and week number to UTC taking leap
2095  // seconds into account
2096  constexpr uint64_t secToNSec = 1000000000;
2097  constexpr uint64_t mSec2NSec = 1000000;
2098  constexpr uint64_t nsOfGpsStart =
2099  315964800 *
2100  secToNSec; // GPS week counter starts at 1980-01-06 which is
2101  // 315964800 seconds since Unix epoch (1970-01-01 UTC)
2102  constexpr uint64_t nsecPerWeek = 7 * 24 * 60 * 60 * secToNSec;
2103 
2104  time_obj = nsOfGpsStart + tow * mSec2NSec + wnc * nsecPerWeek;
2105 
2106  if (current_leap_seconds_ != -128)
2107  time_obj -= current_leap_seconds_ * secToNSec;
2108  // else: warn?
2109 
2110  return time_obj;
2111  }
2112 
2116  template <typename M>
2117  void MessageHandler::publish(const std::string& topic, const M& msg)
2118  {
2119  // TODO: maybe publish only if wnc and tow is valid?
2120  if (!settings_->use_gnss_time ||
2121  (settings_->use_gnss_time && (current_leap_seconds_ != -128)))
2122  {
2123  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2124  {
2125  wait(timestampFromRos(msg.header.stamp));
2126  }
2127  node_->publishMessage<M>(topic, msg);
2128  } else
2129  {
2130  node_->log(
2131  log_level::DEBUG,
2132  "Not publishing message with GNSS time because no leap seconds are available yet.");
2133  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2134  {
2135  node_->log(
2136  log_level::WARN,
2137  "No leap seconds were set and none were received from log yet.");
2138  setLeapSeconds();
2139  }
2140  }
2141  }
2142 
2146  void MessageHandler::publishTf(const LocalizationMsg& msg)
2147  {
2148  // TODO: maybe publish only if wnc and tow is valid?
2149  if (!settings_->use_gnss_time ||
2150  (settings_->use_gnss_time && (current_leap_seconds_ != -128)))
2151  {
2152  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2153  {
2154  wait(timestampFromRos(msg.header.stamp));
2155  }
2156  node_->publishTf(msg);
2157  } else
2158  {
2159  node_->log(
2160  log_level::DEBUG,
2161  "Not publishing tf with GNSS time because no leap seconds are available yet.");
2162  if (settings_->read_from_sbf_log || settings_->read_from_pcap)
2163  {
2164  node_->log(
2165  log_level::WARN,
2166  "No leap seconds were set and none were received from log yet. ");
2167  setLeapSeconds();
2168  };
2169  }
2170  }
2171 
2172  void MessageHandler::parseSbf(const std::shared_ptr<Telegram>& telegram)
2173  {
2174 
2175  uint16_t sbfId = parsing_utilities::getId(telegram->message);
2176 
2177  /*node_->log(log_level::DEBUG, "ROSaic reading SBF block " +
2178  std::to_string(sbfId) + " made up of " +
2179  std::to_string(telegram->message.size()) +
2180  " bytes...");*/
2181 
2182  switch (sbfId)
2183  {
2184  case PVT_CARTESIAN: // Position and velocity in XYZ
2185  {
2186  if (settings_->publish_pvtcartesian)
2187  {
2188  PVTCartesianMsg msg;
2189 
2190  if (!PVTCartesianParser(node_, telegram->message.begin(),
2191  telegram->message.end(), msg))
2192  {
2193  node_->log(log_level::ERROR, "parse error in PVTCartesian");
2194  break;
2195  }
2196  assembleHeader(settings_->frame_id, telegram, msg);
2197  publish<PVTCartesianMsg>("pvtcartesian", msg);
2198  }
2199  break;
2200  }
2201  case PVT_GEODETIC: // Position and velocity in geodetic coordinate frame
2202  // (ENU frame)
2203  {
2204  if (!PVTGeodeticParser(node_, telegram->message.begin(),
2205  telegram->message.end(), last_pvtgeodetic_))
2206  {
2207  node_->log(log_level::ERROR, "parse error in PVTGeodetic");
2208  break;
2209  }
2210  assembleHeader(settings_->frame_id, telegram, last_pvtgeodetic_);
2211  if (settings_->publish_pvtgeodetic)
2212  publish<PVTGeodeticMsg>("pvtgeodetic", last_pvtgeodetic_);
2213  assembleTwist();
2214  assembleNavSatFix();
2215  assemblePoseWithCovarianceStamped();
2216  if (settings_->septentrio_receiver_type == "gnss")
2217  assembleGpsFix();
2218  if (settings_->publish_gpst &&
2219  (settings_->septentrio_receiver_type == "gnss"))
2220  assembleTimeReference(telegram);
2221  break;
2222  }
2223  case BASE_VECTOR_CART:
2224  {
2225  if (settings_->publish_basevectorcart)
2226  {
2227  BaseVectorCartMsg msg;
2228 
2229  if (!BaseVectorCartParser(node_, telegram->message.begin(),
2230  telegram->message.end(), msg))
2231  {
2232  node_->log(log_level::ERROR, "parse error in BaseVectorCart");
2233  break;
2234  }
2235  assembleHeader(settings_->frame_id, telegram, msg);
2236  publish<BaseVectorCartMsg>("basevectorcart", msg);
2237  }
2238  break;
2239  }
2240  case BASE_VECTOR_GEOD:
2241  {
2242  if (settings_->publish_basevectorgeod)
2243  {
2244  BaseVectorGeodMsg msg;
2245 
2246  if (!BaseVectorGeodParser(node_, telegram->message.begin(),
2247  telegram->message.end(), msg))
2248  {
2249  node_->log(log_level::ERROR, "parse error in BaseVectorGeod");
2250  break;
2251  }
2252  assembleHeader(settings_->frame_id, telegram, msg);
2253  publish<BaseVectorGeodMsg>("basevectorgeod", msg);
2254  }
2255  break;
2256  }
2257  case POS_COV_CARTESIAN:
2258  {
2259  if (settings_->publish_poscovcartesian)
2260  {
2261  PosCovCartesianMsg msg;
2262 
2263  if (!PosCovCartesianParser(node_, telegram->message.begin(),
2264  telegram->message.end(), msg))
2265  {
2266  node_->log(log_level::ERROR, "parse error in PosCovCartesian");
2267  break;
2268  }
2269  assembleHeader(settings_->frame_id, telegram, msg);
2270  publish<PosCovCartesianMsg>("poscovcartesian", msg);
2271  }
2272  break;
2273  }
2274  case POS_COV_GEODETIC:
2275  {
2276  if (!PosCovGeodeticParser(node_, telegram->message.begin(),
2277  telegram->message.end(), last_poscovgeodetic_))
2278  {
2279  node_->log(log_level::ERROR, "parse error in PosCovGeodetic");
2280  break;
2281  }
2282  assembleHeader(settings_->frame_id, telegram, last_poscovgeodetic_);
2283  if (settings_->publish_poscovgeodetic)
2284  publish<PosCovGeodeticMsg>("poscovgeodetic", last_poscovgeodetic_);
2285  assembleNavSatFix();
2286  assemblePoseWithCovarianceStamped();
2287  if (settings_->septentrio_receiver_type == "gnss")
2288  assembleGpsFix();
2289  break;
2290  }
2291  case ATT_EULER:
2292  {
2293  if (!AttEulerParser(node_, telegram->message.begin(),
2294  telegram->message.end(), last_atteuler_,
2295  settings_->use_ros_axis_orientation))
2296  {
2297  node_->log(log_level::ERROR, "parse error in AttEuler");
2298  break;
2299  }
2300  assembleHeader(settings_->frame_id, telegram, last_atteuler_);
2301  if (settings_->publish_atteuler)
2302  publish<AttEulerMsg>("atteuler", last_atteuler_);
2303  assemblePoseWithCovarianceStamped();
2304  if (settings_->septentrio_receiver_type == "gnss")
2305  assembleGpsFix();
2306  break;
2307  }
2308  case ATT_COV_EULER:
2309  {
2310  if (!AttCovEulerParser(node_, telegram->message.begin(),
2311  telegram->message.end(), last_attcoveuler_,
2312  settings_->use_ros_axis_orientation))
2313  {
2314  node_->log(log_level::ERROR, "parse error in AttCovEuler");
2315  break;
2316  }
2317  assembleHeader(settings_->frame_id, telegram, last_attcoveuler_);
2318  if (settings_->publish_attcoveuler)
2319  publish<AttCovEulerMsg>("attcoveuler", last_attcoveuler_);
2320  assemblePoseWithCovarianceStamped();
2321  if (settings_->septentrio_receiver_type == "gnss")
2322  assembleGpsFix();
2323  break;
2324  }
2325  case GAL_AUTH_STATUS:
2326  {
2327  if (!GalAuthStatusParser(node_, telegram->message.begin(),
2328  telegram->message.end(), last_gal_auth_status_))
2329  {
2330  node_->log(log_level::ERROR, "parse error in GalAuthStatus");
2331  break;
2332  }
2333  osnma_info_available_ = true;
2334  assembleHeader(settings_->frame_id, telegram, last_gal_auth_status_);
2335  if (settings_->publish_galauthstatus)
2336  {
2337  publish<GalAuthStatusMsg>("galauthstatus", last_gal_auth_status_);
2338  assembleOsnmaDiagnosticArray();
2339  }
2340  break;
2341  }
2342  case RF_STATUS:
2343  {
2344  if (!RfStatusParser(node_, telegram->message.begin(),
2345  telegram->message.end(), last_rf_status_))
2346  {
2347  node_->log(log_level::ERROR, "parse error inRfStatus");
2348  break;
2349  }
2350  assembleHeader(settings_->frame_id, telegram, last_rf_status_);
2351  if (settings_->publish_aimplusstatus)
2352  {
2353  publish<RfStatusMsg>("rfstatus", last_rf_status_);
2354  assembleAimAndDiagnosticArray();
2355  }
2356  break;
2357  }
2358  case INS_NAV_CART: // Position, velocity and orientation in cartesian
2359  // coordinate frame (ENU frame)
2360  {
2361  if (!INSNavCartParser(node_, telegram->message.begin(),
2362  telegram->message.end(), last_insnavcart_,
2363  settings_->use_ros_axis_orientation))
2364  {
2365  node_->log(log_level::ERROR, "parse error in INSNavCart");
2366  break;
2367  }
2368  std::string frame_id;
2369  if (settings_->ins_use_poi)
2370  {
2371  frame_id = settings_->poi_frame_id;
2372  } else
2373  {
2374  frame_id = settings_->frame_id;
2375  }
2376  assembleHeader(frame_id, telegram, last_insnavcart_);
2377  if (settings_->publish_insnavcart)
2378  publish<INSNavCartMsg>("insnavcart", last_insnavcart_);
2379  assembleLocalizationEcef();
2380  break;
2381  }
2382  case INS_NAV_GEOD: // Position, velocity and orientation in geodetic
2383  // coordinate frame (ENU frame)
2384  {
2385  if (!INSNavGeodParser(node_, telegram->message.begin(),
2386  telegram->message.end(), last_insnavgeod_,
2387  settings_->use_ros_axis_orientation))
2388  {
2389  node_->log(log_level::ERROR, "parse error in INSNavGeod");
2390  break;
2391  }
2392  std::string frame_id;
2393  if (settings_->ins_use_poi)
2394  {
2395  frame_id = settings_->poi_frame_id;
2396  } else
2397  {
2398  frame_id = settings_->frame_id;
2399  }
2400 
2401  if (settings_->publish_imu && hasImuMeas_)
2402  {
2403  assembleImu();
2404  }
2405  hasImuMeas_ = false;
2406  assembleHeader(frame_id, telegram, last_insnavgeod_);
2407  if (settings_->publish_insnavgeod)
2408  publish<INSNavGeodMsg>("insnavgeod", last_insnavgeod_);
2409  assembleLocalizationUtm();
2410  assembleLocalizationEcef();
2411  assembleTwist(true);
2412  assemblePoseWithCovarianceStamped();
2413  assembleNavSatFix();
2414  assembleGpsFix();
2415  if (settings_->publish_gpst)
2416  assembleTimeReference(telegram);
2417  break;
2418  }
2419  case IMU_SETUP: // IMU orientation and lever arm
2420  {
2421  if (settings_->publish_imusetup)
2422  {
2423  IMUSetupMsg msg;
2424 
2425  if (!IMUSetupParser(node_, telegram->message.begin(),
2426  telegram->message.end(), msg,
2427  settings_->use_ros_axis_orientation))
2428  {
2429  node_->log(log_level::ERROR, "parse error in IMUSetup");
2430  break;
2431  }
2432  assembleHeader(settings_->vehicle_frame_id, telegram, msg);
2433  publish<IMUSetupMsg>("imusetup", msg);
2434  }
2435  break;
2436  }
2437  case VEL_SENSOR_SETUP: // Velocity sensor lever arm
2438  {
2439  if (settings_->publish_velcovgeodetic)
2440  {
2441  VelSensorSetupMsg msg;
2442 
2443  if (!VelSensorSetupParser(node_, telegram->message.begin(),
2444  telegram->message.end(), msg,
2445  settings_->use_ros_axis_orientation))
2446  {
2447  node_->log(log_level::ERROR, "parse error in VelSensorSetup");
2448  break;
2449  }
2450  assembleHeader(settings_->vehicle_frame_id, telegram, msg);
2451  publish<VelSensorSetupMsg>("velsensorsetup", msg);
2452  }
2453  break;
2454  }
2455  case EXT_EVENT_INS_NAV_CART: // Position, velocity and orientation in
2456  // cartesian coordinate frame (ENU frame)
2457  {
2458  if (settings_->publish_exteventinsnavcart)
2459  {
2460  INSNavCartMsg msg;
2461 
2462  if (!INSNavCartParser(node_, telegram->message.begin(),
2463  telegram->message.end(), msg,
2464  settings_->use_ros_axis_orientation))
2465  {
2466  node_->log(log_level::ERROR,
2467  "parse error in ExtEventINSNavCart");
2468  break;
2469  }
2470  std::string frame_id;
2471  if (settings_->ins_use_poi)
2472  {
2473  frame_id = settings_->poi_frame_id;
2474  } else
2475  {
2476  frame_id = settings_->frame_id;
2477  }
2478  assembleHeader(frame_id, telegram, msg);
2479  publish<INSNavCartMsg>("exteventinsnavcart", msg);
2480  }
2481  break;
2482  }
2483  case EXT_EVENT_INS_NAV_GEOD:
2484  {
2485  if (settings_->publish_exteventinsnavgeod)
2486  {
2487  INSNavGeodMsg msg;
2488 
2489  if (!INSNavGeodParser(node_, telegram->message.begin(),
2490  telegram->message.end(), msg,
2491  settings_->use_ros_axis_orientation))
2492  {
2493  node_->log(log_level::ERROR,
2494  "parse error in ExtEventINSNavGeod");
2495  break;
2496  }
2497  std::string frame_id;
2498  if (settings_->ins_use_poi)
2499  {
2500  frame_id = settings_->poi_frame_id;
2501  } else
2502  {
2503  frame_id = settings_->frame_id;
2504  }
2505  assembleHeader(frame_id, telegram, msg);
2506  publish<INSNavGeodMsg>("exteventinsnavgeod", msg);
2507  }
2508  break;
2509  }
2510  case EXT_SENSOR_MEAS:
2511  {
2512  if (!ExtSensorMeasParser(node_, telegram->message.begin(),
2513  telegram->message.end(), last_extsensmeas_,
2514  settings_->use_ros_axis_orientation,
2515  hasImuMeas_))
2516  {
2517  node_->log(log_level::ERROR, "parse error in ExtSensorMeas");
2518  break;
2519  }
2520  assembleHeader(settings_->imu_frame_id, telegram, last_extsensmeas_);
2521  if (settings_->publish_extsensormeas)
2522  publish<ExtSensorMeasMsg>("extsensormeas", last_extsensmeas_);
2523  break;
2524  }
2525  case CHANNEL_STATUS:
2526  {
2527  if (!ChannelStatusParser(node_, telegram->message.begin(),
2528  telegram->message.end(), last_channelstatus_))
2529  {
2530  node_->log(log_level::ERROR, "parse error in ChannelStatus");
2531  break;
2532  }
2533  if (settings_->septentrio_receiver_type == "gnss")
2534  assembleGpsFix();
2535  break;
2536  }
2537  case MEAS_EPOCH:
2538  {
2539  if (!MeasEpochParser(node_, telegram->message.begin(),
2540  telegram->message.end(), last_measepoch_))
2541  {
2542  node_->log(log_level::ERROR, "parse error in MeasEpoch");
2543  break;
2544  }
2545  assembleHeader(settings_->frame_id, telegram, last_measepoch_);
2546  if (settings_->publish_measepoch)
2547  publish<MeasEpochMsg>("measepoch", last_measepoch_);
2548  if (settings_->septentrio_receiver_type == "gnss")
2549  assembleGpsFix();
2550  break;
2551  }
2552  case DOP:
2553  {
2554  if (!DOPParser(node_, telegram->message.begin(), telegram->message.end(),
2555  last_dop_))
2556  {
2557  node_->log(log_level::ERROR, "parse error in DOP");
2558  break;
2559  }
2560  if (settings_->septentrio_receiver_type == "gnss")
2561  assembleGpsFix();
2562  break;
2563  }
2564  case VEL_COV_CARTESIAN:
2565  {
2566  if (settings_->publish_velcovcartesian)
2567  {
2568  VelCovCartesianMsg msg;
2569  if (!VelCovCartesianParser(node_, telegram->message.begin(),
2570  telegram->message.end(), msg))
2571  {
2572  node_->log(log_level::ERROR, "parse error in VelCovCartesian");
2573  break;
2574  }
2575  assembleHeader(settings_->frame_id, telegram, msg);
2576  publish<VelCovCartesianMsg>("velcovcartesian", msg);
2577  }
2578  break;
2579  }
2580  case VEL_COV_GEODETIC:
2581  {
2582 
2583  if (!VelCovGeodeticParser(node_, telegram->message.begin(),
2584  telegram->message.end(), last_velcovgeodetic_))
2585  {
2586  node_->log(log_level::ERROR, "parse error in VelCovGeodetic");
2587  break;
2588  }
2589  assembleHeader(settings_->frame_id, telegram, last_velcovgeodetic_);
2590  if (settings_->publish_velcovgeodetic)
2591  publish<VelCovGeodeticMsg>("velcovgeodetic", last_velcovgeodetic_);
2592  assembleTwist();
2593  if (settings_->septentrio_receiver_type == "gnss")
2594  assembleGpsFix();
2595  break;
2596  }
2597  case RECEIVER_STATUS:
2598  {
2599  if (!ReceiverStatusParser(node_, telegram->message.begin(),
2600  telegram->message.end(), last_receiverstatus_))
2601  {
2602  node_->log(log_level::ERROR, "parse error in ReceiverStatus");
2603  break;
2604  }
2605  assembleDiagnosticArray(telegram);
2606  break;
2607  }
2608  case QUALITY_IND:
2609  {
2610  if (!QualityIndParser(node_, telegram->message.begin(),
2611  telegram->message.end(), last_qualityind_))
2612  {
2613  node_->log(log_level::ERROR, "parse error in QualityInd");
2614  break;
2615  }
2616  assembleDiagnosticArray(telegram);
2617  break;
2618  }
2619  case RECEIVER_SETUP:
2620  {
2621  if (!ReceiverSetupParser(node_, telegram->message.begin(),
2622  telegram->message.end(), last_receiversetup_))
2623  {
2624  node_->log(log_level::ERROR, "parse error in ReceiverSetup");
2625  break;
2626  }
2627  node_->log(log_level::DEBUG,
2628  "receiver setup firmware: " + last_receiversetup_.rx_version);
2629 
2630  static const int32_t ins_major = 1;
2631  static const int32_t ins_minor = 4;
2632  static const int32_t ins_patch = 0;
2633  static const int32_t gnss_major = 4;
2634  static const int32_t gnss_minor = 12;
2635  static const int32_t gnss_patch = 1;
2636  boost::tokenizer<> tok(last_receiversetup_.rx_version);
2637  boost::tokenizer<>::iterator it = tok.begin();
2638  std::vector<int32_t> major_minor_patch;
2639  major_minor_patch.reserve(3);
2640  for (boost::tokenizer<>::iterator it = tok.begin(); it != tok.end();
2641  ++it)
2642  {
2643  int32_t v = std::atoi(it->c_str());
2644  major_minor_patch.push_back(v);
2645  }
2646  if (major_minor_patch.size() < 3)
2647  {
2648  node_->log(log_level::ERROR, "parse error of firmware version.");
2649  } else
2650  {
2651  if ((settings_->septentrio_receiver_type == "ins") || node_->isIns())
2652  {
2653  if ((major_minor_patch[0] < ins_major) ||
2654  ((major_minor_patch[0] == ins_major) &&
2655  (major_minor_patch[1] < ins_minor)) ||
2656  ((major_minor_patch[0] == ins_major) &&
2657  (major_minor_patch[1] == ins_minor) &&
2658  (major_minor_patch[2] < ins_patch)))
2659  {
2660  node_->log(
2661  log_level::INFO,
2662  "INS receiver has firmware version: " +
2663  last_receiversetup_.rx_version +
2664  ", which does not support all features. Please update to at least " +
2665  std::to_string(ins_major) + "." +
2666  std::to_string(ins_minor) + "." +
2667  std::to_string(ins_patch) + " or consult README.");
2668  } else
2669  node_->setImprovedVsmHandling();
2670  } else if (settings_->septentrio_receiver_type == "gnss")
2671  {
2672  if ((major_minor_patch[0] < gnss_major) ||
2673  ((major_minor_patch[0] == gnss_major) &&
2674  (major_minor_patch[1] < gnss_minor)) ||
2675  ((major_minor_patch[0] == gnss_major) &&
2676  (major_minor_patch[1] == gnss_minor) &&
2677  (major_minor_patch[2] < gnss_patch)))
2678  {
2679  node_->log(
2680  log_level::INFO,
2681  "GNSS receiver has firmware version: " +
2682  last_receiversetup_.rx_version +
2683  ", which may not support all features. Please update to at least " +
2684  std::to_string(gnss_major) + "." +
2685  std::to_string(gnss_minor) + "." +
2686  std::to_string(gnss_patch) + " or consult README.");
2687  }
2688  }
2689  }
2690 
2691  break;
2692  }
2693  case RECEIVER_TIME:
2694  {
2695  ReceiverTimeMsg msg;
2696 
2697  if (!ReceiverTimeParser(node_, telegram->message.begin(),
2698  telegram->message.end(), msg))
2699  {
2700  node_->log(log_level::ERROR, "parse error in ReceiverTime");
2701  break;
2702  }
2703  current_leap_seconds_ = msg.delta_ls;
2704  break;
2705  }
2706  default:
2707  {
2708  node_->log(log_level::DEBUG, "unhandled SBF block " +
2709  std::to_string(sbfId) + " received.");
2710  break;
2711  }
2712  // Many more to be implemented...
2713  }
2714  }
2715 
2716  void MessageHandler::wait(Timestamp time_obj)
2717  {
2718  Timestamp unix_old = unix_time_;
2719  unix_time_ = time_obj;
2720  if ((unix_old != 0) && (unix_time_ > unix_old))
2721  {
2722  auto sleep_nsec = unix_time_ - unix_old;
2723 
2724  std::stringstream ss;
2725  ss << "Waiting for " << sleep_nsec / 1000000 << " milliseconds...";
2726  node_->log(log_level::DEBUG, ss.str());
2727 
2728  std::this_thread::sleep_for(std::chrono::nanoseconds(sleep_nsec));
2729  }
2730  }
2731 
2732  void MessageHandler::parseNmea(const std::shared_ptr<Telegram>& telegram)
2733  {
2734  std::string message(telegram->message.begin(), telegram->message.end());
2735  /*node_->log(
2736  LogLevel::DEBUG,
2737  "The NMEA message contains " + std::to_string(message.size()) +
2738  " bytes and is ready to be parsed. It reads: " + message);*/
2739  boost::char_separator<char> sep_2(",*", "", boost::keep_empty_tokens);
2740  boost::tokenizer<boost::char_separator<char>> tokens(message, sep_2);
2741  std::vector<std::string> body;
2742  body.reserve(20);
2743  for (boost::tokenizer<boost::char_separator<char>>::iterator tok_iter =
2744  tokens.begin();
2745  tok_iter != tokens.end(); ++tok_iter)
2746  {
2747  body.push_back(*tok_iter);
2748  }
2749 
2750  std::string id(body[0].begin() + 1, body[0].end());
2751 
2752  auto it = nmeaMap_.find(body[0]);
2753  if (it != nmeaMap_.end())
2754  {
2755  switch (it->second)
2756  {
2757  case 0:
2758  {
2759  // Create NmeaSentence struct to pass to GpggaParser::parseASCII
2760  NMEASentence gga_message(id, body);
2761  GpggaMsg msg;
2762  GpggaParser parser_obj;
2763  try
2764  {
2765  msg = parser_obj.parseASCII(gga_message, settings_->frame_id,
2766  settings_->use_gnss_time,
2767  telegram->stamp);
2768  } catch (ParseException& e)
2769  {
2770  node_->log(log_level::DEBUG,
2771  "GpggaMsg: " + std::string(e.what()));
2772  break;
2773  }
2774  publish<GpggaMsg>("gpgga", msg);
2775  break;
2776  }
2777  case 1:
2778  {
2779  // Create NmeaSentence struct to pass to GprmcParser::parseASCII
2780  NMEASentence rmc_message(id, body);
2781  GprmcMsg msg;
2782  GprmcParser parser_obj;
2783  try
2784  {
2785  msg = parser_obj.parseASCII(rmc_message, settings_->frame_id,
2786  settings_->use_gnss_time,
2787  telegram->stamp);
2788  } catch (ParseException& e)
2789  {
2790  node_->log(log_level::DEBUG,
2791  "GprmcMsg: " + std::string(e.what()));
2792  break;
2793  }
2794  publish<GprmcMsg>("gprmc", msg);
2795  break;
2796  }
2797  case 2:
2798  {
2799  // Create NmeaSentence struct to pass to GpgsaParser::parseASCII
2800  NMEASentence gsa_message(id, body);
2801  GpgsaMsg msg;
2802  GpgsaParser parser_obj;
2803  try
2804  {
2805  msg = parser_obj.parseASCII(gsa_message, settings_->frame_id,
2806  settings_->use_gnss_time,
2807  node_->getTime());
2808  } catch (ParseException& e)
2809  {
2810  node_->log(log_level::DEBUG,
2811  "GpgsaMsg: " + std::string(e.what()));
2812  break;
2813  }
2814  if (settings_->use_gnss_time)
2815  {
2816  if (settings_->septentrio_receiver_type == "gnss")
2817  {
2818  Timestamp time_obj =
2819  timestampSBF(last_pvtgeodetic_.block_header.tow,
2820  last_pvtgeodetic_.block_header.wnc);
2821  msg.header.stamp = timestampToRos(time_obj);
2822  }
2823  if (settings_->septentrio_receiver_type == "ins")
2824  {
2825  Timestamp time_obj =
2826  timestampSBF(last_insnavgeod_.block_header.tow,
2827  last_insnavgeod_.block_header.wnc);
2828  msg.header.stamp = timestampToRos(time_obj);
2829  }
2830  } else
2831  msg.header.stamp = timestampToRos(telegram->stamp);
2832  publish<GpgsaMsg>("gpgsa", msg);
2833  break;
2834  }
2835  case 4:
2836  {
2837  // Create NmeaSentence struct to pass to GpgsvParser::parseASCII
2838  NMEASentence gsv_message(id, body);
2839  GpgsvMsg msg;
2840  GpgsvParser parser_obj;
2841  try
2842  {
2843  msg = parser_obj.parseASCII(gsv_message, settings_->frame_id,
2844  settings_->use_gnss_time,
2845  node_->getTime());
2846  } catch (ParseException& e)
2847  {
2848  node_->log(log_level::DEBUG,
2849  "GpgsvMsg: " + std::string(e.what()));
2850  break;
2851  }
2852  if (settings_->use_gnss_time)
2853  {
2854 
2855  if (settings_->septentrio_receiver_type == "gnss")
2856  {
2857  Timestamp time_obj =
2858  timestampSBF(last_pvtgeodetic_.block_header.tow,
2859  last_pvtgeodetic_.block_header.wnc);
2860  msg.header.stamp = timestampToRos(time_obj);
2861  }
2862  if (settings_->septentrio_receiver_type == "ins")
2863  {
2864  Timestamp time_obj =
2865  timestampSBF(last_insnavgeod_.block_header.tow,
2866  last_insnavgeod_.block_header.wnc);
2867  msg.header.stamp = timestampToRos(time_obj);
2868  }
2869  } else
2870  msg.header.stamp = timestampToRos(telegram->stamp);
2871  publish<GpgsvMsg>("gpgsv", msg);
2872  break;
2873  }
2874  }
2875  } else
2876  {
2877  node_->log(log_level::DEBUG, "Unknown NMEA message: " + body[0]);
2878  }
2879  }
2880 
2881 } // namespace io
log_level::WARN
@ WARN
Definition: typedefs.hpp:182
GpggaMsg
nmea_msgs::msg::Gpgga GpggaMsg
Definition: typedefs.hpp:145
Settings::publish_pose
bool publish_pose
Whether or not to publish the PoseWithCovarianceStampedMsg message.
Definition: settings.hpp:320
Settings::publish_attcoveuler
bool publish_attcoveuler
Whether or not to publish the AttCovEulerMsg message.
Definition: settings.hpp:298
evMovingBaseRTKFixed
@ evMovingBaseRTKFixed
Definition: message_handler.hpp:102
ExtSensorMeasParser
bool ExtSensorMeasParser(ROSaicNodeBase *node, It it, It itEnd, ExtSensorMeasMsg &msg, bool use_ros_axis_orientation, bool &hasImuMeas)
Qi based parser for the SBF block "ExtSensorMeas".
Definition: sbf_blocks.hpp:1724
Settings::latency_compensation
bool latency_compensation
Wether processing latency shall be compensated for in ROS timestamp.
Definition: settings.hpp:350
Timestamp
uint64_t Timestamp
Definition: typedefs.hpp:101
io::MessageHandler::fixedUtmZone_
std::shared_ptr< std::string > fixedUtmZone_
Fixed UTM zone.
Definition: message_handler.hpp:421
timestampFromRos
Timestamp timestampFromRos(const TimestampRos &tsr)
Convert ROS timestamp to nsec timestamp.
Definition: typedefs.hpp:169
QualityIndParser
bool QualityIndParser(ROSaicNodeBase *node, It it, It itEnd, QualityInd &msg)
Qi based parser for the SBF block "QualityInd".
Definition: sbf_blocks.hpp:1386
Settings::publish_imusetup
bool publish_imusetup
Whether or not to publish the IMUSetupMsg message.
Definition: settings.hpp:304
PosCovGeodeticParser
bool PosCovGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, PosCovGeodeticMsg &msg)
Qi based parser for the SBF block "PosCovGeodetic".
Definition: sbf_blocks.hpp:1278
ChannelStatus::satInfo
std::vector< ChannelSatInfo > satInfo
Definition: sbf_blocks.hpp:128
evDGPS
@ evDGPS
Definition: message_handler.hpp:97
INSNavCartMsg
septentrio_gnss_driver::msg::INSNavCart INSNavCartMsg
Definition: typedefs.hpp:151
Settings::publish_twist
bool publish_twist
Whether or not to publish the TwistWithCovarianceStampedMsg message.
Definition: settings.hpp:330
Settings::publish_localization
bool publish_localization
Whether or not to publish the LocalizationMsg message.
Definition: settings.hpp:326
io::MessageHandler::timestampSBF
Timestamp timestampSBF(const std::vector< uint8_t > &message) const
Calculates the timestamp, in the Unix Epoch time format This is either done using the TOW as transmit...
Definition: message_handler.cpp:2074
INS_NAV_CART
@ INS_NAV_CART
Definition: message_handler.hpp:125
ATT_COV_EULER
@ ATT_COV_EULER
Definition: message_handler.hpp:116
Settings::use_gnss_time
bool use_gnss_time
Definition: settings.hpp:344
RECEIVER_TIME
@ RECEIVER_TIME
Definition: message_handler.hpp:132
parsing_utilities::quaternionToQuaternionMsg
QuaternionMsg quaternionToQuaternionMsg(const Eigen::Quaterniond &q)
Transforms Euler angles to a QuaternionMsg.
Definition: parsing_utilities.hpp:348
INS_NAV_GEOD
@ INS_NAV_GEOD
Definition: message_handler.hpp:126
io::MessageHandler::assembleHeader
void assembleHeader(const std::string &frameId, const std::shared_ptr< Telegram > &telegram, T &msg) const
Header assembling.
Definition: message_handler.cpp:2038
ChannelStatus::block_header
BlockHeaderMsg block_header
Definition: sbf_blocks.hpp:122
ImuMsg
sensor_msgs::msg::Imu ImuMsg
Definition: typedefs.hpp:118
DOP
@ DOP
Definition: message_handler.hpp:119
Settings::publish_pvtcartesian
bool publish_pvtcartesian
Definition: settings.hpp:275
MEAS_EPOCH
@ MEAS_EPOCH
Definition: message_handler.hpp:118
PoseWithCovarianceStampedMsg
geometry_msgs::msg::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg
Definition: typedefs.hpp:109
ReceiverSetup::rx_serial_number
std::string rx_serial_number
Definition: sbf_blocks.hpp:160
Settings::publish_pvtgeodetic
bool publish_pvtgeodetic
Whether or not to publish the PVTGeodeticMsg message.
Definition: settings.hpp:277
POS_COV_GEODETIC
@ POS_COV_GEODETIC
Definition: message_handler.hpp:114
ROSaicNodeBase::log
void log(log_level::LogLevel logLevel, const std::string &s) const
Log function to provide abstraction of ROS loggers.
Definition: typedefs.hpp:286
io::MessageHandler::publishTf
void publishTf(const LocalizationMsg &msg)
Publishing function.
Definition: message_handler.cpp:2146
io::MessageHandler::last_rf_status_
RfStatusMsg last_rf_status_
Stores incoming RFStatus block.
Definition: message_handler.hpp:312
POS_COV_CARTESIAN
@ POS_COV_CARTESIAN
Definition: message_handler.hpp:113
GpgsvMsg
nmea_msgs::msg::Gpgsv GpgsvMsg
Definition: typedefs.hpp:147
parsing_utilities::getTow
uint32_t getTow(const std::vector< uint8_t > &message)
Get the time of week in ms of the SBF message.
Definition: parsing_utilities.cpp:325
TimeReferenceMsg
sensor_msgs::msg::TimeReference TimeReferenceMsg
Definition: typedefs.hpp:117
io::MessageHandler::last_qualityind_
QualityInd last_qualityind_
Since DiagnosticArray needs QualityInd, incoming QualityInd blocks need to be stored.
Definition: message_handler.hpp:291
Settings::publish_imu
bool publish_imu
Whether or not to publish the ImuMsg message.
Definition: settings.hpp:324
parsing_utilities::rad2deg
T rad2deg(T rad)
Definition: parsing_utilities.hpp:93
QUALITY_IND
@ QUALITY_IND
Definition: message_handler.hpp:123
io::MessageHandler::last_receiversetup_
ReceiverSetup last_receiversetup_
Since DiagnosticArray needs ReceiverSetup, incoming ReceiverSetup blocks need to be stored.
Definition: message_handler.hpp:297
DiagnosticArrayMsg
diagnostic_msgs::msg::DiagnosticArray DiagnosticArrayMsg
Definition: typedefs.hpp:106
ChannelStatusParser
bool ChannelStatusParser(ROSaicNodeBase *node, It it, It itEnd, ChannelStatus &msg)
Qi based parser for the SBF block "ChannelStatus".
Definition: sbf_blocks.hpp:448
BaseVectorGeodMsg
septentrio_gnss_driver::msg::BaseVectorGeod BaseVectorGeodMsg
Definition: typedefs.hpp:124
io::MessageHandler::last_atteuler_
AttEulerMsg last_atteuler_
Since GPSFix etc. need AttEuler, incoming AttEuler blocks need to be stored.
Definition: message_handler.hpp:232
timestampToRos
TimestampRos timestampToRos(Timestamp ts)
Convert nsec timestamp to ROS timestamp.
Definition: typedefs.hpp:162
ATT_EULER
@ ATT_EULER
Definition: message_handler.hpp:115
parsing_utilities::convertCovariance
double convertCovariance(double val)
Definition: parsing_utilities.hpp:529
frame_id
std::string frame_id
BASE_VECTOR_CART
@ BASE_VECTOR_CART
Definition: message_handler.hpp:111
IMU_SETUP
@ IMU_SETUP
Definition: message_handler.hpp:129
PVTCartesianMsg
septentrio_gnss_driver::msg::PVTCartesian PVTCartesianMsg
Definition: typedefs.hpp:134
RfStatusParser
bool RfStatusParser(ROSaicNodeBase *node, It it, It itEnd, RfStatusMsg &msg)
Qi based parser for the SBF block "RFStatus".
Definition: sbf_blocks.hpp:669
io::MessageHandler::assemblePoseWithCovarianceStamped
void assemblePoseWithCovarianceStamped()
"Callback" function when constructing PoseWithCovarianceStamped messages
Definition: message_handler.cpp:58
io::MessageHandler::assembleTimeReference
void assembleTimeReference(const std::shared_ptr< Telegram > &telegram)
function when constructing TimeReferenceMsg messages
Definition: message_handler.cpp:2027
EXT_EVENT_INS_NAV_CART
@ EXT_EVENT_INS_NAV_CART
Definition: message_handler.hpp:128
ROSaicNodeBase::publishMessage
void publishMessage(const std::string &topic, const M &msg)
Publishing function.
Definition: typedefs.hpp:322
parsing_utilities::getWnc
uint16_t getWnc(const std::vector< uint8_t > &message)
Get the GPS week counter of the SBF message.
Definition: parsing_utilities.cpp:330
Settings::frame_id
std::string frame_id
The frame ID used in the header of every published ROS message.
Definition: settings.hpp:352
ReceiverTimeParser
bool ReceiverTimeParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverTimeMsg &msg)
Struct for the SBF block "ReceiverTime".
Definition: sbf_blocks.hpp:1482
IMUSetupMsg
septentrio_gnss_driver::msg::IMUSetup IMUSetupMsg
Definition: typedefs.hpp:153
Settings::publish_gpst
bool publish_gpst
Whether or not to publish the TimeReferenceMsg message with GPST.
Definition: settings.hpp:314
io::MessageHandler::assembleGpsFix
void assembleGpsFix()
"Callback" function when constructing GPSFix messages
Definition: message_handler.cpp:1639
ReceiverSetup::block_header
BlockHeaderMsg block_header
Definition: sbf_blocks.hpp:154
Settings::poi_frame_id
std::string poi_frame_id
Definition: settings.hpp:357
MeasEpochParser
bool MeasEpochParser(ROSaicNodeBase *node, It it, It itEnd, MeasEpochMsg &msg)
Qi based parser for the SBF block "MeasEpoch".
Definition: sbf_blocks.hpp:582
Settings::publish_galauthstatus
bool publish_galauthstatus
Whether or not to publish the GALAuthStatus message and diagnostics.
Definition: settings.hpp:272
QualityInd::indicators
std::vector< uint16_t > indicators
Definition: sbf_blocks.hpp:190
io::MessageHandler::last_measepoch_
MeasEpochMsg last_measepoch_
Since GPSFix needs MeasEpoch (for SNRs), incoming MeasEpoch blocks need to be stored.
Definition: message_handler.hpp:268
Settings::publish_velcovcartesian
bool publish_velcovcartesian
Definition: settings.hpp:291
Settings::publish_aimplusstatus
bool publish_aimplusstatus
Definition: settings.hpp:270
evStandAlone
@ evStandAlone
Definition: message_handler.hpp:96
io::MessageHandler::current_leap_seconds_
int32_t current_leap_seconds_
Current leap seconds as received, do not use value is -128.
Definition: message_handler.hpp:322
io::MessageHandler::assembleAimAndDiagnosticArray
void assembleAimAndDiagnosticArray()
"Callback" function when constructing RFStatus DiagnosticArrayMsg messages
Definition: message_handler.cpp:474
GAL_AUTH_STATUS
@ GAL_AUTH_STATUS
Definition: message_handler.hpp:133
parsing_utilities::R_enu_ecef
Eigen::Matrix3d R_enu_ecef(double lat, double lon)
Generates the matrix to rotate from local ENU to ECEF.
Definition: parsing_utilities.hpp:424
io::MessageHandler::assembleDiagnosticArray
void assembleDiagnosticArray(const std::shared_ptr< Telegram > &telegram)
"Callback" function when constructing DiagnosticArrayMsg messages
Definition: message_handler.cpp:214
VEL_COV_GEODETIC
@ VEL_COV_GEODETIC
Definition: message_handler.hpp:121
ReceiverTimeMsg
septentrio_gnss_driver::msg::ReceiverTime ReceiverTimeMsg
Definition: typedefs.hpp:138
io::MessageHandler::assembleLocalizationMsgTwist
void assembleLocalizationMsgTwist(double roll, double pitch, double yaw, LocalizationMsg &msg) const
function to fill twist part of LocalizationMsg
Definition: message_handler.cpp:1242
EXT_SENSOR_MEAS
@ EXT_SENSOR_MEAS
Definition: message_handler.hpp:131
Dop::pdop
double pdop
Definition: sbf_blocks.hpp:140
VelCovCartesianParser
bool VelCovCartesianParser(ROSaicNodeBase *node, It it, It itEnd, VelCovCartesianMsg &msg)
Qi based parser for the SBF block "VelCovCartesian".
Definition: sbf_blocks.hpp:1314
parsing_utilities::convertEulerToQuaternion
Eigen::Quaterniond convertEulerToQuaternion(double roll, double pitch, double yaw)
Transforms Euler angles to a quaternion.
Definition: parsing_utilities.hpp:326
ReceiverStatus::up_time
uint32_t up_time
Definition: sbf_blocks.hpp:214
Dop::block_header
BlockHeaderMsg block_header
Definition: sbf_blocks.hpp:137
Settings::publish_exteventinsnavcart
bool publish_exteventinsnavcart
Whether or not to publish the ExtEventINSNavCartMsg message.
Definition: settings.hpp:310
Settings::publish_gpsfix
bool publish_gpsfix
Whether or not to publish the GpsFixMsg message.
Definition: settings.hpp:318
GalAuthStatusParser
bool GalAuthStatusParser(ROSaicNodeBase *node, It it, It itEnd, GalAuthStatusMsg &msg)
Qi based parser for the SBF block "GALAuthStatus".
Definition: sbf_blocks.hpp:626
PosCovCartesianMsg
septentrio_gnss_driver::msg::PosCovCartesian PosCovCartesianMsg
Definition: typedefs.hpp:136
Dop::tdop
double tdop
Definition: sbf_blocks.hpp:141
io::MessageHandler::settings_
const Settings * settings_
Pointer to settings struct.
Definition: message_handler.hpp:207
Settings::publish_velcovgeodetic
bool publish_velcovgeodetic
Definition: settings.hpp:294
io::MessageHandler::nmeaMap_
std::unordered_map< std::string, uint8_t > nmeaMap_
Map of NMEA messgae IDs and uint8_t.
Definition: message_handler.hpp:212
GpgsvParser
Derived class for parsing GSV messages.
Definition: gpgsv.hpp:76
QualityInd::block_header
BlockHeaderMsg block_header
Definition: sbf_blocks.hpp:186
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_blocks.hpp:1107
io::MessageHandler::last_pvtgeodetic_
PVTGeodeticMsg last_pvtgeodetic_
Since NavSatFix etc. need PVTGeodetic, incoming PVTGeodetic blocks need to be stored.
Definition: message_handler.hpp:220
Settings::publish_poscovgeodetic
bool publish_poscovgeodetic
Definition: settings.hpp:288
Settings::publish_tf_ecef
bool publish_tf_ecef
Whether or not to publish the tf of the localization.
Definition: settings.hpp:334
PVT_GEODETIC
@ PVT_GEODETIC
Definition: message_handler.hpp:110
ReceiverSetup::rx_version
std::string rx_version
Definition: sbf_blocks.hpp:162
io::MessageHandler::setStatus
void setStatus(uint8_t mode, NavSatFixMsg &msg)
Set status of NavSatFix messages.
Definition: message_handler.cpp:1352
Settings::publish_basevectorcart
bool publish_basevectorcart
Definition: settings.hpp:280
PVTCartesianParser
bool PVTCartesianParser(ROSaicNodeBase *node, It it, It itEnd, PVTCartesianMsg &msg)
Qi based parser for the SBF block "PVTCartesian".
Definition: sbf_blocks.hpp:792
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_blocks.hpp:1688
evRTKFixed
@ evRTKFixed
Definition: message_handler.hpp:99
parsing_utilities::convertEulerToQuaternionMsg
QuaternionMsg convertEulerToQuaternionMsg(double roll, double pitch, double yaw)
Convert Eigen quaternion to a QuaternionMsg.
Definition: parsing_utilities.hpp:366
io::MessageHandler::last_gal_auth_status_
GalAuthStatusMsg last_gal_auth_status_
Stores incoming GALAuthStatus block.
Definition: message_handler.hpp:302
INSNavGeodMsg
septentrio_gnss_driver::msg::INSNavGeod INSNavGeodMsg
Definition: typedefs.hpp:152
Settings::use_ros_axis_orientation
bool use_ros_axis_orientation
ROS axis orientation, body: front-left-up, geographic: ENU.
Definition: settings.hpp:211
frameId
std::string const * frameId(const M &m)
parsing_utilities::R_ned_ecef
Eigen::Matrix3d R_ned_ecef(double lat, double lon)
Generates the matrix to rotate from local NED to ECEF.
Definition: parsing_utilities.hpp:452
parsing_utilities::convertAutoCovariance
double convertAutoCovariance(double val)
Definition: parsing_utilities.hpp:524
evFixed
@ evFixed
Definition: message_handler.hpp:98
Settings::publish_localization_ecef
bool publish_localization_ecef
Whether or not to publish the LocalizationMsg message.
Definition: settings.hpp:328
VelCovGeodeticParser
bool VelCovGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, VelCovGeodeticMsg &msg)
Qi based parser for the SBF block "VelCovGeodetic".
Definition: sbf_blocks.hpp:1350
ReceiverStatus::cpu_load
uint8_t cpu_load
Definition: sbf_blocks.hpp:212
io::MessageHandler::parseNmea
void parseNmea(const std::shared_ptr< Telegram > &telegram)
Parse NMEA block.
Definition: message_handler.cpp:2732
TwistWithCovarianceStampedMsg
geometry_msgs::msg::TwistWithCovarianceStamped TwistWithCovarianceStampedMsg
Definition: typedefs.hpp:110
ROSaicNodeBase::isIns
bool isIns()
Check if Rx is INS.
Definition: typedefs.hpp:440
ReceiverStatusParser
bool ReceiverStatusParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverStatus &msg)
Struct for the SBF block "ReceiverStatus".
Definition: sbf_blocks.hpp:1438
Settings::ins_use_poi
bool ins_use_poi
INS solution reference point.
Definition: settings.hpp:243
NavSatFixMsg
sensor_msgs::msg::NavSatFix NavSatFixMsg
Definition: typedefs.hpp:115
ROSaicNodeBase::setImprovedVsmHandling
void setImprovedVsmHandling()
Set improved VSM handling to true.
Definition: typedefs.hpp:435
io
Definition: async_manager.hpp:83
validValue
bool validValue(T s)
Check if value is not set to Do-Not-Use.
Definition: sbf_utilities.hpp:51
message_handler.hpp
GprmcMsg
nmea_msgs::msg::Gprmc GprmcMsg
Definition: typedefs.hpp:148
Settings::publish_atteuler
bool publish_atteuler
Whether or not to publish the AttEulerMsg message.
Definition: settings.hpp:296
QualityInd::n
uint8_t n
Definition: sbf_blocks.hpp:188
Settings::lock_utm_zone
bool lock_utm_zone
Wether the UTM zone of the localization is locked.
Definition: settings.hpp:365
io::MessageHandler::assembleNavSatFix
void assembleNavSatFix()
"Callback" function when constructing NavSatFix messages
Definition: message_handler.cpp:1424
Settings::publish_basevectorgeod
bool publish_basevectorgeod
Whether or not to publish the BaseVectorGeodMsg message.
Definition: settings.hpp:282
io::MessageHandler::assembleImu
void assembleImu()
"Callback" function when constructing ImuMsg messages
Definition: message_handler.cpp:576
Settings::publish_diagnostics
bool publish_diagnostics
Whether or not to publish the DiagnosticArrayMsg message.
Definition: settings.hpp:322
Settings::imu_frame_id
std::string imu_frame_id
The frame ID used in the header of published ROS Imu message.
Definition: settings.hpp:354
parsing_utilities::getId
uint16_t getId(const std::vector< uint8_t > &message)
Get the ID of the SBF message.
Definition: parsing_utilities.cpp:310
GpggaParser
Derived class for parsing GGA messages.
Definition: gpgga.hpp:76
io::MessageHandler::setLeapSeconds
void setLeapSeconds()
Definition: message_handler.hpp:155
ReceiverStatus::rx_error
uint32_t rx_error
Definition: sbf_blocks.hpp:216
io::MessageHandler::assembleLocalizationEcef
void assembleLocalizationEcef()
"Callback" function when constructing LocalizationMsg messages in ECEF
Definition: message_handler.cpp:1088
parsing_utilities::square
T square(T val)
Definition: parsing_utilities.hpp:65
ReceiverStatus::block_header
BlockHeaderMsg block_header
Definition: sbf_blocks.hpp:210
io::MessageHandler::last_attcoveuler_
AttCovEulerMsg last_attcoveuler_
Since GPSFix etc. need AttCovEuler, incoming AttCovEuler blocks need to be stored.
Definition: message_handler.hpp:238
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:55
VelCovCartesianMsg
septentrio_gnss_driver::msg::VelCovCartesian VelCovCartesianMsg
Definition: typedefs.hpp:141
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_blocks.hpp:1650
RECEIVER_SETUP
@ RECEIVER_SETUP
Definition: message_handler.hpp:124
ReceiverStatus::rx_status
uint32_t rx_status
Definition: sbf_blocks.hpp:215
Dop::hdop
double hdop
Definition: sbf_blocks.hpp:142
io::MessageHandler::last_pvt_latency_
uint64_t last_pvt_latency_
Last reported PVT processing latency.
Definition: message_handler.hpp:319
ReceiverStatus::ext_error
uint8_t ext_error
Definition: sbf_blocks.hpp:213
BaseVectorCartMsg
septentrio_gnss_driver::msg::BaseVectorCart BaseVectorCartMsg
Definition: typedefs.hpp:123
io::MessageHandler::osnma_info_available_
bool osnma_info_available_
Wether OSNMA info has been received.
Definition: message_handler.hpp:307
evSBAS
@ evSBAS
Definition: message_handler.hpp:101
Settings::publish_poscovcartesian
bool publish_poscovcartesian
Definition: settings.hpp:285
Settings::publish_insnavcart
bool publish_insnavcart
Whether or not to publish the INSNavCartMsg message.
Definition: settings.hpp:300
BaseVectorCartParser
bool BaseVectorCartParser(ROSaicNodeBase *node, It it, It itEnd, BaseVectorCartMsg &msg)
Qi based parser for the SBF block "BaseVectorCart".
Definition: sbf_blocks.hpp:1008
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:57
log_level::ERROR
@ ERROR
Definition: typedefs.hpp:183
io::MessageHandler::publish
void publish(const std::string &topic, const M &msg)
Publishing function.
Definition: message_handler.cpp:2117
Settings::read_from_sbf_log
bool read_from_sbf_log
Whether or not we are reading from an SBF file.
Definition: settings.hpp:369
ROSaicNodeBase::getTime
Timestamp getTime() const
Gets current timestamp.
Definition: typedefs.hpp:314
PVT_CARTESIAN
@ PVT_CARTESIAN
Definition: message_handler.hpp:109
io::MessageHandler::parseSbf
void parseSbf(const std::shared_ptr< Telegram > &telegram)
Parse SBF block.
Definition: message_handler.cpp:2172
io::MessageHandler::last_dop_
Dop last_dop_
Since GPSFix needs DOP, incoming DOP blocks need to be stored.
Definition: message_handler.hpp:273
evNoPVT
@ evNoPVT
Definition: message_handler.hpp:95
Settings::vehicle_frame_id
std::string vehicle_frame_id
The frame ID of the vehicle frame.
Definition: settings.hpp:363
BaseVectorGeodParser
bool BaseVectorGeodParser(ROSaicNodeBase *node, It it, It itEnd, BaseVectorGeodMsg &msg)
Qi based parser for the SBF block "BaseVectorGeod".
Definition: sbf_blocks.hpp:1070
parsing_utilities::rpyToRot
Eigen::Matrix3d rpyToRot(double roll, double pitch, double yaw)
Definition: parsing_utilities.hpp:101
AimPlusStatusMsg
septentrio_gnss_driver::msg::AIMPlusStatus AimPlusStatusMsg
Definition: typedefs.hpp:122
GprmcParser
Derived class for parsing RMC messages.
Definition: gprmc.hpp:76
io::MessageHandler::last_channelstatus_
ChannelStatus last_channelstatus_
Since GPSFix needs ChannelStatus, incoming ChannelStatus blocks need to be stored.
Definition: message_handler.hpp:262
log_level::DEBUG
@ DEBUG
Definition: typedefs.hpp:180
Dop::vdop
double vdop
Definition: sbf_blocks.hpp:143
VEL_COV_CARTESIAN
@ VEL_COV_CARTESIAN
Definition: message_handler.hpp:120
PosCovCartesianParser
bool PosCovCartesianParser(ROSaicNodeBase *node, It it, It itEnd, PosCovCartesianMsg &msg)
Qi based parser for the SBF block "PosCovCartesian".
Definition: sbf_blocks.hpp:1242
RF_STATUS
@ RF_STATUS
Definition: message_handler.hpp:134
BASE_VECTOR_GEOD
@ BASE_VECTOR_GEOD
Definition: message_handler.hpp:112
GpgsaParser
Derived class for parsing GSA messages.
Definition: gpgsa.hpp:78
VEL_SENSOR_SETUP
@ VEL_SENSOR_SETUP
Definition: message_handler.hpp:130
evPPP
@ evPPP
Definition: message_handler.hpp:104
parsing_utilities::q_ned_ecef
Eigen::Quaterniond q_ned_ecef(double lat, double lon)
Generates the quaternion to rotate from local NED to ECEF.
Definition: parsing_utilities.hpp:408
parsing_utilities::q_enu_ecef
Eigen::Quaterniond q_enu_ecef(double lat, double lon)
Generates the quaternion to rotate from local ENU to ECEF.
Definition: parsing_utilities.hpp:392
io::MessageHandler::last_extsensmeas_
ExtSensorMeasMsg last_extsensmeas_
Since Imu needs ExtSensorMeas, incoming ExtSensorMeas blocks need to be stored.
Definition: message_handler.hpp:256
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:53
parsing_utilities::setQuaternionNaN
void setQuaternionNaN(QuaternionMsg &quaternion)
Definition: parsing_utilities.hpp:371
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_blocks.hpp:1514
VelSensorSetupMsg
septentrio_gnss_driver::msg::VelSensorSetup VelSensorSetupMsg
Definition: typedefs.hpp:154
Settings::publish_navsatfix
bool publish_navsatfix
Whether or not to publish the NavSatFixMsg message.
Definition: settings.hpp:316
EXT_EVENT_INS_NAV_GEOD
@ EXT_EVENT_INS_NAV_GEOD
Definition: message_handler.hpp:127
Settings::publish_tf
bool publish_tf
Whether or not to publish the tf of the localization.
Definition: settings.hpp:332
GpsFixMsg
gps_msgs::msg::GPSFix GpsFixMsg
Definition: typedefs.hpp:113
ChannelStatus::n
uint8_t n
Definition: sbf_blocks.hpp:124
io::MessageHandler::assembleLocalizationUtm
void assembleLocalizationUtm()
"Callback" function when constructing LocalizationMsg messages in UTM
Definition: message_handler.cpp:890
parsing_utilities::deg2radSq
T deg2radSq(T deg)
Definition: parsing_utilities.hpp:83
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_blocks.hpp:904
Settings::publish_measepoch
bool publish_measepoch
Whether or not to publish the MeasEpoch message.
Definition: settings.hpp:267
Settings::publish_exteventinsnavgeod
bool publish_exteventinsnavgeod
Whether or not to publish the ExtEventINSNavGeodMsg message.
Definition: settings.hpp:308
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_blocks.hpp:945
evMovingBaseRTKFloat
@ evMovingBaseRTKFloat
Definition: message_handler.hpp:103
io::MessageHandler::last_receiverstatus_
ReceiverStatus last_receiverstatus_
Since DiagnosticArray needs ReceiverStatus, incoming ReceiverStatus blocks need to be stored.
Definition: message_handler.hpp:285
LocalizationMsg
nav_msgs::msg::Odometry LocalizationMsg
Definition: typedefs.hpp:119
evRTKFloat
@ evRTKFloat
Definition: message_handler.hpp:100
RECEIVER_STATUS
@ RECEIVER_STATUS
Definition: message_handler.hpp:122
ReceiverSetupParser
bool ReceiverSetupParser(ROSaicNodeBase *node, It it, It itEnd, ReceiverSetup &msg)
Qi based parser for the SBF block "ReceiverSetup".
Definition: sbf_blocks.hpp:702
Settings::publish_extsensormeas
bool publish_extsensormeas
Whether or not to publish the ExtSensorMeasMsg message.
Definition: settings.hpp:312
DiagnosticStatusMsg
diagnostic_msgs::msg::DiagnosticStatus DiagnosticStatusMsg
Definition: typedefs.hpp:107
io::MessageHandler::last_velcovgeodetic_
VelCovGeodeticMsg last_velcovgeodetic_
Since GPSFix needs VelCovGeodetic, incoming VelCovGeodetic blocks need to be stored.
Definition: message_handler.hpp:279
io::MessageHandler::node_
ROSaicNodeBase * node_
Pointer to the node.
Definition: message_handler.hpp:202
io::MessageHandler::last_insnavcart_
INSNavCartMsg last_insnavcart_
Since LoclaizationEcef needs INSNavCart, incoming INSNavCart blocks need to be stored.
Definition: message_handler.hpp:250
io::MessageHandler::last_poscovgeodetic_
PosCovGeodeticMsg last_poscovgeodetic_
Since NavSatFix etc. need PosCovGeodetic, incoming PosCovGeodetic blocks need to be stored.
Definition: message_handler.hpp:226
Settings::publish_insnavgeod
bool publish_insnavgeod
Whether or not to publish the INSNavGeodMsg message.
Definition: settings.hpp:302
io::MessageHandler::wait
void wait(Timestamp time_obj)
Waits according to time when reading from file.
Definition: message_handler.cpp:2716
GpgsaMsg
nmea_msgs::msg::Gpgsa GpgsaMsg
Definition: typedefs.hpp:146
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:53
io::MessageHandler::assembleOsnmaDiagnosticArray
void assembleOsnmaDiagnosticArray()
"Callback" function when constructing OSNMA DiagnosticArrayMsg messages
Definition: message_handler.cpp:381
Settings::septentrio_receiver_type
std::string septentrio_receiver_type
Septentrio receiver type, either "gnss" or "ins".
Definition: settings.hpp:340
ROSaicNodeBase::publishTf
void publishTf(const LocalizationMsg &loc)
Publishing function for tf.
Definition: typedefs.hpp:355
log_level::INFO
@ INFO
Definition: typedefs.hpp:181
CHANNEL_STATUS
@ CHANNEL_STATUS
Definition: message_handler.hpp:117
io::MessageHandler::unix_time_
Timestamp unix_time_
Definition: message_handler.hpp:316
Settings::read_from_pcap
bool read_from_pcap
Whether or not we are reading from a PCAP file.
Definition: settings.hpp:371
PVTGeodeticParser
bool PVTGeodeticParser(ROSaicNodeBase *node, It it, It itEnd, PVTGeodeticMsg &msg)
Qi based parser for the SBF block "PVTGeodetic".
Definition: sbf_blocks.hpp:848
io::MessageHandler::last_insnavgeod_
INSNavGeodMsg last_insnavgeod_
Since NavSatFix, GPSFix, Imu and Pose need INSNavGeod, incoming INSNavGeod blocks need to be stored.
Definition: message_handler.hpp:244
io::MessageHandler::hasImuMeas_
bool hasImuMeas_
Definition: message_handler.hpp:374
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:53
DOPParser
bool DOPParser(ROSaicNodeBase *node, It it, It itEnd, Dop &msg)
Qi based parser for the SBF block "DOP".
Definition: sbf_blocks.hpp:488
io::MessageHandler::assembleTwist
void assembleTwist(bool fromIns=false)
"Callback" function when constructing TwistWithCovarianceStampedMsg messages
Definition: message_handler.cpp:675
parsing_utilities::setVector3NaN
void setVector3NaN(Vector3Msg &v)
Definition: parsing_utilities.hpp:379
ParseException
Class to declare error message format when parsing, derived from the public class "std::runtime_error...
Definition: parse_exception.hpp:86
parsing_utilities::deg2rad
T deg2rad(T deg)
Definition: parsing_utilities.hpp:74

septentrio_gnss_driver
Author(s): Tibor Dome, Thomas Emter
autogenerated on Sat May 10 2025 03:03:10