センサからデータが取得できるかのチェックにタイムアウト機能を追加

include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp

@@ -65,6 +65,7 @@ class RtUsb9axisimuRosDriver
   bool startCommunication();
   void stopCommunication(void);
   void checkDataFormat(const double timeout = 5.0);
+  bool checkReadData(const double timeout = 5.0);
   bool hasAsciiDataFormat(void);
   bool hasBinaryDataFormat(void);
   bool hasRefreshedImuData(void);

src/rt_usb_9axisimu_driver.cpp

@@ -366,6 +366,20 @@ void RtUsb9axisimuRosDriver::checkDataFormat(const double timeout)
   }
 }
 
+bool RtUsb9axisimuRosDriver::checkReadData(const double timeout)
+{
+  const auto start_time = std::chrono::system_clock::now();
+  double time_elapsed = 0.0;
+  while (time_elapsed < timeout) {
+    const auto end_time = std::chrono::system_clock::now();
+    time_elapsed = (double)std::chrono::duration_cast<std::chrono::seconds>(end_time - start_time).count();
+    if (readSensorData() == RtUsb9axisimuRosDriver::ReadStatus::SUCCESS) {
+      return true;
+    }
+  }
+  return false;
+}
+
 bool RtUsb9axisimuRosDriver::hasAsciiDataFormat(void)
 {
   return data_format_ == DataFormat::ASCII;

src/rt_usb_9axisimu_driver_component.cpp

@@ -126,8 +126,8 @@ CallbackReturn Driver::on_activate(const rclcpp_lifecycle::State &)
 {
   RCLCPP_INFO(this->get_logger(), "on_activate() is called.");
 
-  if (driver_->readSensorData() == RtUsb9axisimuRosDriver::ReadStatus::FAILURE) {
-    RCLCPP_ERROR(this->get_logger(), "readSensorData() returns FAILURE, please check your devices.");
+  if (!driver_->checkReadData()) {
+    RCLCPP_ERROR(this->get_logger(), "checkReadData() returns false, please check your devices.");
     return CallbackReturn::ERROR;
   }
   imu_data_raw_pub_->on_activate();