message_publisher.h

Go to the documentation of this file.

#ifndef SBG_ROS_MESSAGE_PUBLISHER_H
#define SBG_ROS_MESSAGE_PUBLISHER_H

// Project headers
#include <config_store.h>
#include <message_wrapper.h>

namespace sbg
{
class MessagePublisher
{
private:

  ros::Publisher          m_sbgStatus_pub_;
  ros::Publisher          m_sbgUtcTime_pub_;
  ros::Publisher          m_sbgImuData_pub_;
  ros::Publisher          m_sbgEkfEuler_pub_;
  ros::Publisher          m_sbgEkfQuat_pub_;
  ros::Publisher          m_sbgEkfNav_pub_;
  ros::Publisher          m_sbgShipMotion_pub_;
  ros::Publisher          m_sbgMag_pub_;
  ros::Publisher          m_sbgMagCalib_pub_;
  ros::Publisher          m_sbgGpsVel_pub_;
  ros::Publisher          m_sbgGpsPos_pub_;
  ros::Publisher          m_sbgGpsHdt_pub_;
  ros::Publisher          m_sbgGpsRaw_pub_;
  ros::Publisher          m_sbgOdoVel_pub_;
  ros::Publisher          m_sbgEventA_pub_;
  ros::Publisher          m_sbgEventB_pub_;
  ros::Publisher          m_sbgEventC_pub_;
  ros::Publisher          m_sbgEventD_pub_;
  ros::Publisher          m_sbgEventE_pub_;
  ros::Publisher          m_sbgImuShort_pub_;
  ros::Publisher          m_sbgAirData_pub_;

  ros::Publisher          m_imu_pub_;
  sbg_driver::SbgImuData  m_sbg_imu_message_;
  sbg_driver::SbgEkfQuat  m_sbg_ekf_quat_message_;
  sbg_driver::SbgEkfNav   m_sbg_ekf_nav_message_;
  sbg_driver::SbgEkfEuler m_sbg_ekf_euler_message_;

  ros::Publisher          m_temp_pub_;
  ros::Publisher          m_mag_pub_;
  ros::Publisher          m_fluid_pub_;
  ros::Publisher          m_pos_ecef_pub_;
  ros::Publisher          m_velocity_pub_;
  ros::Publisher          m_utc_reference_pub_;
  ros::Publisher          m_nav_sat_fix_pub_;

  MessageWrapper          m_message_wrapper_;
  SbgEComOutputMode       m_output_mode_;
  uint32_t                m_max_mesages_;

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

  void updateMaxOutputFrequency(SbgEComOutputMode output_mode);

  uint32_t getCorrespondingFrequency(SbgEComOutputMode output_mode) const;

  std::string getOutputTopicName(SbgEComMsgId sbg_message_id) const;

  void initPublisher(ros::NodeHandle& ref_ros_node_handle, SbgEComMsgId sbg_msg_id, SbgEComOutputMode output_conf, const std::string &ref_output_topic);

  void defineRosStandardPublishers(ros::NodeHandle& ref_ros_node_handle);

  void publishIMUData(const SbgBinaryLogData &ref_sbg_log);

  void processRosVelMessage(void);

  void processRosImuMessage(void);
  
  void publishMagData(const SbgBinaryLogData &ref_sbg_log);

  void publishFluidPressureData(const SbgBinaryLogData &ref_sbg_log);

  void publishEkfNavigationData(const SbgBinaryLogData &ref_sbg_log);

  void publishUtcData(const SbgBinaryLogData &ref_sbg_log);

  void publishGpsPosData(const SbgBinaryLogData &ref_sbg_log);

public:

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

  MessagePublisher(void);

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

  uint32_t getMaxOutputFrequency(void) const;

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

  void initPublishers(ros::NodeHandle& ref_ros_node_handle, const ConfigStore &ref_config_store);

  void publish(const ros::Time& ref_ros_time, SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_id, const SbgBinaryLogData &ref_sbg_log);
};
}

#endif // SBG_ROS_MESSAGE_PUBLISHER_H