diff --git a/CMakeLists.txt b/CMakeLists.txt
index e5f20ceda135f093c307fb2817225d484ad53cb7..6cfc6d0ce0ec824e5898807df5c9fa63768c00b4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -5,7 +5,7 @@ project(iri_segway_rmp200)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED)
-find_package(catkin REQUIRED COMPONENTS iri_base_driver iri_segway_rmp_msgs geometry_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver roscpp iri_segway_rmp_msgs geometry_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -79,7 +79,7 @@ generate_dynamic_reconfigure_options(cfg/SegwayRMP200.cfg)
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES 
-  CATKIN_DEPENDS iri_base_driver iri_segway_rmp_msgs
+  CATKIN_DEPENDS iri_base_driver roscpp iri_segway_rmp_msgs
   DEPENDS iriutils comm segway_rmp_200
 )
 
@@ -112,6 +112,7 @@ add_executable(${PROJECT_NAME} src/segway_rmp200_driver.cpp  src/segway_rmp200_d
 # add_dependencies(foo_node foo_generate_messages_cpp)
 
 # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} roscpp_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME} iri_segway_rmp_msgs_generate_messages_cpp)
 
 add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/include/segway_rmp200_driver.h b/include/segway_rmp200_driver.h
index 1ed4f607ffe3b5ed1521b585d0cc355e5f028765..b7b71c151e670731fefdc98ec1a26d8d734e844b 100644
--- a/include/segway_rmp200_driver.h
+++ b/include/segway_rmp200_driver.h
@@ -178,7 +178,7 @@ class SegwayRmp200Driver : public iri_base_driver::IriBaseDriver
     void reset(void);
     void move_platform(float vT, float vR);
     void stop_platform(void);
-    void reset_integrators(void);
+    bool reset_integrators(void);
     const std::list<std::string> & getSegwayEvents() const;
     void setSegwayEvents(void);
     static const std::string & getSegwayEventName(const unsigned int & ev);
diff --git a/include/segway_rmp200_driver_node.h b/include/segway_rmp200_driver_node.h
index def873c68de794de20efefb12f69d919aef581b6..14b5a97e95e3204621f2d909dc6de2075e7723be 100644
--- a/include/segway_rmp200_driver_node.h
+++ b/include/segway_rmp200_driver_node.h
@@ -34,6 +34,7 @@
 // #include <tf/transform_broadcaster.h>
 
 // [service client headers]
+#include <roscpp/Empty.h>
 
 // [action server client headers]
 
@@ -69,6 +70,12 @@ class SegwayRmp200DriverNode : public iri_base_driver::IriBaseNodeDriver<SegwayR
     void cmd_platform_callback(const geometry_msgs::Twist::ConstPtr& msg);
 
     // [service attributes]
+    ros::ServiceServer reset_integrators_server_;
+    bool reset_integratorsCallback(roscpp::Empty::Request &req, roscpp::Empty::Response &res);
+    pthread_mutex_t reset_integrators_mutex_;
+    void reset_integrators_mutex_enter(void);
+    void reset_integrators_mutex_exit(void);
+
 
     // [client attributes]
 
diff --git a/launch/tibi_dabo_segway.launch b/launch/tibi_dabo_segway.launch
index dcf65bdd1ad403120e58688d335bdd72d6e0922d..f5def36222813637783ab19bc75f5b4fbc52fcca 100644
--- a/launch/tibi_dabo_segway.launch
+++ b/launch/tibi_dabo_segway.launch
@@ -15,6 +15,7 @@
     <node pkg ="iri_segway_rmp200"
           type="iri_segway_rmp200"
           name="segway"
+          output="screen"
           machine="nav">
       <param name="velocity_scale_factor" value="1.0"/>
       <param name="acceleration_scale_factor" value="0.25"/>
diff --git a/package.xml b/package.xml
index 91c28a159855a755b0a7156eb14eaefff48f7a8b..183d4e95652bd7947d800960ee8c0b7e0cf3ed9b 100644
--- a/package.xml
+++ b/package.xml
@@ -31,12 +31,14 @@
   <!-- Examples: -->
   <!-- Use build_depend for packages you need at compile time: -->
   <build_depend>iri_base_driver</build_depend>
+  <build_depend>roscpp</build_depend>
   <build_depend>iri_segway_rmp_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <!-- Use buildtool_depend for build tool packages: -->
   <!--   <buildtool_depend>catkin</buildtool_depend> -->
   <!-- Use run_depend for packages you need at runtime: -->
   <run_depend>iri_base_driver</run_depend>
+  <run_depend>roscpp</run_depend>
   <run_depend>iri_segway_rmp_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>
   <!-- Use test_depend for packages you need only for testing: -->
diff --git a/src/segway_rmp200_driver.cpp b/src/segway_rmp200_driver.cpp
index 960e7a79a00046c3658a15752a088a5a49bd0922..339544de753dea2248fcd52726e540dfe332d2a5 100644
--- a/src/segway_rmp200_driver.cpp
+++ b/src/segway_rmp200_driver.cpp
@@ -18,7 +18,7 @@ bool SegwayRmp200Driver::openDriver(void)
     this->lock();
     if(this->serial_number_.compare("NULL") == 0)
     {
-      ROS_DEBUG("No segway serial provided, attempting auto-connect");
+      ROS_WARN("SegwayRmp200Driver::openDriver: No segway serial provided, attempting auto-connect");
       segway_->connect();
       serial_number_ = segway_->get_serial();
     }
@@ -34,7 +34,7 @@ bool SegwayRmp200Driver::openDriver(void)
         serial = ftdi_server_->get_serial_number(ftdi_devs.at(ii));
         if(serial == serial_number_)
         {
-          ROS_DEBUG("Segway serial provided matches FTDI serial list");
+          //ROS_DEBUG("Segway serial provided matches FTDI serial list");
           segway_->connect(serial);
           break;
         }
@@ -54,11 +54,11 @@ bool SegwayRmp200Driver::openDriver(void)
   catch(CException &e)
   {
     this->unlock();
-    ROS_FATAL("'%s'",e.what().c_str());
+    ROS_FATAL("SegwayRmp200Driver::openDriver: '%s'",e.what().c_str());
     return false;
   }
 
-  ROS_INFO("Segway %s successfully connected", serial_number_.c_str());
+  //ROS_INFO("Segway %s successfully connected", serial_number_.c_str());
   return true;
 }
 
