message_publisher.cpp
Go to the documentation of this file.
1 #include "message_publisher.h"
2 
4 
8 //---------------------------------------------------------------------//
9 //- Constructor -//
10 //---------------------------------------------------------------------//
11 
12 MessagePublisher::MessagePublisher(void):
13 m_output_mode_(SBG_ECOM_OUTPUT_MODE_DISABLED),
14 m_max_mesages_(10)
15 {
16 
17 }
18 
19 //---------------------------------------------------------------------//
20 //- Private methods -//
21 //---------------------------------------------------------------------//
22 
24 {
25  //
26  // Update the maximal output frequency if needed.
27  //
29  {
30  m_output_mode_ = output_mode;
31  }
32  else
33  {
35  {
36  m_output_mode_ = output_mode;
37  }
38  }
39 
40  //
41  // In case of sbg output event configuration, just define the output on a 25Hz frequency.
42  //
44  {
46  }
47 }
48 
50 {
51  switch (output_mode)
52  {
54  return 0;
55 
57  return 200;
58 
60  return 100;
61 
63  return 50;
64 
66  return 40;
67 
69  return 25;
70 
72  return 20;
73 
75  return 10;
76 
78  return 5;
79 
81  return 1;
82 
84  return 1000;
85 
87  return 10000;
88 
89  default:
90  return 0;
91  }
92 }
93 
94 std::string MessagePublisher::getOutputTopicName(SbgEComMsgId sbg_message_id) const
95 {
96  switch (sbg_message_id)
97  {
99  return "sbg/status";
100 
102  return "sbg/utc_time";
103 
105  return "sbg/imu_data";
106 
107  case SBG_ECOM_LOG_MAG:
108  return "sbg/mag";
109 
111  return "sbg/mag_calib";
112 
114  return "sbg/ekf_euler";
115 
117  return "sbg/ekf_quat";
118 
120  return "sbg/ekf_nav";
121 
123  return "sbg/ship_motion";
124 
126  return "sbg/gps_vel";
127 
129  return "sbg/gps_pos";
130 
132  return "sbg/gps_hdt";
133 
135  return "sbg/gps_raw";
136 
138  return "sbg/odo_vel";
139 
141  return "sbg/eventA";
142 
144  return "sbg/eventB";
145 
147  return "sbg/eventC";
148 
150  return "sbg/eventD";
151 
153  return "sbg/eventE";
154 
156  return "sbg/air_data";
157 
159  return "sbg/imu_short";
160 
161  default:
162  return "undefined";
163  }
164 }
165 
166 void MessagePublisher::initPublisher(ros::NodeHandle& ref_ros_node_handle, SbgEComMsgId sbg_msg_id, SbgEComOutputMode output_conf, const std::string &ref_output_topic)
167 {
168  //
169  // Check if the publisher has to be initialized.
170  //
171  if (output_conf != SBG_ECOM_OUTPUT_MODE_DISABLED)
172  {
173  updateMaxOutputFrequency(output_conf);
174 
175  switch (sbg_msg_id)
176  {
177  case SBG_ECOM_LOG_STATUS:
178  m_sbgStatus_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgStatus>(ref_output_topic, m_max_mesages_);
179  break;
180 
182  m_sbgUtcTime_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgUtcTime>(ref_output_topic, m_max_mesages_);
183  break;
184 
186  m_sbgImuData_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgImuData>(ref_output_topic, m_max_mesages_);
187  break;
188 
189  case SBG_ECOM_LOG_MAG:
190  m_sbgMag_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgMag>(ref_output_topic, m_max_mesages_);
191  break;
192 
194  m_sbgMagCalib_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgMagCalib>(ref_output_topic, m_max_mesages_);
195  break;
196 
198  m_sbgEkfEuler_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgEkfEuler>(ref_output_topic, m_max_mesages_);
199  break;
200 
202  m_sbgEkfQuat_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgEkfQuat>(ref_output_topic, m_max_mesages_);
203  break;
204 
206 
207  m_sbgEkfNav_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgEkfNav>(ref_output_topic, m_max_mesages_);
208  break;
209 
211 
212  m_sbgShipMotion_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgShipMotion>(ref_output_topic, m_max_mesages_);
213  break;
214 
217 
218  m_sbgGpsVel_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgGpsVel>(ref_output_topic, m_max_mesages_);
219  break;
220 
223 
224  m_sbgGpsPos_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgGpsPos>(ref_output_topic, m_max_mesages_);
225  break;
226 
229 
230  m_sbgGpsHdt_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgGpsHdt>(ref_output_topic, m_max_mesages_);
231  break;
232 
235 
236  m_sbgGpsRaw_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgGpsRaw>(ref_output_topic, m_max_mesages_);
237  break;
238 
240 
241  m_sbgOdoVel_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgOdoVel>(ref_output_topic, m_max_mesages_);
242  break;
243 
245 
246  m_sbgEventA_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgEvent>(ref_output_topic, m_max_mesages_);
247  break;
248 
250 
251  m_sbgEventB_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgEvent>(ref_output_topic, m_max_mesages_);
252  break;
253 
255 
256  m_sbgEventC_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgEvent>(ref_output_topic, m_max_mesages_);
257  break;
258 
260 
261  m_sbgEventD_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgEvent>(ref_output_topic, m_max_mesages_);
262  break;
263 
265 
266  m_sbgEventE_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgEvent>(ref_output_topic, m_max_mesages_);
267  break;
268 
270 
271  m_sbgImuShort_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgImuShort>(ref_output_topic, m_max_mesages_);
272  break;
273 
275 
276  m_sbgAirData_pub_ = ref_ros_node_handle.advertise<sbg_driver::SbgAirData>(ref_output_topic, m_max_mesages_);
277  break;
278 
279  default:
280  break;
281  }
282  }
283 }
284 
286 {
288  {
289  m_imu_pub_ = ref_ros_node_handle.advertise<sensor_msgs::Imu>("imu/data", m_max_mesages_);
290  }
291  else
292  {
293  ROS_WARN("SBG_DRIVER - [Publisher] SBG Imu and/or Quat output are not configured, the standard IMU can not be defined.");
294  }
295 
296  if (m_sbgImuData_pub_)
297  {
298  m_temp_pub_ = ref_ros_node_handle.advertise<sensor_msgs::Temperature>("imu/temp", m_max_mesages_);
299  }
300  else
301  {
302  ROS_WARN("SBG_DRIVER - [Publisher] SBG Imu data output are not configured, the standard Temperature publisher can not be defined.");
303  }
304 
305  if (m_sbgMag_pub_)
306  {
307  m_mag_pub_ = ref_ros_node_handle.advertise<sensor_msgs::MagneticField>("imu/mag", m_max_mesages_);
308  }
309  else
310  {
311  ROS_WARN("SBG_DRIVER - [Publisher] SBG Mag data output are not configured, the standard Magnetic publisher can not be defined.");
312  }
313 
314  //
315  // We need either Euler or quat angles, and we must have Nav and IMU data to
316  // compute Body and angular velocity.
317  //
319  {
320  m_velocity_pub_ = ref_ros_node_handle.advertise<geometry_msgs::TwistStamped>("imu/velocity", m_max_mesages_);
321  }
322  else
323  {
324  ROS_WARN("SBG_DRIVER - [Publisher] SBG Imu, Nav or Angles data outputs are not configured, the standard Velocity publisher can not be defined.");
325  }
326 
327  if (m_sbgAirData_pub_)
328  {
329  m_fluid_pub_ = ref_ros_node_handle.advertise<sensor_msgs::FluidPressure>("imu/pres", m_max_mesages_);
330  }
331  else
332  {
333  ROS_WARN("SBG_DRIVER - [Publisher] SBG AirData output are not configured, the standard FluidPressure publisher can not be defined.");
334  }
335 
336  if (m_sbgEkfNav_pub_)
337  {
338  m_pos_ecef_pub_ = ref_ros_node_handle.advertise<geometry_msgs::PointStamped>("imu/pos_ecef", m_max_mesages_);
339  }
340  else
341  {
342  ROS_WARN("SBG_DRIVER - [Publisher] SBG Ekf data output are not configured, the standard ECEF position publisher can not be defined.");
343  }
344 
345  if (m_sbgUtcTime_pub_)
346  {
347  m_utc_reference_pub_ = ref_ros_node_handle.advertise<sensor_msgs::TimeReference>("imu/utc_ref", m_max_mesages_);
348  }
349  else
350  {
351  ROS_WARN("SBG_DRIVER - [Publisher] SBG Utc data output are not configured, the UTC time reference publisher can not be defined.");
352  }
353 
354  if (m_sbgGpsPos_pub_)
355  {
356  m_nav_sat_fix_pub_ = ref_ros_node_handle.advertise<sensor_msgs::NavSatFix>("imu/nav_sat_fix", m_max_mesages_);
357  }
358  else
359  {
360  ROS_WARN("SBG_DRIVER - [Publisher] SBG GPS Pos data output are not configured, the NavSatFix publisher can not be defined.");
361  }
362 }
363 
365 {
366  if (m_sbgImuData_pub_)
367  {
370  }
371  if (m_temp_pub_)
372  {
374  }
375 
378 }
379 
381 {
382  if (m_velocity_pub_)
383  {
384  if (m_sbgEkfQuat_pub_)
385  {
387  }
388  else if (m_sbgEkfEuler_pub_)
389  {
391  }
392  }
393 }
394 
396 {
397  if (m_imu_pub_)
398  {
399  if (m_sbg_imu_message_.time_stamp == m_sbg_ekf_quat_message_.time_stamp)
400  {
402  }
403  }
404 }
405 
407 {
408  sbg_driver::SbgMag sbg_mag_message;
409  sbg_mag_message = m_message_wrapper_.createSbgMagMessage(ref_sbg_log.magData);
410 
411  if (m_sbgMag_pub_)
412  {
413  m_sbgMag_pub_.publish(sbg_mag_message);
414  }
415  if (m_mag_pub_)
416  {
418  }
419 }
420 
422 {
423  sbg_driver::SbgAirData sbg_air_data_message;
424  sbg_air_data_message = m_message_wrapper_.createSbgAirDataMessage(ref_sbg_log.airData);
425 
426  if (m_sbgAirData_pub_)
427  {
428  m_sbgAirData_pub_.publish(sbg_air_data_message);
429  }
430  if (m_fluid_pub_)
431  {
433  }
434 }
435 
437 {
439 
440  if (m_sbgEkfNav_pub_)
441  {
443  }
444  if (m_pos_ecef_pub_)
445  {
447  }
449 }
450 
452 {
453  sbg_driver::SbgUtcTime sbg_utc_message;
454 
455  sbg_utc_message = m_message_wrapper_.createSbgUtcTimeMessage(ref_sbg_log.utcData);
456 
457  if (m_sbgUtcTime_pub_)
458  {
459  m_sbgUtcTime_pub_.publish(sbg_utc_message);
460  }
462  {
463  if (sbg_utc_message.clock_status.clock_utc_status != SBG_ECOM_UTC_INVALID)
464  {
466  }
467  }
468 }
469 
471 {
472  sbg_driver::SbgGpsPos sbg_gps_pos_message;
473 
474  sbg_gps_pos_message = m_message_wrapper_.createSbgGpsPosMessage(ref_sbg_log.gpsPosData);
475 
476  if (m_sbgGpsPos_pub_)
477  {
478  m_sbgGpsPos_pub_.publish(sbg_gps_pos_message);
479  }
480  if (m_nav_sat_fix_pub_)
481  {
483  }
484 }
485 
486 //---------------------------------------------------------------------//
487 //- Parameters -//
488 //---------------------------------------------------------------------//
489 
491 {
493 }
494 
495 //---------------------------------------------------------------------//
496 //- Operations -//
497 //---------------------------------------------------------------------//
498 
499 void MessagePublisher::initPublishers(ros::NodeHandle& ref_ros_node_handle, const ConfigStore &ref_config_store)
500 {
501  //
502  // Initialize all the publishers with the defined SBG output from the config store.
503  //
504  const std::vector<ConfigStore::SbgLogOutput> &ref_output_modes = ref_config_store.getOutputModes();
505 
506  for (const ConfigStore::SbgLogOutput &ref_output : ref_output_modes)
507  {
508  initPublisher(ref_ros_node_handle, ref_output.message_id, ref_output.output_mode, getOutputTopicName(ref_output.message_id));
509  }
510 
511  if (ref_config_store.checkRosStandardMessages())
512  {
513  defineRosStandardPublishers(ref_ros_node_handle);
514  }
515 }
516 
517 void MessagePublisher::publish(const ros::Time& ref_ros_time, SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_id, const SbgBinaryLogData &ref_sbg_log)
518 {
519  //
520  // Publish the message with the corresponding publisher and SBG message ID.
521  // For each log, check if the publisher has been initialized.
522  //
524 
525  if(sbg_msg_class == SBG_ECOM_CLASS_LOG_ECOM_0)
526  {
527  switch (sbg_msg_id)
528  {
529  case SBG_ECOM_LOG_STATUS:
530 
531  if (m_sbgStatus_pub_)
532  {
534  }
535  break;
536 
538 
539  publishUtcData(ref_sbg_log);
540  break;
541 
543 
544  publishIMUData(ref_sbg_log);
545  break;
546 
547  case SBG_ECOM_LOG_MAG:
548 
549  publishMagData(ref_sbg_log);
550  break;
551 
553 
554  if (m_sbgMagCalib_pub_)
555  {
557  }
558  break;
559 
561 
562  if (m_sbgEkfEuler_pub_)
563  {
567  }
568  break;
569 
571 
572  if (m_sbgEkfQuat_pub_)
573  {
578  }
579  break;
580 
582 
583  publishEkfNavigationData(ref_sbg_log);
584  break;
585 
587 
589  {
591  }
592  break;
593 
596 
597  if (m_sbgGpsVel_pub_)
598  {
600  }
601  break;
602 
605 
606  publishGpsPosData(ref_sbg_log);
607  break;
608 
611 
612  if (m_sbgGpsHdt_pub_)
613  {
615  }
616  break;
617 
620 
621  if (m_sbgGpsRaw_pub_)
622  {
624  }
625  break;
626 
628 
629  if (m_sbgOdoVel_pub_)
630  {
632  }
633  break;
634 
636 
637  if (m_sbgEventA_pub_)
638  {
640  }
641  break;
642 
644 
645  if (m_sbgEventB_pub_)
646  {
648  }
649  break;
650 
652 
653  if (m_sbgEventC_pub_)
654  {
656  }
657  break;
658 
660 
661  if (m_sbgEventD_pub_)
662  {
664  }
665  break;
666 
668 
669  if (m_sbgEventE_pub_)
670  {
672  }
673  break;
674 
676 
677  if (m_sbgImuShort_pub_)
678  {
680  }
681  break;
682 
684 
685  publishFluidPressureData(ref_sbg_log);
686  break;
687 
688  default:
689  break;
690  }
691  }
692  else if (sbg_msg_class == SBG_ECOM_CLASS_LOG_ECOM_1)
693  {
694  switch (sbg_msg_id)
695  {
696  default:
697  break;
698  }
699  }
700 }
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
SbgLogImuData imuData
SbgLogEvent eventMarker
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
#define ROS_WARN(...)
SbgLogGpsVel gpsVelData
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_
SbgLogGpsPos gpsPosData
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
SbgLogImuShort imuShort
const sensor_msgs::FluidPressure createRosFluidPressureMessage(const sbg_driver::SbgAirData &ref_sbg_air_msg) const
uint8_t SbgEComMsgId
Definition: sbgEComIds.h:289
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)
SbgLogAirData airData
SbgLogStatusData statusData
SbgLogUtcData utcData
sbg_driver::SbgEkfNav m_sbg_ekf_nav_message_
SbgLogGpsHdt gpsHdtData
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
SbgLogGpsRaw gpsRawData


sbg_driver
Author(s): SBG Systems
autogenerated on Thu Oct 22 2020 03:47:22