diff --git a/cfg/LoomingModbus.cfg b/cfg/LoomingModbus.cfg
index a7eff50db392227aa38807ba7b04a476908970a5..b23ceb3e86c148152883826b35e7f2500b0fc105 100755
--- a/cfg/LoomingModbus.cfg
+++ b/cfg/LoomingModbus.cfg
@@ -43,5 +43,7 @@ gen.add("rate",            double_t,  SensorLevels.RECONFIGURE_STOP, "Main loop
 gen.add("modbus_address",  str_t,  SensorLevels.RECONFIGURE_STOP, "Modbus IP address",  "")
 gen.add("modbus_port",     int_t,  SensorLevels.RECONFIGURE_STOP, "Modbus Port",  4444,1,65536)
 gen.add("map_frame",       str_t,  SensorLevels.RECONFIGURE_STOP, "Map frame",  "")
+gen.add("call_mode_change", bool_t,  SensorLevels.RECONFIGURE_STOP, "Call mode change service",  False)
+gen.add("mode",     int_t,  SensorLevels.RECONFIGURE_STOP, "Mode to set on call mode change",  3,0,3)
 
 exit(gen.generate(PACKAGE, "LoomingModbusDriver", "LoomingModbus"))
diff --git a/include/looming_modbus_driver_node.h b/include/looming_modbus_driver_node.h
index ce814d12f0d14038a8a06048b4a659f79a1fb5db..3ab29ff736b47cfb6f266df576d0fefae758c23a 100644
--- a/include/looming_modbus_driver_node.h
+++ b/include/looming_modbus_driver_node.h
@@ -83,6 +83,7 @@ class LoomingModbusDriverNode : public iri_base_driver::IriBaseNodeDriver<Loomin
     void move_baseActive();
     void move_baseFeedback(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
 
+    bool change_mode_call(int mode);
 
    /**
     * \brief post open hook
diff --git a/src/looming_modbus_driver_node.cpp b/src/looming_modbus_driver_node.cpp
index 3116818d84e74ed8013d085f3f8b49b6a1b7998b..6169946b51e9016586679d4eef557b8378acbcea 100644
--- a/src/looming_modbus_driver_node.cpp
+++ b/src/looming_modbus_driver_node.cpp
@@ -229,10 +229,46 @@ void LoomingModbusDriverNode::node_config_update(Config& new_cfg, uint32_t level
   this->driver_.lock();
   if(new_cfg.rate!=this->getRate())
     this->setRate(new_cfg.rate);
+
+  if(new_cfg.call_mode_change)
+  {
+    if(this->change_mode_call(new_cfg.mode))
+      ROS_INFO("LoomingModbusDriverNode:node_config_update: success changing mode");
+    else
+      ROS_ERROR("LoomingModbusDriverNode:node_config_update: error changing mode");
+    new_cfg.call_mode_change=false;
+  }
+
   this->config_=new_cfg;
   this->driver_.unlock();
 }
 
+bool LoomingModbusDriverNode::change_mode_call(int mode)
+{
+  bool ok=false;
+
+  this->mode_change_srv_.request.mode.mode=mode;
+  if(this->mode_change_client_.call(this->mode_change_srv_))
+  {
+    if(this->mode_change_srv_.response.accepted)
+    {
+      ROS_INFO("LoomingModbusDriverNode:: Navigation mode changed successfully");
+      ok=true;
+    }
+    else
+    {
+      ROS_WARN("LoomingModbusDriverNode:: Impossible to change navigation mode");
+      ok=false;
+    }
+  }
+  else
+  {
+    ROS_WARN_STREAM("LoomingModbusDriverNode:: Impossible to change navigation mode (no connection to " << this->mode_change_client_.getService() << ")");
+    ok=false;
+  }
+  return ok;
+}
+
 LoomingModbusDriverNode::~LoomingModbusDriverNode(void)
 {
   // [free dynamic memory]