callback_handlers.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 
32 
39 std::pair<std::string, uint32_t> gpsfix_pairs[] = {
40  std::make_pair("4013", 0), std::make_pair("4027", 1), std::make_pair("4001", 2),
41  std::make_pair("4007", 3), std::make_pair("5906", 4), std::make_pair("5908", 5),
42  std::make_pair("5938", 6), std::make_pair("5939", 7), std::make_pair("4226", 8)};
43 
44 std::pair<std::string, uint32_t> navsatfix_pairs[] = {std::make_pair("4007", 0),
45  std::make_pair("5906", 1),
46  std::make_pair("4226",2)};
47 
48 std::pair<std::string, uint32_t> pose_pairs[] = {
49  std::make_pair("4007", 0), std::make_pair("5906", 1), std::make_pair("5938", 2),
50  std::make_pair("5939", 3), std::make_pair("4226",4)};
51 
52 std::pair<std::string, uint32_t> diagnosticarray_pairs[] = {
53  std::make_pair("4014", 0), std::make_pair("4082", 1)};
54 
55 std::pair<std::string, uint32_t> imu_pairs[] = {
56  std::make_pair("4226", 0), std::make_pair("4050", 1)};
57 
58 std::pair<std::string, uint32_t> localization_pairs[] = {
59  std::make_pair("4226", 0)};
60 
61 namespace io_comm_rx {
63 
75  imu_pairs + 2);
76 
79  localization_pairs + 1);
80 
81  std::string CallbackHandlers::do_gpsfix_ = "4007";
82  std::string CallbackHandlers::do_navsatfix_ = "4007";
83  std::string CallbackHandlers::do_pose_ = "4007";
84  std::string CallbackHandlers::do_diagnostics_ = "4014";
85 
86  std::string CallbackHandlers::do_insgpsfix_ = "4226";
87  std::string CallbackHandlers::do_insnavsatfix_ = "4226";
88  std::string CallbackHandlers::do_inspose_ = "4226";
89  std::string CallbackHandlers::do_imu_ = "4226";
90  std::string CallbackHandlers::do_inslocalization_ = "4226";
91 
95  {
96  // Find the ROS message callback handler for the equivalent Rx message
97  // (SBF/NMEA) at hand & call it
98  boost::mutex::scoped_lock lock(callback_mutex_);
99  CallbackMap::key_type key = rx_message_.messageID();
100  std::string ID_temp = rx_message_.messageID();
101  if (!(ID_temp == "4013" || ID_temp == "4001" ||
102  ID_temp == "4014" || ID_temp == "4082" || ID_temp == "5902"))
103  // We only want to handle ChannelStatus, MeasEpoch, DOP, ReceiverStatus,
104  // QualityInd and ReceiverSetup blocks in case GPSFix and DiagnosticArray
105  // messages are to be published, respectively, see few lines below.
106  {
107  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key);
108  callback != callbackmap_.upper_bound(key); ++callback)
109  {
110  try
111  {
112  callback->second->handle(rx_message_, callback->first);
113  } catch (std::runtime_error& e)
114  {
115  throw std::runtime_error(e.what());
116  }
117  }
118  }
119  // Call NavSatFix callback function if it was added for GNSS
120  if (settings_->septentrio_receiver_type == "gnss")
121  {
123  {
124  CallbackMap::key_type key = "NavSatFix";
125  std::string ID_temp = rx_message_.messageID();
126  if (ID_temp == do_navsatfix_)
127  // The last incoming block PVTGeodetic triggers
128  // the publishing of NavSatFix.
129  {
130  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key);
131  callback != callbackmap_.upper_bound(key); ++callback)
132  {
133  try
134  {
135  callback->second->handle(rx_message_, callback->first);
136  } catch (std::runtime_error& e)
137  {
138  throw std::runtime_error(e.what());
139  }
140  }
141  do_navsatfix_ = std::string();
142  }
143  }
144  }
145  // Call NavSatFix callback function if it was added for INS
146  if (settings_->septentrio_receiver_type == "ins")
147  {
149  {
150  CallbackMap::key_type key = "INSNavSatFix";
151  std::string ID_temp = rx_message_.messageID();
152  if (ID_temp == do_insnavsatfix_)
153  // The last incoming block INSNavGeod triggers
154  // the publishing of NavSatFix.
155  {
156  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key);
157  callback != callbackmap_.upper_bound(key); ++callback)
158  {
159  try
160  {
161  callback->second->handle(rx_message_, callback->first);
162  } catch (std::runtime_error& e)
163  {
164  throw std::runtime_error(e.what());
165  }
166  }
167  do_insnavsatfix_ = std::string();
168  }
169  }
170  }
171  // Call PoseWithCovarianceStampedMsg callback function if it was
172  // added for GNSS
173  if (settings_->septentrio_receiver_type == "gnss")
174  {
175  if (settings_->publish_pose)
176  {
177  CallbackMap::key_type key = "PoseWithCovarianceStamped";
178  std::string ID_temp = rx_message_.messageID();
179  if (ID_temp == do_pose_)
180  // The last incoming block among PVTGeodetic, PosCovGeodetic, AttEuler
181  // and AttCovEuler triggers the publishing of PoseWithCovarianceStamped.
182  {
183  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key);
184  callback != callbackmap_.upper_bound(key); ++callback)
185  {
186  try
187  {
188  callback->second->handle(rx_message_, callback->first);
189  } catch (std::runtime_error& e)
190  {
191  throw std::runtime_error(e.what());
192  }
193  }
194  do_pose_ = std::string();
195  }
196  }
197  }
198  // Call PoseWithCovarianceStampedMsg callback function if it was
199  // added for INS
200  if (settings_->septentrio_receiver_type == "ins")
201  {
202  if (settings_->publish_pose)
203  {
204  CallbackMap::key_type key = "INSPoseWithCovarianceStamped";
205  std::string ID_temp = rx_message_.messageID();
206  if (ID_temp == do_inspose_)
207  // The last incoming block INSNavGeod triggers the publishing of PoseWithCovarianceStamped.
208  {
209  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key);
210  callback != callbackmap_.upper_bound(key); ++callback)
211  {
212  try
213  {
214  callback->second->handle(rx_message_, callback->first);
215  } catch (std::runtime_error& e)
216  {
217  throw std::runtime_error(e.what());
218  }
219  }
220  do_inspose_ = std::string();
221  }
222  }
223  }
224  // Call DiagnosticArrayMsg callback function if it was added
225  // for the both type of receivers
227  {
228  CallbackMap::key_type key1 = rx_message_.messageID();
229  std::string ID_temp = rx_message_.messageID();
230  if (ID_temp == "4014" || ID_temp == "4082" || ID_temp == "5902")
231  {
232  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key1);
233  callback != callbackmap_.upper_bound(key1); ++callback)
234  {
235  try
236  {
237  callback->second->handle(rx_message_, callback->first);
238  } catch (std::runtime_error& e)
239  {
240  throw std::runtime_error(e.what());
241  }
242  }
243  }
244  CallbackMap::key_type key2 = "DiagnosticArray";
245  if (ID_temp == do_diagnostics_)
246  // The last incoming block among ReceiverStatus, QualityInd and
247  // ReceiverSetup triggers the publishing of DiagnosticArray.
248  {
249  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key2);
250  callback != callbackmap_.upper_bound(key2); ++callback)
251  {
252  try
253  {
254  callback->second->handle(rx_message_, callback->first);
255  } catch (std::runtime_error& e)
256  {
257  throw std::runtime_error(e.what());
258  }
259  }
260  do_diagnostics_ = std::string();
261  }
262  }
263  // Call ImuMsg callback function if it was
264  // added for INS
265  if (settings_->septentrio_receiver_type == "ins")
266  {
267  if (settings_->publish_imu)
268  {
269  CallbackMap::key_type key = "Imu";
270  std::string ID_temp = rx_message_.messageID();
271  if (ID_temp == do_imu_)
272  // The last incoming block INSNavGeod triggers the publishing of PoseWithCovarianceStamped.
273  {
274  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key);
275  callback != callbackmap_.upper_bound(key); ++callback)
276  {
277  try
278  {
279  callback->second->handle(rx_message_, callback->first);
280  } catch (std::runtime_error& e)
281  {
282  throw std::runtime_error(e.what());
283  }
284  }
285  do_imu_ = std::string();
286  }
287  }
288  }
289  // Call LocalizationMsg callback function if it was
290  // added for INS
291  if (settings_->septentrio_receiver_type == "ins")
292  {
294  {
295  CallbackMap::key_type key = "Localization";
296  std::string ID_temp = rx_message_.messageID();
297  if (ID_temp == do_inslocalization_)
298  // The last incoming block INSNavGeod triggers the publishing of PoseWithCovarianceStamped.
299  {
300  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key);
301  callback != callbackmap_.upper_bound(key); ++callback)
302  {
303  try
304  {
305  callback->second->handle(rx_message_, callback->first);
306  } catch (std::runtime_error& e)
307  {
308  throw std::runtime_error(e.what());
309  }
310  }
311  do_inslocalization_ = std::string();
312  }
313  }
314  }
315  // Call TimeReferenceMsg (with GPST) callback function if it was
316  // added
317  if (settings_->septentrio_receiver_type == "gnss")
318  {
319  if (settings_->publish_gpst)
320  {
321  CallbackMap::key_type key1 = "GPST";
322  std::string ID_temp = rx_message_.messageID();
323  // If no new PVTGeodetic block is coming in, there is no need to publish
324  // TimeReferenceMsg (with GPST) anew.
325  if (ID_temp == "4007")
326  {
327  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key1);
328  callback != callbackmap_.upper_bound(key1); ++callback)
329  {
330  try
331  {
332  callback->second->handle(rx_message_, callback->first);
333  } catch (std::runtime_error& e)
334  {
335  throw std::runtime_error(e.what());
336  }
337  }
338  }
339  }
340  }
341  if (settings_->septentrio_receiver_type == "ins")
342  {
343  if (settings_->publish_gpst)
344  {
345  CallbackMap::key_type key1 = "GPST";
346  std::string ID_temp = rx_message_.messageID();
347  // If no new INSNavGeod block is coming in, there is no need to publish
348  // TimeReferenceMsg (with GPST) anew.
349  if (ID_temp == "4226")
350  {
351  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key1);
352  callback != callbackmap_.upper_bound(key1); ++callback)
353  {
354  try
355  {
356  callback->second->handle(rx_message_, callback->first);
357  } catch (std::runtime_error& e)
358  {
359  throw std::runtime_error(e.what());
360  }
361  }
362  }
363  }
364  }
365  // Call GPSFix callback function if it was added for GNSS
366  if (settings_->septentrio_receiver_type == "gnss")
367  {
369  {
370  std::string ID_temp = rx_message_.messageID();
371  CallbackMap::key_type key1 = rx_message_.messageID();
372  if (ID_temp == "4013" || ID_temp == "4001")
373  // Even though we are not interested in publishing ChannelStatus (4013)
374  // and DOP (4001) ROS messages, we have to save some contents of these
375  // incoming blocks in order to publish the GPSFix message.
376  {
377  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key1);
378  callback != callbackmap_.upper_bound(key1); ++callback)
379  {
380  try
381  {
382  callback->second->handle(rx_message_, callback->first);
383  } catch (std::runtime_error& e)
384  {
385  throw std::runtime_error(e.what());
386  }
387  }
388  }
389  CallbackMap::key_type key2 = "GPSFix";
390  if (ID_temp == do_gpsfix_)
391  // The last incoming block among ChannelStatus (4013), MeasEpoch (4027),
392  // and DOP (4001) triggers the publishing of GPSFix.
393  {
394  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key2);
395  callback != callbackmap_.upper_bound(key2); ++callback)
396  {
397  try
398  {
399  callback->second->handle(rx_message_, callback->first);
400  } catch (std::runtime_error& e)
401  {
402  throw std::runtime_error(e.what());
403  }
404  }
405  do_gpsfix_ = std::string();
406  }
407  }
408  }
409  // Call GPSFix callback function if it was added for INS
410  if (settings_->septentrio_receiver_type == "ins")
411  {
413  {
414  std::string ID_temp = rx_message_.messageID();
415  CallbackMap::key_type key1 = rx_message_.messageID();
416  if (ID_temp == "4013" || ID_temp == "4001")
417  // Even though we are not interested in publishing ChannelStatus (4013)
418  // and DOP (4001) ROS messages, we have to save some contents of these
419  // incoming blocks in order to publish the GPSFix message.
420  {
421  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key1);
422  callback != callbackmap_.upper_bound(key1); ++callback)
423  {
424  try
425  {
426  callback->second->handle(rx_message_, callback->first);
427  } catch (std::runtime_error& e)
428  {
429  throw std::runtime_error(e.what());
430  }
431  }
432  }
433  CallbackMap::key_type key2 = "INSGPSFix";
434  if (ID_temp == do_insgpsfix_)
435  // The last incoming block among ChannelStatus (4013), MeasEpoch (4027) and
436  // DOP (4001) triggers the publishing of
437  // INSGPSFix.
438  {
439  for (CallbackMap::iterator callback = callbackmap_.lower_bound(key2);
440  callback != callbackmap_.upper_bound(key2); ++callback)
441  {
442  try
443  {
444  callback->second->handle(rx_message_, callback->first);
445  } catch (std::runtime_error& e)
446  {
447  throw std::runtime_error(e.what());
448  }
449  }
450  do_insgpsfix_ = std::string();
451  }
452  }
453  }
454  }
455 
456  void CallbackHandlers::readCallback(Timestamp recvTimestamp, const uint8_t* data, std::size_t& size)
457  {
458  rx_message_.newData(recvTimestamp, data, size);
459  // Read !all! (there might be many) messages in the buffer
460  while (rx_message_.search() != rx_message_.getEndBuffer() &&
461  rx_message_.found())
462  {
463  // Print the found message (if NMEA) or just show messageID (if SBF)..
464  if (rx_message_.isSBF())
465  {
466  std::size_t sbf_block_length;
467  std::string ID_temp = rx_message_.messageID();
468  sbf_block_length =
469  static_cast<std::size_t>(rx_message_.getBlockLength());
470  node_->log(LogLevel::DEBUG, "ROSaic reading SBF block " + ID_temp + " made up of " +
471  std::to_string(sbf_block_length) + " bytes...");
472  // If full message did not yet arrive, throw an error message.
473  if (sbf_block_length > rx_message_.getCount())
474  {
476  "Not a valid SBF block, parts of the SBF block are yet to be received. Ignore..");
477  throw(
478  static_cast<std::size_t>(rx_message_.getPosBuffer() - data));
479  }
480  if (settings_->septentrio_receiver_type == "gnss")
481  {
482  if (settings_->publish_gpsfix == true &&
483  (ID_temp == "4013" || ID_temp == "4027" || ID_temp == "4001" ||
484  ID_temp == "4007" || ID_temp == "5906" || ID_temp == "5908" ||
485  ID_temp == "5938" || ID_temp == "5939"))
486  {
488  {
489  do_gpsfix_ = ID_temp;
490  }
491  }
492  }
493  if (settings_->septentrio_receiver_type == "ins")
494  {
495  if (settings_->publish_gpsfix == true &&
496  (ID_temp == "4013" || ID_temp == "4027" || ID_temp == "4001" || ID_temp == "4226"))
497  {
499  {
500  do_insgpsfix_ = ID_temp;
501  }
502  }
503  }
504  if (settings_->septentrio_receiver_type == "gnss")
505  {
506  if (settings_->publish_navsatfix == true &&
507  (ID_temp == "4007" || ID_temp == "5906"))
508  {
510  {
511  do_navsatfix_ = ID_temp;
512  }
513  }
514  }
515  if (settings_->septentrio_receiver_type == "ins")
516  {
517  if (settings_->publish_navsatfix == true &&
518  (ID_temp == "4226"))
519  {
521  {
522  do_insnavsatfix_ = ID_temp;
523  }
524  }
525  }
526  if (settings_->septentrio_receiver_type == "gnss")
527  {
528  if (settings_->publish_pose == true &&
529  (ID_temp == "4007" || ID_temp == "5906" || ID_temp == "5938" ||
530  ID_temp == "5939"))
531  {
533  {
534  do_pose_ = ID_temp;
535  }
536  }
537  }
538  if (settings_->septentrio_receiver_type == "ins")
539  {
540  if (settings_->publish_pose == true &&
541  (ID_temp == "4226"))
542  {
543  if (rx_message_.ins_pose_complete(pose_map[ID_temp]))
544  {
545  do_inspose_ = ID_temp;
546  }
547  }
548  }
549  if (settings_->publish_diagnostics == true &&
550  (ID_temp == "4014" || ID_temp == "4082"))
551  {
553  {
554  do_diagnostics_ = ID_temp;
555  }
556  }
558  (ID_temp == "4226"))
559  {
561  {
562  do_inslocalization_ = ID_temp;
563  }
564  }
565  }
566  if (rx_message_.isNMEA())
567  {
568  boost::char_separator<char> sep("\r"); // Carriage Return (CR)
569  typedef boost::tokenizer<boost::char_separator<char>> tokenizer;
570  std::size_t nmea_size = rx_message_.messageSize();
571  // Syntax: new_string_name (const char* s, size_t n); size_t is
572  // either 2 or 8 bytes, depending on your system
573  std::string block_in_string(
574  reinterpret_cast<const char*>(rx_message_.getPosBuffer()),
575  nmea_size);
576  tokenizer tokens(block_in_string, sep);
577  node_->log(LogLevel::DEBUG, "The NMEA message contains " + std::to_string(nmea_size) +
578  " bytes and is ready to be parsed. It reads: " + *tokens.begin());
579  }
580  if (rx_message_.isResponse()) // If the response is not sent at once, only
581  // first part is ROS_DEBUG-printed
582  {
583  std::size_t response_size = rx_message_.messageSize();
584  std::string block_in_string(
585  reinterpret_cast<const char*>(rx_message_.getPosBuffer()),
586  response_size);
587  node_->log(LogLevel::DEBUG, "The Rx's response contains " + std::to_string(response_size) +
588  " bytes and reads:\n " + block_in_string);
589  {
590  boost::mutex::scoped_lock lock(g_response_mutex);
591  g_response_received = true;
592  lock.unlock();
593  g_response_condition.notify_one();
594  }
596  {
597  node_->log(LogLevel::ERROR, "Invalid command just sent to the Rx! The Rx's response contains " +
598  std::to_string(response_size) + " bytes and reads:\n " + block_in_string);
599  }
600  continue;
601  }
603  {
604  std::string cd(
605  reinterpret_cast<const char*>(rx_message_.getPosBuffer()), 4);
606  g_rx_tcp_port = cd;
607  if (g_cd_count == 0)
608  {
609  node_->log(LogLevel::INFO, "The connection descriptor for the TCP connection is " + cd);
610  }
611  if (g_cd_count < 3)
612  ++g_cd_count;
613  if (g_cd_count == 2)
614  {
615  boost::mutex::scoped_lock lock(g_cd_mutex);
616  g_cd_received = true;
617  lock.unlock();
618  g_cd_condition.notify_one();
619  }
620  continue;
621  }
622  try
623  {
624  handle();
625  } catch (std::runtime_error& e)
626  {
627  node_->log(LogLevel::DEBUG, "Incomplete message: " + std::string(e.what()));
628  throw(static_cast<std::size_t>(rx_message_.getPosBuffer() - data));
629  }
630  }
631  }
632 } // namespace io_comm_rx
bool gnss_gpsfix_complete(uint32_t id)
Wether all blocks from GNSS have arrived for GpsFix Message.
bool isResponse()
Determines whether data_ currently points to an NMEA message.
const uint8_t * search()
Searches the buffer for the beginning of the next message (NMEA or SBF)
std::unordered_map< std::string, uint32_t > PoseWithCovarianceStampedMap
RxMessage rx_message_
RxMessage parser.
bool gnss_pose_complete(uint32_t id)
Wether all blocks from GNSS have arrived for Pose Message.
bool publish_tf
Whether or not to publish the tf of the localization.
Definition: rx_message.hpp:300
std::string messageID()
void readCallback(Timestamp recvTimestamp, const uint8_t *data, std::size_t &size)
Searches for Rx messages that could potentially be decoded/parsed/published.
bool ins_gpsfix_complete(uint32_t id)
Wether all blocks from INS have arrived for GpsFix Message.
std::pair< std::string, uint32_t > diagnosticarray_pairs[]
std::pair< std::string, uint32_t > navsatfix_pairs[]
static LocalizationMap localization_map
void log(LogLevel logLevel, const std::string &s)
Log function to provide abstraction of ROS loggers.
Definition: typedefs.hpp:208
uint64_t Timestamp
Definition: typedefs.hpp:77
boost::mutex g_response_mutex
Mutex to control changes of global variable "g_response_received".
bool isNMEA()
Determines whether data_ currently points to an NMEA message.
const uint8_t * getPosBuffer()
Gets the current position in the read buffer.
bool publish_gpst
Whether or not to publish the TimeReferenceMsg message with GPST.
Definition: rx_message.hpp:286
bool diagnostics_complete(uint32_t id)
Wether all blocks have arrived for Diagnostics Message.
static boost::mutex callback_mutex_
void handle()
Called every time rx_message is found to contain some potentially useful message. ...
static std::string do_inslocalization_
void newData(Timestamp recvTimestamp, const uint8_t *data, std::size_t &size)
Put new data.
Definition: rx_message.hpp:478
bool gnss_navsatfix_complete(uint32_t id)
Wether all blocks from GNSS have arrived for NavSatFix Message.
bool isSBF()
Determines whether data_ currently points to an SBF block.
bool g_response_received
Determines whether a command reply was received from the Rx.
uint32_t g_cd_count
bool ins_pose_complete(uint32_t id)
Wether all blocks from INS have arrived for Pose Message.
bool ins_navsatfix_complete(uint32_t id)
Wether all blocks from INS have arrived for NavSatFix Message.
std::unordered_map< std::string, uint32_t > LocalizationMap
std::pair< std::string, uint32_t > localization_pairs[]
boost::mutex g_cd_mutex
Mutex to control changes of global variable "g_cd_received".
bool ins_localization_complete(uint32_t id)
Wether all blocks have arrived for Localization Message.
std::size_t messageSize()
std::string g_rx_tcp_port
Rx TCP port, e.g. IP10 or IP11, to which ROSaic is connected to.
std::size_t getCount()
Returns the count_ variable.
Definition: rx_message.hpp:516
bool publish_localization
Whether or not to publish the LocalizationMsg message.
Definition: rx_message.hpp:298
ROSaicNodeBase * node_
Pointer to Node.
bool g_cd_received
Determines whether the connection descriptor was received from the Rx.
std::string septentrio_receiver_type
Septentrio receiver type, either "gnss" or "ins".
Definition: rx_message.hpp:302
std::pair< std::string, uint32_t > gpsfix_pairs[]
uint16_t getBlockLength()
Gets the length of the SBF block.
static DiagnosticArrayMap diagnosticarray_map
bool publish_gpsfix
Whether or not to publish the GPSFixMsg message.
Definition: rx_message.hpp:290
boost::condition_variable g_cd_condition
Condition variable complementing "g_cd_mutex".
bool publish_imu
Whether or not to publish the ImuMsg message.
Definition: rx_message.hpp:296
std::unordered_map< std::string, uint32_t > ImuMap
const uint8_t * getEndBuffer()
Gets the end position in the read buffer.
boost::condition_variable g_response_condition
Condition variable complementing "g_response_mutex".
std::unordered_map< std::string, uint32_t > GPSFixMap
std::unordered_map< std::string, uint32_t > NavSatFixMap
Handles callbacks when reading NMEA/SBF messages.
std::pair< std::string, uint32_t > pose_pairs[]
std::unordered_map< std::string, uint32_t > DiagnosticArrayMap
void callback(const sensor_msgs::NavSatFixConstPtr &fix)
bool publish_diagnostics
Whether or not to publish the DiagnosticArrayMsg message.
Definition: rx_message.hpp:294
bool found()
Has an NMEA message, SBF block or command reply been found in the buffer?
bool publish_pose
Whether or not to publish the PoseWithCovarianceStampedMsg message.
Definition: rx_message.hpp:292
static PoseWithCovarianceStampedMap pose_map
bool publish_navsatfix
Whether or not to publish the NavSatFixMsg message.
Definition: rx_message.hpp:288
std::pair< std::string, uint32_t > imu_pairs[]


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