@@ -67,7 +67,7 @@ bool SegwayRmp200Driver::closeDriver(void)
   try
   {
     this->lock();
-    ROS_DEBUG("Stopping and closing platform");
+    //ROS_DEBUG("Stopping and closing platform");
     events_.clear();
     segway_->close();
 //     delete segway_;
@@ -76,7 +76,7 @@ bool SegwayRmp200Driver::closeDriver(void)
   }
   catch (CException & e)
   {
-    ROS_ERROR("'%s'", e.what().c_str());
+    ROS_ERROR("SegwayRmp200Driver::closeDriver: '%s'", e.what().c_str());
     return false;
   }
 
@@ -127,7 +127,7 @@ void SegwayRmp200Driver::config_update(Config& new_cfg, uint32_t level)
   }
   catch(CException &e)
   {
-    ROS_ERROR("'%s'", e.what().c_str());
+    ROS_ERROR("SegwayRmp200Driver::config_update: '%s'", e.what().c_str());
   }
 
   // save the current configuration
@@ -146,7 +146,7 @@ SegwayRmp200Driver::~SegwayRmp200Driver()
   }
   catch(CMutexException &e)
   {
-    ROS_ERROR("'%s'", e.what().c_str());
+    ROS_ERROR("SegwayRmp200Driver::destructor: '%s'", e.what().c_str());
   }
 }
 
@@ -385,15 +385,30 @@ void SegwayRmp200Driver::stop_platform(void)
   }
 }
 
