diff --git a/src/urg_node2.cpp b/src/urg_node2.cpp
index c39fed4..ff03220 100644
--- a/src/urg_node2.cpp
+++ b/src/urg_node2.cpp
@@ -294,7 +294,22 @@ bool UrgNode2::connect()
 
   // 接続先のLiDARが強度出力に対応しているか
   if (publish_intensity_) {
-    use_intensity_ = is_intensity_supported();
+    error_count_ = 0;
+    while (rclcpp::ok()) {
+      RCLCPP_INFO(get_logger(), "Trying to init intensity mode");
+      if (is_intensity_supported()) {
+        RCLCPP_INFO(get_logger(), "Intensity mode init success");
+        use_intensity_ = true;
+        break;
+      }
+      RCLCPP_WARN(get_logger(), "Failed to init intensity mode");
+      error_count_ += 1;
+      if (error_count_ > error_limit_) {
+        error_count_ = 0;
+        use_intensity_ = false;
+        break;
+      }
+    }
     if (!use_intensity_) {
       RCLCPP_WARN(
         get_logger(),
@@ -303,7 +318,22 @@ bool UrgNode2::connect()
   }
   // 接続先のLiDARがマルチエコー出力に対応しているか
   if (publish_multiecho_) {
-    use_multiecho_ = is_multiecho_supported();
+    error_count_ = 0;
+    while (rclcpp::ok()) {
+      RCLCPP_INFO(get_logger(), "Trying to init multiecho mode");
+      if (is_multiecho_supported()) {
+        RCLCPP_INFO(get_logger(), "Multiecho mode init success");
+        use_multiecho_ = true;
+        break;
+      }
+      RCLCPP_WARN(get_logger(), "Failed to init multiecho mode");
+      error_count_ += 1;
+      if (error_count_ > error_limit_) {
+        error_count_ = 0;
+        use_multiecho_ = false;
+        break;
+      }
+    }
     if (!use_multiecho_) {
       RCLCPP_WARN(
         get_logger(),
@@ -396,6 +426,8 @@ void UrgNode2::disconnect()
     urg_close(&urg_);
     is_connected_ = false;
   }
+  if (is_measurement_started_)
+		is_measurement_started_ = false;
 }
 
 // Lidarとの再接続処理
@@ -782,6 +814,8 @@ bool UrgNode2::is_intensity_supported(void)
   int ret = urg_get_distance_intensity(&urg_, &distance_[0], &intensity_[0], NULL);
   if (ret <= 0) {
     // 強度出力非対応
+    urg_stop_measurement(&urg_);
+    is_measurement_started_ = false;
     return false;
   }
 
@@ -806,6 +840,8 @@ bool UrgNode2::is_multiecho_supported(void)
   int ret = urg_get_multiecho(&urg_, &distance_[0], NULL);
   if (ret <= 0) {
     // マルチエコー出力非対応
+    urg_stop_measurement(&urg_);
+    is_measurement_started_ = false;
     return false;
   }