Read the latest data regardless of the operation frequency

include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp

@@ -44,6 +44,13 @@
 #include "sensor_msgs/msg/magnetic_field.hpp"
 #include "std_msgs/msg/float64.hpp"
 
+enum class ReadStatus
+{
+  SUCCESS = 0,
+  NEED_TO_CONTINUE,
+  FAILURE
+};
+
 class RtUsb9axisimuRosDriver : public rt_usb_9axisimu::SerialPort
 {
 private:
@@ -79,9 +86,9 @@ class RtUsb9axisimuRosDriver : public rt_usb_9axisimu::SerialPort
   // Method to extract binary sensor data from communication buffer
   rt_usb_9axisimu::ImuData<int16_t> extractBinarySensorData(unsigned char * imu_data_buf);
   bool isBinarySensorData(unsigned char * imu_data_buf);
-  bool readBinaryData(void);
+  ReadStatus readBinaryData(void);
   bool isValidAsciiSensorData(std::vector<std::string> imu_data_vector_buf);
-  bool readAsciiData(void);
+  ReadStatus readAsciiData(void);
 
 public:
   explicit RtUsb9axisimuRosDriver(std::string serialport);
@@ -102,7 +109,7 @@ class RtUsb9axisimuRosDriver : public rt_usb_9axisimu::SerialPort
   std::unique_ptr<sensor_msgs::msg::Imu> getImuRawDataUniquePtr(const rclcpp::Time timestamp);
   std::unique_ptr<sensor_msgs::msg::MagneticField> getImuMagUniquePtr(const rclcpp::Time timestamp);
   std::unique_ptr<std_msgs::msg::Float64> getImuTemperatureUniquePtr(void);
-  bool readSensorData();
+  ReadStatus readSensorData();
 };
 
 #endif  // RT_USB_9AXISIMU_DRIVER__RT_USB_9AXISIMU_DRIVER_HPP_

src/rt_usb_9axisimu_driver.cpp

@@ -36,6 +36,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include <iostream>
 
 #include "rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp"
 
@@ -93,7 +94,7 @@ bool RtUsb9axisimuRosDriver::isBinarySensorData(unsigned char * imu_data_buf)
   return false;
 }
 
-bool RtUsb9axisimuRosDriver::readBinaryData(void)
+ReadStatus RtUsb9axisimuRosDriver::readBinaryData(void)
 {
   static std::vector<unsigned char> imu_binary_data_buffer;
   unsigned char read_data_buf[256];
@@ -103,14 +104,14 @@ bool RtUsb9axisimuRosDriver::readBinaryData(void)
     consts.IMU_BIN_DATA_SIZE - imu_binary_data_buffer.size());
 
   if(read_data_size == 0){  // The device was unplugged.
-    return false;
+    return ReadStatus::FAILURE;
   }
 
   if(read_data_size < 0){  // read() returns error code.
-    if(errno == EAGAIN || errno == EWOULDBLOCK){  // Wainting for data.
-      return true;
+    if(errno == EAGAIN || errno == EWOULDBLOCK){  // Waiting for data.
+      return ReadStatus::NEED_TO_CONTINUE;
     }else{
-      return false;
+      return ReadStatus::FAILURE;
     }
   }
 
@@ -119,12 +120,12 @@ bool RtUsb9axisimuRosDriver::readBinaryData(void)
   }
 
   if (imu_binary_data_buffer.size() < consts.IMU_BIN_DATA_SIZE){
-    return true;
+    return ReadStatus::NEED_TO_CONTINUE;
   }
 
   if (isBinarySensorData(imu_binary_data_buffer.data()) == false) {
     imu_binary_data_buffer.clear();
-    return false;
+    return ReadStatus::FAILURE;
   }
 
   auto imu_rawdata = extractBinarySensorData(imu_binary_data_buffer.data());
@@ -134,7 +135,7 @@ bool RtUsb9axisimuRosDriver::readBinaryData(void)
   sensor_data_.convertRawDataUnit();        // Convert raw data to physical quantity
   has_refreshed_imu_data_ = true;
 
-  return true;
+  return ReadStatus::SUCCESS;
 }
 
 bool RtUsb9axisimuRosDriver::isValidAsciiSensorData(std::vector<std::string> str_vector)
@@ -147,7 +148,7 @@ bool RtUsb9axisimuRosDriver::isValidAsciiSensorData(std::vector<std::string> str
   return true;
 }
 
