From 8d51d974b9431eb5b8152022ec685172e6750cad Mon Sep 17 00:00:00 2001
From: YusukeKato <yusuke.kato@rt-net.jp>
Date: Wed, 3 Sep 2025 11:28:52 +0900
Subject: [PATCH] =?UTF-8?q?on=5Factivate()=E6=99=82=E3=81=AB=E3=82=BB?=
 =?UTF-8?q?=E3=83=B3=E3=82=B5=E3=81=8B=E3=82=89=E3=83=87=E3=83=BC=E3=82=BF?=
 =?UTF-8?q?=E3=81=8C=E5=8F=96=E5=BE=97=E3=81=A7=E3=81=8D=E3=82=8B=E3=81=8B?=
 =?UTF-8?q?=E3=81=AE=E3=83=81=E3=82=A7=E3=83=83=E3=82=AF=E3=81=AB=E3=82=BF?=
 =?UTF-8?q?=E3=82=A4=E3=83=A0=E3=82=A2=E3=82=A6=E3=83=88=E6=A9=9F=E8=83=BD?=
 =?UTF-8?q?=E3=82=92=E8=BF=BD=E5=8A=A0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../rt_usb_9axisimu_driver.hpp                     |  1 +
 src/rt_usb_9axisimu_driver.cpp                     | 14 ++++++++++++++
 src/rt_usb_9axisimu_driver_component.cpp           |  4 ++--
 3 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp b/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp
index 05a20f8..dbee413 100644
--- a/include/rt_usb_9axisimu_driver/rt_usb_9axisimu_driver.hpp
+++ b/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);
diff --git a/src/rt_usb_9axisimu_driver.cpp b/src/rt_usb_9axisimu_driver.cpp
index 3bb4588..377cfa7 100644
--- a/src/rt_usb_9axisimu_driver.cpp
+++ b/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;
diff --git a/src/rt_usb_9axisimu_driver_component.cpp b/src/rt_usb_9axisimu_driver_component.cpp
index 6a846c6..cebc408 100644
--- a/src/rt_usb_9axisimu_driver_component.cpp
+++ b/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();