diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1835e162c60a8dbca08642663bacc65de5b716f9..c23ca2b96240f5814cb377370c70e6e96e714464 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -35,6 +35,7 @@ add_service_files(
   FILES
   setMode.srv
   getPosition.srv
+  defaultPosition.srv
 #  setComplianceControl.srv
 #  setPIDControl.srv
 #  getPIDControl.srv
diff --git a/cfg/DynamixelPanTilt.cfg b/cfg/DynamixelPanTilt.cfg
index 6b98ab722cbbf72225fd3e9dd15dc1a73c9850cc..6d344ed813776618ea0abd96facd17a79128a6f4 100755
--- a/cfg/DynamixelPanTilt.cfg
+++ b/cfg/DynamixelPanTilt.cfg
@@ -48,8 +48,8 @@ mode_enum = gen.enum([ gen.const("absolute_angle", int_t, 0, "Absolute angle con
 
 gen.add("mode", int_t, SensorLevels.RECONFIGURE_RUNNING, "The control mode which is edited via an enum", 0, 0, 2, edit_method=mode_enum)
 
-gen.add("pan_tilt_configuration_file", str_t, SensorLevels.RECONFIGURE_CLOSE, "The xml configuration file")
-gen.add("server_configuration_file", str_t, SensorLevels.RECONFIGURE_CLOSE, "The xml  configuration file")
+gen.add("pan_tilt_configuration_file", str_t, SensorLevels.RECONFIGURE_CLOSE, "The pant-tilt xml configuration file")
+gen.add("server_configuration_file", str_t, SensorLevels.RECONFIGURE_CLOSE, "The server xml configuration file")
 
 gen.add("pan_tilt_name", str_t, SensorLevels.RECONFIGURE_CLOSE, "The pan&tilt human friendly name", "PAN-TILT-AX-12+")
 gen.add("pan_id", int_t, SensorLevels.RECONFIGURE_CLOSE, "The pan ID", 1, 0, 253)
diff --git a/config/dyn_server_ftdi_config.xml b/config/dyn_server_ftdi_config.xml
index 42f91a261dc88625d0477234d9c7b6b3e411f03a..26b839265ef4b4a4e85ff10a014ebd1818fbd018 100644
--- a/config/dyn_server_ftdi_config.xml
+++ b/config/dyn_server_ftdi_config.xml
@@ -4,5 +4,5 @@
        xsi:noNamespaceSchemaLocation="dyn_server_ftdi_cfg_file.xsd">
 
   <baudrate>1000000</baudrate>
-  <serial_num>AI027ZIZ</serial_num>
+  <serial_num>A4012AD3</serial_num>
 </dyn_server_ftdi_config>
diff --git a/include/dynamixel_pan_tilt_driver_node.h b/include/dynamixel_pan_tilt_driver_node.h
index 6a430baa14a4780c8d102a97af2da8224e42d787..e555813b516b10e51f8e1fb2f52f4aa78eca8a7d 100644
--- a/include/dynamixel_pan_tilt_driver_node.h
+++ b/include/dynamixel_pan_tilt_driver_node.h
@@ -39,6 +39,7 @@
 #include <iri_common_drivers_msgs/setPIDControl.h>
 #include <iri_common_drivers_msgs/setComplianceControl.h>
 #include <iri_dynamixel_pan_tilt/setMode.h>
+#include <iri_dynamixel_pan_tilt/defaultPosition.h>
 
 // [action server client headers]
 #include <iri_action_server/iri_action_server.h>
@@ -139,6 +140,11 @@ class DynamixelPanTiltDriverNode : public iri_base_driver::IriBaseNodeDriver<Dyn
     void set_mode_mutex_enter(void);
     void set_mode_mutex_exit(void);
 
+    ros::ServiceServer default_position_server_;
+    bool default_positionCallback(iri_dynamixel_pan_tilt::defaultPosition::Request &req, iri_dynamixel_pan_tilt::defaultPosition::Response &res);
+    pthread_mutex_t default_position_mutex_;
+    void default_position_mutex_enter(void);
+    void default_position_mutex_exit(void);
 
     // [client attributes]
 
diff --git a/src/dynamixel_pan_tilt_driver.cpp b/src/dynamixel_pan_tilt_driver.cpp
index 5bf19306c0c9c3a00762e889057d00647b0753ec..bad79524e55c6e77cfe831d1017a359e36a7ab8c 100644
--- a/src/dynamixel_pan_tilt_driver.cpp
+++ b/src/dynamixel_pan_tilt_driver.cpp
@@ -76,7 +76,7 @@ bool DynamixelPanTiltDriver::stopDriver(void)
 
 void DynamixelPanTiltDriver::config_update(Config& new_cfg, uint32_t level)
 {
-  this->lock();
+  lock();
   
   // depending on current state
   // update driver with new_cfg data
@@ -98,7 +98,7 @@ void DynamixelPanTiltDriver::config_update(Config& new_cfg, uint32_t level)
   // save the current configuration
   this->mode = (Dynamixel_pan_tilt_mode)new_cfg.mode;
   this->config_=new_cfg;
-  this->unlock();
+  unlock();
 }
 
 DynamixelPanTiltDriver::~DynamixelPanTiltDriver(void)
diff --git a/src/dynamixel_pan_tilt_driver_node.cpp b/src/dynamixel_pan_tilt_driver_node.cpp
index 5e8bb7361fd54a6e0b309780d4c95f96b3220eb6..7318d171463456b697f08056f1a9ac0c0c400e99 100644
--- a/src/dynamixel_pan_tilt_driver_node.cpp
+++ b/src/dynamixel_pan_tilt_driver_node.cpp
@@ -858,6 +858,45 @@ void DynamixelPanTiltDriverNode::set_mode_mutex_exit(void)
   pthread_mutex_unlock(&this->set_mode_mutex_);
 }
 
+bool DynamixelPanTiltDriverNode::default_positionCallback(iri_dynamixel_pan_tilt::defaultPosition::Request &req, iri_dynamixel_pan_tilt::defaultPosition::Response &res)
+{
+  if (ros::ok())
+  {
+    ROS_DEBUG("DynamixelPanTiltDriverNode::default_positionCallback: New Request Received!");
+    try
+    {
+      this->driver_.set_mode((Dynamixel_pan_tilt_mode) 0);
+      Dynamixel_pan_tilt_data angle, sp, pos;
+      angle.pan = req.default_pan;
+      angle.tilt = req.default_tilt;
+      sp.pan = req.pan_speed;
+      sp.tilt = req.tilt_speed;
+      this->driver_.move_absolute_angle(angle, sp);
+      pos = this->driver_.get_position();
+      while (fabs(pos.pan - req.default_pan) > 2 || fabs(pos.tilt - req.default_tilt) > 2)
+        ros::Duration(0.1).sleep();
+      this->driver_.set_mode((Dynamixel_pan_tilt_mode) 0);
+      res.success = true;
+    }
+    catch (CDynamixel_Pan_TiltException &e)
+    {
+      ROS_ERROR("Something wrong happenned while moving to the default position.");
+      res.success = false;
+    }
+  }
+  return true;
+}
+
+void DynamixelPanTiltDriverNode::default_position_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->default_position_mutex_);
+}
+
+void DynamixelPanTiltDriverNode::default_position_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->default_position_mutex_);
+}
+
 
 /*  [action callbacks] */
 void DynamixelPanTiltDriverNode::scanStartCallback(const iri_dynamixel_pan_tilt::scanGoalConstPtr& goal)