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

septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Wed Nov 22 2023 04:04:27