Program Listing for File message_wrapper.h

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

#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>
#include <sbg_utm.h>

// ROS headers
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/temperature.hpp>
#include <sensor_msgs/msg/magnetic_field.hpp>
#include <sensor_msgs/msg/fluid_pressure.hpp>
#include <sensor_msgs/msg/time_reference.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <nav_msgs/msg/odometry.hpp>
#include <nmea_msgs/msg/sentence.hpp>

// SbgRos message headers
#include "sbg_driver/msg/sbg_status.hpp"
#include "sbg_driver/msg/sbg_utc_time.hpp"
#include "sbg_driver/msg/sbg_imu_data.hpp"
#include "sbg_driver/msg/sbg_ekf_euler.hpp"
#include "sbg_driver/msg/sbg_ekf_quat.hpp"
#include "sbg_driver/msg/sbg_ekf_nav.hpp"
#include "sbg_driver/msg/sbg_ekf_vel_body.hpp"
#include "sbg_driver/msg/sbg_ekf_rot_accel.hpp"
#include "sbg_driver/msg/sbg_ship_motion.hpp"
#include "sbg_driver/msg/sbg_mag.hpp"
#include "sbg_driver/msg/sbg_mag_calib.hpp"
#include "sbg_driver/msg/sbg_gps_vel.hpp"
#include "sbg_driver/msg/sbg_gps_pos.hpp"
#include "sbg_driver/msg/sbg_gps_hdt.hpp"
#include "sbg_driver/msg/sbg_gps_raw.hpp"
#include "sbg_driver/msg/sbg_odo_vel.hpp"
#include "sbg_driver/msg/sbg_event.hpp"
#include "sbg_driver/msg/sbg_imu_short.hpp"
#include "sbg_driver/msg/sbg_air_data.hpp"

namespace sbg
{

class MessageWrapper : public rclcpp::Node
{
private:
  sbg_driver::msg::SbgUtcTime         last_sbg_utc_;
  bool                                first_valid_utc_;
  std::string                         frame_id_;
  bool                                use_enu_;
  TimeReference                       time_reference_;

  bool                                odom_enable_;
  bool                                odom_publish_tf_;
  std::string                         odom_frame_id_;
  std::string                         odom_base_frame_id_;
  std::string                         odom_init_frame_id_;

  Utm                                 utm_{};
  double                              first_valid_easting_{};
  double                              first_valid_northing_{};
  double                              first_valid_altitude_{};

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

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

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

  const rclcpp::Time convertUtcTimeToUnix(const sbg_driver::msg::SbgUtcTime& ref_sbg_utc_msg) const;

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

  const sbg_driver::msg::SbgGpsPosStatus createGpsPosStatusMessage(const SbgEComLogGnssPos& ref_log_gps_pos) const;

  const sbg_driver::msg::SbgGpsVelStatus createGpsVelStatusMessage(const SbgEComLogGnssVel& ref_log_gps_vel) const;

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

  const sbg_driver::msg::SbgMagStatus createMagStatusMessage(const SbgEComLogMag& ref_log_mag) const;

  const sbg_driver::msg::SbgShipMotionStatus createShipMotionStatusMessage(const SbgEComLogShipMotion& ref_log_ship_motion) const;

  const sbg_driver::msg::SbgStatusAiding createStatusAidingMessage(const SbgEComLogStatus& ref_log_status) const;

  const sbg_driver::msg::SbgStatusCom createStatusComMessage(const SbgEComLogStatus& ref_log_status) const;

  const sbg_driver::msg::SbgStatusGeneral createStatusGeneralMessage(const SbgEComLogStatus& ref_log_status) const;

  const sbg_driver::msg::SbgUtcTimeStatus createUtcStatusMessage(const SbgEComLogUtc& ref_log_utc) const;

  const sbg_driver::msg::SbgAirDataStatus createAirDataStatusMessage(const SbgEComLogAirData& ref_sbg_air_data) const;

  const geometry_msgs::msg::TwistStamped createRosTwistStampedMessage(const sbg::SbgVector3f& body_vel, const sbg_driver::msg::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::msg::Pose &ref_pose, geometry_msgs::msg::TransformStamped &ref_transform_stamped);

public:

  //---------------------------------------------------------------------//
  //- Transform broadcasters                                            -//
  //---------------------------------------------------------------------//

  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;

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

  MessageWrapper();

  //---------------------------------------------------------------------//
  //- 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::msg::SbgEkfEuler createSbgEkfEulerMessage(const SbgEComLogEkfEuler& ref_log_ekf_euler) const;

  const sbg_driver::msg::SbgEkfNav createSbgEkfNavMessage(const SbgEComLogEkfNav& ref_log_ekf_nav) const;

  const sbg_driver::msg::SbgEkfQuat createSbgEkfQuatMessage(const SbgEComLogEkfQuat& ref_log_ekf_quat) const;

  const sbg_driver::msg::SbgEkfVelBody createSbgEkfVelBodyMessage(const SbgEComLogEkfVelBody& ref_log_ekf_vel_body) const;

  const sbg_driver::msg::SbgEkfRotAccel createSbgEkfRotAccelMessage(const SbgEComLogEkfRotAccel& ref_log_ekf_rot_accel) const;

  const sbg_driver::msg::SbgEvent createSbgEventMessage(const SbgEComLogEvent& ref_log_event) const;

  const sbg_driver::msg::SbgGpsHdt createSbgGpsHdtMessage(const SbgEComLogGnssHdt& ref_log_gps_hdt) const;

  const sbg_driver::msg::SbgGpsPos createSbgGpsPosMessage(const SbgEComLogGnssPos& ref_log_gps_pos) const;

  const sbg_driver::msg::SbgGpsRaw createSbgGpsRawMessage(const SbgEComLogRawData& ref_log_gps_raw) const;

  const sbg_driver::msg::SbgGpsVel createSbgGpsVelMessage(const SbgEComLogGnssVel& ref_log_gps_vel) const;

  const sbg_driver::msg::SbgImuData createSbgImuDataMessage(const SbgEComLogImuLegacy& ref_log_imu_data) const;

  const sbg_driver::msg::SbgMag createSbgMagMessage(const SbgEComLogMag& ref_log_mag) const;

  const sbg_driver::msg::SbgMagCalib createSbgMagCalibMessage(const SbgEComLogMagCalib& ref_log_mag_calib) const;

  const sbg_driver::msg::SbgOdoVel createSbgOdoVelMessage(const SbgEComLogOdometer& ref_log_odo) const;

  const sbg_driver::msg::SbgShipMotion createSbgShipMotionMessage(const SbgEComLogShipMotion& ref_log_ship_motion) const;

  const sbg_driver::msg::SbgStatus createSbgStatusMessage(const SbgEComLogStatus& ref_log_status) const;

  const sbg_driver::msg::SbgUtcTime createSbgUtcTimeMessage(const SbgEComLogUtc& ref_log_utc);

  const sbg_driver::msg::SbgAirData createSbgAirDataMessage(const SbgEComLogAirData& ref_air_data_log) const;

  const sbg_driver::msg::SbgImuShort createSbgImuShortMessage(const SbgEComLogImuShort& ref_short_imu_log) const;

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

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

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

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

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

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

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

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

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

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

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

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

  const nmea_msgs::msg::Sentence createNmeaGGAMessageForNtrip(const SbgEComLogGnssPos& ref_log_gps_pos) const;
};
}

#endif // SBG_ROS_MESSAGE_WRAPPER_H