Merge pull request #54 from SBG-Systems/node-frequency

#47 Remove the computation of the node rate frequency
SBG-Systems · Aug 25, 2021 · 63297ef · 63297ef
2 parents 83bf0b4 + d7288c5
commit 63297ef
Show file tree

Hide file tree

Showing 10 changed files with 18 additions and 139 deletions.
diff --git a/config/example/ellipse_A_default.yaml b/config/example/ellipse_A_default.yaml
@@ -7,8 +7,8 @@
 # Ellipse-D - Dual-antenna GNSS
 
 driver:
-  # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run)
-  frequency: 0
+  # Node frequency (Hz)
+  frequency: 200
 
 # Configuration of the device with ROS.
 confWithRos: true

diff --git a/config/example/ellipse_E_default.yaml b/config/example/ellipse_E_default.yaml
@@ -7,8 +7,8 @@
 # Ellipse-D - Dual-antenna GNSS
 
 driver:
-  # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run)
-  frequency: 0
+  # Node frequency (Hz)
+  frequency: 200
 
 # Configuration of the device with ROS.
 confWithRos: true
@@ -18,7 +18,7 @@ uartConf:
   portName: "/dev/ttyUSB0"
 
   # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600)
-  baudRate: 921600
+  baudRate: 115200
 
   # Port Id
   # 0 PORT_A: Main communication interface. Full duplex.

diff --git a/config/example/ellipse_N_default.yaml b/config/example/ellipse_N_default.yaml
@@ -7,15 +7,15 @@
 # Ellipse-D - Dual-antenna GNSS
 
 driver:
-  # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run)
-  frequency: 0
+  # Node frequency (Hz)
+  frequency: 200
 
 # Configuration of the device with ROS.
 confWithRos: true
 
 uartConf:
   # Port Name
-  portName: "/dev/sbg"
+  portName: "/dev/ttyUSB0"
 
   # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600)
   baudRate: 115200

diff --git a/config/sbg_device_uart_default.yaml b/config/sbg_device_uart_default.yaml
@@ -1,16 +1,16 @@
 # Configuration file for SBG device through an Uart interface.
 
 driver:
-  # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run)
-  frequency: 0
+  # Node frequency (Hz)
+  frequency: 200
 
 # Configuration of the device with ROS.
 confWithRos: false
 
 # Uart configuration
 uartConf:
   # Port Name
-  portName: "/dev/sbg"
+  portName: "/dev/ttyUSB0"
 
   # Baude rate (4800 ,9600 ,19200 ,38400 ,115200 [default],230400 ,460800 ,921600)
   baudRate: 115200

diff --git a/config/sbg_device_udp_default.yaml b/config/sbg_device_udp_default.yaml
@@ -1,8 +1,8 @@
 # Configuration file for SBG device through an Udp interface.
 
 driver:
-  # Node frequency (Hz) (if set to 0, the node will decide the most appropriate frequency to run)
-  frequency: 0
+  # Node frequency (Hz)
+  frequency: 200
 
 # Configuration of the device with ROS.
 confWithRos: false

diff --git a/include/sbg_driver/message_publisher.h b/include/sbg_driver/message_publisher.h
@@ -82,30 +82,13 @@ class MessagePublisher
   ros::Publisher          m_nav_sat_fix_pub_;
 
   MessageWrapper          m_message_wrapper_;
-  SbgEComOutputMode       m_output_mode_;
   uint32_t                m_max_messages_;
   std::string             m_frame_id_;
 
   //---------------------------------------------------------------------//
   //- Private methods                                                   -//
   //---------------------------------------------------------------------//
 
-  /*!
-   * Update the maximal output frequency for the defined pubishers.
-   * Each time a new publisher is defined, update the maximal output frequency if required.
-   * 
-   * \param[in] output_mode_freq        Output mode.
-   */
-  void updateMaxOutputFrequency(SbgEComOutputMode output_mode);
-
-  /*!
-   * Get the corresponding frequency for the SBG output mode.
-   * 
-   * \param[in] output_mode             Output mode.
-   * \return                            Output frequency (in Hz).
-   */
-  uint32_t getCorrespondingFrequency(SbgEComOutputMode output_mode) const;
-
   /*!
    * Get the corresponding topic name output for the SBG output mode.
    * 
@@ -195,17 +178,6 @@ class MessagePublisher
    */
   MessagePublisher(void);
 
-  //---------------------------------------------------------------------//
-  //- Parameters                                                        -//
-  //---------------------------------------------------------------------//
-
-  /*!
-   * Get the maximal output frequency for the publisher.
-   * 
-   * \return                            Maixmal output frequency (in Hz).
-   */
-  uint32_t getMaxOutputFrequency(void) const;
-
   //---------------------------------------------------------------------//
   //- Operations                                                        -//
   //---------------------------------------------------------------------//

diff --git a/src/config_store.cpp b/src/config_store.cpp
@@ -25,7 +25,7 @@ m_ros_standard_output_(false)
 
 void ConfigStore::loadDriverParameters(const ros::NodeHandle& ref_node_handle)
 {
-  m_rate_frequency_ = getParameter<uint32_t>(ref_node_handle, "driver/frequency", 0);
+  m_rate_frequency_ = getParameter<uint32_t>(ref_node_handle, "driver/frequency", 200);
 
   ref_node_handle.param<std::string>("driver/frameId", m_frame_id_, "imu_link_ned");
 }

