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

septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Sat Mar 11 2023 03:12:55