config_store.h
Go to the documentation of this file.
1 
34 #ifndef SBG_ROS_CONFIG_STORE_H
35 #define SBG_ROS_CONFIG_STORE_H
36 
37 // SbgECom headers
38 #include <sbgEComLib.h>
39 
40 // ROS headers
41 #include <ros/ros.h>
42 
43 // Project headers
44 #include "sbg_vector3.h"
45 
46 namespace sbg
47 {
51  enum class TimeReference
52  {
53  ROS = 0,
54  INS_UNIX = 1,
55  };
56 
61 {
62 public:
63 
67  struct SbgLogOutput
68  {
72  };
73 
74 private:
75 
76  std::string m_uart_port_name_;
80 
85 
87 
90 
93 
95 
100 
104 
108 
109  std::vector<SbgLogOutput> m_output_modes_;
111 
113 
115  std::string m_frame_id_;
117 
120  std::string m_odom_frame_id_;
123 
124  //---------------------------------------------------------------------//
125  //- Private methods -//
126  //---------------------------------------------------------------------//
127 
138  template <typename T>
139  T getParameter(const ros::NodeHandle& ref_node_handle, std::string param_key, int default_value) const
140  {
141  if (ref_node_handle.hasParam(param_key))
142  {
143  int parameter;
144  ref_node_handle.param<int>(param_key, parameter, default_value);
145 
146  return static_cast<T>(parameter);
147  }
148  else
149  {
150  return static_cast<T>(default_value);
151  }
152  }
153 
159  void loadDriverParameters(const ros::NodeHandle& ref_node_handle);
160 
166  void loadOdomParameters(const ros::NodeHandle& ref_node_handle);
167 
173  void loadCommunicationParameters(const ros::NodeHandle& ref_node_handle);
174 
180  void loadSensorParameters(const ros::NodeHandle& ref_node_handle);
181 
187  void loadImuAlignementParameters(const ros::NodeHandle& ref_node_handle);
188 
194  void loadAidingAssignementParameters(const ros::NodeHandle& ref_node_handle);
195 
201  void loadMagnetometersParameters(const ros::NodeHandle& ref_node_handle);
202 
208  void loadGnssParameters(const ros::NodeHandle& ref_node_handle);
209 
215  void loadOdometerParameters(const ros::NodeHandle& ref_node_handle);
216 
222  void loadOutputFrameParameters(const ros::NodeHandle& ref_node_handle);
223 
232  void loadOutputConfiguration(const ros::NodeHandle& ref_node_handle, const std::string& ref_key, SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_id);
233 
240  void loadOutputTimeReference(const ros::NodeHandle& ref_node_handle, const std::string& ref_key);
241 
242 public:
243 
244  //---------------------------------------------------------------------//
245  //- Constructor -//
246  //---------------------------------------------------------------------//
247 
251  ConfigStore(void);
252 
253  //---------------------------------------------------------------------//
254  //- Parameters -//
255  //---------------------------------------------------------------------//
256 
262  bool checkConfigWithRos(void) const;
263 
269  bool isInterfaceSerial(void) const;
270 
276  const std::string &getUartPortName(void) const;
277 
283  uint32_t getBaudRate(void) const;
284 
290  SbgEComOutputPort getOutputPort(void) const;
291 
297  bool isInterfaceUdp(void) const;
298 
304  sbgIpAddress getIpAddress(void) const;
305 
311  uint32_t getOutputPortAddress(void) const;
312 
318  uint32_t getInputPortAddress(void) const;
319 
325  const SbgEComInitConditionConf &getInitialConditions(void) const;
326 
332  const SbgEComModelInfo &getMotionProfile(void) const;
333 
339  const SbgEComSensorAlignmentInfo &getSensorAlignement(void) const;
340 
346  const SbgVector3<float> &getSensorLevelArms(void) const;
347 
353  const SbgEComAidingAssignConf &getAidingAssignement(void) const;
354 
360  const SbgEComModelInfo &getMagnetometerModel(void) const;
361 
367  const SbgEComMagRejectionConf &getMagnetometerRejection(void) const;
368 
374  const SbgEComMagCalibMode &getMagnetometerCalibMode(void) const;
375 
381  const SbgEComMagCalibBandwidth &getMagnetometerCalibBandwidth(void) const;
382 
388  const SbgEComModelInfo &getGnssModel(void) const;
389 
395  const SbgEComGnssInstallation &getGnssInstallation(void) const;
396 
402  const SbgEComGnssRejectionConf &getGnssRejection(void) const;
403 
409  const SbgEComOdoConf &getOdometerConf(void) const;
410 
416  const SbgVector3<float> &getOdometerLevelArms(void) const;
417 
423  const SbgEComOdoRejectionConf &getOdometerRejection(void) const;
424 
430  const std::vector<SbgLogOutput> &getOutputModes(void) const;
431 
437  bool checkRosStandardMessages(void) const;
438 
445  uint32_t getReadingRateFrequency(void) const;
446 
452  const std::string &getFrameId(void) const;
453 
459  bool getUseEnu(void) const;
460 
466  bool getOdomEnable(void) const;
467 
473  bool getOdomPublishTf(void) const;
474 
480  const std::string &getOdomFrameId(void) const;
481 
482 
488  const std::string &getOdomBaseFrameId(void) const;
489 
490 
496  const std::string &getOdomInitFrameId(void) const;
497 
503  TimeReference getTimeReference(void) const;
504 
505  //---------------------------------------------------------------------//
506  //- Operations -//
507  //---------------------------------------------------------------------//
508 
514  void loadFromRosNodeHandle(const ros::NodeHandle& ref_node_handle);
515 };
516 }
517 
518 #endif // SBG_ROS_CONFIG_STORE_H
SbgEComOutputPort m_output_port_
Definition: config_store.h:77
SbgEComSensorAlignmentInfo m_sensor_alignement_info_
Definition: config_store.h:91
SbgEComOutputMode output_mode
Definition: config_store.h:71
const std::string & getFrameId(const T &t)
uint32_t m_out_port_address_
Definition: config_store.h:82
uint32_t m_uart_baud_rate_
Definition: config_store.h:78
enum _SbgEComMagCalibMode SbgEComMagCalibMode
SbgEComGnssRejectionConf m_gnss_rejection_conf_
Definition: config_store.h:103
SbgEComMagCalibBandwidth m_mag_calib_bandwidth_
Definition: config_store.h:99
uint32_t m_in_port_address_
Definition: config_store.h:83
enum _SbgEComMagCalibBandwidth SbgEComMagCalibBandwidth
SbgEComModelInfo m_gnss_model_info_
Definition: config_store.h:101
bool m_configure_through_ros_
Definition: config_store.h:86
std::string m_frame_id_
Definition: config_store.h:115
std::vector< SbgLogOutput > m_output_modes_
Definition: config_store.h:109
std::string m_odom_frame_id_
Definition: config_store.h:120
bool m_upd_communication_
Definition: config_store.h:84
bool param(const std::string &param_name, T &param_val, const T &default_val) const
SbgVector3< float > m_odometer_level_arm_
Definition: config_store.h:106
SbgEComModelInfo m_motion_profile_model_info_
Definition: config_store.h:89
SbgEComOdoRejectionConf m_odometer_rejection_conf_
Definition: config_store.h:107
bool m_serial_communication_
Definition: config_store.h:79
SbgEComOdoConf m_odometer_conf_
Definition: config_store.h:105
SbgEComMagRejectionConf m_mag_rejection_conf_
Definition: config_store.h:97
SbgVector3< float > m_sensor_lever_arm_
Definition: config_store.h:92
uint32_t sbgIpAddress
Definition: sbgTypes.h:64
bool hasParam(const std::string &key) const
SbgEComMagCalibMode m_mag_calib_mode_
Definition: config_store.h:98
sbgIpAddress m_sbg_ip_address_
Definition: config_store.h:81
uint8_t SbgEComMsgId
Definition: sbgEComIds.h:289
std::string m_odom_init_frame_id_
Definition: config_store.h:122
std::string m_uart_port_name_
Definition: config_store.h:76
std::string m_odom_base_frame_id_
Definition: config_store.h:121
enum _SbgEComOutputPort SbgEComOutputPort
enum _SbgEComClass SbgEComClass
SbgEComAidingAssignConf m_aiding_assignement_conf_
Definition: config_store.h:94
T getParameter(const ros::NodeHandle &ref_node_handle, std::string param_key, int default_value) const
Definition: config_store.h:139
Handle a three components vector.
TimeReference
Definition: config_store.h:51
enum _SbgEComOutputMode SbgEComOutputMode
bool m_ros_standard_output_
Definition: config_store.h:110
SbgEComGnssInstallation m_gnss_installation_
Definition: config_store.h:102
SbgEComInitConditionConf m_init_condition_conf_
Definition: config_store.h:88
SbgEComModelInfo m_mag_model_info_
Definition: config_store.h:96
Main header file for the SBG Systems Enhanced Communication Library.
TimeReference m_time_reference_
Definition: config_store.h:112
uint32_t m_rate_frequency_
Definition: config_store.h:114


sbg_driver
Author(s): SBG Systems
autogenerated on Sat Sep 3 2022 02:53:35