diff --git a/src/main.cpp b/src/main.cpp
@@ -9,16 +9,16 @@ int main(int argc, char **argv)
 
   try
   {
-	long unsigned int loopFrequency;
+    long unsigned int loopFrequency;
 
     ROS_INFO("SBG DRIVER - Init node, load params and connect to the device.");
     SbgDevice sbg_device(node_handle);
 
     ROS_INFO("SBG DRIVER - Initialize device for receiving data");
     sbg_device.initDeviceForReceivingData();
 
-	loopFrequency = sbg_device.getUpdateFrequency() * 2;
-    ROS_INFO("SBG DRIVER - Loop update frequency : %lu Hz", loopFrequency);
+    loopFrequency = sbg_device.getUpdateFrequency();
+    ROS_INFO("SBG DRIVER - ROS Node frequency : %lu Hz", loopFrequency);
     ros::Rate loop_rate(loopFrequency);
 
     while (ros::ok())

diff --git a/src/message_publisher.cpp b/src/message_publisher.cpp
@@ -10,87 +10,14 @@ using sbg::MessagePublisher;
 //---------------------------------------------------------------------//
 
 MessagePublisher::MessagePublisher(void):
-m_output_mode_(SBG_ECOM_OUTPUT_MODE_DISABLED),
 m_max_messages_(10)
 {
-
 }
 
 //---------------------------------------------------------------------//
 //- Private methods                                                   -//
 //---------------------------------------------------------------------//
 
-void MessagePublisher::updateMaxOutputFrequency(SbgEComOutputMode output_mode)
-{
-  //
-  // Update the maximal output frequency if needed.
-  //
-  if (m_output_mode_ == SBG_ECOM_OUTPUT_MODE_DISABLED)
-  {
-    m_output_mode_ = output_mode;
-  }
-  else
-  {
-    if (getCorrespondingFrequency(output_mode) > getCorrespondingFrequency(m_output_mode_))
-    {
-      m_output_mode_ = output_mode;
-    }
-  }
-
-  //
-  // In case of sbg output event configuration, just define the output on a 25Hz frequency.
-  //
-  if (getCorrespondingFrequency(m_output_mode_) >= getCorrespondingFrequency(SBG_ECOM_OUTPUT_MODE_PPS))
-  {
-    m_output_mode_ = SBG_ECOM_OUTPUT_MODE_DIV_8;
-  }
-}
-
-uint32_t MessagePublisher::getCorrespondingFrequency(SbgEComOutputMode output_mode) const
-{
-  switch (output_mode)
-  {
-  case SBG_ECOM_OUTPUT_MODE_DISABLED:
-    return 0;
-
-  case SBG_ECOM_OUTPUT_MODE_MAIN_LOOP:
-    return 200;
-
-  case SBG_ECOM_OUTPUT_MODE_DIV_2:
-    return 100;
-
-  case SBG_ECOM_OUTPUT_MODE_DIV_4:
-    return 50;
-
-  case SBG_ECOM_OUTPUT_MODE_DIV_5:
-    return 40;
-
-  case SBG_ECOM_OUTPUT_MODE_DIV_8:
-    return 25;
-
-  case SBG_ECOM_OUTPUT_MODE_DIV_10:
-    return 20;
-
-  case SBG_ECOM_OUTPUT_MODE_DIV_20:
-    return 10;
-
-  case SBG_ECOM_OUTPUT_MODE_DIV_40:
-    return 5;
-
-  case SBG_ECOM_OUTPUT_MODE_DIV_200:
-    return 1;
-
-  case SBG_ECOM_OUTPUT_MODE_HIGH_FREQ_LOOP:
-    return 1000;
-
-  case SBG_ECOM_OUTPUT_MODE_PPS:
-    return 10000;
-
-  default:
-    return 0;
-  }
-}
-
 std::string MessagePublisher::getOutputTopicName(SbgEComMsgId sbg_message_id) const
 {
   switch (sbg_message_id)
@@ -170,7 +97,6 @@ void MessagePublisher::initPublisher(ros::NodeHandle& ref_ros_node_handle, SbgEC
   //
   if (output_conf != SBG_ECOM_OUTPUT_MODE_DISABLED)
   {
-    updateMaxOutputFrequency(output_conf);
 	m_frame_id_ = frame_id;
 
     switch (sbg_msg_id)
@@ -484,15 +410,6 @@ void MessagePublisher::publishGpsPosData(const SbgBinaryLogData &ref_sbg_log)
   }
 }
 
-//---------------------------------------------------------------------//
-//- Parameters                                                        -//
-//---------------------------------------------------------------------//
-
-uint32_t MessagePublisher::getMaxOutputFrequency(void) const
-{
-  return getCorrespondingFrequency(m_output_mode_);
-}
-
 //---------------------------------------------------------------------//
 //- Operations                                                        -//
 //---------------------------------------------------------------------//

diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp
@@ -198,17 +198,7 @@ void SbgDevice::initPublishers(void)
 {
   m_message_publisher_.initPublishers(m_ref_node_, m_config_store_);
 
-  //
-  // Check if the rate frequency has to be defined according to the defined publishers.
-  //
-  if(m_config_store_.getReadingRateFrequency() == 0)
-  {
-    m_rate_frequency_ = m_message_publisher_.getMaxOutputFrequency();
-  }
-  else
-  {
-    m_rate_frequency_ = m_config_store_.getReadingRateFrequency();
-  }
+  m_rate_frequency_ = m_config_store_.getReadingRateFrequency();
 }
 
 void SbgDevice::configure(void)