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 
31 // Eigen include
32 #include <Eigen/Geometry>
34 
41 rosaic_node::ROSaicNode::ROSaicNode() : IO_(this, &settings_)
42 {
43  param("activate_debug_log", settings_.activate_debug_log, false);
45  {
48  ros::console::levels::Debug)) // debug is lowest level, shows
49  // everything
51  }
52 
53  this->log(LogLevel::DEBUG, "Called ROSaicNode() constructor..");
54 
56 
57  // Parameters must be set before initializing IO
58  if (!getROSParams())
59  return;
60 
61  // Initializes Connection
62  IO_.initializeIO();
63 
64  // Subscribes to all requested Rx messages by adding entries to the C++ multimap
65  // storing the callback handlers and publishes ROS messages
67 
68  // Sends commands to the Rx regarding which SBF/NMEA messages it should output
69  // and sets all its necessary corrections-related parameters
71  {
72  IO_.configureRx();
73  }
74 
75  this->log(LogLevel::DEBUG, "Leaving ROSaicNode() constructor..");
76 }
77 
79 {
80  param("use_gnss_time", settings_.use_gnss_time, true);
81  param("frame_id", settings_.frame_id, (std::string) "gnss");
82  param("imu_frame_id", settings_.imu_frame_id, (std::string) "imu");
83  param("poi_frame_id", settings_.poi_frame_id, (std::string) "base_link");
84  param("vsm_frame_id", settings_.vsm_frame_id, (std::string) "vsm");
85  param("aux1_frame_id", settings_.aux1_frame_id, (std::string) "aux1");
86  param("vehicle_frame_id", settings_.vehicle_frame_id, settings_.poi_frame_id);
87  param("local_frame_id", settings_.local_frame_id, (std::string) "odom");
88  param("insert_local_frame", settings_.insert_local_frame, false);
89  param("lock_utm_zone", settings_.lock_utm_zone, true);
90  param("leap_seconds", settings_.leap_seconds, -128);
91 
92  // Communication parameters
93  param("device", settings_.device, std::string("/dev/ttyACM0"));
94  getUint32Param("serial/baudrate", settings_.baudrate,
95  static_cast<uint32_t>(921600));
96  param("serial/hw_flow_control", settings_.hw_flow_control, std::string("off"));
97  param("serial/rx_serial_port", settings_.rx_serial_port, std::string("USB1"));
98  param("login/user", settings_.login_user, std::string(""));
99  param("login/password", settings_.login_password, std::string(""));
100  settings_.reconnect_delay_s = 2.0f; // Removed from ROS parameter list.
101  param("receiver_type", settings_.septentrio_receiver_type, std::string("gnss"));
102  if (!((settings_.septentrio_receiver_type == "gnss") ||
103  (settings_.septentrio_receiver_type == "ins") ||
104  (settings_.septentrio_receiver_type == "ins_in_gnss_mode")))
105  {
106  this->log(LogLevel::FATAL, "Unkown septentrio_receiver_type " +
107  settings_.septentrio_receiver_type +
108  " use either gnss or ins.");
109  return false;
110  }
111 
112  if (settings_.septentrio_receiver_type == "ins_in_gnss_mode")
113  {
114  settings_.septentrio_receiver_type = "gnss";
115  settings_.ins_in_gnss_mode = true;
116  }
117 
118  // Polling period parameters
119  getUint32Param("polling_period/pvt", settings_.polling_period_pvt,
120  static_cast<uint32_t>(1000));
121  if (!(validPeriod(settings_.polling_period_pvt,
122  settings_.septentrio_receiver_type == "ins")))
123  {
124  this->log(
126  "Please specify a valid polling period for PVT-related SBF blocks and NMEA messages.");
127  return false;
128  }
129  getUint32Param("polling_period/rest", settings_.polling_period_rest,
130  static_cast<uint32_t>(1000));
131  if (!(validPeriod(settings_.polling_period_rest,
132  settings_.septentrio_receiver_type == "ins")))
133  {
134  this->log(
136  "Please specify a valid polling period for PVT-unrelated SBF blocks and NMEA messages.");
137  return false;
138  }
139 
140  // multi_antenna param
141  param("multi_antenna", settings_.multi_antenna, false);
142 
143  // Publishing parameters
144  param("publish/gpst", settings_.publish_gpst, false);
145  param("publish/navsatfix", settings_.publish_navsatfix, true);
146  param("publish/gpsfix", settings_.publish_gpsfix, false);
147  param("publish/pose", settings_.publish_pose, false);
148  param("publish/diagnostics", settings_.publish_diagnostics, false);
149  param("publish/gpgga", settings_.publish_gpgga, false);
150  param("publish/gprmc", settings_.publish_gprmc, false);
151  param("publish/gpgsa", settings_.publish_gpgsa, false);
152  param("publish/gpgsv", settings_.publish_gpgsv, false);
153  param("publish/measepoch", settings_.publish_measepoch, false);
154  param("publish/pvtcartesian", settings_.publish_pvtcartesian, false);
155  param("publish/pvtgeodetic", settings_.publish_pvtgeodetic,
156  (settings_.septentrio_receiver_type == "gnss"));
157  param("publish/basevectorcart", settings_.publish_basevectorcart, false);
158  param("publish/basevectorgeod", settings_.publish_basevectorgeod, false);
159  param("publish/poscovcartesian", settings_.publish_poscovcartesian, false);
160  param("publish/poscovgeodetic", settings_.publish_poscovgeodetic,
161  (settings_.septentrio_receiver_type == "gnss"));
162  param("publish/velcovgeodetic", settings_.publish_velcovgeodetic,
163  (settings_.septentrio_receiver_type == "gnss"));
164  param(
165  "publish/atteuler", settings_.publish_atteuler,
166  ((settings_.septentrio_receiver_type == "gnss") && settings_.multi_antenna));
167  param(
168  "publish/attcoveuler", settings_.publish_attcoveuler,
169  ((settings_.septentrio_receiver_type == "gnss") && settings_.multi_antenna));
170  param("publish/insnavcart", settings_.publish_insnavcart, false);
171  param("publish/insnavgeod", settings_.publish_insnavgeod,
172  (settings_.septentrio_receiver_type == "ins"));
173  param("publish/imusetup", settings_.publish_imusetup, false);
174  param("publish/velsensorsetup", settings_.publish_velsensorsetup, false);
175  param("publish/exteventinsnavgeod", settings_.publish_exteventinsnavgeod, false);
176  param("publish/exteventinsnavcart", settings_.publish_exteventinsnavcart, false);
177  param("publish/extsensormeas", settings_.publish_extsensormeas, false);
178  param("publish/imu", settings_.publish_imu, false);
179  param("publish/localization", settings_.publish_localization, false);
180  param("publish/twist", settings_.publish_twist, false);
181  param("publish/tf", settings_.publish_tf, false);
182 
183  // Datum and marker-to-ARP offset
184  param("datum", settings_.datum, std::string("Default"));
185  param("ant_type", settings_.ant_type, std::string("Unknown"));
186  param("ant_aux1_type", settings_.ant_aux1_type, std::string("Unknown"));
187  param("ant_serial_nr", settings_.ant_serial_nr, std::string());
188  if (settings_.ant_serial_nr.empty())
189  {
190  uint32_t sn_tmp;
191  if (getUint32Param("ant_serial_nr", sn_tmp, static_cast<uint32_t>(0)))
192  settings_.ant_serial_nr = std::to_string(sn_tmp);
193  else
194  settings_.ant_serial_nr = "Unknown";
195  }
196  param("ant_aux1_serial_nr", settings_.ant_aux1_serial_nr, std::string());
197  if (settings_.ant_aux1_serial_nr.empty())
198  {
199  uint32_t sn_tmp;
200  if (getUint32Param("ant_aux1_serial_nr", sn_tmp, static_cast<uint32_t>(0)))
201  settings_.ant_aux1_serial_nr = std::to_string(sn_tmp);
202  else
203  settings_.ant_aux1_serial_nr = "Unknown";
204  }
205  param("poi_to_arp/delta_e", settings_.delta_e, 0.0f);
206  param("poi_to_arp/delta_n", settings_.delta_n, 0.0f);
207  param("poi_to_arp/delta_u", settings_.delta_u, 0.0f);
208 
209  param("use_ros_axis_orientation", settings_.use_ros_axis_orientation, true);
210 
211  // INS Spatial Configuration
212  bool getConfigFromTf;
213  param("get_spatial_config_from_tf", getConfigFromTf, false);
214  if (getConfigFromTf)
215  {
216  if (settings_.septentrio_receiver_type == "ins")
217  {
218  TransformStampedMsg T_imu_vehicle;
219  getTransform(settings_.vehicle_frame_id, settings_.imu_frame_id,
220  T_imu_vehicle);
221  TransformStampedMsg T_poi_imu;
222  getTransform(settings_.imu_frame_id, settings_.poi_frame_id, T_poi_imu);
223  TransformStampedMsg T_vsm_imu;
224  getTransform(settings_.imu_frame_id, settings_.vsm_frame_id, T_vsm_imu);
225  TransformStampedMsg T_ant_imu;
226  getTransform(settings_.imu_frame_id, settings_.frame_id, T_ant_imu);
227 
228  // IMU orientation parameter
229  double roll, pitch, yaw;
230  getRPY(T_imu_vehicle.transform.rotation, roll, pitch, yaw);
231  settings_.theta_x = parsing_utilities::rad2deg(roll);
232  settings_.theta_y = parsing_utilities::rad2deg(pitch);
233  settings_.theta_z = parsing_utilities::rad2deg(yaw);
234  // INS antenna lever arm offset parameter
235  settings_.ant_lever_x = T_ant_imu.transform.translation.x;
236  settings_.ant_lever_y = T_ant_imu.transform.translation.y;
237  settings_.ant_lever_z = T_ant_imu.transform.translation.z;
238  // INS POI ofset paramter
239  settings_.poi_x = T_poi_imu.transform.translation.x;
240  settings_.poi_y = T_poi_imu.transform.translation.y;
241  settings_.poi_z = T_poi_imu.transform.translation.z;
242  // INS velocity sensor lever arm offset parameter
243  settings_.vsm_x = T_vsm_imu.transform.translation.x;
244  settings_.vsm_y = T_vsm_imu.transform.translation.y;
245  settings_.vsm_z = T_vsm_imu.transform.translation.z;
246 
247  if (settings_.multi_antenna)
248  {
249  TransformStampedMsg T_aux1_imu;
250  getTransform(settings_.imu_frame_id, settings_.aux1_frame_id,
251  T_aux1_imu);
252  // Antenna Attitude Determination parameter
253  double dy = T_aux1_imu.transform.translation.y -
254  T_ant_imu.transform.translation.y;
255  double dx = T_aux1_imu.transform.translation.x -
256  T_ant_imu.transform.translation.x;
257  settings_.heading_offset =
258  parsing_utilities::rad2deg(std::atan2(dy, dx));
259  double dz = T_aux1_imu.transform.translation.z -
260  T_ant_imu.transform.translation.z;
261  double dr = std::sqrt(parsing_utilities::square(dx) +
263  settings_.pitch_offset =
264  parsing_utilities::rad2deg(std::atan2(-dz, dr));
265  }
266  }
267  if ((settings_.septentrio_receiver_type == "gnss") &&
268  settings_.multi_antenna)
269  {
270  TransformStampedMsg T_ant_vehicle;
271  getTransform(settings_.vehicle_frame_id, settings_.frame_id,
272  T_ant_vehicle);
273  TransformStampedMsg T_aux1_vehicle;
274  getTransform(settings_.vehicle_frame_id, settings_.aux1_frame_id,
275  T_aux1_vehicle);
276 
277  // Antenna Attitude Determination parameter
278  double dy = T_aux1_vehicle.transform.translation.y -
279  T_ant_vehicle.transform.translation.y;
280  double dx = T_aux1_vehicle.transform.translation.x -
281  T_ant_vehicle.transform.translation.x;
282  settings_.heading_offset =
283  parsing_utilities::rad2deg(std::atan2(dy, dx));
284  double dz = T_aux1_vehicle.transform.translation.z -
285  T_ant_vehicle.transform.translation.z;
286  double dr = std::sqrt(parsing_utilities::square(dx) +
288  settings_.pitch_offset = parsing_utilities::rad2deg(std::atan2(-dz, dr));
289  }
290  } else
291  {
292  // IMU orientation parameter
293  param("ins_spatial_config/imu_orientation/theta_x", settings_.theta_x, 0.0);
294  param("ins_spatial_config/imu_orientation/theta_y", settings_.theta_y, 0.0);
295  param("ins_spatial_config/imu_orientation/theta_z", settings_.theta_z, 0.0);
296  // INS antenna lever arm offset parameter
297  param("ins_spatial_config/ant_lever_arm/x", settings_.ant_lever_x, 0.0);
298  param("ins_spatial_config/ant_lever_arm/y", settings_.ant_lever_y, 0.0);
299  param("ins_spatial_config/ant_lever_arm/z", settings_.ant_lever_z, 0.0);
300  // INS POI ofset paramter
301  param("ins_spatial_config/poi_lever_arm/delta_x", settings_.poi_x, 0.0);
302  param("ins_spatial_config/poi_lever_arm/delta_y", settings_.poi_y, 0.0);
303  param("ins_spatial_config/poi_lever_arm/delta_z", settings_.poi_z, 0.0);
304  // INS velocity sensor lever arm offset parameter
305  param("ins_spatial_config/vsm_lever_arm/vsm_x", settings_.vsm_x, 0.0);
306  param("ins_spatial_config/vsm_lever_arm/vsm_y", settings_.vsm_y, 0.0);
307  param("ins_spatial_config/vsm_lever_arm/vsm_z", settings_.vsm_z, 0.0);
308  // Antenna Attitude Determination parameter
309  param("att_offset/heading", settings_.heading_offset, 0.0);
310  param("att_offset/pitch", settings_.pitch_offset, 0.0);
311  }
312 
313  if (settings_.use_ros_axis_orientation)
314  {
315  settings_.theta_x =
316  parsing_utilities::wrapAngle180to180(settings_.theta_x + 180.0);
317  settings_.theta_y *= -1.0;
318  settings_.theta_z *= -1.0;
319  settings_.ant_lever_y *= -1.0;
320  settings_.ant_lever_z *= -1.0;
321  settings_.poi_y *= -1.0;
322  settings_.poi_z *= -1.0;
323  settings_.vsm_y *= -1.0;
324  settings_.vsm_z *= -1.0;
325  settings_.heading_offset *= -1.0;
326  settings_.pitch_offset *= -1.0;
327  }
328 
329  if (std::abs(settings_.heading_offset) > std::numeric_limits<double>::epsilon())
330  {
331  if (settings_.publish_atteuler)
332  {
333  this->log(
335  "Pitch angle output by topic /atteuler is a tilt angle rotated by " +
336  std::to_string(settings_.heading_offset) + ".");
337  }
338  if (settings_.publish_pose && (settings_.septentrio_receiver_type == "gnss"))
339  {
340  this->log(
342  "Pitch angle output by topic /pose is a tilt angle rotated by " +
343  std::to_string(settings_.heading_offset) + ".");
344  }
345  }
346 
347  this->log(LogLevel::DEBUG,
348  "IMU roll offset: " + std::to_string(settings_.theta_x));
349  this->log(LogLevel::DEBUG,
350  "IMU pitch offset: " + std::to_string(settings_.theta_y));
351  this->log(LogLevel::DEBUG,
352  "IMU yaw offset: " + std::to_string(settings_.theta_z));
353  this->log(LogLevel::DEBUG,
354  "Ant heading offset: " + std::to_string(settings_.heading_offset));
355  this->log(LogLevel::DEBUG,
356  "Ant pitch offset: " + std::to_string(settings_.pitch_offset));
357 
358  // ins_initial_heading param
359  param("ins_initial_heading", settings_.ins_initial_heading, std::string("auto"));
360 
361  // ins_std_dev_mask
362  param("ins_std_dev_mask/att_std_dev", settings_.att_std_dev, 5.0f);
363  param("ins_std_dev_mask/pos_std_dev", settings_.pos_std_dev, 10.0f);
364 
365  // INS solution reference point
366  param("ins_use_poi", settings_.ins_use_poi, false);
367 
368  if (settings_.publish_tf && !settings_.ins_use_poi)
369  {
370  this->log(
372  "If tf shall be published, ins_use_poi has to be set to true! It is set automatically to true.");
373  settings_.ins_use_poi = true;
374  }
375 
376  // RTK correction parameters
377  // NTRIP
378  for (uint8_t i = 1; i < 4; ++i)
379  {
380  RtkNtrip ntripSettings;
381  std::string ntrip = "ntrip_" + std::to_string(i);
382 
383  param("rtk_settings/" + ntrip + "/id", ntripSettings.id, std::string());
384  if (ntripSettings.id.empty())
385  continue;
386 
387  param("rtk_settings/" + ntrip + "/caster", ntripSettings.caster,
388  std::string());
389  getUint32Param("rtk_settings/" + ntrip + "/caster_port",
390  ntripSettings.caster_port, static_cast<uint32_t>(0));
391  param("rtk_settings/" + ntrip + "/username", ntripSettings.username,
392  std::string());
393  if (!param("rtk_settings/" + ntrip + "/password", ntripSettings.password,
394  std::string()))
395  {
396  uint32_t pwd_tmp;
397  getUint32Param("rtk_settings/" + ntrip + "/password", pwd_tmp,
398  static_cast<uint32_t>(0));
399  ntripSettings.password = std::to_string(pwd_tmp);
400  }
401  param("rtk_settings/" + ntrip + "/mountpoint", ntripSettings.mountpoint,
402  std::string());
403  param("rtk_settings/" + ntrip + "/version", ntripSettings.version,
404  std::string("v2"));
405  param("rtk_settings/" + ntrip + "/tls", ntripSettings.tls, false);
406  if (ntripSettings.tls)
407  param("rtk_settings/" + ntrip + "/fingerprint",
408  ntripSettings.fingerprint, std::string(""));
409  param("rtk_settings/" + ntrip + "/rtk_standard", ntripSettings.rtk_standard,
410  std::string("auto"));
411  param("rtk_settings/" + ntrip + "/send_gga", ntripSettings.send_gga,
412  std::string("auto"));
413  if (ntripSettings.send_gga.empty())
414  ntripSettings.send_gga = "off";
415  param("rtk_settings/" + ntrip + "/keep_open", ntripSettings.keep_open, true);
416 
417  settings_.rtk_settings.ntrip.push_back(ntripSettings);
418  }
419  // IP server
420  for (uint8_t i = 1; i < 6; ++i)
421  {
422  RtkIpServer ipSettings;
423  std::string ips = "ip_server_" + std::to_string(i);
424 
425  param("rtk_settings/" + ips + "/id", ipSettings.id, std::string(""));
426  if (ipSettings.id.empty())
427  continue;
428 
429  getUint32Param("rtk_settings/" + ips + "/port", ipSettings.port,
430  static_cast<uint32_t>(0));
431  param("rtk_settings/" + ips + "/rtk_standard", ipSettings.rtk_standard,
432  std::string("auto"));
433  param("rtk_settings/" + ips + "/send_gga", ipSettings.send_gga,
434  std::string("auto"));
435  if (ipSettings.send_gga.empty())
436  ipSettings.send_gga = "off";
437  param("rtk_settings/" + ips + "/keep_open", ipSettings.keep_open, true);
438 
439  settings_.rtk_settings.ip_server.push_back(ipSettings);
440  }
441  // Serial
442  for (uint8_t i = 1; i < 6; ++i)
443  {
444  RtkSerial serialSettings;
445  std::string serial = "serial_" + std::to_string(i);
446 
447  param("rtk_settings/" + serial + "/port", serialSettings.port,
448  std::string());
449  if (serialSettings.port.empty())
450  continue;
451 
452  getUint32Param("rtk_settings/" + serial + "/baud_rate",
453  serialSettings.baud_rate, static_cast<uint32_t>(115200));
454  param("rtk_settings/" + serial + "/rtk_standard",
455  serialSettings.rtk_standard, std::string("auto"));
456  param("rtk_settings/" + serial + "/send_gga", serialSettings.send_gga,
457  std::string("auto"));
458  if (serialSettings.send_gga.empty())
459  serialSettings.send_gga = "off";
460  param("rtk_settings/" + serial + "/keep_open", serialSettings.keep_open,
461  true);
462 
463  settings_.rtk_settings.serial.push_back(serialSettings);
464  }
465 
466  {
467  // deprecation warnings
468  std::string tempString;
469  int32_t tempInt;
470  bool tempBool;
471  param("ntrip_settings/mode", tempString, std::string(""));
472  if (tempString != "")
473  this->log(
475  "Deprecation warning: parameter ntrip_settings/mode has been removed, see README under section rtk_settings.");
476  param("ntrip_settings/caster", tempString, std::string(""));
477  if (tempString != "")
478  this->log(
480  "Deprecation warning: parameter ntrip_settings/caster has been removed, see README under section rtk_settings.");
481  param("ntrip_settings/rx_has_internet", tempBool, false);
482  if (tempBool)
483  this->log(
485  "Deprecation warning: parameter ntrip_settings/rx_has_internet has been removed, see README under section rtk_settings.");
486  param("ntrip_settings/rx_input_corrections_tcp", tempInt, 0);
487  if (tempInt != 0)
488  this->log(
490  "Deprecation warning: parameter ntrip_settings/rx_input_corrections_tcp has been removed, see README under section rtk_settings.");
491  param("ntrip_settings/rx_input_corrections_serial", tempString,
492  std::string(""));
493  if (tempString != "")
494  this->log(
496  "Deprecation warning: parameter ntrip_settings/rx_input_corrections_serial has been removed, see README under section rtk_settings.");
497  }
498 
499  if (settings_.publish_atteuler)
500  {
501  if (!settings_.multi_antenna)
502  {
503  this->log(
505  "AttEuler needs multi-antenna receiver. Multi-antenna setting automatically activated. Deactivate publishing of AttEuler if multi-antenna operation is not available.");
506  settings_.multi_antenna = true;
507  }
508  }
509 
510  // VSM - velocity sensor measurements for INS
511  if (settings_.septentrio_receiver_type == "ins")
512  {
513  param("ins_vsm/ros/source", settings_.ins_vsm_ros_source, std::string(""));
514 
515  bool ins_use_vsm = false;
516  ins_use_vsm = ((settings_.ins_vsm_ros_source == "odometry") ||
517  (settings_.ins_vsm_ros_source == "twist"));
518  if (!settings_.ins_vsm_ros_source.empty() && !ins_use_vsm)
519  this->log(LogLevel::ERROR, "unknown ins_vsm/ros/source " +
520  settings_.ins_vsm_ros_source +
521  " -> VSM input will not be used!");
522  param("ins_vsm/ip_server/id", settings_.ins_vsm_ip_server_id,
523  std::string(""));
524  if (!settings_.ins_vsm_ip_server_id.empty())
525  {
526  getUint32Param("ins_vsm/ip_server/port",
527  settings_.ins_vsm_ip_server_port,
528  static_cast<uint32_t>(0));
529  param("ins_vsm/ip_server/keep_open",
530  settings_.ins_vsm_ip_server_keep_open, true);
531  this->log(
533  "external velocity sensor measurements via ip_server are used.");
534  }
535 
536  param("ins_vsm/serial/port", settings_.ins_vsm_serial_port, std::string(""));
537  if (!settings_.ins_vsm_serial_port.empty())
538  {
539  getUint32Param("ins_vsm/serial/baud_rate",
540  settings_.ins_vsm_serial_baud_rate,
541  static_cast<uint32_t>(115200));
542  param("ins_vsm/serial/keep_open", settings_.ins_vsm_serial_keep_open,
543  true);
544  this->log(LogLevel::INFO,
545  "external velocity sensor measurements via serial are used.");
546  }
547 
548  param("ins_vsm/ros/config", settings_.ins_vsm_ros_config,
549  std::vector<bool>());
550  if (ins_use_vsm && (settings_.ins_vsm_ros_config.size() == 3))
551  {
552  if (std::all_of(settings_.ins_vsm_ros_config.begin(),
553  settings_.ins_vsm_ros_config.end(),
554  [](bool v) { return !v; }))
555  {
556  ins_use_vsm = false;
557  this->log(
559  "all elements of ins_vsm/ros/config have been set to false -> VSM input will not be used!");
560  } else
561  {
562  param("ins_vsm/ros/variances_by_parameter",
563  settings_.ins_vsm_ros_variances_by_parameter, false);
564  if (settings_.ins_vsm_ros_variances_by_parameter)
565  {
566  param("ins_vsm/ros/variances", settings_.ins_vsm_ros_variances,
567  std::vector<double>());
568  if (settings_.ins_vsm_ros_variances.size() != 3)
569  {
570  this->log(
572  "ins_vsm/ros/variances has to be of size 3 for var_x, var_y, and var_z -> VSM input will not be used!");
573  ins_use_vsm = false;
574  settings_.ins_vsm_ros_source = "";
575  } else
576  {
577  for (size_t i = 0; i < settings_.ins_vsm_ros_config.size();
578  ++i)
579  {
580  if (settings_.ins_vsm_ros_config[i] &&
581  (settings_.ins_vsm_ros_variances[i] <= 0.0))
582  {
583  this->log(
585  "ins_vsm/ros/config of element " +
586  std::to_string(i) +
587  " has been set to be used but its variance is not > 0.0 -> its VSM input will not be used!");
588  settings_.ins_vsm_ros_config[i] = false;
589  }
590  }
591  }
592  if (std::all_of(settings_.ins_vsm_ros_config.begin(),
593  settings_.ins_vsm_ros_config.end(),
594  [](bool v) { return !v; }))
595  {
596  ins_use_vsm = false;
597  settings_.ins_vsm_ros_source = "";
598  this->log(
600  "all elements of ins_vsm/ros/config have been set to false due to invalid covariances -> VSM input will not be used!");
601  }
602  }
603  }
604  } else if (ins_use_vsm)
605  {
606  settings_.ins_vsm_ros_source = "";
607  this->log(
609  "ins_vsm/ros/config has to be of size 3 to signal wether to use v_x, v_y, and v_z -> VSM input will not be used!");
610  }
611  if (ins_use_vsm)
612  {
613  this->log(LogLevel::INFO, "ins_vsm/ros/source " +
614  settings_.ins_vsm_ros_source +
615  " will be used.");
616  registerSubscriber();
617  }
618  }
619 
620  // To be implemented: RTCM, raw data settings, PPP, SBAS ...
621  this->log(LogLevel::DEBUG, "Finished getROSParams() method");
622  return true;
623 }
624 
625 bool rosaic_node::ROSaicNode::validPeriod(uint32_t period, bool isIns)
626 {
627  return ((period == 0) || ((period == 5 && isIns)) || (period == 10) ||
628  (period == 20) || (period == 40) || (period == 50) || (period == 100) ||
629  (period == 200) || (period == 500) || (period == 1000) ||
630  (period == 2000) || (period == 5000) || (period == 10000) ||
631  (period == 15000) || (period == 30000) || (period == 60000) ||
632  (period == 120000) || (period == 300000) || (period == 600000) ||
633  (period == 900000) || (period == 1800000) || (period == 3600000));
634 }
635 
636 void rosaic_node::ROSaicNode::getTransform(const std::string& targetFrame,
637  const std::string& sourceFrame,
638  TransformStampedMsg& T_s_t)
639 {
640  bool found = false;
641  while (!found)
642  {
643  try
644  {
645  // try to get tf from source frame to target frame
646  T_s_t = tfBuffer_.lookupTransform(targetFrame, sourceFrame, ros::Time(0),
647  ros::Duration(2.0));
648  found = true;
649  } catch (const tf2::TransformException& ex)
650  {
651  this->log(LogLevel::WARN, "Waiting for transform from " + sourceFrame +
652  " to " + targetFrame + ": " + ex.what() +
653  ".");
654  found = false;
655  }
656  }
657 }
658 
659 void rosaic_node::ROSaicNode::getRPY(const QuaternionMsg& qm, double& roll,
660  double& pitch, double& yaw)
661 {
662  Eigen::Quaterniond q(qm.w, qm.x, qm.y, qm.z);
663  Eigen::Quaterniond::RotationMatrixType C = q.matrix();
664 
665  roll = std::atan2(C(2, 1), C(2, 2));
666  pitch = std::asin(-C(2, 0));
667  yaw = std::atan2(C(1, 0), C(0, 0));
668 }
669 
670 void rosaic_node::ROSaicNode::sendVelocity(const std::string& velNmea)
671 {
672  IO_.sendVelocity(velNmea);
673 }
rosaic_node::ROSaicNode::getRPY
void getRPY(const QuaternionMsg &qm, double &roll, double &pitch, double &yaw)
Gets Euler angles from quaternion message.
Definition: rosaic_node.cpp:659
RtkNtrip::id
std::string id
Id of the NTRIP port.
Definition: settings.h:40
RtkIpServer::keep_open
bool keep_open
Wether RTK connections shall be kept open on shutdown.
Definition: settings.h:77
parsing_utilities::rad2deg
T rad2deg(T rad)
Definition: parsing_utilities.hpp:89
RtkSerial::baud_rate
uint32_t baud_rate
Baud rate of the serial port on which Rx receives the corrections.
Definition: settings.h:86
parsing_utilities::wrapAngle180to180
double wrapAngle180to180(double angle)
Wraps an angle between -180 and 180 degrees.
Definition: parsing_utilities.cpp:49
io_comm_rx::Comm_IO::initializeIO
void initializeIO()
Initializes the I/O handling.
Definition: communication_core.cpp:206
ERROR
@ ERROR
Definition: typedefs.hpp:166
RtkNtrip::tls
bool tls
Wether to use TLS.
Definition: settings.h:54
RtkSerial::send_gga
std::string send_gga
Whether (and at which rate) or not to send GGA to the serial port.
Definition: settings.h:90
RtkSerial::port
std::string port
Definition: settings.h:84
Settings::activate_debug_log
bool activate_debug_log
Set logger level to DEBUG.
Definition: settings.h:106
ros::console::set_logger_level
ROSCONSOLE_DECL bool set_logger_level(const std::string &name, levels::Level level)
INFO
@ INFO
Definition: typedefs.hpp:164
RtkNtrip::send_gga
std::string send_gga
Whether (and at which rate) or not to send GGA to the NTRIP caster.
Definition: settings.h:60
tf2_ros::TransformListener
RtkNtrip::fingerprint
std::string fingerprint
Self-signed certificate fingerprint.
Definition: settings.h:56
RtkNtrip::caster
std::string caster
Hostname or IP address of the NTRIP caster to connect to.
Definition: settings.h:42
ros::console::levels::Debug
Debug
rosaic_node::ROSaicNode::tfListener_
std::unique_ptr< tf2_ros::TransformListener > tfListener_
Definition: rosaic_node.hpp:134
rosaic_node.hpp
The heart of the ROSaic driver: The ROS node that represents it.
rosaic_node::ROSaicNode::sendVelocity
void sendVelocity(const std::string &velNmea)
Send velocity to communication layer (virtual)
Definition: rosaic_node.cpp:670
RtkNtrip
Definition: settings.h:37
RtkSerial
Definition: settings.h:80
RtkSerial::keep_open
bool keep_open
Wether RTK connections shall be kept open on shutdown.
Definition: settings.h:92
RtkNtrip::caster_port
uint32_t caster_port
IP port number of NTRIP caster to connect to.
Definition: settings.h:44
rosaic_node::ROSaicNode::getROSParams
bool getROSParams()
Gets the node parameters from the ROS Parameter Server, parts of which are specified in a YAML file.
Definition: rosaic_node.cpp:78
DEBUG
@ DEBUG
Definition: typedefs.hpp:163
rosaic_node::ROSaicNode::IO_
io_comm_rx::Comm_IO IO_
Handles communication with the Rx.
Definition: rosaic_node.hpp:131
RtkNtrip::version
std::string version
NTRIP version for NTRIP service.
Definition: settings.h:52
parsing_utilities::square
T square(T val)
Definition: parsing_utilities.hpp:61
ROSaicNodeBase::param
bool param(const std::string &name, T &val, const T &defaultVal)
Gets parameter of type T from the parameter server.
Definition: typedefs.hpp:221
TransformStampedMsg
geometry_msgs::TransformStamped TransformStampedMsg
Definition: typedefs.hpp:98
RtkNtrip::rtk_standard
std::string rtk_standard
RTCM version for correction data.
Definition: settings.h:58
RtkIpServer::port
uint32_t port
Definition: settings.h:71
ROSaicNodeBase::settings_
Settings settings_
Settings.
Definition: typedefs.hpp:461
RtkNtrip::username
std::string username
Username for NTRIP service.
Definition: settings.h:46
RtkSerial::rtk_standard
std::string rtk_standard
RTCM version for correction data.
Definition: settings.h:88
rosaic_node::ROSaicNode::ROSaicNode
ROSaicNode()
Definition: rosaic_node.cpp:41
FATAL
@ FATAL
Definition: typedefs.hpp:167
Settings::read_from_sbf_log
bool read_from_sbf_log
Whether or not we are reading from an SBF file.
Definition: settings.h:282
RtkIpServer::rtk_standard
std::string rtk_standard
RTCM version for correction data.
Definition: settings.h:73
rosaic_node::ROSaicNode::getTransform
void getTransform(const std::string &targetFrame, const std::string &sourceFrame, TransformStampedMsg &T_s_t)
Gets transforms from tf2.
Definition: rosaic_node.cpp:636
ros::console::notifyLoggerLevelsChanged
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
ros::Time
ROSaicNodeBase::log
void log(LogLevel logLevel, const std::string &s)
Log function to provide abstraction of ROS loggers.
Definition: typedefs.hpp:231
RtkNtrip::keep_open
bool keep_open
Wether RTK connections shall be kept open on shutdown.
Definition: settings.h:62
rosaic_node::ROSaicNode::validPeriod
bool validPeriod(uint32_t period, bool isIns)
Checks if the period has a valid value.
Definition: rosaic_node.cpp:625
io_comm_rx::Comm_IO::configureRx
void configureRx()
Configures Rx: Which SBF/NMEA messages it should output and later correction settings.
Definition: communication_core.cpp:389
param
T param(const std::string &param_name, const T &default_val)
RtkIpServer
Definition: settings.h:65
WARN
@ WARN
Definition: typedefs.hpp:165
QuaternionMsg
geometry_msgs::Quaternion QuaternionMsg
Definition: typedefs.hpp:95
ROSCONSOLE_DEFAULT_NAME
#define ROSCONSOLE_DEFAULT_NAME
RtkIpServer::id
std::string id
The IP server id.
Definition: settings.h:68
tf2::TransformException
rosaic_node::ROSaicNode::tfBuffer_
tf2_ros::Buffer tfBuffer_
tf2 buffer and listener
Definition: rosaic_node.hpp:133
RtkNtrip::mountpoint
std::string mountpoint
Mountpoint for NTRIP service.
Definition: settings.h:50
ros::Duration
RtkNtrip::password
std::string password
Password for NTRIP service.
Definition: settings.h:48
Settings::read_from_pcap
bool read_from_pcap
Whether or not we are reading from a PCAP file.
Definition: settings.h:284
io_comm_rx::Comm_IO::defineMessages
void defineMessages()
Defines which Rx messages to read and which ROS messages to publish.
Definition: communication_core.cpp:985
RtkIpServer::send_gga
std::string send_gga
Whether (and at which rate) or not to send GGA to the NTRIP caster.
Definition: settings.h:75


septentrio_gnss_driver
Author(s): Tibor Dome
autogenerated on Thu Nov 10 2022 04:02:01