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>
#include <rtcm_msgs/Message.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           com_handle_;
  SbgInterface            sbg_interface_;
  ros::NodeHandle&        ref_node_;
  MessagePublisher        message_publisher_;
  ConfigStore             config_store_;
 
  uint32_t                rate_frequency_;
 
  bool                    mag_calibration_ongoing_;
  bool                    mag_calibration_done_;
  SbgEComMagCalibResults  mag_calib_results_;
  ros::ServiceServer      calib_service_;
  ros::ServiceServer      calib_save_service_;
 
  ros::Subscriber         rtcm_sub_;
 
  //---------------------------------------------------------------------//
  //- 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 connect();
 
  void readDeviceInfo();
 
  std::string getVersionAsString(uint32 sbg_version_enc) const;
 
  void initPublishers();
 
  void initSubscribers();
 
  void configure();
 
  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();
 
  bool endMagCalibration();
 
  bool uploadMagCalibrationToDevice();
 
  void displayMagCalibrationStatusResult() const;
 
  void exportMagCalibrationResults() const;
 
  void writeRtcmMessageToDevice(const rtcm_msgs::Message::ConstPtr &msg);
 
public:
 
  //---------------------------------------------------------------------//
  //- Constructor                                                       -//
  //---------------------------------------------------------------------//
 
  SbgDevice(ros::NodeHandle& ref_node_handle);
 
  ~SbgDevice();
 
  //---------------------------------------------------------------------//
  //- Parameters                                                        -//
  //---------------------------------------------------------------------//
 
  uint32_t getUpdateFrequency() const;
 
  //---------------------------------------------------------------------//
  //- Public  methods                                                   -//
  //---------------------------------------------------------------------//
 
  void initDeviceForReceivingData();
 
  void initDeviceForMagCalibration();
 
  void periodicHandle();
};
}
 
#endif // SBG_ROS_SBG_DEVICE_H