Program Listing for File sbg_device.h

↰ Return to documentation for file (include/sbg_driver/sbg_device.h)

#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/srv/set_bool.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <rtcm_msgs/msg/message.hpp>

// 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_;
  rclcpp::Node&                                             ref_node_;
  MessagePublisher                                          message_publisher_;
  ConfigStore                                               config_store_;

  uint32_t                                                  rate_frequency_;

  bool                                                      mag_calibration_ongoing_;
  bool                                                      mag_calibration_done_;
  SbgEComMagCalibResults                                    mag_calib_results_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr        calib_service_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr        calib_save_service_;

  rclcpp::Subscription<rtcm_msgs::msg::Message>::SharedPtr  rtcm_sub_;

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

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

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

  void loadParameters();

  void connect();

  void readDeviceInfo();

  std::string getVersionAsString(uint32_t sbg_version_enc) const;

  void initPublishers();

  void initSubscribers();

  void configure();

  bool processMagCalibration(const std::shared_ptr<std_srvs::srv::Trigger::Request> ref_ros_request, std::shared_ptr<std_srvs::srv::Trigger::Response> ref_ros_response);

  bool saveMagCalibration(const std::shared_ptr<std_srvs::srv::Trigger::Request> ref_ros_request, std::shared_ptr<std_srvs::srv::Trigger::Response> ref_ros_response);

  bool startMagCalibration();

  bool endMagCalibration();

  bool uploadMagCalibrationToDevice();

  void displayMagCalibrationStatusResult() const;

  void exportMagCalibrationResults() const;

  void writeRtcmMessageToDevice(const rtcm_msgs::msg::Message::SharedPtr msg);

public:

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

  SbgDevice(rclcpp::Node& ref_node_handle);

  ~SbgDevice();

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

  uint32_t getUpdateFrequency() const;

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

  void initDeviceForReceivingData();

  void initDeviceForMagCalibration();

  void periodicHandle();
};
}

#endif // SBG_ROS_SBG_DEVICE_H