ROSaic
rosaic_node.cpp
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // © Copyright 2020, Septentrio NV/SA.
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // 1. Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // 2. Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // 3. Neither the name of the copyright holder nor the names of its
14 // contributors may be used to endorse or promote products derived
15 // from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 // ****************************************************************************
30 
32 
40 {
41  ROS_DEBUG("Called ROSaicNode() constructor..");
42 
43  // Parameters must be set before initializing IO
44  connected_ = false;
45  getROSParams();
46 
47  // Initializes Connection
48  initializeIO();
49 
50  // Subscribes to all requested Rx messages by adding entries to the C++ multimap
51  // storing the callback handlers and publishes ROS messages
53 
54  // Sends commands to the Rx regarding which SBF/NMEA messages it should output and sets all
55  // its necessary corrections-related parameters
56  boost::mutex::scoped_lock lock(connection_mutex_);
57  connection_condition_.wait(lock, [this](){return connected_;});
58  configureRx();
59 
60  // Since we already have a ros::Spin() elsewhere, we use waitForShutdown() here
61  ros::waitForShutdown();
62  ROS_DEBUG("Leaving ROSaicNode() constructor..");
63 }
64 
71 {
72  ROS_DEBUG("Called configureRx() method");
73 
74  // It is imperative to hold a lock on the mutex "g_response_mutex" while modifying the variable
75  // "g_response_received". Same for "g_cd_mutex" and "g_cd_received".
76  boost::mutex::scoped_lock lock(g_response_mutex);
77  boost::mutex::scoped_lock lock_cd(g_cd_mutex);
78 
79  // Escape sequence (escape from correction mode), ensuring that we can send our real commands afterwards...
80  IO.send("\x0DSSSSSSSSSSSSSSSSSSS\x0D\x0D");
81  // We wait for the connection descriptor before we send another command, otherwise the latter would not be processed.
82  g_cd_condition.wait(lock_cd, [](){return g_cd_received;});
83  g_cd_received = false;
84 
85  // Turning off all current SBF/NMEA output
86  IO.send("sso, all, none, none, off \x0D");
87  g_response_condition.wait(lock, [](){return g_response_received;});
88  g_response_received = false;
89  IO.send("sno, all, none, none, off \x0D");
90  g_response_condition.wait(lock, [](){return g_response_received;});
91  g_response_received = false;
92 
93  // Setting the datum to be used by the Rx (not the NMEA output though, which only provides MSL and undulation
94  // (by default with respect to WGS84), but not ellipsoidal height)
95  {
96  std::stringstream ss;
97  ss << "sgd, " << datum_ << "\x0D";
98  IO.send(ss.str());
99  }
100  g_response_condition.wait(lock, [](){return g_response_received;});
101  g_response_received = false;
102 
103  // Setting SBF/NMEA output of Rx
104  unsigned stream = 1;
105  boost::smatch match;
106  boost::regex_match(device_, match, boost::regex("(tcp|udp)://(.+):(\\d+)"));
107  std::string proto(match[1]);
108  std::string rx_port;
109  if (proto == "tcp")
110  {
111  rx_port = g_rx_tcp_port;
112  }
113  else
114  {
115  rx_port = rx_serial_port_;
116  }
119  std::string pvt_sec_or_msec;
120  std::string rest_sec_or_msec;
121  if (polling_period_pvt_ == 1000 || polling_period_pvt_ == 2000 || polling_period_pvt_ == 5000 ||
122  polling_period_pvt_ == 10000) pvt_sec_or_msec = "sec";
123  else pvt_sec_or_msec = "msec";
124  if (polling_period_rest_ == 1000 || polling_period_rest_ == 2000 || polling_period_rest_ == 5000 ||
125  polling_period_rest_ == 10000) rest_sec_or_msec = "sec";
126  else rest_sec_or_msec = "msec";
127 
128 
129  if (publish_gpgga_ == true)
130  {
131  std::stringstream ss;
132 
133  ss << "sno, Stream" << std::to_string(stream) << ", " << rx_port << ", GGA, " << pvt_sec_or_msec <<
134  std::to_string(rx_period_pvt) << "\x0D";
135  IO.send(ss.str());
136  ++stream;
137  g_response_condition.wait(lock, [](){return g_response_received;});
138  g_response_received = false;
139  }
140  if (publish_gprmc_ == true)
141  {
142  std::stringstream ss;
143 
144  ss << "sno, Stream" << std::to_string(stream) << ", " << rx_port << ", RMC, " << pvt_sec_or_msec <<
145  std::to_string(rx_period_pvt) << "\x0D";
146  IO.send(ss.str());
147  ++stream;
148  g_response_condition.wait(lock, [](){return g_response_received;});
149  g_response_received = false;
150  }
151  if (publish_gpgsa_ == true)
152  {
153  std::stringstream ss;
154 
155  ss << "sno, Stream" << std::to_string(stream) << ", " << rx_port << ", GSA, " << pvt_sec_or_msec <<
156  std::to_string(rx_period_pvt) << "\x0D";
157  IO.send(ss.str());
158  ++stream;
159  g_response_condition.wait(lock, [](){return g_response_received;});
160  g_response_received = false;
161  }
162  if (publish_gpgsv_ == true)
163  {
164  std::stringstream ss;
165 
166  ss << "sno, Stream" << std::to_string(stream) << ", " << rx_port << ", GSV, " << rest_sec_or_msec <<
167  std::to_string(rx_period_rest) << "\x0D";
168  IO.send(ss.str());
169  ++stream;
170  g_response_condition.wait(lock, [](){return g_response_received;});
171  g_response_received = false;
172  }
173  if (publish_pvtcartesian_ == true)
174  {
175  std::stringstream ss;
176  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", PVTCartesian, " << pvt_sec_or_msec <<
177  std::to_string(rx_period_pvt) << "\x0D";
178  IO.send(ss.str());
179  ++stream;
180  g_response_condition.wait(lock, [](){return g_response_received;});
181  g_response_received = false;
182  }
183  if (publish_pvtgeodetic_ == true)
184  {
185  std::stringstream ss;
186  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", PVTGeodetic, " << pvt_sec_or_msec <<
187  std::to_string(rx_period_pvt) << "\x0D";
188  IO.send(ss.str());
189  ++stream;
190  g_response_condition.wait(lock, [](){return g_response_received;});
191  g_response_received = false;
192  }
193  if (publish_poscovcartesian_ == true)
194  {
195  std::stringstream ss;
196  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", PosCovCartesian, " << pvt_sec_or_msec <<
197  std::to_string(rx_period_pvt) << "\x0D";
198  IO.send(ss.str());
199  ++stream;
200  g_response_condition.wait(lock, [](){return g_response_received;});
201  g_response_received = false;
202  }
203  if (publish_poscovgeodetic_ == true)
204  {
205  std::stringstream ss;
206  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", PosCovGeodetic, " << pvt_sec_or_msec <<
207  std::to_string(rx_period_pvt) << "\x0D";
208  IO.send(ss.str());
209  ++stream;
210  g_response_condition.wait(lock, [](){return g_response_received;});
211  g_response_received = false;
212  }
213  if (publish_atteuler_ == true)
214  {
215  std::stringstream ss;
216  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", AttEuler, " << rest_sec_or_msec <<
217  std::to_string(rx_period_rest) << "\x0D";
218  IO.send(ss.str());
219  ++stream;
220  g_response_condition.wait(lock, [](){return g_response_received;});
221  g_response_received = false;
222  }
223  if (publish_attcoveuler_ == true)
224  {
225  std::stringstream ss;
226  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", AttCovEuler, " << rest_sec_or_msec <<
227  std::to_string(rx_period_rest) << "\x0D";
228  IO.send(ss.str());
229  ++stream;
230  g_response_condition.wait(lock, [](){return g_response_received;});
231  g_response_received = false;
232  }
233  if (g_publish_gpsfix == true)
234  {
235  std::stringstream ss;
236  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", ChannelStatus, " << pvt_sec_or_msec <<
237  std::to_string(rx_period_pvt) << "\x0D";
238  IO.send(ss.str());
239  ++stream;
240  g_response_condition.wait(lock, [](){return g_response_received;});
241  g_response_received = false;
242  ss.str(std::string()); // avoids invoking the std::string constructor
243  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", MeasEpoch, " << pvt_sec_or_msec <<
244  std::to_string(rx_period_pvt) << "\x0D";
245  IO.send(ss.str());
246  ++stream;
247  g_response_condition.wait(lock, [](){return g_response_received;});
248  g_response_received = false;
249  ss.str(std::string());
250  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", DOP, " << pvt_sec_or_msec <<
251  std::to_string(rx_period_pvt) << "\x0D";
252  IO.send(ss.str());
253  ++stream;
254  g_response_condition.wait(lock, [](){return g_response_received;});
255  g_response_received = false;
256  ss.str(std::string());
257  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", VelCovGeodetic, " << pvt_sec_or_msec <<
258  std::to_string(rx_period_pvt) << "\x0D";
259  IO.send(ss.str());
260  ++stream;
261  g_response_condition.wait(lock, [](){return g_response_received;});
262  g_response_received = false;
263  }
264  if (g_publish_diagnostics == true)
265  {
266  std::stringstream ss;
267  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", ReceiverStatus, " << rest_sec_or_msec <<
268  std::to_string(rx_period_rest) << "\x0D";
269  IO.send(ss.str());
270  ++stream;
271  g_response_condition.wait(lock, [](){return g_response_received;});
272  g_response_received = false;
273  ss.str(std::string());
274  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", QualityInd, " << rest_sec_or_msec <<
275  std::to_string(rx_period_rest) << "\x0D";
276  IO.send(ss.str());
277  ++stream;
278  g_response_condition.wait(lock, [](){return g_response_received;});
279  g_response_received = false;
280  ss.str(std::string());
281  ss << "sso, Stream" << std::to_string(stream) << ", " << rx_port << ", ReceiverSetup, " << rest_sec_or_msec <<
282  std::to_string(rx_period_rest) << "\x0D";
283  IO.send(ss.str());
284  ++stream;
285  g_response_condition.wait(lock, [](){return g_response_received;});
286  g_response_received = false;
287  }
288 
289  // Setting the marker-to-ARP offsets. This comes after the "sso, ..., ReceiverSetup, ..." command,
290  // since the latter is only generated when a user-command is entered to change one or more values in the block.
291  {
292  std::stringstream ss;
293  ss << "sao, Main, " << string_utilities::trimString(std::to_string(delta_e_)) << ", " <<
294  string_utilities::trimString(std::to_string(delta_n_)) << ", " <<
295  string_utilities::trimString(std::to_string(delta_u_)) << ", \"" << ant_type_ << "\", \"" << ant_serial_nr_ <<
296  "\", 0 \x0D";
297  IO.send(ss.str());
298  }
299  g_response_condition.wait(lock, [](){return g_response_received;});
300  g_response_received = false;
301 
302  // Configuring the NTRIP connection
303  // First disable any existing NTRIP connection on NTR1
304  {
305  std::stringstream ss;
306  ss << "snts, NTR1, off \x0D";
307  IO.send(ss.str());
308  }
309  g_response_condition.wait(lock, [](){return g_response_received;});
310  g_response_received = false;
311  if (rx_has_internet_)
312  {
313  if (mode_ == "off")
314  {
315  }
316  else if (mode_ == "Client")
317  {
318  {
319  std::stringstream ss;
320  ss << "snts, NTR1, " << mode_ << ", " << caster_ << ", " << std::to_string(caster_port_) << ", " <<
321  username_ << ", " << password_ << ", " << mountpoint_ << ", " << ntrip_version_ << ", " << send_gga_ << " \x0D";
322  IO.send(ss.str());
323  }
324  g_response_condition.wait(lock, [](){return g_response_received;});
325  g_response_received = false;
326  }
327  else if (mode_ == "Client-Sapcorda")
328  {
329  {
330  std::stringstream ss;
331  ss << "snts, NTR1, Client-Sapcorda, , , , , , , , \x0D";
332  IO.send(ss.str());
333  }
334  g_response_condition.wait(lock, [](){return g_response_received;});
335  g_response_received = false;
336  }
337  else
338  {
339  ROS_ERROR("Invalid mode specified for NTRIP settings.");
340  }
341  }
342  else // Since the Rx does not have internet (and you will not be able to share it via USB),
343  //we need to forward the corrections ourselves, though not on the same port.
344  {
345  if (proto == "tcp")
346  {
347  {
348  std::stringstream ss;
349  // In case IPS1 was used before, old configuration is lost of course.
350  ss << "siss, IPS1, " << std::to_string(rx_input_corrections_tcp_) << ", TCP2Way \x0D";
351  IO.send(ss.str());
352  }
353  g_response_condition.wait(lock, [](){return g_response_received;});
354  g_response_received = false;
355  {
356  std::stringstream ss;
357  ss << "sno, Stream" << std::to_string(stream) << ", IPS1, GGA, " << pvt_sec_or_msec <<
358  std::to_string(rx_period_pvt) << " \x0D";
359  ++stream;
360  IO.send(ss.str());
361  }
362  g_response_condition.wait(lock, [](){return g_response_received;});
363  g_response_received = false;
364  }
365  {
366  std::stringstream ss;
367  if (proto == "tcp")
368  {
369  ss << "sdio, IPS1, " << rtcm_version_ << ", +SBF+NMEA \x0D";
370  }
371  else
372  {
373  ss << "sdio, " << rx_input_corrections_serial_ << ", " << rtcm_version_ << ", +SBF+NMEA \x0D";
374  }
375  IO.send(ss.str());
376  }
377  }
378  ROS_DEBUG("Leaving configureRx() method");
379 }
381 {
382  // Communication parameters
383  g_nh->param("device", device_, std::string("/dev/ttyACM0"));
384  getROSInt("serial/baudrate", baudrate_, static_cast<uint32_t>(115200));
385  g_nh->param("serial/hw_flow_control", hw_flow_control_, std::string("off"));
386  g_nh->param("serial/rx_serial_port", rx_serial_port_, std::string("USB1"));
387 
388  g_nh->param("reconnect_delay_s", reconnect_delay_s_, 4.0f);
389 
390  // Polling period parameters
391  getROSInt("polling_period/pvt", polling_period_pvt_, static_cast<uint32_t>(1000));
393  && polling_period_pvt_ != 200 && polling_period_pvt_ != 250 && polling_period_pvt_ != 500 && polling_period_pvt_ != 1000
394  && polling_period_pvt_ != 2000 && polling_period_pvt_ != 5000 && polling_period_pvt_ != 10000)
395  {
396  ROS_ERROR("Please specify a valid polling period for PVT-related SBF blocks and NMEA messages.");
397  }
398  getROSInt("polling_period/rest", polling_period_rest_, static_cast<uint32_t>(1000));
401  && polling_period_rest_ != 2000 && polling_period_rest_ != 5000 && polling_period_rest_ != 10000)
402  {
403  ROS_ERROR("Please specify a valid polling period for PVT-unrelated SBF blocks and NMEA messages.");
404  }
405 
406  // Datum and marker-to-ARP offset
407  g_nh->param("datum", datum_, std::string("ETRS89"));
408  g_nh->param("ant_type", ant_type_, std::string("Unknown"));
409  g_nh->param("ant_serial_nr", ant_serial_nr_, std::string("Unknown"));
410  g_nh->param("marker_to_arp/delta_e", delta_e_, 0.0f);
411  g_nh->param("marker_to_arp/delta_n", delta_n_, 0.0f);
412  g_nh->param("marker_to_arp/delta_u", delta_u_, 0.0f);
413 
414  // Correction service parameters
415  g_nh->param("ntrip_settings/mode", mode_, std::string("off"));
416  g_nh->param("ntrip_settings/caster", caster_, std::string());
417  getROSInt("ntrip_settings/caster_port", caster_port_, static_cast<uint32_t>(0));
418  g_nh->param("ntrip_settings/username", username_, std::string());
419  g_nh->param("ntrip_settings/password", password_, std::string());
420  g_nh->param("ntrip_settings/mountpoint", mountpoint_, std::string());
421  g_nh->param("ntrip_settings/ntrip_version", ntrip_version_, std::string("v2"));
422  g_nh->param("ntrip_settings/send_gga", send_gga_, std::string("auto"));
423  g_nh->param("ntrip_settings/rx_has_internet", rx_has_internet_, false);
424  g_nh->param("ntrip_settings/rtcm_version", rtcm_version_, std::string("RTCMv3"));
425  getROSInt("ntrip_settings/rx_input_corrections_tcp", rx_input_corrections_tcp_, static_cast<uint32_t>(28785));
426  g_nh->param("ntrip_settings/rx_input_corrections_serial", rx_input_corrections_serial_, std::string("USB2"));
427 
428  // Publishing parameters, the others remained global since they need to be accessed in the callbackhandlers.hpp file
429  g_nh->param("publish/gpgga", publish_gpgga_, true);
430  g_nh->param("publish/gprmc", publish_gprmc_, true);
431  g_nh->param("publish/gpgsa", publish_gpgsa_, true);
432  g_nh->param("publish/gpgsv", publish_gpgsv_, true);
433  g_nh->param("publish/pvtcartesian", publish_pvtcartesian_, true);
434  g_nh->param("publish/pvtgeodetic", publish_pvtgeodetic_, true);
435  g_nh->param("publish/poscovcartesian", publish_poscovcartesian_, true);
436  g_nh->param("publish/poscovgeodetic", publish_poscovgeodetic_, true);
437  g_nh->param("publish/atteuler", publish_atteuler_, true);
438  g_nh->param("publish/attcoveuler", publish_attcoveuler_, true);
439 
440  // To be implemented: RTCM, setting datum, raw data settings, PPP, SBAS, fix mode...
441  ROS_DEBUG("Finished getROSParams() method");
442 
443 }
444 
446 {
447  ROS_DEBUG("Called initializeIO() method");
448  boost::smatch match;
449  // In fact: smatch is a typedef of match_results<string::const_iterator>
450  if (boost::regex_match(device_, match, boost::regex("(tcp)://(.+):(\\d+)")))
451  // \d means decimal, however, in the regular expression, the \ is a special character, which needs
452  // to be escaped on its own as well..
453  // Note that regex_match can be used with a smatch object to store results, or without. In any case,
454  // true is returned if and only if it matches the !complete! string.
455  {
456  // The first sub_match (index 0) contained in a match_result always represents the full match
457  // within a target sequence made by a regex, and subsequent sub_matches represent sub-expression
458  // matches corresponding in sequence to the left parenthesis delimiting the sub-expression in the regex,
459  // i.e. $n Perl is equivalent to m[n] in boost regex.
460  std::string proto(match[1]);
461  if (proto == "tcp")
462  {
463  tcp_host_ = match[2];
464  tcp_port_ = match[3];
465 
466  serial_ = false;
467  boost::thread temporary_thread(boost::bind(&ROSaicNode::connect, this));
468  temporary_thread.detach();
469  }
470  else
471  {
472  {
473  std::stringstream ss;
474  ss << "Protocol '" << proto << "' is unsupported.";
475  ROS_ERROR("%s", ss.str().c_str());
476  }
477  }
478  }
479  else
480  {
481  serial_ = true;
482  boost::thread temporary_thread(boost::bind(&ROSaicNode::connect, this));
483  temporary_thread.detach();
484  }
485  ROS_DEBUG("Leaving initializeIO() method");
486 }
487 
489 {
490  ROS_DEBUG("Called connect() method");
491  ROS_DEBUG("Setting ROS timer for calling reconnect() method until connection succeds");
492  reconnect_timer_ = g_nh->createTimer(ros::Duration(reconnect_delay_s_), &ROSaicNode::reconnect, this);
493  reconnect_timer_.start();
494  ROS_DEBUG("Started ROS timer for calling reconnect() method until connection succeds");
495  ros::spin();
496  ROS_DEBUG("Leaving connect() method"); // This will never be output since ros::spin() is on the line above.
497 }
498 
501 void rosaic_node::ROSaicNode::reconnect(const ros::TimerEvent& event)
502 {
503  ROS_DEBUG("Called reconnect() method");
504  if (connected_ == true)
505  {
506  reconnect_timer_.stop();
507  ROS_DEBUG("Stopped ROS timer since successully connected.");
508  }
509  else
510  {
511  if (serial_)
512  {
513  bool initialize_serial_return = false;
514  try
515  {
516  ROS_INFO("Connecting serially to device %s, targeted baudrate: %u", device_.c_str(), baudrate_);
517  initialize_serial_return = IO.initializeSerial(device_, baudrate_, hw_flow_control_);
518  }
519  catch (std::runtime_error& e)
520  {
521  {
522  std::stringstream ss;
523  ss << "IO.initializeSerial() failed for device " << device_ << " due to: " << e.what();
524  ROS_ERROR("%s", ss.str().c_str());
525  }
526  }
527  if (initialize_serial_return)
528  {
529  boost::mutex::scoped_lock lock(connection_mutex_);
530  connected_ = true;
531  lock.unlock();
532  connection_condition_.notify_one();
533  }
534  }
535  else
536  {
537  bool initialize_tcp_return = false;
538  try
539  {
540  ROS_INFO("Connecting to tcp://%s:%s ...", tcp_host_.c_str(), tcp_port_.c_str());
541  initialize_tcp_return = IO.initializeTCP(tcp_host_, tcp_port_);
542  }
543  catch (std::runtime_error& e)
544  {
545  {
546  std::stringstream ss;
547  ss << "IO.initializeTCP() failed for host " << tcp_host_ << " on port " << tcp_port_ <<
548  " due to: " << e.what();
549  ROS_ERROR("%s", ss.str().c_str());
550  }
551  }
552  if (initialize_tcp_return)
553  {
554  boost::mutex::scoped_lock lock(connection_mutex_);
555  connected_ = true;
556  lock.unlock();
557  connection_condition_.notify_one();
558  }
559  }
560  }
561  ROS_DEBUG("Leaving reconnect() method");
562 }
563 
570 {
571  ROS_DEBUG("Called defineMessages() method");
572 
573  if (publish_gpgga_ == true)
574  {
575  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::Gpgga>("$GPGGA");
576  }
577  if (publish_gprmc_ == true)
578  {
579  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::Gprmc>("$GPRMC");
580  }
581  if (publish_gpgsa_ == true)
582  {
583  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::Gpgsa>("$GPGSA");
584  }
585  if (publish_gpgsv_ == true)
586  {
587  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::Gpgsv>("$GPGSV");
588  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::Gpgsv>("$GLGSV");
589  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::Gpgsv>("$GAGSV");
590  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::Gpgsv>("$GBGSV");
591  }
592  if (publish_pvtcartesian_ == true)
593  {
594  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::PVTCartesian>("4006");
595  }
596  if (publish_pvtgeodetic_ == true)
597  {
598  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::PVTGeodetic>("4007");
599  }
600  if (publish_poscovcartesian_ == true)
601  {
602  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::PosCovCartesian>("5905");
603  }
604  if (publish_poscovgeodetic_ == true)
605  {
606  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::PosCovGeodetic>("5906");
607  }
608  if (publish_atteuler_ == true)
609  {
610  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::AttEuler>("5938");
611  }
612  if (publish_attcoveuler_ == true)
613  {
614  IO.handlers_.callbackmap_ = IO.getHandlers().insert<rosaic::AttCovEuler>("5939");
615  }
616  if (g_publish_gpst == true)
617  {
618  IO.handlers_.callbackmap_ = IO.getHandlers().insert<int32_t>("GPST");
619  }
620  if (g_publish_navsatfix == true)
621  {
622  if (publish_pvtgeodetic_ == false || publish_poscovgeodetic_ == false)
623  {
624  ROS_ERROR("For a proper NavSatFix message, please set the publish/pvtgeodetic and the publish/poscovgeodetic ROSaic parameters both to true.");
625  }
626  IO.handlers_.callbackmap_ = IO.getHandlers().insert<sensor_msgs::NavSatFix>("NavSatFix");
627  }
628  if (g_publish_gpsfix == true)
629  {
630  if (publish_pvtgeodetic_ == false || publish_poscovgeodetic_ == false)
631  {
632  ROS_ERROR("For a proper GPSFix message, please set the publish/pvtgeodetic and the publish/poscovgeodetic ROSaic parameters both to true.");
633  }
634  IO.handlers_.callbackmap_ = IO.getHandlers().insert<gps_common::GPSFix>("GPSFix");
635  // The following blocks are never published, yet are needed for the construction of the GPSFix message, hence we have empty callbacks.
636  IO.handlers_.callbackmap_ = IO.getHandlers().insert<int32_t>("4013"); // ChannelStatus block
637  IO.handlers_.callbackmap_ = IO.getHandlers().insert<int32_t>("4027"); // MeasEpoch block
638  IO.handlers_.callbackmap_ = IO.getHandlers().insert<int32_t>("4001"); // DOP block
639  IO.handlers_.callbackmap_ = IO.getHandlers().insert<int32_t>("5908"); // VelCovGeodetic block
640  }
641  if (g_publish_pose == true)
642  {
643  if (publish_pvtgeodetic_ == false || publish_poscovgeodetic_ == false || publish_atteuler_ == false ||
644  publish_attcoveuler_ == false)
645  {
646  ROS_ERROR("For a proper PoseWithCovarianceStamped message, please set the publish/pvtgeodetic, publish/poscovgeodetic, publish_atteuler and publish_attcoveuler ROSaic parameters all to true.");
647  }
648  IO.handlers_.callbackmap_ = IO.getHandlers().insert<geometry_msgs::PoseWithCovarianceStamped>("PoseWithCovarianceStamped");
649  }
650  if (g_publish_diagnostics == true)
651  {
652  IO.handlers_.callbackmap_ = IO.getHandlers().insert<diagnostic_msgs::DiagnosticArray>("DiagnosticArray");
653  IO.handlers_.callbackmap_ = IO.getHandlers().insert<int32_t>("4014"); // ReceiverStatus block
654  IO.handlers_.callbackmap_ = IO.getHandlers().insert<int32_t>("4082"); // QualityInd block
655  IO.handlers_.callbackmap_ = IO.getHandlers().insert<int32_t>("5902"); // ReceiverSetup block
656  }
657  // so on and so forth...
658  ROS_DEBUG("Leaving defineMessages() method");
659 }
660 
661 
677 std::string g_frame_id;
679 uint32_t g_leap_seconds;
681 boost::mutex g_response_mutex;
685 boost::condition_variable g_response_condition;
687 boost::mutex g_cd_mutex;
691 boost::condition_variable g_cd_condition;
696 std::string g_rx_tcp_port;
698 uint32_t g_cd_count;
732 std::map<std::string, uint32_t> g_GPSFixMap;
734 std::map<std::string, uint32_t> g_NavSatFixMap;
736 std::map<std::string, uint32_t> g_PoseWithCovarianceStampedMap;
738 std::map<std::string, uint32_t> g_DiagnosticArrayMap;
743 boost::shared_ptr<ros::NodeHandle> g_nh;
745 const uint32_t g_ROS_QUEUE_SIZE = 1;
746 
747 int main(int argc, char** argv)
748 {
749  ros::init(argc, argv, "septentrio_gnss");
750  g_nh.reset(new ros::NodeHandle("~"));
751  g_nh->param("use_gnss_time", g_use_gnss_time, true);
752  g_nh->param("frame_id", g_frame_id, (std::string) "gnss");
753  g_nh->param("publish/gpst", g_publish_gpst, true);
754  g_nh->param("publish/navsatfix", g_publish_navsatfix, true);
755  g_nh->param("publish/gpsfix", g_publish_gpsfix, true);
756  g_nh->param("publish/pose", g_publish_pose, true);
757  g_nh->param("publish/diagnostics", g_publish_diagnostics, true);
758  rosaic_node::getROSInt("leap_seconds", g_leap_seconds, static_cast<uint32_t>(18));
759 
760  g_response_received = false;
761  g_cd_received = false;
762  g_read_cd = true;
763  g_cd_count = 0;
766  g_dop_has_arrived_gpsfix = false;
780 
781  // The info logging level seems to be default, hence we modify log level momentarily..
782  // The following is the C++ version of rospy.init_node('my_ros_node', log_level=rospy.DEBUG)
783  if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
784  ros::console::levels::Debug)) //debug is lowest level, shows everything
785  ros::console::notifyLoggerLevelsChanged();
786 
787  rosaic_node::ROSaicNode rx_node; // This launches everything we need, in theory :)
788  return 0;
789 }
std::string rtcm_version_
RTCM version for NTRIP service (if Rx does not have internet)
bool g_atteuler_has_arrived_gpsfix
For GPSFix: Whether the AttEuler block of the current epoch has arrived or not.
uint32_t polling_period_pvt_
Polling period for PVT-related SBF blocks.
boost::shared_ptr< ros::NodeHandle > g_nh
std::string password_
Password for NTRIP service.
std::string g_frame_id
The frame ID used in the header of every published ROS message.
bool publish_attcoveuler_
Whether or not to publish the rosaic::AttCovEuler message.
bool g_channelstatus_has_arrived_gpsfix
For GPSFix: Whether the ChannelStatus block of the current epoch has arrived or not.
boost::mutex connection_mutex_
bool publish_gprmc_
Whether or not to publish the RMC message.
bool serial_
Whether yet-to-be-established connection to Rx will be serial or TCP.
std::map< std::string, uint32_t > g_PoseWithCovarianceStampedMap
A C++ map for keeping track of SBF blocks necessary to construct the PoseWithCovarianceStamped ROS me...
uint32_t g_leap_seconds
The number of leap seconds that have been inserted into the UTC time.
bool g_cd_received
Determines whether the connection descriptor was received from the Rx.
bool g_dop_has_arrived_gpsfix
For GPSFix: Whether the DOP block of the current epoch has arrived or not.
std::map< std::string, uint32_t > g_GPSFixMap
A C++ map for keeping track of the SBF blocks necessary to construct the GPSFix ROS message...
boost::mutex g_response_mutex
Mutex to control changes of global variable "g_response_received".
std::string g_rx_tcp_port
Rx TCP port, e.g. IP10 or IP11, to which ROSaic is connected to.
uint32_t polling_period_rest_
Polling period for all other SBF blocks and NMEA messages.
bool rx_has_internet_
Whether Rx has internet or not.
bool publish_pvtcartesian_
Whether or not to publish the rosaic::PVTCartesian message.
bool publish_poscovcartesian_
Whether or not to publish the rosaic::PosCovCartesian message.
bool g_attcoveuler_has_arrived_pose
For PoseWithCovarianceStamped: Whether the AttCovEuler block of the current epoch has arrived or not...
CallbackMap insert(std::string message_key)
Adds a pair to the multimap "callbackmap_", with the message_key being the key.
std::string ant_type_
Antenna type, from the list returned by the command "lstAntennaInfo, Overview".
bool initializeTCP(std::string host, std::string port)
Initializes the TCP I/O.
int main(int argc, char **argv)
std::string rx_serial_port_
In case of serial communication to Rx, rx_serial_port_ specifies Rx&#39;s serial port connected to...
CallbackHandlers getHandlers() const
bool g_attcoveuler_has_arrived_gpsfix
For GPSFix: Whether the AttCovEuler block of the current epoch has arrived or not.
void connect()
Calles the reconnect() method.
void getROSParams()
Gets the node parameters from the ROS Parameter Server, parts of which are specified in a YAML file...
uint32_t rx_input_corrections_tcp_
Rx TCP port number, e.g. 28785, on which Rx receives the corrections (can&#39;t be the same as main conne...
bool publish_poscovgeodetic_
Whether or not to publish the rosaic::PosCovGeodetic message.
bool publish_atteuler_
Whether or not to publish the rosaic::AttEuler message.
bool g_qualityind_has_arrived_diagnostics
For DiagnosticArray: Whether the QualityInd block of the current epoch has arrived or not...
float delta_n_
Marker-to-ARP offset in the northward direction.
bool g_publish_diagnostics
Whether or not to publish the diagnostic_msgs::DiagnosticArray message.
bool initializeSerial(std::string port, uint32_t baudrate=115200, std::string flowcontrol="None")
Initializes the serial port.
bool publish_pvtgeodetic_
Whether or not to publish the rosaic::PVTGeodetic message.
void send(std::string cmd)
void defineMessages()
Defines which Rx messages to read and which ROS messages to publish.
void initializeIO()
Initializes the I/O handling.
bool publish_gpgsv_
Whether or not to publish the GSV message.
bool g_publish_pose
Whether or not to publish the geometry_msgs::PoseWithCovarianceStamped message.
uint32_t caster_port_
IP port number of NTRIP caster to connect to.
std::string tcp_port_
TCP port number.
boost::condition_variable connection_condition_
boost::mutex g_cd_mutex
Mutex to control changes of global variable "g_cd_received".
std::string ant_serial_nr_
Serial number of your particular antenna.
void reconnect(const ros::TimerEvent &event)
Attempts to (re)connect every reconnect_delay_s_ seconds.
uint32_t baudrate_
Baudrate.
std::string username_
Username for NTRIP service.
std::string mountpoint_
Mountpoint for NTRIP service.
std::map< std::string, uint32_t > g_NavSatFixMap
A C++ map for keeping track of the SBF blocks necessary to construct the NavSatFix ROS message...
bool g_poscovgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PosCovGeodetic block of the current epoch has arrived or not.
bool g_velcovgeodetic_has_arrived_gpsfix
For GPSFix: Whether the VelCovGeodetic block of the current epoch has arrived or not.
bool publish_gpgga_
Whether or not to publish the GGA message.
bool getROSInt(const std::string &key, U &u)
Gets an integer or unsigned integer value from the parameter server.
float reconnect_delay_s_
Delay in seconds between reconnection attempts to the connection type specified in the parameter conn...
bool g_read_cd
std::string tcp_host_
Host name of TCP server.
boost::condition_variable g_cd_condition
Condition variable complementing "g_cd_mutex".
std::string mode_
Type of NTRIP connection.
std::string send_gga_
Whether (and at which rate) or not to send GGA to the NTRIP caster.
bool g_poscovgeodetic_has_arrived_navsatfix
For NavSatFix: Whether the PosCovGeodetic block of the current epoch has arrived or not...
uint32_t convertUserPeriodToRxCommand(uint32_t period_user)
Transforms the input polling period [milliseconds] into a uint32_t number that can be appended to eit...
bool connected_
Whether connecting to Rx was successful.
bool g_atteuler_has_arrived_pose
For PoseWithCovarianceStamped: Whether the AttEuler block of the current epoch has arrived or not...
bool g_measepoch_has_arrived_gpsfix
For GPSFix: Whether the MeasEpoch block of the current epoch has arrived or not.
void configureRx()
Configures Rx: Which SBF/NMEA messages it should output and later correction settings.
Definition: rosaic_node.cpp:70
bool g_publish_navsatfix
Whether or not to publish the sensor_msgs::NavSatFix message.
bool g_pvtgeodetic_has_arrived_navsatfix
For NavSatFix: Whether the PVTGeodetic block of the current epoch has arrived or not.
float delta_e_
Marker-to-ARP offset in the eastward direction.
std::string device_
Device port.
CallbackHandlers handlers_
Callback handlers for the inwards streaming messages.
std::string rx_input_corrections_serial_
Rx serial port, e.g. USB2, on which Rx receives the corrections (can&#39;t be the same as main connection...
bool g_publish_gpsfix
Whether or not to publish the gps_common::GPSFix message.
bool g_receiverstatus_has_arrived_diagnostics
For DiagnosticArray: Whether the ReceiverStatus block of the current epoch has arrived or not...
bool g_use_gnss_time
std::string trimString(std::string str)
Removes trailing zeros from a string representing a float or double except for the first zero after t...
bool publish_gpgsa_
Whether or not to publish the GSA message.
std::string datum_
Datum to be used.
std::string caster_
Hostname or IP address of the NTRIP caster to connect to.
bool g_poscovgeodetic_has_arrived_pose
For PoseWithCovarianceStamped: Whether the PosCovGeodetic block of the current epoch has arrived or n...
ros::Timer reconnect_timer_
Our ROS timer governing the reconnection.
std::map< std::string, uint32_t > g_DiagnosticArrayMap
A C++ map for keeping track of SBF blocks necessary to construct the DiagnosticArray ROS message...
std::string ntrip_version_
NTRIP version for NTRIP service.
This class represents the ROsaic node, to be extended..
bool g_response_received
Determines whether a command reply was received from the Rx.
bool g_pvtgeodetic_has_arrived_pose
For PoseWithCovarianceStamped: Whether the PVTGeodetic block of the current epoch has arrived or not...
const uint32_t g_ROS_QUEUE_SIZE
Queue size for ROS publishers.
bool g_publish_gpst
Whether or not to publish the sensor_msgs::TimeReference message with GPST.
uint32_t g_cd_count
Since after SSSSSSSSSSS we need to wait for second connection descriptor, we have to count the connec...
boost::condition_variable g_response_condition
Condition variable complementing "g_response_mutex".
The heart of the ROSaic driver: The ROS node that represents it.
io_comm_rx::Comm_IO IO
Handles communication with the Rx.
Definition: rosaic_node.hpp:96
float delta_u_
Marker-to-ARP offset in the upward direction.
bool g_pvtgeodetic_has_arrived_gpsfix
For GPSFix: Whether the PVTGeodetic block of the current epoch has arrived or not.
std::string hw_flow_control_
HW flow control.