sbg_device.h

Go to the documentation of this file.

#ifndef SBG_ROS_SBG_DEVICE_H
#define SBG_ROS_SBG_DEVICE_H

// Standard headers
#include <iostream>
#include <map>
#include <string>

// ROS headers
#include <std_srvs/SetBool.h>
#include <std_srvs/Trigger.h>

// Project headers
#include <config_applier.h>
#include <config_store.h>
#include <message_publisher.h>

namespace sbg
{
class SbgDevice
{
private:

  //---------------------------------------------------------------------//
  //- Static members definition                                         -//
  //---------------------------------------------------------------------//

  static std::map<SbgEComMagCalibQuality, std::string>    g_mag_calib_quality_;
  static std::map<SbgEComMagCalibConfidence, std::string> g_mag_calib_confidence_;
  static std::map<SbgEComMagCalibMode, std::string>       g_mag_calib_mode_;
  static std::map<SbgEComMagCalibBandwidth, std::string>  g_mag_calib_bandwidth;

  //---------------------------------------------------------------------//
  //- Private variables                                                 -//
  //---------------------------------------------------------------------//

  SbgEComHandle           m_com_handle_;
  SbgInterface            m_sbg_interface_;
  ros::NodeHandle&        m_ref_node_;
  MessagePublisher        m_message_publisher_;
  ConfigStore             m_config_store_;

  uint32_t                m_rate_frequency_;

  bool                    m_mag_calibration_ongoing_;
  bool                    m_mag_calibration_done_;
  SbgEComMagCalibResults  m_magCalibResults;
  ros::ServiceServer      m_calib_service_;
  ros::ServiceServer      m_calib_save_service_;

  ros::Time               m_ros_processing_time_;

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

  static SbgErrorCode onLogReceivedCallback(SbgEComHandle* p_handle, SbgEComClass msg_class, SbgEComMsgId msg, const SbgBinaryLogData* p_log_data, void* p_user_arg);

  void onLogReceived(SbgEComClass msg_class, SbgEComMsgId msg, const SbgBinaryLogData& ref_sbg_data);

  void loadParameters(void);

  void connect(void);

  void readDeviceInfo(void);

  std::string getVersionAsString(uint32 sbg_version_enc) const;

  void initPublishers(void);

  void configure(void);

  bool processMagCalibration(std_srvs::Trigger::Request& ref_ros_request, std_srvs::Trigger::Response& ref_ros_response);

  bool saveMagCalibration(std_srvs::Trigger::Request& ref_ros_request, std_srvs::Trigger::Response& ref_ros_response);

  bool startMagCalibration(void);

  bool endMagCalibration(void);

  bool uploadMagCalibrationToDevice(void);

  void displayMagCalibrationStatusResult(void) const;

  void exportMagCalibrationResults(void) const;

public:

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

  SbgDevice(ros::NodeHandle& ref_node_handle);

  ~SbgDevice(void);

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

  uint32_t getUpdateFrequency(void) const;

  //---------------------------------------------------------------------//
  //- Public  methods                                                   -//
  //---------------------------------------------------------------------//

  void initDeviceForReceivingData(void);

  void initDeviceForMagCalibration(void);

  void periodicHandle(void);
};
}

#endif // SBG_ROS_SBG_DEVICE_H