-bool RtUsb9axisimuRosDriver::readAsciiData(void)
+ReadStatus RtUsb9axisimuRosDriver::readAsciiData(void)
 {
   static std::vector<std::string> imu_data_vector_buf;
 
@@ -160,8 +161,16 @@ bool RtUsb9axisimuRosDriver::readAsciiData(void)
 
   int data_size_of_buf = readFromDevice(imu_data_buf, sizeof(imu_data_buf));
 
-  if (data_size_of_buf <= 0) {
-    return false;  // Raise communication error
+  if (data_size_of_buf == 0) {  // The device was unplugged.
+    return ReadStatus::FAILURE;
+  }
+
+  if(data_size_of_buf < 0){  // read() returns error code.
+    if(errno == EAGAIN || errno == EWOULDBLOCK){  // Waiting for data.
+      return ReadStatus::NEED_TO_CONTINUE;
+    }else{
+      return ReadStatus::FAILURE;
+    }
   }
 
   for (int char_count = 0; char_count < data_size_of_buf; char_count++) {
@@ -201,7 +210,7 @@ bool RtUsb9axisimuRosDriver::readAsciiData(void)
     }
   }
 
-  return true;
+  return ReadStatus::SUCCESS;
 }
 
 RtUsb9axisimuRosDriver::RtUsb9axisimuRosDriver(std::string port = "")
@@ -368,13 +377,13 @@ std::unique_ptr<std_msgs::msg::Float64> RtUsb9axisimuRosDriver::getImuTemperatur
 
 // Method to receive IMU data, convert those units to SI, and publish to ROS
 // topic
-bool RtUsb9axisimuRosDriver::readSensorData()
+ReadStatus RtUsb9axisimuRosDriver::readSensorData()
 {
   if (data_format_ == DataFormat::BINARY) {
     return readBinaryData();
   } else if (data_format_ == DataFormat::ASCII) {
     return readAsciiData();
   }
 
-  return false;
+  return ReadStatus::FAILURE;
 }

src/rt_usb_9axisimu_driver_component.cpp

@@ -65,15 +65,30 @@ Driver::Driver(const rclcpp::NodeOptions & options)
 
 void Driver::on_publish_timer()
 {
-  if (driver_->readSensorData()) {
+  std::unique_ptr<sensor_msgs::msg::Imu> raw_imu;
+  std::unique_ptr<sensor_msgs::msg::MagneticField> mag;
+  std::unique_ptr<std_msgs::msg::Float64> temperature;
+
+  // Keep reading to get the latest data
+  auto read_result = ReadStatus::FAILURE;
+  while ((read_result = driver_->readSensorData()) == ReadStatus::SUCCESS) {
     if (driver_->hasRefreshedImuData()) {
-      rclcpp::Time timestamp = this->now();
-      imu_data_raw_pub_->publish(std::move(driver_->getImuRawDataUniquePtr(timestamp)));
-      imu_mag_pub_->publish(std::move(driver_->getImuMagUniquePtr(timestamp)));
-      imu_temperature_pub_->publish(std::move(driver_->getImuTemperatureUniquePtr()));
+      auto timestamp = this->now();
+      raw_imu = driver_->getImuRawDataUniquePtr(timestamp);
+      mag = driver_->getImuMagUniquePtr(timestamp);
+      temperature = driver_->getImuTemperatureUniquePtr();
     }
-  } else {
-    RCLCPP_ERROR(this->get_logger(), "readSensorData() returns false, please check your devices.");
+  }
+
+  if (read_result == ReadStatus::FAILURE) {
+    RCLCPP_ERROR(this->get_logger(), "readSensorData() returns FAILURE, please check your devices.");
+    return;
+  }
+
+  if(raw_imu && mag && temperature) {
+    imu_data_raw_pub_->publish(std::move(raw_imu));
+    imu_mag_pub_->publish(std::move(mag));
+    imu_temperature_pub_->publish(std::move(temperature));
   }
 }
 
@@ -124,8 +139,8 @@ CallbackReturn Driver::on_activate(const rclcpp_lifecycle::State &)
 {
   RCLCPP_INFO(this->get_logger(), "on_activate() is called.");
 
-  if (!driver_->readSensorData()) {
-    RCLCPP_ERROR(this->get_logger(), "readSensorData() returns false, please check your devices.");
+  if (driver_->readSensorData() == ReadStatus::FAILURE) {
+    RCLCPP_ERROR(this->get_logger(), "readSensorData() returns FAILURE, please check your devices.");
     return CallbackReturn::ERROR;
   }
   imu_data_raw_pub_->on_activate();