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 #include <linux/serial.h>
33 
34 // Boost includes
35 #include <boost/regex.hpp>
37 
38 static const int16_t ANGLE_MAX = 180;
39 static const int16_t ANGLE_MIN = -180;
40 static const int8_t THETA_Y_MAX = 90;
41 static const int8_t THETA_Y_MIN = -90;
42 static const int8_t LEVER_ARM_MAX = 100;
43 static const int8_t LEVER_ARM_MIN = -100;
44 static const int16_t HEADING_MAX = 360;
45 static const int16_t HEADING_MIN = -360;
46 static const int8_t PITCH_MAX = 90;
47 static const int8_t PITCH_MIN = -90;
48 static const int8_t ATTSTD_DEV_MIN = 0;
49 static const int8_t ATTSTD_DEV_MAX = 5;
50 static const int8_t POSSTD_DEV_MIN = 0;
51 static const int8_t POSSTD_DEV_MAX = 100;
52 
59 namespace io {
60 
62  node_(node), settings_(node->settings()), telegramHandler_(node),
63  running_(true)
64  {
65  running_ = true;
66 
68  std::thread(std::bind(&CommunicationCore::processTelegrams, this));
69  }
70 
72  {
74 
75  resetSettings();
76 
77  running_ = false;
78  std::shared_ptr<Telegram> telegram(new Telegram);
79  telegramQueue_.push(telegram);
80  processingThread_.join();
81  }
82 
84  {
87  {
89  send("sdio, " + mainConnectionPort_ + ", auto, none\x0D");
90  // Turning off all current SBF/NMEA output
91  send("sso, all, none, none, off \x0D");
92  send("sno, all, none, none, off \x0D");
93  if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty()))
94  {
95  send("siss, " + settings_->udp_ip_server + ", 0\x0D");
96  }
97  for (auto ntrip : settings_->rtk.ntrip)
98  {
99  if (!ntrip.id.empty() && !ntrip.keep_open)
100  {
101  send("snts, " + ntrip.id + ", off \x0D");
102  }
103  }
104  for (auto ip_server : settings_->rtk.ip_server)
105  {
106  if (!ip_server.id.empty() && !ip_server.keep_open)
107  {
108  send("sdio, " + ip_server.id + ", auto, none\x0D");
109  send("siss, " + ip_server.id + ", 0\x0D");
110  }
111  }
112  for (auto serial : settings_->rtk.serial)
113  {
114  if (!serial.port.empty() && !serial.keep_open)
115  {
116  send("sdio, " + serial.port + ", auto, none\x0D");
117  if (serial.port.rfind("COM", 0) == 0)
118  send("scs, " + serial.port +
119  ", baud115200, bits8, No, bit1, none\x0D");
120  }
121  }
122  if (!settings_->ins_vsm_ip_server_id.empty())
123  {
125  {
126  send("sdio, " + settings_->ins_vsm_ip_server_id +
127  ", auto, none\x0D");
128  send("siss, " + settings_->ins_vsm_ip_server_id + ", 0\x0D");
129  }
130  }
131  if (!settings_->ins_vsm_serial_port.empty())
132  {
134  {
135  if (settings_->ins_vsm_serial_port.rfind("COM", 0) == 0)
136  send("scs, " + settings_->ins_vsm_serial_port +
137  ", baud115200, bits8, No, bit1, none\x0D");
138  send("sdio, " + settings_->ins_vsm_serial_port +
139  ", auto, none\x0D");
140  }
141  }
142  if (!settings_->osnma.keep_open && (settings_->osnma.mode == "loose" ||
143  settings_->osnma.mode == "strict"))
144  {
145  std::stringstream ss;
146  ss << "sou, off \x0D";
147  send(ss.str());
148 
149  if (!settings_->osnma.ntp_server.empty())
150  {
151  std::stringstream ss;
152  ss << "snc, off \x0D";
153  send(ss.str());
154  }
155  }
156 
157  if (!settings_->login_user.empty() && !settings_->login_password.empty())
158  send("logout \x0D");
159  }
160  }
161 
163  {
164  node_->log(log_level::DEBUG, "Called connect() method");
165  node_->log(
167  "Started timer for calling connect() method until connection succeeds");
168 
169  boost::asio::io_service io;
170  boost::posix_time::millisec wait_ms(
171  static_cast<uint32_t>(settings_->reconnect_delay_s * 1000));
172  if (initializeIo())
173  {
174  while (running_)
175  {
176  boost::asio::deadline_timer t(io, wait_ms);
177 
178  if (manager_->connect())
179  {
180  initializedIo_ = true;
181  break;
182  }
183 
184  t.wait();
185  }
186  }
187 
188  // Sends commands to the Rx regarding which SBF/NMEA messages it should
189  // output
190  // and sets all its necessary corrections-related parameters
192  {
193  node_->log(log_level::DEBUG, "Configure Rx.");
194  if (settings_->configure_rx)
195  configureRx();
196  }
197 
198  node_->log(log_level::INFO, "Setup complete.");
199 
201  "Successully connected. Leaving connect() method");
202  }
203 
204  [[nodiscard]] bool CommunicationCore::initializeIo()
205  {
206  bool client = false;
207  node_->log(log_level::DEBUG, "Called initializeIo() method");
208  if ((settings_->tcp_port != 0) && (!settings_->tcp_ip_server.empty()))
209  {
211  tcpClient_->setPort(std::to_string(settings_->tcp_port));
212  if (!settings_->configure_rx)
213  tcpClient_->connect();
214  client = true;
215  }
216  if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty()))
217  {
218  udpClient_.reset(
220  client = true;
221  }
222 
223  switch (settings_->device_type)
224  {
225  case device_type::TCP:
226  {
228  break;
229  }
230  case device_type::SERIAL:
231  {
233  break;
234  }
236  {
238  break;
239  }
241  {
243  break;
244  }
245  default:
246  {
247  if (!client || settings_->configure_rx ||
248  (settings_->ins_vsm_ros_source == "odometry") ||
249  (settings_->ins_vsm_ros_source == "twist"))
250  {
251  node_->log(log_level::DEBUG, "Unsupported device.");
252  return false;
253  }
254  break;
255  }
256  }
257  return true;
258  }
259 
268  {
269  node_->log(log_level::DEBUG, "Called configureRx() method");
270 
271  if (!initializedIo_)
272  {
274  "Called configureRx() method but IO is not initialized.");
275  return;
276  }
277 
278  uint8_t stream = 1;
279  // Determining communication mode: TCP vs USB/Serial
280  boost::smatch match;
281  boost::regex_match(settings_->device, match,
282  boost::regex("(tcp)://(.+):(\\d+)"));
283  std::string proto(match[1]);
286  "The connection descriptor is " + mainConnectionPort_);
288  if ((settings_->tcp_port != 0) && (!settings_->tcp_ip_server.empty()))
289  {
291  send("siss, " + streamPort_ + ", " +
292  std::to_string(settings_->tcp_port) + ", TCP, " + "\x0D");
293  tcpClient_->connect();
294  } else if ((settings_->udp_port != 0) && (!settings_->udp_ip_server.empty()))
295  {
297  std::string destination;
298  if (!settings_->udp_unicast_ip.empty())
299  destination = settings_->udp_unicast_ip;
300  else
301  destination = "255.255.255.255";
302  send("siss, " + streamPort_ + ", " +
303  std::to_string(settings_->udp_port) + ", UDP, " + destination +
304  "\x0D");
305  }
306 
307  node_->log(log_level::INFO, "Setting up Rx.");
308 
309  std::string pvt_interval = parsing_utilities::convertUserPeriodToRxCommand(
311 
312  std::string rest_interval = parsing_utilities::convertUserPeriodToRxCommand(
314 
315  // Credentials for login
316  if (!settings_->login_user.empty() && !settings_->login_password.empty())
317  {
319  send("login, " + settings_->login_user + ", \"" +
320  settings_->login_password + "\" \x0D");
321  else
322  send("login, " + settings_->login_user + ", " +
323  settings_->login_password + " \x0D");
324  }
325 
326  // Turning off all current SBF/NMEA output
327  send("sso, all, none, none, off \x0D");
328  send("sno, all, none, none, off \x0D");
329 
330  // Get Rx capabilities
331  send("grc \x0D");
333 
334  // Activate NTP server
336  send("sntp, on \x0D");
337 
338  // Setting the datum to be used by the Rx (not the NMEA output though, which
339  // only provides MSL and undulation (by default with respect to WGS84), but
340  // not ellipsoidal height)
341  {
342  std::stringstream ss;
343  ss << "sgd, " << settings_->datum << "\x0D";
344  send(ss.str());
345  }
346 
347  if ((settings_->septentrio_receiver_type == "ins") || node_->isIns())
348  {
349  {
350  std::stringstream ss;
351  ss << "sat, Main, \"" << settings_->ant_type << "\""
352  << "\x0D";
353  send(ss.str());
354  }
355 
356  // Configure Aux1 antenna
358  {
359  std::stringstream ss;
360  ss << "sat, Aux1, \"" << settings_->ant_type << "\""
361  << "\x0D";
362  send(ss.str());
363  }
364  } else if (settings_->septentrio_receiver_type == "gnss")
365  {
366  // Setting the marker-to-ARP offsets. This comes after the "sso, ...,
367  // ReceiverSetup, ..." command, since the latter is only generated when a
368  // user-command is entered to change one or more values in the block.
369  {
370  std::stringstream ss;
371  ss << "sao, Main, "
375  << ", \"" << settings_->ant_type << "\", "
376  << settings_->ant_serial_nr << "\x0D";
377  send(ss.str());
378  }
379 
380  // Configure Aux1 antenna
382  {
383  std::stringstream ss;
384  ss << "sao, Aux1, " << string_utilities::trimDecimalPlaces(0.0)
385  << ", " << string_utilities::trimDecimalPlaces(0.0) << ", "
386  << string_utilities::trimDecimalPlaces(0.0) << ", \""
387  << settings_->ant_aux1_type << "\", "
388  << settings_->ant_aux1_serial_nr << "\x0D";
389  send(ss.str());
390  }
391  }
392 
393  // Configuring the corrections connection
394  for (auto ntrip : settings_->rtk.ntrip)
395  {
396  if (!ntrip.id.empty())
397  {
398  // First disable any existing NTRIP connection on NTR1
399  send("snts, " + ntrip.id + ", off \x0D");
400  {
401  std::stringstream ss;
402  ss << "snts, " << ntrip.id << ", Client, " << ntrip.caster
403  << ", " << std::to_string(ntrip.caster_port) << ", "
404  << ntrip.username << ", " << ntrip.password << ", "
405  << ntrip.mountpoint << ", " << ntrip.version << ", "
406  << ntrip.send_gga << " \x0D";
407  send(ss.str());
408  }
409  if (ntrip.tls)
410  {
411  std::stringstream ss;
412  ss << "sntt, " << ntrip.id << ", on, \"" << ntrip.fingerprint
413  << "\" \x0D";
414  send(ss.str());
415  } else
416  {
417  std::stringstream ss;
418  ss << "sntt, " << ntrip.id << ", off \x0D";
419  send(ss.str());
420  }
421  }
422  }
423 
424  for (auto ip_server : settings_->rtk.ip_server)
425  {
426  if (!ip_server.id.empty())
427  // Since the Rx does not have internet (and you will not
428  // be able to share it via USB), we need to forward the
429  // corrections ourselves, though not on the same port.
430  {
431  {
432  std::stringstream ss;
433  // In case this IP server was used before, old
434  // configuration is lost of course.
435  ss << "siss, " << ip_server.id << ", "
436  << std::to_string(ip_server.port) << ", TCP2Way \x0D";
437  send(ss.str());
438  }
439  {
440  std::stringstream ss;
441  ss << "sdio, " << ip_server.id << ", " << ip_server.rtk_standard
442  << ", +SBF+NMEA \x0D";
443  send(ss.str());
444  }
445  if (ip_server.send_gga != "off")
446  {
447  std::string rate = ip_server.send_gga;
448  if (ip_server.send_gga == "auto")
449  rate = "sec1";
450  std::stringstream ss;
451  ss << "sno, Stream" << std::to_string(stream) << ", "
452  << ip_server.id << ", GGA, " << rate << " \x0D";
453  ++stream;
454  send(ss.str());
455  }
456  }
457  }
458 
459  for (auto serial : settings_->rtk.serial)
460  {
461  if (!serial.port.empty())
462  {
463  if (serial.port.rfind("COM", 0) == 0)
464  send("scs, " + serial.port + ", baud" +
465  std::to_string(serial.baud_rate) +
466  ", bits8, No, bit1, none\x0D");
467 
468  std::stringstream ss;
469  ss << "sdio, " << serial.port << ", " << serial.rtk_standard
470  << ", +SBF+NMEA \x0D";
471  send(ss.str());
472  if (serial.send_gga != "off")
473  {
474  std::string rate = serial.send_gga;
475  if (serial.send_gga == "auto")
476  rate = "sec1";
477  std::stringstream ss;
478  ss << "sno, Stream" << std::to_string(stream) << ", "
479  << serial.port << ", GGA, " << rate << " \x0D";
480  ++stream;
481  send(ss.str());
482  }
483  }
484  }
485 
486  // Setting multi antenna
488  {
489  if (node_->hasHeading())
490  send("sga, MultiAntenna \x0D");
491  else
493  "Multi antenna requested but Rx does not support it.");
494  } else
495  {
496  send("sga, none \x0D");
497  }
498 
499  // Setting the Attitude Determination
500  {
505  {
506  std::stringstream ss;
507  ss << "sto, "
509  << ", "
511  << " \x0D";
512  send(ss.str());
513  } else
514  {
516  "Please specify a valid parameter for heading and pitch");
517  }
518  }
519 
520  // Setting the INS-related commands
521  if (settings_->septentrio_receiver_type == "ins")
522  {
523  // IMU orientation
524  {
525  std::stringstream ss;
526  if (settings_->theta_x >= ANGLE_MIN &&
532  {
533  ss << " sio, "
534  << "manual"
535  << ", "
537  << ", "
539  << ", "
541  << " \x0D";
542  send(ss.str());
543  } else
544  {
545  node_->log(
547  "Please specify a correct value for IMU orientation angles");
548  }
549  }
550 
551  // Setting the INS antenna lever arm offset
552  {
559  {
560  std::stringstream ss;
561  ss << "sial, "
563  << ", "
565  << ", "
567  << " \x0D";
568  send(ss.str());
569  } else
570  {
571  node_->log(
573  "Please specify a correct value for x, y and z in the config file under ant_lever_arm");
574  }
575  }
576 
577  // Setting the user defined point offset
578  {
579  if (settings_->poi_x >= LEVER_ARM_MIN &&
585  {
586  std::stringstream ss;
587  ss << "sipl, POI1, "
589  << ", "
591  << ", "
593  << " \x0D";
594  send(ss.str());
595  } else
596  {
597  node_->log(
599  "Please specify a correct value for poi_x, poi_y and poi_z in the config file under poi_lever_arm");
600  }
601  }
602 
603  // Setting the Velocity sensor lever arm offset
604  {
605  if (settings_->vsm_x >= LEVER_ARM_MIN &&
611  {
612  std::stringstream ss;
613  ss << "sivl, VSM1, "
615  << ", "
617  << ", "
619  << " \x0D";
620  send(ss.str());
621  } else
622  {
623  node_->log(
625  "Please specify a correct value for vsm_x, vsm_y and vsm_z in the config file under vsm_lever_arm");
626  }
627  }
628 
629  // Setting the INS Solution Reference Point: MainAnt or POI1
630  // First disable any existing INS sub-block connection
631  {
632  std::stringstream ss;
633  ss << "sinc, off, all, MainAnt \x0D";
634  send(ss.str());
635  }
636 
637  // INS solution reference point
638  {
639  std::stringstream ss;
640  if (settings_->ins_use_poi)
641  {
642  ss << "sinc, on, all, "
643  << "POI1"
644  << " \x0D";
645  send(ss.str());
646  } else
647  {
648  ss << "sinc, on, all, "
649  << "MainAnt"
650  << " \x0D";
651  send(ss.str());
652  }
653  }
654 
655  // Setting the INS heading
656  {
657  std::stringstream ss;
658  if (settings_->ins_initial_heading == "auto")
659  {
660  ss << "siih, " << settings_->ins_initial_heading << " \x0D";
661  send(ss.str());
662  } else if (settings_->ins_initial_heading == "stored")
663  {
664  ss << "siih, " << settings_->ins_initial_heading << " \x0D";
665  send(ss.str());
666  } else
667  {
669  "Invalid mode specified for ins_initial_heading.");
670  }
671  }
672 
673  // Setting the INS navigation filter
674  {
679  {
680  std::stringstream ss;
681  ss << "sism, "
683  << ", "
685  << " \x0D";
686  send(ss.str());
687  } else
688  {
690  "Please specify a valid AttStsDev and PosStdDev");
691  }
692  }
693  }
694 
695  // OSNMA
696  if (settings_->osnma.mode == "loose" || settings_->osnma.mode == "strict")
697  {
698  std::stringstream ss;
699  ss << "sou, " << settings_->osnma.mode << " \x0D";
700  send(ss.str());
701 
702  if (!settings_->osnma.ntp_server.empty())
703  {
704  std::stringstream ss;
705  ss << "snc, on, " << settings_->osnma.ntp_server << " \x0D";
706  send(ss.str());
707  } else
708  {
709  if (settings_->osnma.mode == "strict")
710  node_->log(
712  "OSNMA mode set to strict but no NTP server provided. In Strict mode an NTP server is mandatory!");
713  }
714  }
715 
716  // Setting up SBF blocks with rx_period_rest
717  {
718  std::stringstream blocks;
719  if (settings_->septentrio_receiver_type == "ins")
720  {
722  {
723  blocks << " +IMUSetup";
724  }
726  {
727  blocks << " +VelSensorSetup";
728  }
729  }
731  {
732  blocks << " +ReceiverStatus +QualityInd";
733  }
735  {
736  blocks << " +RFStatus";
737  }
739  (settings_->osnma.mode == "loose") ||
740  (settings_->osnma.mode == "strict"))
741  {
742  blocks << " +GALAuthStatus";
743  }
744 
745  blocks << " +ReceiverSetup";
746 
747  std::stringstream ss;
748  ss << "sso, Stream" << std::to_string(stream) << ", " << streamPort_
749  << "," << blocks.str() << ", " << rest_interval << "\x0D";
750  send(ss.str());
751  ++stream;
752  }
753 
754  // send command to trigger emission of receiver setup
755  send("sop, \"\", \"\" \x0D");
756 
757  // Setting up NMEA streams
758  {
759  if (settings_->septentrio_receiver_type == "ins")
760  send("snti, auto\x0D");
761  else
762  send("snti, GP\x0D");
763 
764  std::stringstream blocks;
766  {
767  blocks << " +GGA";
768  }
770  {
771  blocks << " +RMC";
772  }
774  {
775  blocks << " +GSA";
776  }
778  {
779  blocks << " +GSV";
780  }
781 
782  std::stringstream ss;
783  ss << "sno, Stream" << std::to_string(stream) << ", " << streamPort_
784  << "," << blocks.str() << ", " << pvt_interval << "\x0D";
785  send(ss.str());
786  ++stream;
787  }
788 
789  // Setting up SBF blocks with rx_period_pvt
790  {
791  std::stringstream blocks;
793  {
794  blocks << " +ReceiverTime";
795  }
797  {
798  blocks << " +PVTCartesian";
799  }
802  (settings_->septentrio_receiver_type == "gnss")) ||
804  (settings_->septentrio_receiver_type == "gnss")) ||
806  (settings_->septentrio_receiver_type == "gnss")) ||
808  (settings_->septentrio_receiver_type == "gnss")) ||
810  {
811  blocks << " +PVTGeodetic";
812  }
814  {
815  blocks << " +BaseVectorCart";
816  }
818  {
819  blocks << " +BaseVectorGeod";
820  }
822  {
823  blocks << " +PosCovCartesian";
824  }
827  (settings_->septentrio_receiver_type == "gnss")) ||
829  (settings_->septentrio_receiver_type == "gnss")) ||
831  (settings_->septentrio_receiver_type == "gnss")))
832  {
833  blocks << " +PosCovGeodetic";
834  }
836  {
837  blocks << " +VelCovCartesian";
838  }
841  (settings_->septentrio_receiver_type == "gnss")))
842  {
843  blocks << " +VelCovGeodetic";
844  }
847  (settings_->septentrio_receiver_type == "gnss")) ||
849  (settings_->septentrio_receiver_type == "gnss")))
850  {
851  blocks << " +AttEuler";
852  }
855  (settings_->septentrio_receiver_type == "gnss")) ||
857  (settings_->septentrio_receiver_type == "gnss")))
858  {
859  blocks << " +AttCovEuler";
860  }
862  {
863  blocks << " +MeasEpoch";
864  }
866  {
867  blocks << " +ChannelStatus +DOP";
868  }
869  // Setting SBF output of Rx depending on the receiver type
870  // If INS then...
871  if (settings_->septentrio_receiver_type == "ins")
872  {
876  {
877  blocks << " +INSNavCart";
878  }
885  {
886  blocks << " +INSNavGeod";
887  }
889  {
890  blocks << " +ExtEventINSNavGeod";
891  }
893  {
894  blocks << " +ExtEventINSNavCart";
895  }
897  {
898  blocks << " +ExtSensorMeas";
899  }
900  }
901  std::stringstream ss;
902  ss << "sso, Stream" << std::to_string(stream) << ", " << streamPort_
903  << "," << blocks.str() << ", " << pvt_interval << "\x0D";
904  send(ss.str());
905  ++stream;
906  }
907 
908  if (settings_->septentrio_receiver_type == "ins")
909  {
910  if (!settings_->ins_vsm_ip_server_id.empty())
911  {
912  send("siss, " + settings_->ins_vsm_ip_server_id + ", " +
913  std::to_string(settings_->ins_vsm_ip_server_port) +
914  ", TCP2Way \x0D");
915  send("sdio, IPS2, NMEA, none\x0D");
916  }
917  if (!settings_->ins_vsm_serial_port.empty())
918  {
919  if (settings_->ins_vsm_serial_port.rfind("COM", 0) == 0)
920  send("scs, " + settings_->ins_vsm_serial_port + ", baud" +
921  std::to_string(settings_->ins_vsm_serial_baud_rate) +
922  ", bits8, No, bit1, none\x0D");
923  send("sdio, " + settings_->ins_vsm_serial_port + ", NMEA\x0D");
924  }
925  if ((settings_->ins_vsm_ros_source == "odometry") ||
926  (settings_->ins_vsm_ros_source == "twist"))
927  {
928  std::string s;
929  s = "sdio, " + mainConnectionPort_ + ", NMEA, +NMEA +SBF\x0D";
930  send(s);
931  nmeaActivated_ = true;
932  }
933  }
934 
935  node_->log(log_level::DEBUG, "Leaving configureRx() method");
936  }
937 
938  void CommunicationCore::sendVelocity(const std::string& velNmea)
939  {
940  if (nmeaActivated_)
941  manager_.get()->send(velNmea);
942  }
943 
945  {
946  // Escape sequence (escape from correction mode), ensuring that we
947  // can send our real commands afterwards... has to be sent multiple times.
948  std::string cmd("\x0DSSSSSSSSSS\x0D\x0D");
950  manager_.get()->send(cmd);
951  std::ignore = telegramHandler_.getMainCd();
953  manager_.get()->send(cmd);
954  std::ignore = telegramHandler_.getMainCd();
956  manager_.get()->send(cmd);
957  return telegramHandler_.getMainCd();
958  }
959 
961  {
962  while (running_)
963  {
964  std::shared_ptr<Telegram> telegram;
965  telegramQueue_.pop(telegram);
966 
967  if (telegram->type != telegram_type::EMPTY)
969  }
970  }
971 
972  void CommunicationCore::send(const std::string& cmd)
973  {
974  manager_.get()->send(cmd);
976  }
977 
978 } // namespace io
Settings::osnma
Osnma osnma
OSNMA settings.
Definition: settings.hpp:225
log_level::DEBUG
@ DEBUG
Definition: typedefs.hpp:172
Settings::publish_pose
bool publish_pose
Whether or not to publish the PoseWithCovarianceStampedMsg message.
Definition: settings.hpp:288
PITCH_MIN
static const int8_t PITCH_MIN
Definition: communication_core.cpp:47
Settings::publish_attcoveuler
bool publish_attcoveuler
Whether or not to publish the AttCovEulerMsg message.
Definition: settings.hpp:266
io::CommunicationCore::resetSettings
void resetSettings()
Resets Rx settings.
Definition: communication_core.cpp:83
device_type::TCP
@ TCP
Definition: settings.hpp:115
Settings::ins_initial_heading
std::string ins_initial_heading
For heading computation when unit is powered-cycled.
Definition: settings.hpp:217
THETA_Y_MIN
static const int8_t THETA_Y_MIN
Definition: communication_core.cpp:41
io::UdpClient
Definition: io.hpp:58
Settings::latency_compensation
bool latency_compensation
Wether processing latency shall be compensated for in ROS timestamp.
Definition: settings.hpp:314
Settings::ins_vsm_ip_server_keep_open
bool ins_vsm_ip_server_keep_open
Wether VSM shall be kept open om shutdown.
Definition: settings.hpp:349
Osnma::keep_open
bool keep_open
Wether OSNMA shall be kept open on shutdown.
Definition: settings.hpp:44
Settings::publish_imusetup
bool publish_imusetup
Whether or not to publish the IMUSetupMsg message.
Definition: settings.hpp:272
Settings::publish_twist
bool publish_twist
Whether or not to publish the TwistWithCovarianceStampedMsg message.
Definition: settings.hpp:298
Settings::publish_localization
bool publish_localization
Whether or not to publish the LocalizationMsg message.
Definition: settings.hpp:294
Settings::pos_std_dev
float pos_std_dev
Position deviation mask.
Definition: settings.hpp:221
Rtk::serial
std::vector< RtkSerial > serial
Definition: settings.hpp:109
Settings::use_gnss_time
bool use_gnss_time
Definition: settings.hpp:312
io::TelegramHandler::resetWaitforMainCd
void resetWaitforMainCd()
Returns the connection descriptor.
Definition: telegram_handler.hpp:133
Settings::login_password
std::string login_password
Password for login.
Definition: settings.hpp:150
communication_core.hpp
Highest-Level view on communication services.
Settings::publish_pvtcartesian
bool publish_pvtcartesian
Definition: settings.hpp:243
Settings::udp_ip_server
std::string udp_ip_server
UDP IP server id.
Definition: settings.hpp:140
io::TelegramHandler::getMainCd
std::string getMainCd()
Returns the connection descriptor.
Definition: telegram_handler.hpp:136
io::AsyncManager
This is the central interface between ROSaic and the Rx(s), managing I/O operations such as reading m...
Definition: async_manager.hpp:108
Settings::publish_pvtgeodetic
bool publish_pvtgeodetic
Whether or not to publish the PVTGeodeticMsg message.
Definition: settings.hpp:245
Settings::heading_offset
double heading_offset
Attitude offset determination in longitudinal direction.
Definition: settings.hpp:209
ROSaicNodeBase::log
void log(log_level::LogLevel logLevel, const std::string &s) const
Log function to provide abstraction of ROS loggers.
Definition: typedefs.hpp:257
Settings::publish_gpgga
bool publish_gpgga
Whether or not to publish the GGA message.
Definition: settings.hpp:227
s
XmlRpcServer s
io::TelegramHandler::clearSemaphores
void clearSemaphores()
Definition: telegram_handler.hpp:121
Settings::publish_imu
bool publish_imu
Whether or not to publish the ImuMsg message.
Definition: settings.hpp:292
Osnma::ntp_server
std::string ntp_server
Server for NTP synchronization.
Definition: settings.hpp:42
Settings::multi_antenna
bool multi_antenna
INS multiantenna.
Definition: settings.hpp:213
Settings::vsm_x
double vsm_x
INS velocity sensor lever arm x-offset.
Definition: settings.hpp:203
Settings::udp_port
uint32_t udp_port
UDP port.
Definition: settings.hpp:136
parsing_utilities::convertUserPeriodToRxCommand
std::string convertUserPeriodToRxCommand(uint32_t period_user)
Transforms the input polling period [milliseconds] into a std::string number that can be appended to ...
Definition: parsing_utilities.cpp:400
ANGLE_MIN
static const int16_t ANGLE_MIN
Definition: communication_core.cpp:39
Settings::delta_n
float delta_n
Marker-to-ARP offset in the northward direction.
Definition: settings.hpp:169
Settings::polling_period_pvt
uint32_t polling_period_pvt
Polling period for PVT-related SBF blocks.
Definition: settings.hpp:163
Osnma::mode
std::string mode
OSNMA mode.
Definition: settings.hpp:40
io::CommunicationCore::tcpClient_
std::unique_ptr< AsyncManager< TcpIo > > tcpClient_
Definition: communication_core.hpp:169
io::CommunicationCore::nmeaActivated_
bool nmeaActivated_
Definition: communication_core.hpp:172
Settings::datum
std::string datum
Datum to be used.
Definition: settings.hpp:161
Settings::device_type
device_type::DeviceType device_type
Device type.
Definition: settings.hpp:130
Settings::theta_z
double theta_z
IMU orientation z-angle.
Definition: settings.hpp:189
Settings::publish_gpst
bool publish_gpst
Whether or not to publish the TimeReferenceMsg message with GPST.
Definition: settings.hpp:282
Settings::poi_y
double poi_y
INS POI offset in y-dimension.
Definition: settings.hpp:199
Settings::publish_galauthstatus
bool publish_galauthstatus
Whether or not to publish the GALAuthStatus message and diagnostics.
Definition: settings.hpp:240
POSSTD_DEV_MAX
static const int8_t POSSTD_DEV_MAX
Definition: communication_core.cpp:51
Settings::publish_velcovcartesian
bool publish_velcovcartesian
Definition: settings.hpp:259
Settings::publish_aimplusstatus
bool publish_aimplusstatus
Definition: settings.hpp:238
Settings::poi_x
double poi_x
INS POI offset in x-dimension.
Definition: settings.hpp:197
log_level::INFO
@ INFO
Definition: typedefs.hpp:173
io::CommunicationCore::configureRx
void configureRx()
Configures Rx: Which SBF/NMEA messages it should output and later correction settings.
Definition: communication_core.cpp:267
io::CommunicationCore::sendVelocity
void sendVelocity(const std::string &velNmea)
Hands over NMEA velocity message over to the send() method of manager_.
Definition: communication_core.cpp:938
ROSaicNodeBase
This class is the base class for abstraction.
Definition: typedefs.hpp:184
Settings::tcp_ip_server
std::string tcp_ip_server
TCP IP server id.
Definition: settings.hpp:144
ConcurrentQueue::pop
void pop(T &output) noexcept
Definition: telegram.hpp:191
Settings::theta_x
double theta_x
IMU orientation x-angle.
Definition: settings.hpp:185
Settings::publish_velsensorsetup
bool publish_velsensorsetup
Whether or not to publish the VelSensorSetupMsg message.
Definition: settings.hpp:274
Settings::theta_y
double theta_y
IMU orientation y-angle.
Definition: settings.hpp:187
Settings::ant_aux1_serial_nr
std::string ant_aux1_serial_nr
Serial number of your particular Aux1 antenna.
Definition: settings.hpp:181
ATTSTD_DEV_MIN
static const int8_t ATTSTD_DEV_MIN
Definition: communication_core.cpp:48
Settings::publish_exteventinsnavcart
bool publish_exteventinsnavcart
Whether or not to publish the ExtEventINSNavCartMsg message.
Definition: settings.hpp:278
Settings::publish_gpsfix
bool publish_gpsfix
Whether or not to publish the GpsFixMsg message.
Definition: settings.hpp:286
io::CommunicationCore::processingThread_
std::thread processingThread_
Processing thread.
Definition: communication_core.hpp:162
Settings::configure_rx
bool configure_rx
Definition: settings.hpp:159
Settings::publish_gpgsa
bool publish_gpgsa
Whether or not to publish the GSA message.
Definition: settings.hpp:231
io::CommunicationCore::settings_
const Settings * settings_
Settings.
Definition: communication_core.hpp:156
Settings::publish_velcovgeodetic
bool publish_velcovgeodetic
Definition: settings.hpp:262
Settings::tcp_port
uint32_t tcp_port
TCP port.
Definition: settings.hpp:142
Settings::publish_poscovgeodetic
bool publish_poscovgeodetic
Definition: settings.hpp:256
Settings::publish_tf_ecef
bool publish_tf_ecef
Whether or not to publish the tf of the localization.
Definition: settings.hpp:302
Settings::vsm_y
double vsm_y
INS velocity sensor lever arm y-offset.
Definition: settings.hpp:205
io::CommunicationCore::mainConnectionPort_
std::string mainConnectionPort_
Main communication port.
Definition: communication_core.hpp:178
Settings::publish_basevectorcart
bool publish_basevectorcart
Definition: settings.hpp:248
HEADING_MAX
static const int16_t HEADING_MAX
Definition: communication_core.cpp:44
Rtk::ntrip
std::vector< RtkNtrip > ntrip
Definition: settings.hpp:107
device_type::SBF_FILE
@ SBF_FILE
Definition: settings.hpp:117
io::CommunicationCore::CommunicationCore
CommunicationCore(ROSaicNodeBase *node)
Constructor of the class CommunicationCore.
Definition: communication_core.cpp:61
LEVER_ARM_MIN
static const int8_t LEVER_ARM_MIN
Definition: communication_core.cpp:43
Settings::vsm_z
double vsm_z
INS velocity sensor lever arm z-offset.
Definition: settings.hpp:207
Settings::pitch_offset
double pitch_offset
Attitude offset determination in latitudinal direction.
Definition: settings.hpp:211
string_utilities::trimDecimalPlaces
std::string trimDecimalPlaces(double num)
Trims decimal places to three.
Definition: string_utilities.cpp:190
Settings::ant_aux1_type
std::string ant_aux1_type
Definition: settings.hpp:177
Settings::publish_localization_ecef
bool publish_localization_ecef
Whether or not to publish the LocalizationMsg message.
Definition: settings.hpp:296
ROSaicNodeBase::isIns
bool isIns()
Check if Rx is INS.
Definition: typedefs.hpp:394
Settings::delta_u
float delta_u
Marker-to-ARP offset in the upward direction.
Definition: settings.hpp:171
io::CommunicationCore::node_
ROSaicNodeBase * node_
Pointer to Node.
Definition: communication_core.hpp:154
Settings::ins_use_poi
bool ins_use_poi
INS solution reference point.
Definition: settings.hpp:215
Settings::ant_serial_nr
std::string ant_serial_nr
Serial number of your particular Main antenna.
Definition: settings.hpp:179
PITCH_MAX
static const int8_t PITCH_MAX
Definition: communication_core.cpp:46
io
Definition: async_manager.hpp:83
Settings::ins_vsm_ip_server_port
uint32_t ins_vsm_ip_server_port
VSM tcp port.
Definition: settings.hpp:347
Settings::ant_type
std::string ant_type
Definition: settings.hpp:174
Settings::publish_atteuler
bool publish_atteuler
Whether or not to publish the AttEulerMsg message.
Definition: settings.hpp:264
Settings::login_user
std::string login_user
Username for login.
Definition: settings.hpp:148
THETA_Y_MAX
static const int8_t THETA_Y_MAX
Definition: communication_core.cpp:40
Settings::publish_basevectorgeod
bool publish_basevectorgeod
Whether or not to publish the BaseVectorGeodMsg message.
Definition: settings.hpp:250
Settings::publish_diagnostics
bool publish_diagnostics
Whether or not to publish the DiagnosticArrayMsg message.
Definition: settings.hpp:290
io::CommunicationCore::udpClient_
std::unique_ptr< UdpClient > udpClient_
Definition: communication_core.hpp:170
ConcurrentQueue::push
void push(const T &input) noexcept
Definition: telegram.hpp:181
Settings::polling_period_rest
uint32_t polling_period_rest
Polling period for all other SBF blocks and NMEA messages.
Definition: settings.hpp:165
Telegram
Definition: telegram.hpp:104
Rtk::ip_server
std::vector< RtkIpServer > ip_server
Definition: settings.hpp:108
log_level::ERROR
@ ERROR
Definition: typedefs.hpp:175
device_type::SERIAL
@ SERIAL
Definition: settings.hpp:116
ROSaicNodeBase::hasHeading
bool hasHeading()
Check if Rx has heading.
Definition: typedefs.hpp:399
Settings::poi_z
double poi_z
INS POI offset in z-dimension.
Definition: settings.hpp:201
Settings::ins_vsm_serial_port
std::string ins_vsm_serial_port
VSM serial port.
Definition: settings.hpp:351
ANGLE_MAX
static const int16_t ANGLE_MAX
Definition: communication_core.cpp:38
io::CommunicationCore::send
void send(const std::string &)
Hands over to the send() method of manager_.
Definition: communication_core.cpp:972
Settings::publish_poscovcartesian
bool publish_poscovcartesian
Definition: settings.hpp:253
Settings::publish_insnavcart
bool publish_insnavcart
Whether or not to publish the INSNavCartMsg message.
Definition: settings.hpp:268
Settings::delta_e
float delta_e
Marker-to-ARP offset in the eastward direction.
Definition: settings.hpp:167
device_type::PCAP_FILE
@ PCAP_FILE
Definition: settings.hpp:118
io::CommunicationCore::initializeIo
bool initializeIo()
Initializes the I/O handling.
Definition: communication_core.cpp:204
Settings::read_from_sbf_log
bool read_from_sbf_log
Whether or not we are reading from an SBF file.
Definition: settings.hpp:333
string_utilities::containsSpace
bool containsSpace(const std::string str)
Checks if a string contains spaces.
Definition: string_utilities.cpp:201
io::CommunicationCore::streamPort_
std::string streamPort_
Definition: communication_core.hpp:180
Settings::ant_lever_z
double ant_lever_z
INS antenna lever arm z-offset.
Definition: settings.hpp:195
Settings::publish_gprmc
bool publish_gprmc
Whether or not to publish the RMC message.
Definition: settings.hpp:229
Settings::ins_vsm_ros_source
std::string ins_vsm_ros_source
VSM source for INS.
Definition: settings.hpp:337
io::CommunicationCore::connect
void connect()
Connects the data stream.
Definition: communication_core.cpp:162
io::TelegramHandler::waitForCapabilities
void waitForCapabilities()
Waits for capabilities.
Definition: telegram_handler.hpp:146
Settings::ins_vsm_serial_baud_rate
uint32_t ins_vsm_serial_baud_rate
VSM serial baud rate.
Definition: settings.hpp:353
telegram_type::EMPTY
@ EMPTY
Definition: telegram.hpp:93
io::CommunicationCore::processTelegrams
void processTelegrams()
Definition: communication_core.cpp:960
log_level::WARN
@ WARN
Definition: typedefs.hpp:174
io::CommunicationCore::telegramQueue_
TelegramQueue telegramQueue_
TelegramQueue.
Definition: communication_core.hpp:158
Settings::publish_navsatfix
bool publish_navsatfix
Whether or not to publish the NavSatFixMsg message.
Definition: settings.hpp:284
Settings::reconnect_delay_s
float reconnect_delay_s
Definition: settings.hpp:153
Settings::publish_tf
bool publish_tf
Whether or not to publish the tf of the localization.
Definition: settings.hpp:300
Settings::udp_unicast_ip
std::string udp_unicast_ip
UDP unicast destination ip.
Definition: settings.hpp:138
POSSTD_DEV_MIN
static const int8_t POSSTD_DEV_MIN
Definition: communication_core.cpp:50
Settings::att_std_dev
float att_std_dev
Attitude deviation mask.
Definition: settings.hpp:219
io::CommunicationCore::running_
std::atomic< bool > running_
Indicator for threads to run.
Definition: communication_core.hpp:175
Settings::publish_measepoch
bool publish_measepoch
Whether or not to publish the MeasEpoch message.
Definition: settings.hpp:235
Settings::publish_exteventinsnavgeod
bool publish_exteventinsnavgeod
Whether or not to publish the ExtEventINSNavGeodMsg message.
Definition: settings.hpp:276
ATTSTD_DEV_MAX
static const int8_t ATTSTD_DEV_MAX
Definition: communication_core.cpp:49
Settings::ins_vsm_ip_server_id
std::string ins_vsm_ip_server_id
VSM IP server id.
Definition: settings.hpp:345
Settings::ant_lever_x
double ant_lever_x
INS antenna lever arm x-offset.
Definition: settings.hpp:191
Settings::publish_extsensormeas
bool publish_extsensormeas
Whether or not to publish the ExtSensorMeasMsg message.
Definition: settings.hpp:280
io::TelegramHandler::waitForResponse
void waitForResponse()
Waits for response.
Definition: telegram_handler.hpp:143
io::CommunicationCore::resetMainConnection
std::string resetMainConnection()
Reset main connection so it can receive commands.
Definition: communication_core.cpp:944
Settings::device
std::string device
Device.
Definition: settings.hpp:128
io::CommunicationCore::~CommunicationCore
~CommunicationCore()
Default destructor of the class CommunicationCore.
Definition: communication_core.cpp:71
HEADING_MIN
static const int16_t HEADING_MIN
Definition: communication_core.cpp:45
Settings::publish_insnavgeod
bool publish_insnavgeod
Whether or not to publish the INSNavGeodMsg message.
Definition: settings.hpp:270
Settings::rtk
Rtk rtk
RTK corrections settings.
Definition: settings.hpp:223
io::TelegramHandler::handleTelegram
void handleTelegram(const std::shared_ptr< Telegram > &telegram)
Called every time a telegram is received.
Definition: telegram_handler.cpp:41
Settings::septentrio_receiver_type
std::string septentrio_receiver_type
Septentrio receiver type, either "gnss" or "ins".
Definition: settings.hpp:308
io::CommunicationCore::manager_
std::unique_ptr< AsyncManagerBase > manager_
Definition: communication_core.hpp:167
t
geometry_msgs::TransformStamped t
LEVER_ARM_MAX
static const int8_t LEVER_ARM_MAX
Definition: communication_core.cpp:42
io::CommunicationCore::telegramHandler_
TelegramHandler telegramHandler_
TelegramHandler.
Definition: communication_core.hpp:160
Settings::read_from_pcap
bool read_from_pcap
Whether or not we are reading from a PCAP file.
Definition: settings.hpp:335
Settings::ins_vsm_serial_keep_open
bool ins_vsm_serial_keep_open
Wether VSM shall be kept open om shutdown.
Definition: settings.hpp:355
Settings::publish_gpgsv
bool publish_gpgsv
Whether or not to publish the GSV message.
Definition: settings.hpp:233
Settings::ant_lever_y
double ant_lever_y
INS antenna lever arm y-offset.
Definition: settings.hpp:193
io::CommunicationCore::initializedIo_
bool initializedIo_
Whether connecting was successful.
Definition: communication_core.hpp:164


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