message_wrapper.h

Go to the documentation of this file.

#ifndef SBG_ROS_MESSAGE_WRAPPER_H
#define SBG_ROS_MESSAGE_WRAPPER_H

// SbgECom headers
#include <sbgEComLib.h>
#include <sbgEComIds.h>

// Sbg header
#include <sbg_matrix3.h>
#include <config_store.h>

// ROS headers
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PointStamped.h>
#include "ros/ros.h"
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Temperature.h>
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/FluidPressure.h>
#include <sensor_msgs/TimeReference.h>
#include <sensor_msgs/NavSatFix.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

// SbgRos message headers
#include "sbg_driver/SbgStatus.h"
#include "sbg_driver/SbgUtcTime.h"
#include "sbg_driver/SbgImuData.h"
#include "sbg_driver/SbgEkfEuler.h"
#include "sbg_driver/SbgEkfQuat.h"
#include "sbg_driver/SbgEkfNav.h"
#include "sbg_driver/SbgShipMotion.h"
#include "sbg_driver/SbgMag.h"
#include "sbg_driver/SbgMagCalib.h"
#include "sbg_driver/SbgGpsVel.h"
#include "sbg_driver/SbgGpsPos.h"
#include "sbg_driver/SbgGpsHdt.h"
#include "sbg_driver/SbgGpsRaw.h"
#include "sbg_driver/SbgOdoVel.h"
#include "sbg_driver/SbgEvent.h"
#include "sbg_driver/SbgImuShort.h"
#include "sbg_driver/SbgAirData.h"

namespace sbg
{
typedef struct _UTM0
{
        double                  easting;
        double                  northing;
        double                  altitude;
        int                             zone;
} UTM0;

class MessageWrapper
{
private:

  ros::Time                           m_ros_processing_time_;
  sbg_driver::SbgUtcTime              m_last_sbg_utc_;
  bool                                m_first_valid_utc_;
  std::string                         m_frame_id_;
  bool                                m_use_enu_;
  TimeReference                       m_time_reference_;
  UTM0                                                m_utm0_;
  tf2_ros::TransformBroadcaster       m_tf_broadcaster_;
  tf2_ros::StaticTransformBroadcaster m_static_tf_broadcaster_;

  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_;

  //---------------------------------------------------------------------//
  //- Internal methods                                                  -//
  //---------------------------------------------------------------------//

  float wrapAngle2Pi(float angle_rad) const;

  float wrapAngle360(float angle_deg) const;

  double computeMeridian(int zone_number) const;

  const std_msgs::Header createRosHeader(uint32_t device_timestamp) const;

  const ros::Time convertInsTimeToUnix(uint32_t device_timestamp) const;

  const sbg_driver::SbgEkfStatus createEkfStatusMessage(uint32_t ekf_status) const;

  const sbg_driver::SbgGpsPosStatus createGpsPosStatusMessage(const SbgLogGpsPos& ref_log_gps_pos) const;

  const sbg_driver::SbgGpsVelStatus createGpsVelStatusMessage(const SbgLogGpsVel& ref_log_gps_vel) const;

  const sbg_driver::SbgImuStatus createImuStatusMessage(uint16_t sbg_imu_status) const;

  const sbg_driver::SbgMagStatus createMagStatusMessage(const SbgLogMag& ref_log_mag) const;

  const sbg_driver::SbgShipMotionStatus createShipMotionStatusMessage(const SbgLogShipMotionData& ref_log_ship_motion) const;

  const sbg_driver::SbgStatusAiding createStatusAidingMessage(const SbgLogStatusData& ref_log_status) const;

  const sbg_driver::SbgStatusCom createStatusComMessage(const SbgLogStatusData& ref_log_status) const;

  const sbg_driver::SbgStatusGeneral createStatusGeneralMessage(const SbgLogStatusData& ref_log_status) const;

  const sbg_driver::SbgUtcTimeStatus createUtcStatusMessage(const SbgLogUtcData& ref_log_utc) const;

  uint32_t getNumberOfDaysInYear(uint16_t year) const;

  uint32_t getNumberOfDaysInMonth(uint16_t year, uint8_t month_index) const;

  bool isLeapYear(uint16_t year) const;

  const ros::Time convertUtcToUnix(const sbg_driver::SbgUtcTime& ref_sbg_utc_msg) const;

  const sbg_driver::SbgAirDataStatus createAirDataStatusMessage(const SbgLogAirData& ref_sbg_air_data) const;

  const geometry_msgs::TwistStamped createRosTwistStampedMessage(const sbg::SbgVector3f& body_vel, const sbg_driver::SbgImuData& ref_sbg_imu_msg) const;

   void fillTransform(const std::string &ref_parent_frame_id, const std::string &ref_child_frame_id, const geometry_msgs::Pose &ref_pose, geometry_msgs::TransformStamped &ref_transform_stamped);

   char UTMLetterDesignator(double Lat);

   void initUTM(double Lat, double Long, double altitude);

   void LLtoUTM(double Lat, double Long, int zoneNumber, double &UTMNorthing, double &UTMEasting) const;

public:

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

  MessageWrapper(void);

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

  void setTimeReference(TimeReference time_reference);

  void setFrameId(const std::string &frame_id);

  void setUseEnu(bool enu);

