12 MessagePublisher::MessagePublisher(
void):
96 switch (sbg_message_id)
102 return "sbg/utc_time";
105 return "sbg/imu_data";
111 return "sbg/mag_calib";
114 return "sbg/ekf_euler";
117 return "sbg/ekf_quat";
120 return "sbg/ekf_nav";
123 return "sbg/ship_motion";
126 return "sbg/gps_vel";
129 return "sbg/gps_pos";
132 return "sbg/gps_hdt";
135 return "sbg/gps_raw";
138 return "sbg/odo_vel";
156 return "sbg/air_data";
159 return "sbg/imu_short";
293 ROS_WARN(
"SBG_DRIVER - [Publisher] SBG Imu and/or Quat output are not configured, the standard IMU can not be defined.");
302 ROS_WARN(
"SBG_DRIVER - [Publisher] SBG Imu data output are not configured, the standard Temperature publisher can not be defined.");
311 ROS_WARN(
"SBG_DRIVER - [Publisher] SBG Mag data output are not configured, the standard Magnetic publisher can not be defined.");
324 ROS_WARN(
"SBG_DRIVER - [Publisher] SBG Imu, Nav or Angles data outputs are not configured, the standard Velocity publisher can not be defined.");
333 ROS_WARN(
"SBG_DRIVER - [Publisher] SBG AirData output are not configured, the standard FluidPressure publisher can not be defined.");
342 ROS_WARN(
"SBG_DRIVER - [Publisher] SBG Ekf data output are not configured, the standard ECEF position publisher can not be defined.");
351 ROS_WARN(
"SBG_DRIVER - [Publisher] SBG Utc data output are not configured, the UTC time reference publisher can not be defined.");
360 ROS_WARN(
"SBG_DRIVER - [Publisher] SBG GPS Pos data output are not configured, the NavSatFix publisher can not be defined.");
408 sbg_driver::SbgMag sbg_mag_message;
423 sbg_driver::SbgAirData sbg_air_data_message;
453 sbg_driver::SbgUtcTime sbg_utc_message;
472 sbg_driver::SbgGpsPos sbg_gps_pos_message;
504 const std::vector<ConfigStore::SbgLogOutput> &ref_output_modes = ref_config_store.
getOutputModes();
ros::Publisher m_sbgGpsVel_pub_
SbgLogEkfNavData ekfNavData
SbgEComOutputMode m_output_mode_
SbgLogEkfEulerData ekfEulerData
SbgLogEkfQuatData ekfQuatData
ros::Publisher m_imu_pub_
const sensor_msgs::NavSatFix createRosNavSatFixMessage(const sbg_driver::SbgGpsPos &ref_sbg_gps_msg) const
ros::Publisher m_pos_ecef_pub_
const sbg_driver::SbgEkfEuler createSbgEkfEulerMessage(const SbgLogEkfEulerData &ref_log_ekf_euler) const
const sensor_msgs::Temperature createRosTemperatureMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg) const
void publish(const boost::shared_ptr< M > &message) const
std::string getOutputTopicName(SbgEComMsgId sbg_message_id) const
const std::vector< SbgLogOutput > & getOutputModes(void) const
void processRosImuMessage(void)
void processRosVelMessage(void)
ros::Publisher m_sbgImuShort_pub_
Manage publishment of messages from logs.
const sensor_msgs::MagneticField createRosMagneticMessage(const sbg_driver::SbgMag &ref_sbg_mag_msg) const
const sbg_driver::SbgShipMotion createSbgShipMotionMessage(const SbgLogShipMotionData &ref_log_ship_motion) const
MessageWrapper m_message_wrapper_
void defineRosStandardPublishers(ros::NodeHandle &ref_ros_node_handle)
const sensor_msgs::Imu createRosImuMessage(const sbg_driver::SbgImuData &ref_sbg_imu_msg, const sbg_driver::SbgEkfQuat &ref_sbg_quat_msg) const
ros::Publisher m_sbgImuData_pub_
void publishIMUData(const SbgBinaryLogData &ref_sbg_log)
ros::Publisher m_sbgEkfQuat_pub_
const sbg_driver::SbgMagCalib createSbgMagCalibMessage(const SbgLogMagCalib &ref_log_mag_calib) const
const sbg_driver::SbgGpsRaw createSbgGpsRawMessage(const SbgLogGpsRaw &ref_log_gps_raw) const
void publishEkfNavigationData(const SbgBinaryLogData &ref_sbg_log)
SbgLogOdometerData odometerData
ros::Publisher m_sbgUtcTime_pub_
void publishMagData(const SbgBinaryLogData &ref_sbg_log)
ros::Publisher m_sbgEventD_pub_
const sbg_driver::SbgImuShort createSbgImuShortMessage(const SbgLogImuShort &ref_short_imu_log) const
const sbg_driver::SbgOdoVel createSbgOdoVelMessage(const SbgLogOdometerData &ref_log_odo) const
const sbg_driver::SbgUtcTime createSbgUtcTimeMessage(const SbgLogUtcData &ref_log_utc)
void initPublisher(ros::NodeHandle &ref_ros_node_handle, SbgEComMsgId sbg_msg_id, SbgEComOutputMode output_conf, const std::string &ref_output_topic)
const sbg_driver::SbgEkfQuat createSbgEkfQuatMessage(const SbgLogEkfQuatData &ref_log_ekf_quat) const
const sbg_driver::SbgStatus createSbgStatusMessage(const SbgLogStatusData &ref_log_status) const
ros::Publisher m_sbgGpsPos_pub_
ros::Publisher m_sbgEkfNav_pub_
ros::Publisher m_sbgEventB_pub_
void publishUtcData(const SbgBinaryLogData &ref_sbg_log)
sbg_driver::SbgEkfQuat m_sbg_ekf_quat_message_
const sbg_driver::SbgGpsHdt createSbgGpsHdtMessage(const SbgLogGpsHdt &ref_log_gps_hdt) const
ros::Publisher m_temp_pub_
const geometry_msgs::TwistStamped createRosTwistStampedMessage(const sbg::SbgVector3f &body_vel, const sbg_driver::SbgImuData &ref_sbg_imu_msg) const
const sbg_driver::SbgGpsPos createSbgGpsPosMessage(const SbgLogGpsPos &ref_log_gps_pos) const
ros::Publisher m_mag_pub_
ros::Publisher m_utc_reference_pub_
ros::Publisher m_sbgGpsRaw_pub_
const sbg_driver::SbgAirData createSbgAirDataMessage(const SbgLogAirData &ref_air_data_log) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void publishGpsPosData(const SbgBinaryLogData &ref_sbg_log)
ros::Publisher m_sbgEventC_pub_
const sbg_driver::SbgImuData createSbgImuDataMessage(const SbgLogImuData &ref_log_imu_data) const
const sbg_driver::SbgEvent createSbgEventMessage(const SbgLogEvent &ref_log_event) const
ros::Publisher m_sbgMagCalib_pub_
ros::Publisher m_sbgEventA_pub_
void setRosProcessingTime(const ros::Time &ref_ros_time)
SbgLogMagCalib magCalibData
const sensor_msgs::FluidPressure createRosFluidPressureMessage(const sbg_driver::SbgAirData &ref_sbg_air_msg) const
ros::Publisher m_sbgGpsHdt_pub_
ros::Publisher m_sbgEventE_pub_
ros::Publisher m_sbgAirData_pub_
const geometry_msgs::PointStamped createRosPointStampedMessage(const sbg_driver::SbgEkfNav &ref_sbg_ekf_msg) const
ros::Publisher m_sbgShipMotion_pub_
void publishFluidPressureData(const SbgBinaryLogData &ref_sbg_log)
void initPublishers(ros::NodeHandle &ref_ros_node_handle, const ConfigStore &ref_config_store)
const sbg_driver::SbgGpsVel createSbgGpsVelMessage(const SbgLogGpsVel &ref_log_gps_vel) const
void updateMaxOutputFrequency(SbgEComOutputMode output_mode)
ros::Publisher m_sbgStatus_pub_
enum _SbgEComClass SbgEComClass
ros::Publisher m_fluid_pub_
ros::Publisher m_nav_sat_fix_pub_
const sbg_driver::SbgMag createSbgMagMessage(const SbgLogMag &ref_log_mag) const
sbg_driver::SbgEkfEuler m_sbg_ekf_euler_message_
SbgLogShipMotionData shipMotionData
const sensor_msgs::TimeReference createRosUtcTimeReferenceMessage(const sbg_driver::SbgUtcTime &ref_sbg_utc_msg) const
const sbg_driver::SbgEkfNav createSbgEkfNavMessage(const SbgLogEkfNavData &ref_log_ekf_nav) const
uint32_t getMaxOutputFrequency(void) const
enum _SbgEComOutputMode SbgEComOutputMode
sbg_driver::SbgImuData m_sbg_imu_message_
ros::Publisher m_sbgOdoVel_pub_
void publish(const ros::Time &ref_ros_time, SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_id, const SbgBinaryLogData &ref_sbg_log)
SbgLogStatusData statusData
sbg_driver::SbgEkfNav m_sbg_ekf_nav_message_
ros::Publisher m_sbgEkfEuler_pub_
uint32_t getCorrespondingFrequency(SbgEComOutputMode output_mode) const
ros::Publisher m_velocity_pub_
ros::Publisher m_sbgMag_pub_
bool checkRosStandardMessages(void) const