diff --git a/src/looming_modbus_driver_node.cpp b/src/looming_modbus_driver_node.cpp
index b475cf147565218e8de7f7055ca1cb84dcb58128..a737f8258b8b01d6fef3284c2f9e5c36fc4fbeb0 100644
--- a/src/looming_modbus_driver_node.cpp
+++ b/src/looming_modbus_driver_node.cpp
@@ -32,7 +32,6 @@ void LoomingModbusDriverNode::mainNodeThread(void)
   this->driver_.lock();
   ROS_DEBUG("LoomingModbusDriverNode::mainNodeThread");
 
-  this->driver_.set_current_position(1.5,2.5,3.5);
   // [fill msg Header if necessary]
 
   // [fill msg structures]
@@ -54,7 +53,7 @@ void LoomingModbusDriverNode::mainNodeThread(void)
     this->driver_.set_success(false);
     this->driver_.set_canceled(false);
     this->driver_.set_navigating(false);
-    this->driver_.set_ready(false);
+    this->driver_.set_ready(true);
   }
   else if(move_base_state==actionlib::SimpleClientGoalState::SUCCEEDED)
   {
@@ -62,7 +61,7 @@ void LoomingModbusDriverNode::mainNodeThread(void)
     this->driver_.set_success(true);
     this->driver_.set_canceled(false);
     this->driver_.set_navigating(false);
-    this->driver_.set_ready(false);
+    this->driver_.set_ready(true);
   }
   else if(move_base_state==actionlib::SimpleClientGoalState::PREEMPTED)
   {
@@ -70,7 +69,7 @@ void LoomingModbusDriverNode::mainNodeThread(void)
     this->driver_.set_success(false);
     this->driver_.set_canceled(true);
     this->driver_.set_navigating(false);
-    this->driver_.set_ready(false);
+    this->driver_.set_ready(true);
   }
   else if(move_base_state==actionlib::SimpleClientGoalState::ACTIVE)
   {
@@ -82,9 +81,6 @@ void LoomingModbusDriverNode::mainNodeThread(void)
   }
   else
   {
-    this->driver_.set_aborted(false);
-    this->driver_.set_success(false);
-    this->driver_.set_canceled(false);
     this->driver_.set_navigating(false);
     this->driver_.set_ready(true);
   }