34 #ifndef SBG_ROS_MESSAGE_WRAPPER_H 35 #define SBG_ROS_MESSAGE_WRAPPER_H 46 #include <geometry_msgs/TwistStamped.h> 47 #include <geometry_msgs/PointStamped.h> 49 #include <sensor_msgs/Imu.h> 50 #include <sensor_msgs/Temperature.h> 51 #include <sensor_msgs/MagneticField.h> 52 #include <sensor_msgs/FluidPressure.h> 53 #include <sensor_msgs/TimeReference.h> 54 #include <sensor_msgs/NavSatFix.h> 58 #include <nav_msgs/Odometry.h> 61 #include "sbg_driver/SbgStatus.h" 62 #include "sbg_driver/SbgUtcTime.h" 63 #include "sbg_driver/SbgImuData.h" 64 #include "sbg_driver/SbgEkfEuler.h" 65 #include "sbg_driver/SbgEkfQuat.h" 66 #include "sbg_driver/SbgEkfNav.h" 67 #include "sbg_driver/SbgShipMotion.h" 68 #include "sbg_driver/SbgMag.h" 69 #include "sbg_driver/SbgMagCalib.h" 70 #include "sbg_driver/SbgGpsVel.h" 71 #include "sbg_driver/SbgGpsPos.h" 72 #include "sbg_driver/SbgGpsHdt.h" 73 #include "sbg_driver/SbgGpsRaw.h" 74 #include "sbg_driver/SbgOdoVel.h" 75 #include "sbg_driver/SbgEvent.h" 76 #include "sbg_driver/SbgImuShort.h" 77 #include "sbg_driver/SbgAirData.h" 122 float wrapAngle2Pi(
float angle_rad)
const;
130 float wrapAngle360(
float angle_deg)
const;
138 double computeMeridian(
int zone_number)
const;
146 const std_msgs::Header createRosHeader(uint32_t device_timestamp)
const;
154 const ros::Time convertInsTimeToUnix(uint32_t device_timestamp)
const;
162 const sbg_driver::SbgEkfStatus createEkfStatusMessage(uint32_t ekf_status)
const;
170 const sbg_driver::SbgGpsPosStatus createGpsPosStatusMessage(
const SbgLogGpsPos& ref_log_gps_pos)
const;
178 const sbg_driver::SbgGpsVelStatus createGpsVelStatusMessage(
const SbgLogGpsVel& ref_log_gps_vel)
const;
186 const sbg_driver::SbgImuStatus createImuStatusMessage(uint16_t sbg_imu_status)
const;
194 const sbg_driver::SbgMagStatus createMagStatusMessage(
const SbgLogMag& ref_log_mag)
const;
202 const sbg_driver::SbgShipMotionStatus createShipMotionStatusMessage(
const SbgLogShipMotionData& ref_log_ship_motion)
const;
210 const sbg_driver::SbgStatusAiding createStatusAidingMessage(
const SbgLogStatusData& ref_log_status)
const;
218 const sbg_driver::SbgStatusCom createStatusComMessage(
const SbgLogStatusData& ref_log_status)
const;
226 const sbg_driver::SbgStatusGeneral createStatusGeneralMessage(
const SbgLogStatusData& ref_log_status)
const;
234 const sbg_driver::SbgUtcTimeStatus createUtcStatusMessage(
const SbgLogUtcData& ref_log_utc)
const;
242 uint32_t getNumberOfDaysInYear(uint16_t year)
const;
251 uint32_t getNumberOfDaysInMonth(uint16_t year, uint8_t month_index)
const;
259 bool isLeapYear(uint16_t year)
const;
267 const ros::Time convertUtcToUnix(
const sbg_driver::SbgUtcTime& ref_sbg_utc_msg)
const;
275 const sbg_driver::SbgAirDataStatus createAirDataStatusMessage(
const SbgLogAirData& ref_sbg_air_data)
const;
284 const geometry_msgs::TwistStamped createRosTwistStampedMessage(
const sbg::SbgVector3f& body_vel,
const sbg_driver::SbgImuData& ref_sbg_imu_msg)
const;
294 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);
302 char UTMLetterDesignator(
double Lat);
311 void initUTM(
double Lat,
double Long,
double altitude);
322 void LLtoUTM(
double Lat,
double Long,
int zoneNumber,
double &UTMNorthing,
double &UTMEasting)
const;
351 void setFrameId(
const std::string &frame_id);
358 void setUseEnu(
bool enu);
365 void setOdomEnable(
bool odom_enable);
372 void setOdomPublishTf(
bool publish_tf);
379 void setOdomFrameId(
const std::string &ref_frame_id);
386 void setOdomBaseFrameId(
const std::string &ref_frame_id);
393 void setOdomInitFrameId(
const std::string &ref_frame_id);
405 const sbg_driver::SbgEkfEuler createSbgEkfEulerMessage(
const SbgLogEkfEulerData& ref_log_ekf_euler)
const;
413 const sbg_driver::SbgEkfNav createSbgEkfNavMessage(
const SbgLogEkfNavData& ref_log_ekf_nav)
const;
421 const sbg_driver::SbgEkfQuat createSbgEkfQuatMessage(
const SbgLogEkfQuatData& ref_log_ekf_quat)
const;
429 const sbg_driver::SbgEvent createSbgEventMessage(
const SbgLogEvent& ref_log_event)
const;
437 const sbg_driver::SbgGpsHdt createSbgGpsHdtMessage(
const SbgLogGpsHdt& ref_log_gps_hdt)
const;
445 const sbg_driver::SbgGpsPos createSbgGpsPosMessage(
const SbgLogGpsPos& ref_log_gps_pos)
const;
453 const sbg_driver::SbgGpsRaw createSbgGpsRawMessage(
const SbgLogGpsRaw& ref_log_gps_raw)
const;
461 const sbg_driver::SbgGpsVel createSbgGpsVelMessage(
const SbgLogGpsVel& ref_log_gps_vel)
const;
469 const sbg_driver::SbgImuData createSbgImuDataMessage(
const SbgLogImuData& ref_log_imu_data)
const;
477 const sbg_driver::SbgMag createSbgMagMessage(
const SbgLogMag& ref_log_mag)
const;
485 const sbg_driver::SbgMagCalib createSbgMagCalibMessage(
const SbgLogMagCalib& ref_log_mag_calib)
const;
493 const sbg_driver::SbgOdoVel createSbgOdoVelMessage(
const SbgLogOdometerData& ref_log_odo)
const;
501 const sbg_driver::SbgShipMotion createSbgShipMotionMessage(
const SbgLogShipMotionData& ref_log_ship_motion)
const;
509 const sbg_driver::SbgStatus createSbgStatusMessage(
const SbgLogStatusData& ref_log_status)
const;
517 const sbg_driver::SbgUtcTime createSbgUtcTimeMessage(
const SbgLogUtcData& ref_log_utc);
525 const sbg_driver::SbgAirData createSbgAirDataMessage(
const SbgLogAirData& ref_air_data_log)
const;
533 const sbg_driver::SbgImuShort createSbgImuShortMessage(
const SbgLogImuShort& ref_short_imu_log)
const;
542 const sensor_msgs::Imu createRosImuMessage(
const sbg_driver::SbgImuData& ref_sbg_imu_msg,
const sbg_driver::SbgEkfQuat& ref_sbg_quat_msg)
const;
553 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);
563 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);
574 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);
582 const sensor_msgs::Temperature createRosTemperatureMessage(
const sbg_driver::SbgImuData& ref_sbg_imu_msg)
const;
590 const sensor_msgs::MagneticField createRosMagneticMessage(
const sbg_driver::SbgMag& ref_sbg_mag_msg)
const;
600 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;
610 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;
618 const geometry_msgs::PointStamped createRosPointStampedMessage(
const sbg_driver::SbgEkfNav& ref_sbg_ekf_msg)
const;
634 const sensor_msgs::NavSatFix createRosNavSatFixMessage(
const sbg_driver::SbgGpsPos& ref_sbg_gps_msg)
const;
642 const sensor_msgs::FluidPressure createRosFluidPressureMessage(
const sbg_driver::SbgAirData& ref_sbg_air_msg)
const;
646 #endif // SBG_ROS_MESSAGE_WRAPPER_H
tf2_ros::TransformBroadcaster m_tf_broadcaster_
sbg_driver::SbgUtcTime m_last_sbg_utc_
std::string m_odom_frame_id_
Defines all sbgECom commands identifiers.
Class to handle the device configuration.
TimeReference m_time_reference_
std::string m_odom_init_frame_id_
ros::Time m_ros_processing_time_
Main header file for the SBG Systems Enhanced Communication Library.
std::string m_odom_base_frame_id_
tf2_ros::StaticTransformBroadcaster m_static_tf_broadcaster_