-void SegwayRmp200Driver::reset_integrators(void)
+bool SegwayRmp200Driver::reset_integrators(void)
 {
-  if(segway_!=NULL)
+  bool ok=false;
+  if(segway_!=NULL && this->isRunning())
   {
     segway_->reset_right_wheel_integrator();
     segway_->reset_left_wheel_integrator();
     segway_->reset_yaw_integrator();
     segway_->reset_forward_integrator();
+
+    if(segway_->get_right_wheel_displacement()==0 &&
+      segway_->get_left_wheel_displacement() ==0 &&
+      segway_->get_yaw_displacement()        ==0 &&
+      segway_->get_forward_displacement()    ==0 )
+    {
+      ok=true;
+    }
+    else
+      ok=false;
   }
+  else
+    ok=false;
+
+  return ok;
 }
 
 const std::list<std::string> & SegwayRmp200Driver::getSegwayEvents() const
diff --git a/src/segway_rmp200_driver_node.cpp b/src/segway_rmp200_driver_node.cpp
index b7c1c04b1bef616b6df01656ed10e8aa3bda77e7..65eb0f6ef5225de24fdde4c493f0987b9ce0a03a 100644
--- a/src/segway_rmp200_driver_node.cpp
+++ b/src/segway_rmp200_driver_node.cpp
@@ -17,6 +17,9 @@ SegwayRmp200DriverNode::SegwayRmp200DriverNode(ros::NodeHandle &nh) try :
   this->cmd_platform_subscriber_ = this->public_node_handle_.subscribe("cmd_vel", 100, &SegwayRmp200DriverNode::cmd_platform_callback, this);
   
   // [init services]
+  this->reset_integrators_server_ = this->public_node_handle_.advertiseService("reset_integrators", &SegwayRmp200DriverNode::reset_integratorsCallback, this);
+  pthread_mutex_init(&this->reset_integrators_mutex_,NULL);
+
   
   // [init clients]
   
@@ -112,6 +115,41 @@ void SegwayRmp200DriverNode::cmd_platform_callback(const geometry_msgs::Twist::C
 }
 
 /*  [service callbacks] */
+bool SegwayRmp200DriverNode::reset_integratorsCallback(roscpp::Empty::Request &req, roscpp::Empty::Response &res)
+{
+  //ROS_INFO("SegwayRmp200DriverNode::reset_integratorsCallback: New Request Received!");
+
+  //use appropiate mutex to shared variables if necessary
+  this->driver_.lock();
+  //this->reset_integrators_mutex_enter();
+
+  //ROS_INFO("SegwayRmp200DriverNode::reset_integratorsCallback: Processing New Request!");
+  //do operations with req and output on res
+  //res.data2 = req.data1 + my_var;
+  bool ok=false;
+  ok= this->driver_.reset_integrators();
+  if(ok)
+    ROS_INFO("SegwayRmp200DriverNode::reset_integratorsCallback: integrators reset!");
+  else
+    ROS_ERROR("SegwayRmp200DriverNode::reset_integratorsCallback: integrators couldn't reset!");
+
+  //unlock previously blocked shared variables
+  //this->reset_integrators_mutex_exit();
+  this->driver_.unlock();
+
+  return ok;
+}
+
+void SegwayRmp200DriverNode::reset_integrators_mutex_enter(void)
+{
+  pthread_mutex_lock(&this->reset_integrators_mutex_);
+}
+
+void SegwayRmp200DriverNode::reset_integrators_mutex_exit(void)
+{
+  pthread_mutex_unlock(&this->reset_integrators_mutex_);
+}
+
 
 /*  [action callbacks] */
 
@@ -206,6 +244,7 @@ void SegwayRmp200DriverNode::reconfigureNodeHook(int level)
 SegwayRmp200DriverNode::~SegwayRmp200DriverNode()
 {
   //[free dynamic memory]
+  pthread_mutex_destroy(&this->reset_integrators_mutex_);
 }
 
 void SegwayRmp200DriverNode::reset_cmd_vel_watchdog(void)