communication_core.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 <chrono>
32 
33 // Boost includes
34 #include <boost/regex.hpp>
35 
38 
39 #ifndef ANGLE_MAX
40 #define ANGLE_MAX 180
41 #endif
42 
43 #ifndef ANGLE_MIN
44 #define ANGLE_MIN -180
45 #endif
46 
47 #ifndef THETA_Y_MAX
48 #define THETA_Y_MAX 90
49 #endif
50 
51 #ifndef THETA_Y_MIN
52 #define THETA_Y_MIN -90
53 #endif
54 
55 #ifndef LEVER_ARM_MAX
56 #define LEVER_ARM_MAX 100
57 #endif
58 
59 #ifndef LEVER_ARM_MIN
60 #define LEVER_ARM_MIN -100
61 #endif
62 
63 #ifndef HEADING_MAX
64 #define HEADING_MAX 360
65 #endif
66 
67 #ifndef HEADING_MIN
68 #define HEADING_MIN -360
69 #endif
70 
71 #ifndef PITCH_MAX
72 #define PITCH_MAX 90
73 #endif
74 
75 #ifndef PITCH_MIN
76 #define PITCH_MIN -90
77 #endif
78 
79 #ifndef ATTSTD_DEV_MIN
80 #define ATTSTD_DEV_MIN 0
81 #endif
82 
83 #ifndef ATTSTD_DEV_MAX
84 #define ATTSTD_DEV_MAX 5
85 #endif
86 
87 #ifndef POSSTD_DEV_MIN
88 #define POSSTD_DEV_MIN 0
89 #endif
90 
91 #ifndef POSSTD_DEV_MAX
92 #define POSSTD_DEV_MAX 100
93 #endif
94 
101 boost::mutex g_response_mutex;
106 boost::condition_variable g_response_condition;
108 boost::mutex g_cd_mutex;
112 boost::condition_variable g_cd_condition;
117 std::string g_rx_tcp_port;
120 uint32_t g_cd_count;
121 
123  node_(node),
124  handlers_(node, settings),
125  settings_(settings),
126  stopping_(false)
127 {
128  g_response_received = false;
129  g_cd_received = false;
130  g_read_cd = true;
131  g_cd_count = 0;
132 }
133 
135 {
136  node_->log(LogLevel::DEBUG, "Called initializeIO() method");
137  boost::smatch match;
138  // In fact: smatch is a typedef of match_results<string::const_iterator>
139  if (boost::regex_match(settings_->device, match, boost::regex("(tcp)://(.+):(\\d+)")))
140  // C++ needs \\d instead of \d: Both mean decimal.
141  // Note that regex_match can be used with a smatch object to store results, or
142  // without. In any case, true is returned if and only if it matches the
143  // !complete! string.
144  {
145  // The first sub_match (index 0) contained in a match_result always
146  // represents the full match within a target sequence made by a regex, and
147  // subsequent sub_matches represent sub-expression matches corresponding in
148  // sequence to the left parenthesis delimiting the sub-expression in the
149  // regex, i.e. $n Perl is equivalent to m[n] in boost regex.
150  tcp_host_ = match[2];
151  tcp_port_ = match[3];
152 
153  serial_ = false;
154  settings_->read_from_sbf_log = false;
155  settings_->read_from_pcap = false;
156  connectionThread_.reset(new boost::thread (boost::bind(&Comm_IO::connect, this)));
157  } else if (boost::regex_match(settings_->device, match,
158  boost::regex("(file_name):(/|(?:/[\\w-]+)+.sbf)")))
159  {
160  serial_ = false;
162  settings_->read_from_pcap = false;
163  settings_->use_gnss_time = true;
164  connectionThread_.reset(new boost::thread (
165  boost::bind(&Comm_IO::prepareSBFFileReading, this, match[2])));
166 
167  } else if (boost::regex_match(
168  settings_->device, match,
169  boost::regex("(file_name):(/|(?:/[\\w-]+)+.pcap)")))
170  {
171  serial_ = false;
172  settings_->read_from_sbf_log = false;
173  settings_->read_from_pcap = true;
174  settings_->use_gnss_time = true;
175  connectionThread_.reset(new boost::thread (
176  boost::bind(&Comm_IO::preparePCAPFileReading, this, match[2])));
177 
178  } else if (boost::regex_match(settings_->device, match, boost::regex("(serial):(.+)")))
179  {
180  serial_ = true;
181  settings_->read_from_sbf_log = false;
182  settings_->read_from_pcap = false;
183  std::string proto(match[2]);
184  std::stringstream ss;
185  ss << "Searching for serial port" << proto;
186  settings_->device = proto;
187  node_->log(LogLevel::DEBUG, ss.str());
188  connectionThread_.reset(new boost::thread (boost::bind(&Comm_IO::connect, this)));
189  } else
190  {
191  std::stringstream ss;
192  ss << "Device is unsupported. Perhaps you meant 'tcp://host:port' or 'file_name:xxx.sbf' or 'serial:/path/to/device'?";
193  node_->log(LogLevel::ERROR, ss.str());
194  }
195  node_->log(LogLevel::DEBUG, "Leaving initializeIO() method");
196 }
197 
199 {
200  try
201  {
202  std::stringstream ss;
203  ss << "Setting up everything needed to read from" << file_name;
204  node_->log(LogLevel::DEBUG, ss.str());
205  initializeSBFFileReading(file_name);
206  } catch (std::runtime_error& e)
207  {
208  std::stringstream ss;
209  ss << "Comm_IO::initializeSBFFileReading() failed for SBF File" << file_name
210  << " due to: " << e.what();
211  node_->log(LogLevel::ERROR, ss.str());
212  }
213 }
214 
216 {
217  try
218  {
219  std::stringstream ss;
220  ss << "Setting up everything needed to read from " << file_name;
221  node_->log(LogLevel::DEBUG, ss.str());
222  initializePCAPFileReading(file_name);
223  } catch (std::runtime_error& e)
224  {
225  std::stringstream ss;
226  ss << "CommIO::initializePCAPFileReading() failed for SBF File " << file_name
227  << " due to: " << e.what();
228  node_->log(LogLevel::ERROR, ss.str());
229  }
230 }
231 
233 {
234  node_->log(LogLevel::DEBUG, "Called connect() method");
236  "Started timer for calling reconnect() method until connection succeeds");
237 
238  boost::asio::io_service io;
239  boost::posix_time::millisec wait_ms(static_cast<uint32_t>(settings_->reconnect_delay_s * 1000));
240  while (!connected_ && !stopping_)
241  {
242  boost::asio::deadline_timer t(io, wait_ms);
243  reconnect();
244  t.wait();
245  }
246  node_->log(LogLevel::DEBUG, "Successully connected. Leaving connect() method");
247 }
248 
253 {
254  node_->log(LogLevel::DEBUG, "Called reconnect() method");
255  if (serial_)
256  {
257  bool initialize_serial_return = false;
258  try
259  {
260  node_->log(LogLevel::INFO, "Connecting serially to device" + settings_->device +
261  ", targeted baudrate: " + std::to_string(settings_->baudrate));
262  initialize_serial_return =
264  } catch (std::runtime_error& e)
265  {
266  {
267  std::stringstream ss;
268  ss << "initializeSerial() failed for device " << settings_->device
269  << " due to: " << e.what();
270  node_->log(LogLevel::ERROR, ss.str());
271  }
272  }
273  if (initialize_serial_return)
274  {
275  boost::mutex::scoped_lock lock(connection_mutex_);
276  connected_ = true;
277  lock.unlock();
278  connection_condition_.notify_one();
279  }
280  } else
281  {
282  bool initialize_tcp_return = false;
283  try
284  {
285  node_->log(LogLevel::INFO, "Connecting to tcp://" + tcp_host_ + ":" + tcp_port_ +
286  "...");
287  initialize_tcp_return = initializeTCP(tcp_host_, tcp_port_);
288  } catch (std::runtime_error& e)
289  {
290  {
291  std::stringstream ss;
292  ss << "initializeTCP() failed for host " << tcp_host_
293  << " on port " << tcp_port_ << " due to: " << e.what();
294  node_->log(LogLevel::ERROR, ss.str());
295  }
296  }
297  if (initialize_tcp_return)
298  {
299  boost::mutex::scoped_lock lock(connection_mutex_);
300  connected_ = true;
301  lock.unlock();
302  connection_condition_.notify_one();
303  }
304  }
305  node_->log(LogLevel::DEBUG, "Leaving reconnect() method");
306 }
307 
316 {
317  node_->log(LogLevel::DEBUG, "Called configureRx() method");
318  {
319  // wait for connection
320  boost::mutex::scoped_lock lock(connection_mutex_);
321  connection_condition_.wait(lock, [this]() { return connected_; });
322  }
323 
324 
325 
326  // Determining communication mode: TCP vs USB/Serial
327  unsigned stream = 1;
328  boost::smatch match;
329  boost::regex_match(settings_->device, match, boost::regex("(tcp)://(.+):(\\d+)"));
330  std::string proto(match[1]);
331  std::string rx_port;
332  if (proto == "tcp")
333  {
334  // It is imperative to hold a lock on the mutex "g_cd_mutex" while
335  // modifying the variable and "g_cd_received".
336  boost::mutex::scoped_lock lock_cd(g_cd_mutex);
337  // Escape sequence (escape from correction mode), ensuring that we can send
338  // our real commands afterwards...
339  std::string cmd("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D");
340  manager_.get()->send(cmd, cmd.size());
341  // We wait for the connection descriptor before we send another command,
342  // otherwise the latter would not be processed.
343  g_cd_condition.wait(lock_cd, []() { return g_cd_received; });
344  g_cd_received = false;
345  rx_port = g_rx_tcp_port;
346  } else
347  {
348  rx_port = settings_->rx_serial_port;
349  // After booting, the Rx sends the characters "x?" to all ports, which could
350  // potentially mingle with our first command. Hence send a safeguard command
351  // "lif", whose potentially false processing is harmless.
352  send("lif, Identification \x0D");
353  }
354 
355  std::string pvt_interval;
356  if (settings_->polling_period_pvt == 0)
357  {
358  pvt_interval = "OnChange";
359  }
360  else
361  {
362  uint32_t rx_period_pvt =
364  std::string pvt_sec_or_msec;
365  if (settings_->polling_period_pvt == 1000 || settings_->polling_period_pvt == 2000 ||
367  pvt_sec_or_msec = "sec";
368  else
369  pvt_sec_or_msec = "msec";
370 
371  pvt_interval = pvt_sec_or_msec + std::to_string(rx_period_pvt);
372  }
373 
374  std::string rest_interval;
375  {
376  uint32_t rx_period_rest =
378  std::string rest_sec_or_msec;
379  if (settings_->polling_period_rest == 1000 || settings_->polling_period_rest == 2000 ||
381  rest_sec_or_msec = "sec";
382  else
383  rest_sec_or_msec = "msec";
384 
385  rest_interval = rest_sec_or_msec + std::to_string(rx_period_rest);
386  }
387 
388  // Credentials for login
389  if (!settings_->login_user.empty() &&
390  !settings_->login_password.empty())
391  send("login, " + settings_->login_user + ", " + settings_->login_password + " \x0D");
392 
393  // Turning off all current SBF/NMEA output
394  send("sso, all, none, none, off \x0D");
395  send("sno, all, none, none, off \x0D");
396 
397  // Activate NTP server
399  send("sntp, on \x0D");
400 
401  // Setting the datum to be used by the Rx (not the NMEA output though, which only
402  // provides MSL and undulation (by default with respect to WGS84), but not
403  // ellipsoidal height)
404  {
405  std::stringstream ss;
406  ss << "sgd, " << settings_->datum << "\x0D";
407  send(ss.str());
408  }
409 
410  // Setting up SBF blocks with rx_period_pvt
411  {
412  std::stringstream blocks;
414  {
415  blocks << " +PVTCartesian";
416  }
421  {
422  blocks << " +PVTGeodetic";
423  }
425  {
426  blocks << " +PosCovCartesian";
427  }
432  {
433  blocks << " +PosCovGeodetic";
434  }
437  {
438  blocks << " +VelCovGeodetic";
439  }
443  {
444  blocks << " +AttEuler";
445  }
449  {
450  blocks << " +AttCovEuler";
451  }
454  {
455  blocks << " +MeasEpoch";
456  }
458  {
459  blocks << " +ChannelStatus +DOP";
460  }
461  // Setting SBF output of Rx depending on the receiver type
462  // If INS then...
463  if (settings_->septentrio_receiver_type == "ins")
464  {
466  {
467  blocks << " +INSNavCart";
468  }
476  {
477  blocks << " +INSNavGeod";
478  }
480  {
481  blocks << " +ExtEventINSNavGeod";
482  }
484  {
485  blocks << " +ExtEventINSNavCart";
486  }
489  {
490  blocks << " +ExtSensorMeas";
491  }
492  }
493  std::stringstream ss;
494  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port
495  << "," << blocks.str() << ", " << pvt_interval
496  << "\x0D";
497  send(ss.str());
498  ++stream;
499  }
500  // Setting up SBF blocks with rx_period_rest
501  {
502  std::stringstream blocks;
503  if (settings_->septentrio_receiver_type == "ins")
504  {
506  {
507  blocks << " +IMUSetup";
508  }
510  {
511  blocks << " +VelSensorSetup";
512  }
513  }
515  {
516  blocks << " +ReceiverStatus +QualityInd +ReceiverSetup";
517  }
518 
519  std::stringstream ss;
520  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port
521  << "," << blocks.str() << ", " << rest_interval
522  << "\x0D";
523  send(ss.str());
524  ++stream;
525  }
526 
527  // Setting up NMEA streams
528  {
529  std::stringstream ss;
530 
531  ss << "snti, GP" << "\x0D";
532  send(ss.str());
533  }
535  {
536  std::stringstream ss;
537 
538  ss << "sno, Stream" << std::to_string(stream) << ", " << rx_port << ", GGA, "
539  << pvt_interval << "\x0D";
540  send(ss.str());
541  ++stream;
542  }
544  {
545  std::stringstream ss;
546 
547  ss << "sno, Stream" << std::to_string(stream) << ", " << rx_port << ", RMC, "
548  << pvt_interval << "\x0D";
549  send(ss.str());
550  ++stream;
551  }
553  {
554  std::stringstream ss;
555 
556  ss << "sno, Stream" << std::to_string(stream) << ", " << rx_port << ", GSA, "
557  << pvt_interval << "\x0D";
558  send(ss.str());
559  ++stream;
560  }
562  {
563  std::stringstream ss;
564 
565  ss << "sno, Stream" << std::to_string(stream) << ", " << rx_port << ", GSV, "
566  << rest_interval << "\x0D";
567  send(ss.str());
568  ++stream;
569  }
570 
571  if (settings_->septentrio_receiver_type == "gnss")
572  {
573  // Setting the marker-to-ARP offsets. This comes after the "sso, ...,
574  // ReceiverSetup, ..." command, since the latter is only generated when a
575  // user-command is entered to change one or more values in the block.
576  {
577  std::stringstream ss;
578  ss << "sao, Main, " << string_utilities::trimString(std::to_string(settings_->delta_e))
579  << ", " << string_utilities::trimString(std::to_string(settings_->delta_n)) << ", "
580  << string_utilities::trimString(std::to_string(settings_->delta_u)) << ", \""
581  << settings_->ant_type << "\", " << settings_->ant_serial_nr << "\x0D";
582  send(ss.str());
583  }
584 
585  // Configure Aux1 antenna
586  {
587  std::stringstream ss;
588  ss << "sao, Aux1, " << string_utilities::trimString(std::to_string(0.0))
589  << ", " << string_utilities::trimString(std::to_string(0.0)) << ", "
590  << string_utilities::trimString(std::to_string(0.0)) << ", \""
591  << settings_->ant_aux1_type << "\", " << settings_->ant_aux1_serial_nr << "\x0D";
592  send(ss.str());
593  }
594  }
595  else if (settings_->septentrio_receiver_type == "ins")
596  {
597  {
598  std::stringstream ss;
599  ss << "sat, Main, \"" << settings_->ant_type << "\"" << "\x0D";
600  send(ss.str());
601  }
602 
603  // Configure Aux1 antenna
604  {
605  std::stringstream ss;
606  ss << "sat, Aux1, \"" << settings_->ant_type << "\"" << "\x0D";
607  send(ss.str());
608  }
609  }
610 
611  // Configuring the NTRIP connection
612  // First disable any existing NTRIP connection on NTR1
613  {
614  std::stringstream ss;
615  ss << "snts, NTR1, off \x0D";
616  send(ss.str());
617  }
619  {
620  if (settings_->ntrip_mode == "off")
621  {
622  } else if (settings_->ntrip_mode == "Client")
623  {
624  {
625  std::stringstream ss;
626  ss << "snts, NTR1, " << settings_->ntrip_mode << ", " << settings_->caster << ", "
627  << std::to_string(settings_->caster_port) << ", " << settings_->ntrip_username << ", "
629  << ", " << settings_->send_gga << " \x0D";
630  send(ss.str());
631  }
632  } else if (settings_->ntrip_mode == "Client-Sapcorda")
633  {
634  {
635  std::stringstream ss;
636  ss << "snts, NTR1, Client-Sapcorda, , , , , , , , \x0D";
637  send(ss.str());
638  }
639  } else
640  {
641  node_->log(LogLevel::ERROR, "Invalid mode specified for NTRIP settings_->");
642  }
643  } else // Since the Rx does not have internet (and you will not be able to share
644  // it via USB),
645  // we need to forward the corrections ourselves, though not on the same
646  // port.
647  {
648  if (proto == "tcp")
649  {
650  {
651  std::stringstream ss;
652  // In case IPS1 was used before, old configuration is lost of course.
653  ss << "siss, IPS1, " << std::to_string(settings_->rx_input_corrections_tcp)
654  << ", TCP2Way \x0D";
655  send(ss.str());
656  }
657  {
658  std::stringstream ss;
659  ss << "sno, Stream" << std::to_string(stream) << ", IPS1, GGA, "
660  << pvt_interval << " \x0D";
661  ++stream;
662  send(ss.str());
663  }
664  }
665  {
666  std::stringstream ss;
667  if (proto == "tcp")
668  {
669  ss << "sdio, IPS1, " << settings_->rtcm_version << ", +SBF+NMEA \x0D";
670  } else
671  {
672  ss << "sdio, " << settings_->rx_input_corrections_serial << ", "
673  << settings_->rtcm_version << ", +SBF+NMEA \x0D";
674  }
675  send(ss.str());
676  }
677  }
678 
679  // Setting multi antenna
681  {
682  send("sga, MultiAntenna \x0D");
683  }
684  else
685  {
686  send("sga, none \x0D");
687  }
688 
689  // Setting the Attitude Determination
690  {
692  {
693  std::stringstream ss;
696  send(ss.str());
697  }
698  else
699  {
700  node_->log(LogLevel::ERROR, "Please specify a valid parameter for heading and pitch");
701  }
702  }
703 
704  // Setting the INS-related commands
705  if (settings_->septentrio_receiver_type == "ins")
706  {
707  // IMU orientation
708  {
709  std::stringstream ss;
712  {
713  ss << " sio, " << "manual" << ", " << string_utilities::trimString(std::to_string(settings_->theta_x)) << ", "
714  << string_utilities::trimString(std::to_string(settings_->theta_y)) << ", "
715  << string_utilities::trimString(std::to_string(settings_->theta_z)) << " \x0D";
716  send(ss.str());
717  }
718  else
719  {
720  node_->log(LogLevel::ERROR, "Please specify a correct value for IMU orientation angles");
721  }
722 
723  }
724 
725  // Setting the INS antenna lever arm offset
726  {
729  {
730  std::stringstream ss;
731  ss << "sial, " << string_utilities::trimString(std::to_string(settings_->ant_lever_x))
732  << ", " << string_utilities::trimString(std::to_string(settings_->ant_lever_y)) << ", "
733  << string_utilities::trimString(std::to_string(settings_->ant_lever_z)) << " \x0D";
734  send(ss.str());
735  }
736  else
737  {
738  node_->log(LogLevel::ERROR, "Please specify a correct value for x, y and z in the config file under ant_lever_arm");
739  }
740  }
741 
742  // Setting the user defined point offset
743  {
746  {
747  std::stringstream ss;
748  ss << "sipl, POI1, " << string_utilities::trimString(std::to_string(settings_->poi_x))
749  << ", " << string_utilities::trimString(std::to_string(settings_->poi_y)) << ", "
750  << string_utilities::trimString(std::to_string(settings_->poi_z)) << " \x0D";
751  send(ss.str());
752  }
753  else
754  {
755  node_->log(LogLevel::ERROR, "Please specify a correct value for poi_x, poi_y and poi_z in the config file under poi_lever_arm");
756  }
757  }
758 
759  // Setting the Velocity sensor lever arm offset
760  {
763  {
764  std::stringstream ss;
765  ss << "sivl, VSM1, " << string_utilities::trimString(std::to_string(settings_->vsm_x))
766  << ", " << string_utilities::trimString(std::to_string(settings_->vsm_y)) << ", "
767  << string_utilities::trimString(std::to_string(settings_->vsm_z)) << " \x0D";
768  send(ss.str());
769  }
770  else
771  {
772  node_->log(LogLevel::ERROR, "Please specify a correct value for vsm_x, vsm_y and vsm_z in the config file under vel_sensor_lever_arm");
773  }
774 
775  }
776 
777  // Setting the INS Solution Reference Point: MainAnt or POI1
778  // First disable any existing INS sub-block connection
779  {
780  std::stringstream ss;
781  ss << "sinc, off, all, MainAnt \x0D";
782  send(ss.str());
783  }
784 
785  // INS solution reference point
786  {
787  std::stringstream ss;
788  if (settings_->ins_use_poi)
789  {
790  ss << "sinc, on, all, " << "POI1" << " \x0D";
791  send(ss.str());
792  }
793  else
794  {
795  ss << "sinc, on, all, " << "MainAnt" << " \x0D";
796  send(ss.str());
797  }
798  }
799 
800  // Setting the INS heading
801  {
802  std::stringstream ss;
803  if (settings_->ins_initial_heading == "auto")
804  {
805  ss << "siih, " << settings_->ins_initial_heading << " \x0D";
806  send(ss.str());
807  }
808  else if (settings_->ins_initial_heading == "stored")
809  {
810  ss << "siih, " << settings_->ins_initial_heading << " \x0D";
811  send(ss.str());
812  }
813  else
814  {
815  node_->log(LogLevel::ERROR, "Invalid mode specified for ins_initial_heading.");
816  }
817  }
818 
819  // Setting the INS navigation filter
820  {
823  {
824  std::stringstream ss;
825  ss << "sism, " << string_utilities::trimString(std::to_string(settings_->att_std_dev)) << ", " << string_utilities::trimString(std::to_string(settings_->pos_std_dev))
826  << " \x0D";
827  send(ss.str());
828  }
829  else
830  {
831  node_->log(LogLevel::ERROR, "Please specify a valid AttStsDev and PosStdDev");
832  }
833  }
834  }
835  node_->log(LogLevel::DEBUG, "Leaving configureRx() method");
836 }
837 
844 {
845  node_->log(LogLevel::DEBUG, "Called defineMessages() method");
846 
848  {
850  handlers_.insert<GpggaMsg>("$GPGGA");
851  }
853  {
855  handlers_.insert<GprmcMsg>("$GPRMC");
856  }
858  {
860  handlers_.insert<GpgsaMsg>("$GPGSA");
861  }
863  {
865  handlers_.insert<GpgsvMsg>("$GPGSV");
867  handlers_.insert<GpgsvMsg>("$GLGSV");
869  handlers_.insert<GpgsvMsg>("$GAGSV");
871  handlers_.insert<GpgsvMsg>("$GBGSV");
872  }
874  {
877  }
882  {
885  }
887  {
890  }
895  {
898  }
901  {
904  }
908  {
910  handlers_.insert<AttEulerMsg>("5938");
911  }
915  {
918  }
921  {
923  handlers_.insert<int32_t>("4027"); // MeasEpoch block
924  }
925 
926  // INS-related SBF blocks
928  {
930  handlers_.insert<INSNavCartMsg>("4225");
931  }
939  {
941  handlers_.insert<INSNavGeodMsg>("4226");
942  }
944  {
946  handlers_.insert<IMUSetupMsg>("4224");
947  }
950  {
953  }
955  {
957  handlers_.insert<INSNavGeodMsg>("4230");
958  }
960  {
963  }
965  {
967  handlers_.insert<INSNavCartMsg>("4229");
968  }
969  if (settings_->publish_gpst)
970  {
971  handlers_.callbackmap_ = handlers_.insert<int32_t>("GPST");
972  }
973  if (settings_->septentrio_receiver_type == "gnss")
974  {
976  {
978  handlers_.insert<NavSatFixMsg>("NavSatFix");
979  }
980  }
981  if (settings_->septentrio_receiver_type == "ins")
982  {
984  {
986  handlers_.insert<NavSatFixMsg>("INSNavSatFix");
987  }
988  }
989  if (settings_->septentrio_receiver_type == "gnss")
990  {
992  {
994  handlers_.insert<GPSFixMsg>("GPSFix");
995  // The following blocks are never published, yet are needed for the
996  // construction of the GPSFix message, hence we have empty callbacks.
998  handlers_.insert<int32_t>("4013"); // ChannelStatus block
1000  handlers_.insert<int32_t>("4001"); // DOP block
1001  }
1002  }
1003  if (settings_->septentrio_receiver_type == "ins")
1004  {
1006  {
1008  handlers_.insert<GPSFixMsg>("INSGPSFix");
1010  handlers_.insert<int32_t>("4013"); // ChannelStatus block
1012  handlers_.insert<int32_t>("4001"); // DOP block
1013  }
1014  }
1015  if (settings_->septentrio_receiver_type == "gnss")
1016  {
1017  if (settings_->publish_pose)
1018  {
1021  "PoseWithCovarianceStamped");
1022  }
1023  }
1024  if (settings_->septentrio_receiver_type == "ins")
1025  {
1026  if (settings_->publish_pose)
1027  {
1030  "INSPoseWithCovarianceStamped");
1031  }
1032  }
1034  {
1037  "DiagnosticArray");
1039  handlers_.insert<int32_t>("4014"); // ReceiverStatus block
1041  handlers_.insert<int32_t>("4082"); // QualityInd block
1043  handlers_.insert<int32_t>("5902"); // ReceiverSetup block
1044  }
1045  if (settings_->septentrio_receiver_type == "ins")
1046  {
1048  {
1051  "Localization");
1052  }
1053  }
1054  // so on and so forth...
1055  node_->log(LogLevel::DEBUG, "Leaving defineMessages() method");
1056 }
1057 
1058 void io_comm_rx::Comm_IO::send(std::string cmd)
1059 {
1060  // It is imperative to hold a lock on the mutex "g_response_mutex" while
1061  // modifying the variable "g_response_received".
1062  boost::mutex::scoped_lock lock(g_response_mutex);
1063  // Determine byte size of cmd and hand over to send() method of manager_
1064  manager_.get()->send(cmd, cmd.size());
1065  g_response_condition.wait(lock, []() { return g_response_received; });
1066  g_response_received = false;
1067 }
1068 
1069 bool io_comm_rx::Comm_IO::initializeTCP(std::string host, std::string port)
1070 {
1071  node_->log(LogLevel::DEBUG, "Calling initializeTCP() method..");
1072  host_ = host;
1073  port_ = port;
1074  // The io_context, of which io_service is a typedef of; it represents your
1075  // program's link to the operating system's I/O services.
1077  new boost::asio::io_service);
1078  boost::asio::ip::tcp::resolver::iterator endpoint;
1079 
1080  try
1081  {
1082  boost::asio::ip::tcp::resolver resolver(*io_service);
1083  // Note that tcp::resolver::query takes the host to resolve or the IP as the
1084  // first parameter and the name of the service (as defined e.g. in
1085  // /etc/services on Unix hosts) as second parameter. For the latter, one can
1086  // also use a numeric service identifier (aka port number). In any case, it
1087  // returns a list of possible endpoints, as there might be several entries
1088  // for a single host.
1089  endpoint = resolver.resolve(boost::asio::ip::tcp::resolver::query(
1090  host, port)); // Resolves query object..
1091  } catch (std::runtime_error& e)
1092  {
1093  throw std::runtime_error("Could not resolve " + host + " on port " + port +
1094  ": " + e.what());
1095  return false;
1096  }
1097 
1099  new boost::asio::ip::tcp::socket(*io_service));
1100 
1101  try
1102  {
1103  // The list of endpoints obtained above may contain both IPv4 and IPv6
1104  // endpoints, so we need to try each of them until we find one that works.
1105  // This keeps the client program independent of a specific IP version. The
1106  // boost::asio::connect() function does this for us automatically.
1107  socket->connect(*endpoint);
1108  socket->set_option(boost::asio::ip::tcp::no_delay(true));
1109  } catch (std::runtime_error& e)
1110  {
1111  throw std::runtime_error("Could not connect to " + endpoint->host_name() +
1112  ": " + endpoint->service_name() + ": " + e.what());
1113  return false;
1114  }
1115 
1116  node_->log(LogLevel::INFO, "Connected to " + endpoint->host_name() +
1117  ":" + endpoint->service_name() + ".");
1118 
1119  if (manager_)
1120  {
1122  "You have called the InitializeTCP() method though an AsyncManager object is already available! Start all anew..");
1123  return false;
1124  }
1126  new AsyncManager<boost::asio::ip::tcp::socket>(node_, socket, io_service)));
1127  node_->log(LogLevel::DEBUG, "Leaving initializeTCP() method..");
1128  return true;
1129 }
1130 
1132 {
1133  node_->log(LogLevel::DEBUG, "Calling initializeSBFFileReading() method..");
1134  std::size_t buffer_size = 8192;
1135  uint8_t* to_be_parsed;
1136  to_be_parsed = new uint8_t[buffer_size];
1137  std::ifstream bin_file(file_name, std::ios::binary);
1138  std::vector<uint8_t> vec_buf;
1139  if (bin_file.good())
1140  {
1141  /* Reads binary data using streambuffer iterators.
1142  Copies all SBF file content into bin_data. */
1143  std::vector<uint8_t> v_buf((std::istreambuf_iterator<char>(bin_file)),
1144  (std::istreambuf_iterator<char>()));
1145  vec_buf = v_buf;
1146  bin_file.close();
1147  } else
1148  {
1149  throw std::runtime_error("I could not find your file. Or it is corrupted.");
1150  }
1151  // The spec now guarantees that vectors store their elements contiguously.
1152  to_be_parsed = vec_buf.data();
1153  std::stringstream ss;
1154  ss << "Opened and copied over from " << file_name;
1155  node_->log(LogLevel::DEBUG, ss.str());
1156 
1157  while (!stopping_) // Loop will stop if we are done reading the SBF file
1158  {
1159  try
1160  {
1162  "Calling read_callback_() method, with number of bytes to be parsed being " +
1163  buffer_size);
1164  handlers_.readCallback(node_->getTime(), to_be_parsed, buffer_size);
1165  } catch (std::size_t& parsing_failed_here)
1166  {
1167  if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t))
1168  {
1169  break;
1170  }
1171  to_be_parsed = to_be_parsed + parsing_failed_here;
1172  node_->log(LogLevel::DEBUG, "Parsing_failed_here is " + parsing_failed_here);
1173  continue;
1174  }
1175  if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t))
1176  {
1177  break;
1178  }
1179  to_be_parsed = to_be_parsed + buffer_size;
1180  }
1181  node_->log(LogLevel::DEBUG, "Leaving initializeSBFFileReading() method..");
1182 }
1183 
1185 {
1186  node_->log(LogLevel::DEBUG, "Calling initializePCAPFileReading() method..");
1187  pcapReader::buffer_t vec_buf;
1188  pcapReader::PcapDevice device(node_, vec_buf);
1189 
1190  if (!device.connect(file_name.c_str()))
1191  {
1192  node_->log(LogLevel::ERROR, "Unable to find file or either it is corrupted");
1193  return;
1194  }
1195 
1196  node_->log(LogLevel::INFO, "Reading ...");
1197  while (device.isConnected() && device.read() == pcapReader::READ_SUCCESS)
1198  ;
1199  device.disconnect();
1200 
1201  std::size_t buffer_size = pcapReader::PcapDevice::BUFFSIZE;
1202  uint8_t* to_be_parsed = new uint8_t[buffer_size];
1203  to_be_parsed = vec_buf.data();
1204 
1205  while (!stopping_) // Loop will stop if we are done reading the SBF file
1206  {
1207  try
1208  {
1210  "Calling read_callback_() method, with number of bytes to be parsed being " +
1211  buffer_size);
1212  handlers_.readCallback(node_->getTime(), to_be_parsed, buffer_size);
1213  } catch (std::size_t& parsing_failed_here)
1214  {
1215  if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t))
1216  {
1217  break;
1218  }
1219  if (!parsing_failed_here)
1220  parsing_failed_here = 1;
1221 
1222  to_be_parsed = to_be_parsed + parsing_failed_here;
1223  node_->log(LogLevel::DEBUG, "Parsing_failed_here is " + parsing_failed_here);
1224  continue;
1225  }
1226  if (to_be_parsed - vec_buf.data() >= vec_buf.size() * sizeof(uint8_t))
1227  {
1228  break;
1229  }
1230  to_be_parsed = to_be_parsed + buffer_size;
1231  }
1232  node_->log(LogLevel::DEBUG, "Leaving initializePCAPFileReading() method..");
1233 }
1234 
1235 bool io_comm_rx::Comm_IO::initializeSerial(std::string port, uint32_t baudrate,
1236  std::string flowcontrol)
1237 {
1238  node_->log(LogLevel::DEBUG, "Calling initializeSerial() method..");
1239  serial_port_ = port;
1240  baudrate_ = baudrate;
1241  // The io_context, of which io_service is a typedef of; it represents your
1242  // program's link to the operating system's I/O services.
1244  new boost::asio::io_service);
1245  // To perform I/O operations the program needs an I/O object, here "serial".
1247  new boost::asio::serial_port(*io_service));
1248 
1249  // We attempt the opening of the serial port..
1250  try
1251  {
1252  serial->open(serial_port_);
1253  } catch (std::runtime_error& e)
1254  {
1255  // and return an error message in case it fails.
1256  throw std::runtime_error("Could not open serial port : " + serial_port_ +
1257  ": " + e.what());
1258  return false;
1259  }
1260 
1261  node_->log(LogLevel::INFO, "Opened serial port " + serial_port_);
1262  node_->log(LogLevel::DEBUG, "Our boost version is " + std::to_string(BOOST_VERSION) + ".");
1263  if (BOOST_VERSION < 106600) // E.g. for ROS melodic (i.e. Ubuntu 18.04), the
1264  // version is 106501, standing for 1.65.1.
1265  {
1266  // Workaround to set some options for the port manually,
1267  // cf. https://github.com/mavlink/mavros/pull/971/files
1268  // This function native_handle() may be used to obtain
1269  // the underlying representation of the serial port.
1270  // Conversion from type native_handle_type to int is done implicitly.
1271  int fd = serial->native_handle();
1272  termios tio;
1273  // Get terminal attribute, follows the syntax
1274  // int tcgetattr(int fd, struct termios *termios_p);
1275  tcgetattr(fd, &tio);
1276 
1277  // Hardware flow control settings_->.
1278  if (flowcontrol == "RTS|CTS")
1279  {
1280  tio.c_iflag &= ~(IXOFF | IXON);
1281  tio.c_cflag |= CRTSCTS;
1282  } else
1283  {
1284  tio.c_iflag &= ~(IXOFF | IXON);
1285  tio.c_cflag &= ~CRTSCTS;
1286  }
1287  // Setting serial port to "raw" mode to prevent EOF exit..
1288  cfmakeraw(&tio);
1289 
1290  // Commit settings, syntax is
1291  // int tcsetattr(int fd, int optional_actions, const struct termios
1292  // *termios_p);
1293  tcsetattr(fd, TCSANOW, &tio);
1294  }
1295 
1296  // Set the I/O manager
1297  if (manager_)
1298  {
1300  "You have called the initializeSerial() method though an AsyncManager object is already available! Start all anew..");
1301  return false;
1302  }
1303  node_->log(LogLevel::DEBUG, "Creating new Async-Manager object..");
1305  new AsyncManager<boost::asio::serial_port>(node_, serial, io_service)));
1306 
1307  // Setting the baudrate, incrementally..
1308  node_->log(LogLevel::DEBUG, "Gradually increasing the baudrate to the desired value...");
1309  boost::asio::serial_port_base::baud_rate current_baudrate;
1310  node_->log(LogLevel::DEBUG, "Initiated current_baudrate object...");
1311  try
1312  {
1313  serial->get_option(
1314  current_baudrate); // Note that this sets current_baudrate.value() often
1315  // to 115200, since by default, all Rx COM ports,
1316  // at least for mosaic Rxs, are set to a baudrate of 115200 baud, using 8
1317  // data-bits, no parity and 1 stop-bit.
1318  } catch (boost::system::system_error& e)
1319  {
1320 
1321  node_->log(LogLevel::ERROR, "get_option failed due to " +std::string( e.what()));
1322  node_->log(LogLevel::INFO, "Additional info about error is " +
1323  boost::diagnostic_information(e));
1324  /*
1325  boost::system::error_code e_loop;
1326  do // Caution: Might cause infinite loop..
1327  {
1328  serial->get_option(current_baudrate, e_loop);
1329  } while(e_loop);
1330  */
1331  return false;
1332  }
1333  // Gradually increase the baudrate to the desired value
1334  // The desired baudrate can be lower or larger than the
1335  // current baudrate; the for loop takes care of both scenarios.
1336  node_->log(LogLevel::DEBUG, "Current baudrate is " + std::to_string(current_baudrate.value()));
1337  for (uint8_t i = 0; i < sizeof(BAUDRATES) / sizeof(BAUDRATES[0]); i++)
1338  {
1339  if (current_baudrate.value() == baudrate_)
1340  {
1341  break; // Break if the desired baudrate has been reached.
1342  }
1343  if (current_baudrate.value() >= BAUDRATES[i] && baudrate_ > BAUDRATES[i])
1344  {
1345  continue;
1346  }
1347  // Increment until Baudrate[i] matches current_baudrate.
1348  try
1349  {
1350  serial->set_option(
1351  boost::asio::serial_port_base::baud_rate(BAUDRATES[i]));
1352  } catch (boost::system::system_error& e)
1353  {
1354 
1355  node_->log(LogLevel::ERROR, "set_option failed due to " + std::string(e.what()));
1356  node_->log(LogLevel::INFO, "Additional info about error is " +
1357  boost::diagnostic_information(e));
1358  return false;
1359  }
1360  usleep(SET_BAUDRATE_SLEEP_);
1361  // boost::this_thread::sleep(boost::posix_time::milliseconds(SET_BAUDRATE_SLEEP_*1000));
1362  // Boost's sleep would yield an error message with exit code -7 the second
1363  // time it is called, hence we use sleep() or usleep().
1364  try
1365  {
1366  serial->get_option(current_baudrate);
1367  } catch (boost::system::system_error& e)
1368  {
1369 
1370  node_->log(LogLevel::ERROR, "get_option failed due to " + std::string(e.what()));
1371  node_->log(LogLevel::INFO, "Additional info about error is " +
1372  boost::diagnostic_information(e));
1373  /*
1374  boost::system::error_code e_loop;
1375  do // Caution: Might cause infinite loop..
1376  {
1377  serial->get_option(current_baudrate, e_loop);
1378  } while(e_loop);
1379  */
1380  return false;
1381  }
1382  node_->log(LogLevel::DEBUG, "Set ASIO baudrate to " + std::to_string(current_baudrate.value()));
1383  }
1384  node_->log(LogLevel::INFO, "Set ASIO baudrate to " + std::to_string(current_baudrate.value()) +
1385  ", leaving InitializeSerial() method");
1386  return true;
1387 }
1388 
1390 {
1391  node_->log(LogLevel::DEBUG, "Called setManager() method");
1392  if (manager_)
1393  return;
1394  manager_ = manager;
1395  manager_->setCallback(
1396  boost::bind(&CallbackHandlers::readCallback, &handlers_, _1, _2, _3));
1397  node_->log(LogLevel::DEBUG, "Leaving setManager() method");
1398 }
1399 
1400 void io_comm_rx::Comm_IO::resetSerial(std::string port)
1401 {
1402  serial_port_ = port;
1404  new boost::asio::io_service);
1406  new boost::asio::serial_port(*io_service));
1407 
1408  // Try to open serial port
1409  try
1410  {
1411  serial->open(serial_port_);
1412  } catch (std::runtime_error& e)
1413  {
1414  throw std::runtime_error("Could not open serial port :" + serial_port_ +
1415  " " + e.what());
1416  }
1417 
1418  node_->log(LogLevel::INFO, "Reset serial port " + serial_port_);
1419 
1420  // Sets the I/O worker
1421  if (manager_)
1422  return;
1424  new AsyncManager<boost::asio::serial_port>(node_, serial, io_service)));
1425 
1426  // Set the baudrate
1427  serial->set_option(boost::asio::serial_port_base::baud_rate(baudrate_));
1428 }
double theta_z
IMU orientation z-angle.
Definition: rx_message.hpp:184
septentrio_gnss_driver::VelCovGeodetic VelCovGeodeticMsg
Definition: typedefs.hpp:107
bool connected_
Whether connecting to Rx was successful.
nmea_msgs::Gpgsa GpgsaMsg
Definition: typedefs.hpp:111
uint32_t polling_period_rest
Polling period for all other SBF blocks and NMEA messages.
Definition: rx_message.hpp:160
ReadResult read()
Attempt to read a packet and store data to buffer.
Definition: pcap_reader.cpp:87
boost::shared_ptr< Manager > manager_
septentrio_gnss_driver::AttCovEuler AttCovEulerMsg
Definition: typedefs.hpp:100
uint32_t baudrate_
Baudrate at the moment, unless InitializeSerial or ResetSerial fail.
bool publish_insnavcart
Whether or not to publish the INSNavCartMsg message.
Definition: rx_message.hpp:272
bool publish_attcoveuler
Whether or not to publish the AttCovEulerMsg message.
Definition: rx_message.hpp:270
float delta_e
Marker-to-ARP offset in the eastward direction.
Definition: rx_message.hpp:162
bool publish_tf
Whether or not to publish the tf of the localization.
Definition: rx_message.hpp:300
bool read_from_sbf_log
Whether or not we are reading from an SBF file.
Definition: rx_message.hpp:324
std::string hw_flow_control
HW flow control.
Definition: rx_message.hpp:151
string cmd
void readCallback(Timestamp recvTimestamp, const uint8_t *data, std::size_t &size)
Searches for Rx messages that could potentially be decoded/parsed/published.
bool publish_poscovgeodetic
Definition: rx_message.hpp:263
#define THETA_Y_MAX
std::vector< uint8_t > buffer_t
Definition: pcap_reader.hpp:49
Settings * settings_
Settings.
void prepareSBFFileReading(std::string file_name)
Sets up the stage for SBF file reading.
double poi_x
INS POI offset in x-dimension.
Definition: rx_message.hpp:192
uint32_t rx_input_corrections_tcp
Definition: rx_message.hpp:237
std::string ntrip_password
Password for NTRIP service.
Definition: rx_message.hpp:226
std::string rtcm_version
RTCM version for NTRIP service (if Rx does not have internet)
Definition: rx_message.hpp:234
Comm_IO(ROSaicNodeBase *node, Settings *settings)
Constructor of the class Comm_IO.
void defineMessages()
Defines which Rx messages to read and which ROS messages to publish.
bool serial_
Whether yet-to-be-established connection to Rx will be serial or TCP.
static const unsigned int SET_BAUDRATE_SLEEP_
std::string ant_serial_nr
Serial number of your particular Main antenna.
Definition: rx_message.hpp:174
bool g_response_received
Determines whether a command reply was received from the Rx.
septentrio_gnss_driver::PVTGeodetic PVTGeodeticMsg
Definition: typedefs.hpp:103
std::string rx_input_corrections_serial
Definition: rx_message.hpp:240
septentrio_gnss_driver::VelSensorSetup VelSensorSetupMsg
Definition: typedefs.hpp:119
void disconnect()
Close connected file.
Definition: pcap_reader.cpp:75
bool isConnected() const
Check if file is open and healthy.
Definition: pcap_reader.cpp:85
septentrio_gnss_driver::INSNavCart INSNavCartMsg
Definition: typedefs.hpp:116
bool initializeSerial(std::string port, uint32_t baudrate=115200, std::string flowcontrol="None")
Initializes the serial port.
void initializeIO()
Initializes the I/O handling.
#define PITCH_MIN
#define HEADING_MAX
void initializeSBFFileReading(std::string file_name)
Initializes SBF file reading and reads SBF file by repeatedly calling read_callback_() ...
std::unique_ptr< boost::thread > connectionThread_
Connection or reading thread.
Highest-Level view on communication services.
bool ins_use_poi
INS solution reference point.
Definition: rx_message.hpp:210
void reconnect()
Attempts to (re)connect every reconnect_delay_s_ seconds.
#define THETA_Y_MIN
std::string login_user
Username for login.
Definition: rx_message.hpp:142
septentrio_gnss_driver::ExtSensorMeas ExtSensorMeasMsg
Definition: typedefs.hpp:120
double vsm_x
INS velocity sensor lever arm x-offset.
Definition: rx_message.hpp:198
double vsm_y
INS velocity sensor lever arm y-offset.
Definition: rx_message.hpp:200
void log(LogLevel logLevel, const std::string &s)
Log function to provide abstraction of ROS loggers.
Definition: typedefs.hpp:208
bool connect(const char *device)
Try to open a pcap file.
Definition: pcap_reader.cpp:57
#define ANGLE_MIN
septentrio_gnss_driver::AttEuler AttEulerMsg
Definition: typedefs.hpp:101
geometry_msgs::TransformStamped t
septentrio_gnss_driver::IMUSetup IMUSetupMsg
Definition: typedefs.hpp:118
boost::mutex g_cd_mutex
Mutex to control changes of global variable "g_cd_received".
std::string serial_port_
Saves the port description.
nmea_msgs::Gpgsv GpgsvMsg
Definition: typedefs.hpp:112
Data read successfully.
Definition: pcap_reader.hpp:55
std::string rx_serial_port
Definition: rx_message.hpp:154
nmea_msgs::Gprmc GprmcMsg
Definition: typedefs.hpp:113
bool publish_gpst
Whether or not to publish the TimeReferenceMsg message with GPST.
Definition: rx_message.hpp:286
double poi_y
INS POI offset in y-dimension.
Definition: rx_message.hpp:194
ROSaicNodeBase * node_
Pointer to Node.
static const size_t BUFFSIZE
Definition: pcap_reader.hpp:72
double ant_lever_x
INS antenna lever arm x-offset.
Definition: rx_message.hpp:186
#define POSSTD_DEV_MAX
nmea_msgs::Gpgga GpggaMsg
Definition: typedefs.hpp:110
uint32_t polling_period_pvt
Polling period for PVT-related SBF blocks.
Definition: rx_message.hpp:158
std::string port_
Port over which TCP/IP connection is currently established.
bool publish_insnavgeod
Whether or not to publish the INSNavGeodMsg message.
Definition: rx_message.hpp:274
bool publish_gprmc
Whether or not to publish the RMC message.
Definition: rx_message.hpp:246
bool rx_has_internet
Whether Rx has internet or not.
Definition: rx_message.hpp:232
double poi_z
INS POI offset in z-dimension.
Definition: rx_message.hpp:196
void resetSerial(std::string port)
Reset the Serial I/O port, e.g. after a Rx reset.
bool publish_gpgsa
Whether or not to publish the GSA message.
Definition: rx_message.hpp:248
bool initializeTCP(std::string host, std::string port)
Initializes the TCP I/O.
bool use_gnss_time
Definition: rx_message.hpp:306
float delta_n
Marker-to-ARP offset in the northward direction.
Definition: rx_message.hpp:164
std::string login_password
Password for login.
Definition: rx_message.hpp:144
#define HEADING_MIN
bool publish_poscovcartesian
Definition: rx_message.hpp:260
bool publish_measepoch
Whether or not to publish the MeasEpoch message.
Definition: rx_message.hpp:252
std::string trimDecimalPlaces(double num)
Trims decimal places to two.
CallbackMap insert(std::string message_key)
Adds a pair to the multimap "callbackmap_", with the message_key being the key.
bool publish_extsensormeas
Whether or not to publish the ExtSensorMeasMsg message.
Definition: rx_message.hpp:284
double heading_offset
Attitude offset determination in longitudinal direction.
Definition: rx_message.hpp:204
Settings struct.
Definition: rx_message.hpp:135
bool publish_gpgga
Whether or not to publish the GGA message.
Definition: rx_message.hpp:244
boost::condition_variable g_response_condition
Condition variable complementing "g_response_mutex".
std::string mountpoint
Mountpoint for NTRIP service.
Definition: rx_message.hpp:228
bool publish_localization
Whether or not to publish the LocalizationMsg message.
Definition: rx_message.hpp:298
septentrio_gnss_driver::INSNavGeod INSNavGeodMsg
Definition: typedefs.hpp:117
std::string ant_aux1_serial_nr
Serial number of your particular Aux1 antenna.
Definition: rx_message.hpp:176
std::string septentrio_receiver_type
Septentrio receiver type, either "gnss" or "ins".
Definition: rx_message.hpp:302
static const uint32_t BAUDRATES[]
Possible baudrates for the Rx.
std::string g_rx_tcp_port
Rx TCP port, e.g. IP10 or IP11, to which ROSaic is connected to.
uint32_t convertUserPeriodToRxCommand(uint32_t period_user)
Transforms the input polling period [milliseconds] into a uint32_t number that can be appended to eit...
Class for handling a pcap file.
Definition: pcap_reader.hpp:69
bool publish_pvtcartesian
Definition: rx_message.hpp:255
uint32_t g_cd_count
#define PITCH_MAX
uint32_t baudrate
Baudrate.
Definition: rx_message.hpp:149
bool read_from_pcap
Whether or not we are reading from a PCAP file.
Definition: rx_message.hpp:326
bool publish_velcovgeodetic
Definition: rx_message.hpp:266
double theta_x
IMU orientation x-angle.
Definition: rx_message.hpp:180
bool publish_velsensorsetup
Whether or not to publish the VelSensorSetupMsg message.
Definition: rx_message.hpp:278
void preparePCAPFileReading(std::string file_name)
Sets up the stage for PCAP file reading.
std::string device
Device port.
Definition: rx_message.hpp:140
std::string caster
Hostname or IP address of the NTRIP caster to connect to.
Definition: rx_message.hpp:220
septentrio_gnss_driver::PosCovGeodetic PosCovGeodeticMsg
Definition: typedefs.hpp:105
bool publish_pvtgeodetic
Whether or not to publish the PVTGeodeticMsg message.
Definition: rx_message.hpp:257
std::string ntrip_mode
Type of NTRIP connection.
Definition: rx_message.hpp:218
bool publish_exteventinsnavcart
Whether or not to publish the ExtEventINSNavCartMsg message.
Definition: rx_message.hpp:282
bool publish_gpsfix
Whether or not to publish the GPSFixMsg message.
Definition: rx_message.hpp:290
septentrio_gnss_driver::PosCovCartesian PosCovCartesianMsg
Definition: typedefs.hpp:104
void configureRx()
Configures Rx: Which SBF/NMEA messages it should output and later correction settings.
bool g_cd_received
Determines whether the connection descriptor was received from the Rx.
float reconnect_delay_s
Definition: rx_message.hpp:147
std::string tcp_port_
TCP port number.
boost::condition_variable connection_condition_
#define POSSTD_DEV_MIN
std::string send_gga
Whether (and at which rate) or not to send GGA to the NTRIP caster.
Definition: rx_message.hpp:242
std::atomic< bool > stopping_
Indicator for threads to exit.
bool publish_imu
Whether or not to publish the ImuMsg message.
Definition: rx_message.hpp:296
bool publish_imusetup
Whether or not to publish the IMUSetupMsg message.
Definition: rx_message.hpp:276
double ant_lever_y
INS antenna lever arm y-offset.
Definition: rx_message.hpp:188
boost::condition_variable g_cd_condition
Condition variable complementing "g_cd_mutex".
diagnostic_msgs::DiagnosticArray DiagnosticArrayMsg
Definition: typedefs.hpp:82
Declares a class for handling pcap files.
float att_std_dev
Attitude deviation mask.
Definition: rx_message.hpp:214
void setManager(const boost::shared_ptr< Manager > &manager)
Set the I/O manager.
float pos_std_dev
Position deviation mask.
Definition: rx_message.hpp:216
bool g_read_cd
This is the central interface between ROSaic and the Rx(s), managing I/O operations such as reading m...
std::string ant_aux1_type
Definition: rx_message.hpp:172
bool publish_exteventinsnavgeod
Whether or not to publish the ExtEventINSNavGeodMsg message.
Definition: rx_message.hpp:280
Timestamp getTime()
Gets current timestamp.
Definition: typedefs.hpp:236
nav_msgs::Odometry LocalizationUtmMsg
Definition: typedefs.hpp:93
CallbackHandlers handlers_
Callback handlers for the inwards streaming messages.
void send(std::string cmd)
Hands over to the send() method of manager_.
double vsm_z
INS velocity sensor lever arm z-offset.
Definition: rx_message.hpp:202
#define ANGLE_MAX
double pitch_offset
Attitude offset determination in latitudinal direction.
Definition: rx_message.hpp:206
bool publish_atteuler
Whether or not to publish the AttEulerMsg message.
Definition: rx_message.hpp:268
std::string ntrip_version
NTRIP version for NTRIP service.
Definition: rx_message.hpp:230
std::string datum
Datum to be used.
Definition: rx_message.hpp:156
This class is the base class for abstraction.
Definition: typedefs.hpp:160
double theta_y
IMU orientation y-angle.
Definition: rx_message.hpp:182
#define ATTSTD_DEV_MIN
void initializePCAPFileReading(std::string file_name)
Initializes PCAP file reading and reads PCAP file by repeatedly calling read_callback_() ...
std::string trimString(std::string str)
Removes trailing zeros from a string representing a float or double except for the first zero after t...
boost::mutex g_response_mutex
Mutex to control changes of global variable "g_response_received".
#define ATTSTD_DEV_MAX
bool publish_gpgsv
Whether or not to publish the GSV message.
Definition: rx_message.hpp:250
uint32_t caster_port
IP port number of NTRIP caster to connect to.
Definition: rx_message.hpp:222
bool publish_diagnostics
Whether or not to publish the DiagnosticArrayMsg message.
Definition: rx_message.hpp:294
bool publish_pose
Whether or not to publish the PoseWithCovarianceStampedMsg message.
Definition: rx_message.hpp:292
std::string ins_initial_heading
For heading computation when unit is powered-cycled.
Definition: rx_message.hpp:212
septentrio_gnss_driver::PVTCartesian PVTCartesianMsg
Definition: typedefs.hpp:102
#define LEVER_ARM_MIN
void connect()
Calls the reconnect() method.
sensor_msgs::NavSatFix NavSatFixMsg
Definition: typedefs.hpp:89
std::string tcp_host_
Host name of TCP server.
gps_common::GPSFix GPSFixMsg
Definition: typedefs.hpp:87
std::string ntrip_username
Username for NTRIP service.
Definition: rx_message.hpp:224
geometry_msgs::PoseWithCovarianceStamped PoseWithCovarianceStampedMsg
Definition: typedefs.hpp:85
bool multi_antenna
INS multiantenna.
Definition: rx_message.hpp:208
bool publish_navsatfix
Whether or not to publish the NavSatFixMsg message.
Definition: rx_message.hpp:288
#define LEVER_ARM_MAX
std::string host_
Host currently connected to.
float delta_u
Marker-to-ARP offset in the upward direction.
Definition: rx_message.hpp:166
double ant_lever_z
INS antenna lever arm z-offset.
Definition: rx_message.hpp:190
std::string ant_type
Definition: rx_message.hpp:169


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