   void setOdomEnable(bool odom_enable);

   void setOdomPublishTf(bool publish_tf);

  void setOdomFrameId(const std::string &ref_frame_id);

  void setOdomBaseFrameId(const std::string &ref_frame_id);

  void setOdomInitFrameId(const std::string &ref_frame_id);

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

  const sbg_driver::SbgEkfEuler createSbgEkfEulerMessage(const SbgLogEkfEulerData& ref_log_ekf_euler) const;

  const sbg_driver::SbgEkfNav createSbgEkfNavMessage(const SbgLogEkfNavData& ref_log_ekf_nav) const;

  const sbg_driver::SbgEkfQuat createSbgEkfQuatMessage(const SbgLogEkfQuatData& ref_log_ekf_quat) const;

  const sbg_driver::SbgEvent createSbgEventMessage(const SbgLogEvent& ref_log_event) const;

  const sbg_driver::SbgGpsHdt createSbgGpsHdtMessage(const SbgLogGpsHdt& ref_log_gps_hdt) const;

  const sbg_driver::SbgGpsPos createSbgGpsPosMessage(const SbgLogGpsPos& ref_log_gps_pos) const;

  const sbg_driver::SbgGpsRaw createSbgGpsRawMessage(const SbgLogGpsRaw& ref_log_gps_raw) const;

  const sbg_driver::SbgGpsVel createSbgGpsVelMessage(const SbgLogGpsVel& ref_log_gps_vel) const;

  const sbg_driver::SbgImuData createSbgImuDataMessage(const SbgLogImuData& ref_log_imu_data) const;

  const sbg_driver::SbgMag createSbgMagMessage(const SbgLogMag& ref_log_mag) const;

  const sbg_driver::SbgMagCalib createSbgMagCalibMessage(const SbgLogMagCalib& ref_log_mag_calib) const;

  const sbg_driver::SbgOdoVel createSbgOdoVelMessage(const SbgLogOdometerData& ref_log_odo) const;

  const sbg_driver::SbgShipMotion createSbgShipMotionMessage(const SbgLogShipMotionData& ref_log_ship_motion) const;

  const sbg_driver::SbgStatus createSbgStatusMessage(const SbgLogStatusData& ref_log_status) const;

  const sbg_driver::SbgUtcTime createSbgUtcTimeMessage(const SbgLogUtcData& ref_log_utc);

  const sbg_driver::SbgAirData createSbgAirDataMessage(const SbgLogAirData& ref_air_data_log) const;

  const sbg_driver::SbgImuShort createSbgImuShortMessage(const SbgLogImuShort& ref_short_imu_log) const;

   const sensor_msgs::Imu createRosImuMessage(const sbg_driver::SbgImuData& ref_sbg_imu_msg, const sbg_driver::SbgEkfQuat& ref_sbg_quat_msg) const;

  const nav_msgs::Odometry createRosOdoMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg, const sbg_driver::SbgEkfNav &ref_sbg_ekf_nav_msg, const sbg_driver::SbgEkfQuat &ref_sbg_ekf_quat_msg, const sbg_driver::SbgEkfEuler &ref_sbg_ekf_euler_msg);

  const nav_msgs::Odometry createRosOdoMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg, const sbg_driver::SbgEkfNav &ref_sbg_ekf_nav_msg, const sbg_driver::SbgEkfEuler &ref_sbg_ekf_euler_msg);

  const nav_msgs::Odometry createRosOdoMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg, const sbg_driver::SbgEkfNav &ref_sbg_ekf_nav_msg, const tf2::Quaternion &ref_orientation, const sbg_driver::SbgEkfEuler &ref_sbg_ekf_euler_msg);

  const sensor_msgs::Temperature createRosTemperatureMessage(const sbg_driver::SbgImuData& ref_sbg_imu_msg) const;

  const sensor_msgs::MagneticField createRosMagneticMessage(const sbg_driver::SbgMag& ref_sbg_mag_msg) const;

  const geometry_msgs::TwistStamped createRosTwistStampedMessage(const sbg_driver::SbgEkfEuler& ref_sbg_ekf_vel_msg, const sbg_driver::SbgEkfNav& ref_sbg_ekf_nav_msg, const sbg_driver::SbgImuData& ref_sbg_imu_msg) const;

  const geometry_msgs::TwistStamped createRosTwistStampedMessage(const sbg_driver::SbgEkfQuat& ref_sbg_ekf_vel_msg, const sbg_driver::SbgEkfNav& ref_sbg_ekf_nav_msg, const sbg_driver::SbgImuData& ref_sbg_imu_msg) const;

  const geometry_msgs::PointStamped createRosPointStampedMessage(const sbg_driver::SbgEkfNav& ref_sbg_ekf_msg) const;

  const sensor_msgs::TimeReference createRosUtcTimeReferenceMessage(const sbg_driver::SbgUtcTime& ref_sbg_utc_msg) const;

  const sensor_msgs::NavSatFix createRosNavSatFixMessage(const sbg_driver::SbgGpsPos& ref_sbg_gps_msg) const;

  const sensor_msgs::FluidPressure createRosFluidPressureMessage(const sbg_driver::SbgAirData& ref_sbg_air_msg) const;
};
}

#endif // SBG_ROS_MESSAGE_WRAPPER_H