diff --git a/src/subscriber_imu_enablable.cpp b/src/subscriber_imu_enablable.cpp
index 69c3bacf80cab7a22bdab5c2f54abab8b0f5ed5a..e9f2f77a8851ded1548ce3557ace9047c2906d67 100644
--- a/src/subscriber_imu_enablable.cpp
+++ b/src/subscriber_imu_enablable.cpp
@@ -27,12 +27,11 @@ void SubscriberImuEnablable::callback(const sensor_msgs::Imu::ConstPtr& msg)
 
 void SubscriberImuEnablable::enableCallback(const std_msgs::Bool::ConstPtr& msg)
 {
-    if (msg->data)
-    {
+    if (!enabled_ and msg->data)
         ROS_INFO("IMU has been enabled!");
-        enabled_ = true;
-        enable_sub_.shutdown();
-    }
+    if (enabled_ and !msg->data)
+        ROS_INFO("IMU has been disabled!");
+    enabled_ = msg->data;
 }
 
 }