config_store.h

Go to the documentation of this file.

#ifndef SBG_ROS_CONFIG_STORE_H
#define SBG_ROS_CONFIG_STORE_H

// SbgECom headers
#include <sbgEComLib.h>

// ROS headers
#include <ros/ros.h>

// Project headers
#include "sbg_vector3.h"

namespace sbg
{
  enum class TimeReference
  {
    ROS = 0,
    INS_UNIX = 1,
  };

class ConfigStore
{
public:

  struct SbgLogOutput
  {
    SbgEComClass      message_class;
    SbgEComMsgId      message_id;
    SbgEComOutputMode output_mode;
  };

private:

  std::string                 m_uart_port_name_;
  SbgEComOutputPort           m_output_port_;
  uint32_t                    m_uart_baud_rate_;
  bool                        m_serial_communication_;

  sbgIpAddress                m_sbg_ip_address_;
  uint32_t                    m_out_port_address_;
  uint32_t                    m_in_port_address_;
  bool                        m_upd_communication_;

  bool                        m_configure_through_ros_;

  SbgEComInitConditionConf    m_init_condition_conf_;
  SbgEComModelInfo            m_motion_profile_model_info_;

  SbgEComSensorAlignmentInfo  m_sensor_alignement_info_;
  SbgVector3<float>           m_sensor_lever_arm_;

  SbgEComAidingAssignConf     m_aiding_assignement_conf_;

  SbgEComModelInfo            m_mag_model_info_;
  SbgEComMagRejectionConf     m_mag_rejection_conf_;
  SbgEComMagCalibMode         m_mag_calib_mode_;
  SbgEComMagCalibBandwidth    m_mag_calib_bandwidth_;

  SbgEComModelInfo            m_gnss_model_info_;
  SbgEComGnssInstallation     m_gnss_installation_;
  SbgEComGnssRejectionConf    m_gnss_rejection_conf_;

  SbgEComOdoConf              m_odometer_conf_;
  SbgVector3<float>           m_odometer_level_arm_;
  SbgEComOdoRejectionConf     m_odometer_rejection_conf_;

  std::vector<SbgLogOutput>   m_output_modes_;
  bool                        m_ros_standard_output_;

  TimeReference               m_time_reference_;

  uint32_t                    m_rate_frequency_;
  std::string                 m_frame_id_;
  bool                                            m_use_enu_;

  bool                                            m_odom_enable_;
  bool                                            m_odom_publish_tf_;
  std::string                 m_odom_frame_id_;
  std::string                 m_odom_base_frame_id_;
  std::string                 m_odom_init_frame_id_;

  //---------------------------------------------------------------------//
  //- Private  methods                                                  -//
  //---------------------------------------------------------------------//

  template <typename T>
  T getParameter(const ros::NodeHandle& ref_node_handle, std::string param_key, int default_value) const
  {
    if (ref_node_handle.hasParam(param_key))
    {
      int parameter;
      ref_node_handle.param<int>(param_key, parameter, default_value);

      return static_cast<T>(parameter);
    }
    else
    {
      return static_cast<T>(default_value);
    }
  }

  void loadDriverParameters(const ros::NodeHandle& ref_node_handle);

  void loadOdomParameters(const ros::NodeHandle& ref_node_handle);

  void loadCommunicationParameters(const ros::NodeHandle& ref_node_handle);

  void loadSensorParameters(const ros::NodeHandle& ref_node_handle);

  void loadImuAlignementParameters(const ros::NodeHandle& ref_node_handle);

  void loadAidingAssignementParameters(const ros::NodeHandle& ref_node_handle);

  void loadMagnetometersParameters(const ros::NodeHandle& ref_node_handle);

  void loadGnssParameters(const ros::NodeHandle& ref_node_handle);

  void loadOdometerParameters(const ros::NodeHandle& ref_node_handle);

   void loadOutputFrameParameters(const ros::NodeHandle& ref_node_handle);

  void loadOutputConfiguration(const ros::NodeHandle& ref_node_handle, const std::string& ref_key, SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_id);

  void loadOutputTimeReference(const ros::NodeHandle& ref_node_handle, const std::string& ref_key);

public:

  //---------------------------------------------------------------------//
  //- Constructor                                                       -//
  //---------------------------------------------------------------------//

  ConfigStore(void);

  //---------------------------------------------------------------------//
  //- Parameters                                                        -//
  //---------------------------------------------------------------------//

  bool checkConfigWithRos(void) const;

  bool isInterfaceSerial(void) const;

  const std::string &getUartPortName(void) const;

  uint32_t getBaudRate(void) const;

  SbgEComOutputPort getOutputPort(void) const;

  bool isInterfaceUdp(void) const;

  sbgIpAddress getIpAddress(void) const;

  uint32_t getOutputPortAddress(void) const;

  uint32_t getInputPortAddress(void) const;

  const SbgEComInitConditionConf &getInitialConditions(void) const;

  const SbgEComModelInfo &getMotionProfile(void) const;

  const SbgEComSensorAlignmentInfo &getSensorAlignement(void) const;

  const SbgVector3<float> &getSensorLevelArms(void) const;

  const SbgEComAidingAssignConf &getAidingAssignement(void) const;

  const SbgEComModelInfo &getMagnetometerModel(void) const;

  const SbgEComMagRejectionConf &getMagnetometerRejection(void) const;

  const SbgEComMagCalibMode &getMagnetometerCalibMode(void) const;

  const SbgEComMagCalibBandwidth &getMagnetometerCalibBandwidth(void) const;

  const SbgEComModelInfo &getGnssModel(void) const;

  const SbgEComGnssInstallation &getGnssInstallation(void) const;

  const SbgEComGnssRejectionConf &getGnssRejection(void) const;

  const SbgEComOdoConf &getOdometerConf(void) const;

  const SbgVector3<float> &getOdometerLevelArms(void) const;

  const SbgEComOdoRejectionConf &getOdometerRejection(void) const;

  const std::vector<SbgLogOutput> &getOutputModes(void) const;

  bool checkRosStandardMessages(void) const;

  uint32_t getReadingRateFrequency(void) const;

  const std::string &getFrameId(void) const;

   bool getUseEnu(void) const;

   bool getOdomEnable(void) const;

   bool getOdomPublishTf(void) const;

  const std::string &getOdomFrameId(void) const;


  const std::string &getOdomBaseFrameId(void) const;


  const std::string &getOdomInitFrameId(void) const;

  TimeReference getTimeReference(void) const;

  //---------------------------------------------------------------------//
  //- Operations                                                        -//
  //---------------------------------------------------------------------//

  void loadFromRosNodeHandle(const ros::NodeHandle& ref_node_handle);
};
}

#endif // SBG_ROS_CONFIG_STORE_H